自动驾驶感知入门：用Python手搓一个UKF滤波器（CTRV模型实战）

自动驾驶感知入门：用Python手搓一个UKF滤波器（CTRV模型实战）

在自动驾驶和机器人定位领域，准确估计车辆或机器人的状态（如位置、速度、方向）至关重要。无迹卡尔曼滤波（UKF）作为一种非线性滤波方法，因其在处理非线性系统时的优异表现而广受欢迎。本文将带您从零开始，用Python实现一个基于CTRV（Constant Turn Rate and Velocity）模型的UKF滤波器，避开复杂的数学推导，专注于代码实现和实际应用。

1. 环境准备与基础知识

在开始之前，确保您已安装以下Python库：

• NumPy：用于矩阵运算
• Matplotlib：用于结果可视化
• SciPy：提供一些数学工具

可以通过以下命令安装这些依赖：

pip install numpy matplotlib scipy

UKF的核心思想是通过一组精心选择的采样点（称为Sigma点）来近似非线性变换后的概率分布。与传统的扩展卡尔曼滤波（EKF）不同，UKF不需要计算雅可比矩阵，这使得它在处理高度非线性系统时更为鲁棒。

提示：虽然本文会尽量简化数学部分，但了解一些基本的概率和线性代数知识会更有帮助。特别是对均值、协方差和矩阵运算的理解。

2. CTRV模型简介

CTRV模型假设车辆以恒定转向率和速度运动，这对于大多数城市驾驶场景是一个合理的近似。模型的状态向量通常包含以下元素：

状态变量	描述
x	车辆x坐标
y	车辆y坐标
v	速度大小
ψ	航向角
ψ̇	转向率

在Python中，我们可以这样定义状态向量：

import numpy as np # 初始状态 [x, y, v, ψ, ψ̇] state = np.array([0.0, 0.0, 5.0, 0.0, 0.1]) # 示例值

CTRV模型的状态转移函数可以表示为：

def ctrv_model(state, dt): x, y, v, psi, psi_dot = state if abs(psi_dot) < 1e-5: # 避免除以零 x_new = x + v * np.cos(psi) * dt y_new = y + v * np.sin(psi) * dt else: x_new = x + (v/psi_dot) * (np.sin(psi + psi_dot*dt) - np.sin(psi)) y_new = y + (v/psi_dot) * (np.cos(psi) - np.cos(psi + psi_dot*dt)) psi_new = psi + psi_dot * dt return np.array([x_new, y_new, v, psi_new, psi_dot])

3. UKF实现步骤

3.1 Sigma点生成

Sigma点是UKF的核心，它们代表了状态分布的统计特性。生成Sigma点的步骤如下：

1. 计算状态协方差矩阵P的平方根
2. 根据状态维度和缩放参数选择Sigma点
3. 为每个Sigma点分配适当的权重

以下是Python实现：

def generate_sigma_points(state, P, alpha=1e-3, kappa=0, beta=2): n = len(state) lambda_ = alpha**2 * (n + kappa) - n # 计算矩阵平方根 sqrt_P = np.linalg.cholesky((n + lambda_) * P) # 生成Sigma点 sigma_points = np.zeros((2*n+1, n)) sigma_points[0] = state for i in range(n): sigma_points[i+1] = state + sqrt_P[i] sigma_points[n+i+1] = state - sqrt_P[i] # 计算权重 Wm = np.full(2*n+1, 1/(2*(n+lambda_))) Wc = np.copy(Wm) Wm[0] = lambda_/(n+lambda_) Wc[0] = Wm[0] + (1 - alpha**2 + beta) return sigma_points, Wm, Wc

3.2 状态预测

使用生成的Sigma点进行状态预测：

def predict(sigma_points, Wm, Wc, Q, dt): # 通过CTRV模型传播每个Sigma点 n_points = sigma_points.shape[0] predicted_points = np.zeros_like(sigma_points) for i in range(n_points): predicted_points[i] = ctrv_model(sigma_points[i], dt) # 计算预测状态和协方差 predicted_state = np.sum(Wm[:, None] * predicted_points, axis=0) residuals = predicted_points - predicted_state predicted_P = (Wc * residuals.T) @ residuals.T + Q return predicted_state, predicted_P, predicted_points

3.3 测量更新

当获得新的测量值时，我们需要更新状态估计：

def update(predicted_state, predicted_P, predicted_points, Wm, Wc, z, R, measurement_function): # 将Sigma点转换到测量空间 n_points = predicted_points.shape[0] measurement_points = np.zeros((n_points, len(z))) for i in range(n_points): measurement_points[i] = measurement_function(predicted_points[i]) # 计算预测测量值和协方差 predicted_z = np.sum(Wm[:, None] * measurement_points, axis=0) residuals_z = measurement_points - predicted_z Pzz = (Wc * residuals_z.T) @ residuals_z.T + R # 计算状态与测量的互协方差 residuals_x = predicted_points - predicted_state Pxz = (Wc * residuals_x.T) @ residuals_z.T # 计算卡尔曼增益 K = Pxz @ np.linalg.inv(Pzz) # 更新状态和协方差 updated_state = predicted_state + K @ (z - predicted_z) updated_P = predicted_P - K @ Pzz @ K.T return updated_state, updated_P

4. 完整实现与可视化

现在我们将所有部分组合起来，创建一个完整的UKF滤波器类：

class UKF: def __init__(self, initial_state, initial_P, Q, R, alpha=1e-3, kappa=0, beta=2): self.state = initial_state self.P = initial_P self.Q = Q self.R = R self.alpha = alpha self.kappa = kappa self.beta = beta self.n = len(initial_state) def predict(self, dt): sigma_points, Wm, Wc = generate_sigma_points(self.state, self.P, self.alpha, self.kappa, self.beta) self.state, self.P, self.predicted_points = predict(sigma_points, Wm, Wc, self.Q, dt) def update(self, z, measurement_function): self.state, self.P = update(self.state, self.P, self.predicted_points, self.Wm, self.Wc, z, self.R, measurement_function)

为了测试我们的UKF实现，我们可以创建一个简单的模拟场景：

import matplotlib.pyplot as plt # 初始化UKF initial_state = np.array([0, 0, 5, 0, 0.1]) # [x, y, v, ψ, ψ̇] initial_P = np.diag([0.1, 0.1, 0.1, 0.1, 0.01]) Q = np.diag([0.01, 0.01, 0.01, 0.01, 0.001]) # 过程噪声 R = np.diag([0.1, 0.1]) # 测量噪声 ukf = UKF(initial_state, initial_P, Q, R) # 模拟参数 dt = 0.1 n_steps = 100 # 存储结果 true_states = [] estimated_states = [] measurements = [] # 模拟循环 true_state = initial_state.copy() for _ in range(n_steps): # 真实状态演化 true_state = ctrv_model(true_state, dt) + np.random.multivariate_normal(np.zeros(5), Q) true_states.append(true_state) # 生成噪声测量 (只测量x和y) z = true_state[:2] + np.random.multivariate_normal(np.zeros(2), R) measurements.append(z) # UKF预测和更新 ukf.predict(dt) ukf.update(z, lambda x: x[:2]) # 测量函数只返回x和y estimated_states.append(ukf.state.copy()) # 转换为数组 true_states = np.array(true_states) estimated_states = np.array(estimated_states) measurements = np.array(measurements) # 绘制结果 plt.figure(figsize=(10, 6)) plt.plot(true_states[:, 0], true_states[:, 1], 'g-', label='真实轨迹') plt.plot(estimated_states[:, 0], estimated_states[:, 1], 'b-', label='UKF估计') plt.plot(measurements[:, 0], measurements[:, 1], 'r.', label='测量值', alpha=0.5) plt.xlabel('X位置') plt.ylabel('Y位置') plt.title('UKF滤波结果对比') plt.legend() plt.grid(True) plt.show()

5. 调参与性能优化

在实际应用中，UKF的性能很大程度上取决于参数的选择。以下是一些调参建议：

• 过程噪声Q：反映模型的不确定性。如果模型预测与实际运动差异较大，可以适当增大Q中的对应元素。
• 测量噪声R：反映传感器的精度。如果传感器噪声较大，应增大R中的值。
• 缩放参数(α, κ, β)：
  • α控制Sigma点的分布范围（通常1e-4 ≤ α ≤ 1）
  • κ是次要缩放参数（通常设为0或3-n）
  • β用于合并先验分布信息（对于高斯分布，β=2是最优的）

注意：UKF对初始状态和协方差的估计比较敏感。如果初始估计偏差太大，滤波器可能需要较长时间才能收敛。

为了评估UKF性能，我们可以计算估计误差：

position_errors = np.sqrt((true_states[:, 0] - estimated_states[:, 0])**2 + (true_states[:, 1] - estimated_states[:, 1])**2) plt.figure(figsize=(10, 4)) plt.plot(position_errors) plt.xlabel('时间步') plt.ylabel('位置误差') plt.title('UKF位置估计误差') plt.grid(True) plt.show()

6. 实际应用中的挑战与解决方案

在实际自动驾驶系统中应用UKF时，会遇到一些额外的挑战：

1. 计算效率：UKF需要生成和传播多个Sigma点，计算量比EKF大。可以通过以下方式优化：

   • 使用高效的矩阵运算库（如NumPy）
   • 在C++等高性能语言中实现核心部分
   • 考虑降维或简化模型

2. 模型失配：CTRV模型假设恒定转向率和速度，这在急加速或制动时可能不成立。解决方案包括：

   • 使用更复杂的模型（如CTRA，包含加速度）
   • 自适应调整过程噪声Q
   • 结合多个模型（交互多模型方法）

3. 传感器融合：UKF可以轻松扩展到多传感器融合。例如，同时处理GPS、IMU和轮速计数据：

def measurement_function(state): # GPS提供x,y # IMU提供航向角ψ # 轮速计提供速度v return np.array([state[0], state[1], state[2], state[3]]) # 更新R矩阵以反映不同传感器的噪声特性 R_fused = np.diag([0.1, 0.1, 0.01, 0.05]) # GPS_x, GPS_y, 轮速计, IMU

4. 异常值处理：实际传感器数据可能包含异常值。可以添加简单的有效性检查：

def update_with_outlier_rejection(self, z, measurement_function, max_mahalanobis=3.0): # 计算马氏距离 predicted_z = np.sum(self.Wm[:, None] * measurement_function(self.predicted_points), axis=0) residuals_z = measurement_function(self.predicted_points) - predicted_z Pzz = (self.Wc * residuals_z.T) @ residuals_z.T + self.R mahalanobis = (z - predicted_z).T @ np.linalg.inv(Pzz) @ (z - predicted_z) if mahalanobis < max_mahalanobis: self.update(z, measurement_function) else: print(f"拒绝异常测量，马氏距离: {mahalanobis:.2f}")

7. 扩展与进阶方向

掌握了基本的UKF实现后，您可以考虑以下进阶方向：

• 交互多模型(IMM)：结合多个运动模型（如CTRV和恒定加速度模型）来提高适应性
• 自适应UKF：动态调整过程噪声Q和测量噪声R
• 分布式UKF：用于多传感器或多智能体系统
• Unscented粒子滤波(UPF)：结合UKF和粒子滤波的优势

对于希望深入研究的读者，以下是一些有用的资源：

• 《Probabilistic Robotics》 by Thrun, Burgard, and Fox
• 《Estimation with Applications to Tracking and Navigation》 by Bar-Shalom, Li, and Kirubarajan
• 开源项目如Kalman and Bayesian Filters in Python（GitHub）

在实际项目中实现UKF时，我发现初始参数的选择对滤波器性能影响很大。一个实用的技巧是从较大的过程噪声开始，然后逐渐减小，这有助于滤波器快速收敛而不失稳定性。另外，定期检查协方差矩阵的正定性也很重要，可以使用np.linalg.cholesky来验证，如果失败则可以考虑添加小的正则化项。