智能驾驶中的惯性导航与融合定位(一)：惯性导航实现
前言
知乎是很棒的知识分享平台，我个人从中学习了很多。最近工作中有涉及惯性导航和多传感器融合定位相关的内容，特此开个系列文章记录总结这部分工作并分享给大家。
写这个专栏的目的主要有三点：
- 知识升华：对一个知识点的认识，从看懂到功能实现再到讲清楚，是完全不同的境界，希望通过这个专栏进行知识升华；
- 知识分享：知乎很多大佬的文章对我有很大的帮助，解决了我的诸多疑惑，我也很期望自己能够分享给大家有用的知识和技巧；
- 督促自己：我是个很懒的人，但是工作越久越觉得知识总结的重要性，知乎是个开放的平台，我所写的东西会被大家看到，希望借此督促自己持续高质量的交付。
系列的文章我会更侧重于代码实现，帮助解决工程实际问题。
第一篇文章我们从惯性导航开始。会包括两部分内容，先简述惯性导航的基本原理，然后着重进行代码的实现与讲解。希望读者在阅读前对惯性导航的原理有一定了解，推荐秦永元老师的惯性导航一书。
一、惯性导航的原理
- 常用坐标系定义
- 地心惯性坐标系i：在空间保持静止或匀速直线运动(无加速度)的坐标系称为惯性坐标系。惯性导航中的惯性参考系一般选用地心惯性坐标系，其以地球质心为坐标系原点，z轴沿着地球自转轴指向协议地极，x轴位于赤道平面内,并指向春分点;
- 地心地固坐标系： 地心地固坐标系与地心惯性坐标系有着相同的坐标原点和坐标轴轴定义,但是地心地固坐标系与地球保持同步旋转(所以命名为地固)。
- 地球坐标系e：地球坐标系可认为是地心地固坐标系的极坐标表示，最常用的是GPS系统采用的WGS-84坐标系，地球上的任一点通过经度纬度和高度表达；
- 地理坐标系/导航坐标系n：一般取载体所在位置的东北天或者北东地坐标系；
- 载体坐标系b：固联在载体上，原点在重心，一般取右前上或者前右下坐标系。
惯性器件测到的惯性量是惯性器件固联的坐标系相对惯性坐标系的值在固联坐标系的表达，读者可能会注意到地心惯性坐标系并不完全符合惯性坐标系的定义，因为地球还在围绕太阳转，太阳还在银河系中运动，这里将这些运动小量忽略。
2、惯性导航系统分类
惯性导航系统按照有无物理平台可分为：
- 平台式惯导系统：早期的惯导实现方式，惯导系统中有一个真实的物理稳定平台，惯性敏感器(陀螺仪和加速度计)安装在用三个环架支承的平台台体上，通过控制平台的旋转使其跟踪导航坐标系（如东北天或北东地坐标系），这样加速度计的敏感轴始终与要求的导航坐标系一致，测量到的加速度是导航坐标系n相对惯性坐标系i的结果 a_{in}^{n} ,同时载体姿态角可以从平台直接读取。
- 捷联式惯导系统：由于平台式惯导系统需要维持一个精密的真实物理平台，生产、使用、维护都比较困难，捷联式惯导系统指的是将惯性敏感器(陀螺仪和加速度计)直接固联在载体上，测量到的加速度和加速度是载体坐标系b相对惯性坐标系i的结果 a_{ib}^{b} 和 \omega_{ib}^{b} ,经陀螺仪估算出载体姿态后，通过数学计算的方式将载体系测量到的惯性数据转换到导航坐标系中，构建了虚拟的“数字平台”。
简单来说，平台式的惯导系统包含一个稳定平台，该平台能够通过一定的方式隔绝载体运动，使得惯性器件始终敏感导航坐标系的结果，方便进行导航解算；捷联式的惯导系统将惯性器件直接固联在载体上，通过构建“数学平台”将惯性器件敏感到的载体系测量值转换到导航系下，再进行导航解算。
平常大家接触到的都是捷联式惯导系统，下面简述捷联式惯导系统的原理。
3. 捷联式惯导系统的力学编排
捷联式惯导系统递推方程包括三块内容，分别是：
- 姿态更新：将陀螺仪测量到的角速度 \omega_{ib}^{b} 减去地球自转角速度 \omega_{ie}^{b} 和载体运动角速度 \omega_{en}^{b} ，得到 \omega_{nb}^{b} ，据此对旋转矩阵 C_{b}^{n} 更新；（ \omega_{in}^{b} 也称作指令角速度，因为在平台式惯导中，需要对稳定平台施加该角速度以保持对导航坐标系的跟踪）
- 速度更新：将加速度计测量到的比力 a_{ib}^{b} 转换到导航坐标系下 a_{ib}^{n} ，清除有害加速度（包括哥氏加速度和向心加速度）和重力加速度g，得到 a_{eb}^{n} ，积分后即为导航坐标系下的速度增量；
- 位置更新：根据导航坐标系下的三向速度可以方便的进行位置更新。
二、代码实现与讲解
1、定义数据类型
class FusionPose:
def __init__(self, timestamp=0.0, r=np.zeros(3, dtype=np.float), v=np.zeros(3, dtype=np.float),
q_bn=np.array([0, 0, 0, 1], dtype=np.float)):
self.timestamp = timestamp
self.r = r #位置：经纬高
self.v = v #速度：北东地
self.q_bn = q_bn #姿态四元数 n->b
def copy(self):
fusionPose = FusionPose(self.timestamp, self.r, self.v, self.q_bn)
return fusionPose
class Imu:
def __init__(self):
self.timestamp = 0
self.linear_acceleration = np.array([0, 0, 0], dtype=np.float)
self.angular_velocity = np.array([0, 0, 0], dtype=np.float)
FusionPose中包含时间戳、位置、速度和姿态，其中姿态用四元数表达；IMU数据包含时间戳、三轴加速度和三轴角速度。
2、定义必要的常量和惯性递推主函数
import numpy as np
from scipy.spatial.transform import Rotation as R
RTOD = 180.0 / np.pi
DTOR = np.pi / 180.0
a = 6378137.0 # Earths radius (m)
f = 1.0/298.257223563
e = np.sqrt(f*(2.0-f))
omega_ie = 7.2921151467E-05 # Earth ROT rate (rad/s)
def INS_Equations_NED(meas_pre, meas_cur, fusion_pose):
# INS update Equations from k-1 to k
dt = meas_cur[0] - meas_pre[0]
3、计算当地重力加速度 （惯性导航第二版P178）
sin_L_2 = (np.sin(fusion_pose.r[1])) ** 2.0
g0 = 9.7803267714*(1 + 0.00193185138639*sin_L_2)/np.sqrt(1-0.00669437999013*sin_L_2)
ch = (a/(a + fusion_pose.r[2])) ** 2.0
g_n = np.array([0.0, 0.0, ch * g0])
4、计算地球自转角速度和载体运动角速度在导航坐标系中的表达 （惯性导航第二版P175）
# Earth rotation rate expressed in navigation frame
omega_ie_n = omega_ie*np.array([np.cos(fusion_pose.r[1]), 0.0, -np.sin(fusion_pose.r[1])])
# Transport Rate
RN = a*(1.0-e**2)/(1.0-e**2.0*(np.sin(fusion_pose.r[1]))**2.0)**1.5
RE = a/np.sqrt(1.0-e**2.0*(np.sin(fusion_pose.r[1]))**2.0)
dum4 = float(fusion_pose.v[1]/(RE+fusion_pose.r[2])) #东向速度
dum5 = float(-fusion_pose.v[0]/(RN+fusion_pose.r[2])) #北向速度
dum6 = float(-fusion_pose.v[1]*np.tan(fusion_pose.r[1])/(RE+fusion_pose.r[2]))
omega_en_n = np.array([dum4, dum5, dum6])
5、进行姿态更新
# Attitude Update
rotation_pre = R.from_quat(fusion_pose.q_bn) #from_quat
omega_in_b = rotation_pre.apply(omega_ie_n + omega_en_n) #四元数更新
meas_omega_nb_b = meas_cur[4:7] - omega_in_b
rot_quat = rotation_quat(-meas_omega_nb_b, dt) #^cnb = -w*cnb
fusion_pose.q_bn = R.as_quat(R.from_quat(rot_quat) * rotation_pre)
以\omega_{nb}^{b} 的角速度旋转dt时间，得到的旋转矩阵为 C_{b^{k+1}}^{b^{k}} ，这里把角速度取个负号，因此得到 C_{b^{k}}^{b^{k+1}} ，右乘以 C_{n}^{b} ，即完成了姿态矩阵的更新。
其中，根据角速度计算更新四元数的函数如下：
def rotation_quat(w, dt):
Args:
w: angular velocity, rad/s.
dt: sample period, sec.
Returns:
rot_quat: rotation quaternion corresponds to w and dt
rot_vec = w * dt # rotation vector
theta = math.sqrt(np.dot(rot_vec, rot_vec)) # rotation angle
half_theta = 0.5 * theta # half rotation angle
s = math.sin(half_theta)
c = math.cos(half_theta)
if theta == 0.0:
return np.array([0.0, 0.0, 0.0, 1.0])
elif c >= 0:
tmp = s / theta
return np.array([tmp*rot_vec[0], tmp*rot_vec[1], tmp*rot_vec[2], c])
else:
tmp = -s / theta
return np.array([tmp*rot_vec[0], tmp*rot_vec[1], tmp*rot_vec[2], -c])
6、速度更新
# Velocity Update
old_v_eb_n = fusion_pose.v
meas_f_ib_n = rotation_pre.inv().apply(meas_cur[1:4])
dum3 = 2.0*omega_ie_n+omega_en_n
a_eb_n = meas_f_ib_n + g_n - skew(dum3)@old_v_eb_n
new_v_eb_n = old_v_eb_n + dt*a_eb_n
7、位置更新
# Position Update
old_P_lla = fusion_pose.r
T = np.array([[0.0, 1.0/((RE+fusion_pose.r[2])*np.cos(fusion_pose.r[1])), 0.0],
[1.0/(RN+fusion_pose.r[2]), 0.0, 0.0],
[0.0, 0.0, -1.0]])
new_P_lla = old_P_lla + dt*T@old_v_eb_n
至此，纯惯性导航递推的功能即实现完毕，本节中我们简述了惯性导航常用的坐标系、惯导系统的分类以及捷联惯性导航系统的力学编排，使用python实现了捷联惯性导航系统的递推过程。
这个代码离工程实际应用还相去甚远，至少有以下问题：
- 惯性导航系统的初始位置、速度、姿态如何设置？
- 惯性导航系统不可避免的会随时间发散，怎样引入其他传感器信息做融合？
- 离散状态进行k到k+1时刻的递推，完全使用的是k时刻的状态量(欧拉法)，是否有更优的递推方法？
以上问题我们接下来一个个讨论。
三、参考文献
- 秦永元. 惯性导航[M]. 北京: 科学出版社, 第二版, 2014.
- 客居：地心地固坐标系-WGS84坐标系-东北天坐标系
- https://github.com/maysjeff/LS_INS_GNSS_EKF.git
如果您觉得文章不错，对您有帮助，欢迎点赞，有问题欢迎留言，我会尽我所力细致解答。