Deadreckoning-航位推算基本原理及代码实现-1

航位推算是一个很常见的定位方法。在知道当前时刻的位置，然后通过imu等传感器去估计下一个时刻的位置。在自动驾驶车辆定位的时候，GPS提供10Hz的定位信息。这每个ＧＰＳ信息来临的0.1s的间隔里面，车辆位置也会移动很多。那么这个时候就需要航位推算来判断车辆到底移动了多少距离，在哪个地方。所以，航位推算是自动驾驶车辆最基本的，也是必须的一种算法之一。比如推算车辆在隧道中的位置。

航位推算英文叫deadreckoning。初看有没有觉得航位推算应该是什么navigationreckoning之类的？为了好理解，我把dead理解为死角，测量不到的点。这样就好理解很多了。理解为推算测量不到的位置。航位推算广泛应用于船舶，飞机，车辆，机器人等领域。各种各样的估计位置的算法，广义来说也是(我认为)一种航位推算。

那么如何推算？

既然要推算，基本的传感器是必不可少的。既然因为因为GPS的数据间隔太长，不能只靠GPS。所以要用IMU。IMU的更新频率是Khz为单位的。比如随便找的ps-imu-3000a的更新频率就是30Khz左右。假如用的是这款IMU，那么在上面120km/h的例子中，每33米更新一次位置-》通过IMU的航位推算，每0.011米更新一次位置。

但是！GPS可以给出位置和姿态，但是IMU不行，所以IMU要通过累积各轴的变化量来判断现在的位置和姿态。

“诶？基于ＩＭＵ的推算这么牛？那不用GPS了，只用IMU做航位推算好了。”

哪有这么好的事情。凡事皆有两面性。基于IMU的推算，由于只靠IMU提供的各轴速度，加速度&各轴的旋转速度，加速度，传感器肯定有微小的误差，会导致推算出来的位置误差会随着时间越发的增大。

比如，现在要推算\phi角。

\phi_{0}：是初始角度。初始值。这东西，要看硬件情况设定，这里暂时设为0。看做已知确定的数。

\dot{\phi}：特定时刻的角速度。从传感器得到。看做已知确定的数。

\eta_{}：特定时刻的角速度测量误差。噪声的分布\simN(0,\sigma^{2})。参数是固定，但是噪声会累积。

推测下一个时间点的\phi值。

\phi_{1}=\phi_{0}+(\dot{\phi_{0}}+\eta_{0})\Deltat(1)

=\phi_{0}+\dot{\phi_{0}}\Deltat+\eta_{0}\Deltatwhere\eta_{0}\Deltat属于传感器误差\simN(0,\sigma^{2}\Deltat^{2})

\phi_{2}=\phi_{1}+(\dot{\phi_{1}}+\eta_{1})\Deltat(2)

=\phi_{1}+\dot{\phi_{1}}\Deltat+\eta_{1}\Deltat

=\phi_{0}+(\dot{\phi_{0}}+\eta_{0})\Deltat+\dot{\phi_{1}}\Deltat+\eta_{1}\Deltat

=\phi_{0}+\dot{\phi_{0}}\Deltat+\eta_{0}\Deltat+\dot{\phi_{1}}\Deltat+\eta_{1}\Deltat

=\phi_{0}+(\dot{\phi_{0}}+\dot{\phi_{1}})\Deltat+(\eta_{0}+\eta_{1})\Deltatwhere(\eta_{0}+\eta_{1})\Deltat是属于累积的误差\simN(0,2\sigma^{2}\Deltat^{2})

同理

\phi_{3}=\phi_{2}+(\dot{\phi_{2}}+\eta_{2})\Deltat(3)

=\phi_{0}+(\dot{\phi_{0}}+\dot{\phi_{1}}+\dot{\phi_{2}})\Deltat+(\eta_{0}+\eta_{1}+\eta_{2})\Deltatwhere(\eta_{0}+\eta_{1}+\eta_{2})\Deltat是属于累积的误差\simN(0,3\sigma^{2}\Deltat^{2})

最终可以看出，出去那些已知的值，噪声项的大小在不断的随着\Deltat累积。也就是说，只用IMU做推算的情况下，推算的误差会随着时间的积累慢慢增长。

从此可以看出，时间一长，IMU不行，短距离推算GPS不行，但是这两个结合呢？

完美。这就是为什么将GPS和IMU一起使用并进行航位推算的原因。

点个赞，关个注啊~　

20191212

下次继续更新~