📚 课程目录第2章转向盘转角传感器信号处理

2. 转向盘转角传感器信号处理

任务定义

转向盘转角传感器（SAS）输出信号通常叠加有测量噪声与轻微抖动，直接影响线控转向系统的路感反馈精度与稳定性控制。本案例设计一个一维卡尔曼滤波器，对含噪的转向盘转角测量值进行实时滤波，输出平滑的转角估计值，并给出可执行的验证方法。

卡尔曼滤波模型

针对单变量转角信号，建立如下离散线性系统模型：

状态方程：x_k = x_k-1 + w_k-1，其中过程噪声 w ~ N(0, Q)
观测方程：z_k = x_k + v_k，其中测量噪声 v ~ N(0, R)

采用匀速转角假设（即转角在相邻采样周期内近似不变），通过卡尔曼滤波递推公式实时估计真实转角。滤波器参数 Q（过程噪声协方差）与 R（测量噪声协方差）需根据传感器特性设定：Q 反映转角真实变化的剧烈程度，R 反映传感器噪声水平。

代码实现


import numpy as np
import matplotlib.pyplot as plt

class KalmanFilter1D:
    """一维卡尔曼滤波器，用于转向盘转角信号平滑"""
    def __init__(self, Q=0.01, R=1.0):
        self.Q = Q          # 过程噪声协方差
        self.R = R          # 测量噪声协方差
        self.x = 0.0        # 状态估计值（转角）
        self.P = 1.0        # 误差协方差

    def update(self, z):
        """输入测量值z，返回滤波后的转角估计"""
        # 预测步骤
        x_pred = self.x
        P_pred = self.P + self.Q

        # 更新步骤
        K = P_pred / (P_pred + self.R)          # 卡尔曼增益
        self.x = x_pred + K * (z - x_pred)      # 状态更新
        self.P = (1 - K) * P_pred               # 协方差更新
        return self.x

def generate_noisy_signal(true_angles, noise_std=2.0, seed=42):
    """生成含高斯噪声的转角测量信号"""
    rng = np.random.default_rng(seed)
    noise = rng.normal(0, noise_std, size=len(true_angles))
    return true_angles + noise

if __name__ == "__main__":
    # 生成仿真数据：模拟转向盘正弦摆动 + 阶跃变化
    t = np.linspace(0, 10, 500)
    true_angle = 90 * np.sin(0.5 * t) + 30 * (t > 5)   # 真实转角

    # 添加测量噪声（标准差2°）
    measured = generate_noisy_signal(true_angle, noise_std=2.0)

    # 初始化卡尔曼滤波器
    kf = KalmanFilter1D(Q=0.01, R=4.0)   # R=σ²=4

    # 逐点滤波
    filtered = []
    for z in measured:
        est = kf.update(z)
        filtered.append(est)

    # 计算滤波前后误差（与真实值对比）
    meas_error = np.mean((measured - true_angle) ** 2)
    filt_error = np.mean((np.array(filtered) - true_angle) ** 2)

    print(f"测量信号均方误差: {meas_error:.4f} deg²")
    print(f"滤波信号均方误差: {filt_error:.4f} deg²")

    # 绘图对比
    plt.figure(figsize=(10, 5))
    plt.plot(t, true_angle, 'k-', linewidth=2, label='真实转角')
    plt.plot(t, measured, 'r.', markersize=2, alpha=0.5, label='含噪测量')
    plt.plot(t, filtered, 'b-', linewidth=1.5, label='卡尔曼滤波输出')
    plt.xlabel('时间 (s)')
    plt.ylabel('转向盘转角 (deg)')
    plt.legend()
    plt.grid(True)
    plt.title('转向盘转角卡尔曼滤波效果')
    plt.savefig('steering_angle_kalman.png', dpi=100)
    plt.show()

落地与运行方式

本案例代码为独立Python脚本，依赖NumPy和Matplotlib库。运行前需安装依赖：


pip install numpy matplotlib

将上述代码保存为 steering_kalman.py，在终端执行：


python steering_kalman.py

验证方法与判定标准

运行脚本后，控制台输出两组均方误差（MSE）数值，并弹出对比图。判定滤波有效的标准为：

滤波后MSE显著小于测量MSE（通常降低50%以上）；
图中蓝色滤波曲线明显比红色测量点平滑，且能跟随黑色真实转角曲线的趋势变化，无显著滞后或发散。

由于代码中设置了随机种子（seed=42），每次运行结果可复现。上述MSE数值为确定性输出，可直接用于验证。