自动驾驶感知入门用Python手搓一个UKF滤波器CTRV模型实战在自动驾驶和机器人定位领域准确估计车辆或机器人的状态如位置、速度、方向至关重要。无迹卡尔曼滤波UKF作为一种非线性滤波方法因其在处理非线性系统时的优异表现而广受欢迎。本文将带您从零开始用Python实现一个基于CTRVConstant Turn Rate and Velocity模型的UKF滤波器避开复杂的数学推导专注于代码实现和实际应用。1. 环境准备与基础知识在开始之前确保您已安装以下Python库NumPy用于矩阵运算Matplotlib用于结果可视化SciPy提供一些数学工具可以通过以下命令安装这些依赖pip install numpy matplotlib scipyUKF的核心思想是通过一组精心选择的采样点称为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点的步骤如下计算状态协方差矩阵P的平方根根据状态维度和缩放参数选择Sigma点为每个Sigma点分配适当的权重以下是Python实现def generate_sigma_points(state, P, alpha1e-3, kappa0, beta2): n len(state) lambda_ alpha**2 * (n kappa) - n # 计算矩阵平方根 sqrt_P np.linalg.cholesky((n lambda_) * P) # 生成Sigma点 sigma_points np.zeros((2*n1, n)) sigma_points[0] state for i in range(n): sigma_points[i1] state sqrt_P[i] sigma_points[ni1] state - sqrt_P[i] # 计算权重 Wm np.full(2*n1, 1/(2*(nlambda_))) Wc np.copy(Wm) Wm[0] lambda_/(nlambda_) Wc[0] Wm[0] (1 - alpha**2 beta) return sigma_points, Wm, Wc3.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, axis0) residuals predicted_points - predicted_state predicted_P (Wc * residuals.T) residuals.T Q return predicted_state, predicted_P, predicted_points3.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, axis0) 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_P4. 完整实现与可视化现在我们将所有部分组合起来创建一个完整的UKF滤波器类class UKF: def __init__(self, initial_state, initial_P, Q, R, alpha1e-3, kappa0, beta2): 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-, labelUKF估计) plt.plot(measurements[:, 0], measurements[:, 1], r., label测量值, alpha0.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时会遇到一些额外的挑战计算效率UKF需要生成和传播多个Sigma点计算量比EKF大。可以通过以下方式优化使用高效的矩阵运算库如NumPy在C等高性能语言中实现核心部分考虑降维或简化模型模型失配CTRV模型假设恒定转向率和速度这在急加速或制动时可能不成立。解决方案包括使用更复杂的模型如CTRA包含加速度自适应调整过程噪声Q结合多个模型交互多模型方法传感器融合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异常值处理实际传感器数据可能包含异常值。可以添加简单的有效性检查def update_with_outlier_rejection(self, z, measurement_function, max_mahalanobis3.0): # 计算马氏距离 predicted_z np.sum(self.Wm[:, None] * measurement_function(self.predicted_points), axis0) 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 PythonGitHub在实际项目中实现UKF时我发现初始参数的选择对滤波器性能影响很大。一个实用的技巧是从较大的过程噪声开始然后逐渐减小这有助于滤波器快速收敛而不失稳定性。另外定期检查协方差矩阵的正定性也很重要可以使用np.linalg.cholesky来验证如果失败则可以考虑添加小的正则化项。