【原】《航机推算：从起点到未知，如何用数学 “走” 出精准轨迹？（一）》

SLAM之路 2025-05-06

展开全文

什么是航迹推算

What

航迹推算（Dead Reckoning/DR），也被称为航位推算，是一种通过已知的初始位置、运动速度、运动方向和时间来推算物体当前位置的方法。

自动驾驶车辆上，常用于航迹推算的传感器有：轮速编码器、方向盘、imu(6轴即陀螺仪+加速度计）等；

根据功能场景/算力需求等，常见的航迹推算方案如下：

方案	传感器依赖	难度	实现方案
1	轮速编码器	易	轮速差确定方向，轮速确定长度；
2	轮速编码器方向盘	易	方向盘转角确定方向轮速确定长度
3	陀螺仪z轴轮速编码器	中	陀螺仪z轴确定方向yaw 轮速编码器确定长度
4	陀螺仪轮速编码器	中	陀螺仪z轴确定方向rpy 轮速编码器确定长度
5	陀螺仪加速度计轮速编码器	难	ESKF(陀螺仪rpy预测，加计用作重力观测) 轮速编码器确定长度
6	陀螺仪加速度计轮速编码器	难	ESKF (陀螺仪rpy+加计预测，轮速观测）采用IMU运动学模型预测轨迹，轮速作为对于车辆X方向上速度的观测，对预测的轨迹进行修正；

为什么用航迹推算

提供连续的位置估计

通过航迹推算，不必依赖外界输入，保证即使在卫星导航信号暂时丢失或不准确的情况下，也能为车辆提供连续的位置估计，保证自动驾驶系统的正常运行。

预测车辆运动趋势

根据当前的运动状态和历史数据，航迹推算可以预测车辆未来的位置和姿态，帮助自动驾驶系统进行路径规划、决策制定和避障等操作

方便计算

基于全局坐标系下的计算（涉及经纬度或大地坐标系）通常数值巨大或者不易计算，将车辆历史帧和当前帧转换至dr坐标系通常是更优的选择；

航迹推算方案-1

1、坐标系转换

以t1时刻坐标系为世界系，计算任意时刻相对世界系的姿态，即可确定任意时刻的里程计；

1、已知初始时刻t1姿态为单位矩阵I；

2、根据轮速编码器，可以确定任意相邻两帧之间的姿态变化Ti,i+1

3、因此可知，Ti+1 = Ti * Ti,i+1

4、最后可知，Ti+1 = T1 * T12 * T23 …… * Ti-1,i * Ti,i+1

由于轮速脉冲编码器的频率较高，通常为50hz或100hz，可以进行离散积分。

2、计算相邻时刻姿态

假设n-1时刻和n时刻，后轮走过的位置如图所示，则 Δs 即为左右轮的距离差, L为两个后轮中心线之间的距离；

则可知，n-1时刻到n时刻，Δyaw = ΔS/L；ΔS= 两个后轮平均移动距离；

以上是近似计算公式：当Δyaw趋近于0时，sin(Δyaw)≈Δyaw

因此帧间位移 delta_X = ΔS * sin(Δyaw)/Δyaw;

因此帧间位移 delta_Y = ΔS *(1-cos(Δyaw))/Δyaw;

// 左后右后轮相邻时刻编码器计数变化量uint64_t delta_pulse_rear_left = 4;uint64_t delta_pulse_rear_right = 1;// 单个编码器计数对应的距离，使用前需要标定double coeff_per_pulse = 0.022;double delta_S = ( delta_pulse_rear_right + delta_pulse_rear_left)*coeff_per_pulse/2.0// 轮中心间距double wheelBase = 1.62;double delta_yaw = (delta_pulse_rear_left - delta_pulse_rear_right)/wheelbase;/// 计算相邻两个时刻旋转变换；roll/pitch视为0；Eigen::Quaterniond q = Eigen::Quaterniond(1, 0.0, 0.0, delta_yaw/2);/// 计算相邻两帧平移；double delta_X = delta_S * sin(delta_yaw)/delta_yaw;double delta_Y = delta_S *(1-cos(delta_yaw))/delta_yaw;Eigen::Vector3d delta_trans = Eigen::Vector3d(delta_X, delta_Y, 0);