智能驾驶中的惯性导航与融合定位(一)：惯性导航实现

来源: | 作者:佚名 | 发布时间: 2023-09-14 | 521 次浏览 | 分享到:

一、惯性导航的原理

1、常用坐标系定义

地心惯性坐标系i：在空间保持静止或匀速直线运动(无加速度)的坐标系称为惯性坐标系。惯性导航中的惯性参考系一般选用地心惯性坐标系，其以地球质心为坐标系原点，z轴沿着地球自转轴指向协议地极，x轴位于赤道平面内,并指向春分点;
地心地固坐标系：地心地固坐标系与地心惯性坐标系有着相同的坐标原点和坐标轴轴定义,但是地心地固坐标系与地球保持同步旋转(所以命名为地固)。
地球坐标系e：地球坐标系可认为是地心地固坐标系的极坐标表示，最常用的是GPS系统采用的WGS-84坐标系，地球上的任一点通过经度纬度和高度表达；
地理坐标系/导航坐标系n：一般取载体所在位置的东北天或者北东地坐标系；
载体坐标系b：固联在载体上，原点在重心，一般取右前上或者前右下坐标系。

2、惯性导航系统分类

惯性导航系统按照有无物理平台可分为：

平台式惯导系统：早期的惯导实现方式，惯导系统中有一个真实的物理稳定平台，惯性敏感器(陀螺仪和加速度计)安装在用三个环架支承的平台台体上，通过控制平台的旋转使其跟踪导航坐标系（如东北天或北东地坐标系），这样加速度计的敏感轴始终与要求的导航坐标系一致，测量到的加速度是导航坐标系n相对惯性坐标系i的结果a_inⁿ $a_{i n}^{n}$ ,同时载体姿态角可以从平台直接读取。
捷联式惯导系统：由于平台式惯导系统需要维持一个精密的真实物理平台，生产、使用、维护都比较困难，捷联式惯导系统指的是将惯性敏感器(陀螺仪和加速度计)直接固联在载体上，测量到的加速度和加速度是载体坐标系b相对惯性坐标系i的结果a_ib^b $a_{i b}^{b}$ 和w_ib^b $ω_{i b}^{b}$ ,经陀螺仪估算出载体姿态后，通过数学计算的方式将载体系测量到的惯性数据转换到导航坐标系中，构建了虚拟的“数字平台”。

简单来说，平台式的惯导系统包含一个稳定平台，该平台能够通过一定的方式隔绝载体运动，使得

惯性器件始终敏感导航坐标系的结果，方便进行导航解算；捷联式的惯导系统将惯性器件直接固联

在载体上，通过构建“数学平台”将惯性器件敏感到的载体系测量值转换到导航系下，再进行导航

解算。

平常大家接触到的都是捷联式惯导系统，下面简述捷联式惯导系统的原理。

3. 捷联式惯导系统的力学编排

捷联式惯导系统递推方程包括三块内容，分别是：

姿态更新：将陀螺仪测量到的角速度W_ib^b $ω_{i b}^{b}$ 减去地球自转角速度W_ie^b $ω_{i b}^{b}$ $ω_{i e}^{b}$ 和载体运动角速度 W_en^b $ω_{i b}^{b}$ $ω_{e n}^{b}$ ，得到W_nb^b $ω_{n b}^{b}$ ，据此对旋转矩阵 C_bⁿ $C_{b}^{n}$ 更新；W_in^b $ω_{i b}^{b}$ $ω_{i n}^{b}$ 也称作指令角速度，因为在平台式惯导中，需要对稳定平台施加该角速度以保持对导航坐标系的跟踪
速度更新：将加速度计测量到的比力a_ib^b $a_{i b}^{b}$ $a_{i b}^{b}$ 转换到导航坐标系下a_ibⁿ $a_{i b}^{b}$ ，清除有害加速度（包括哥氏加速度和向心加速度）和重力加速度g，得到a_ebⁿ ，积分后即为导航坐标系下的速度增量；
位置更新：根据导航坐标系下的三向速度可以方便的进行位置更新。

二、代码实现与讲解

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 fusionPoseclass 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  # Earth's 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)

其中，根据角速度计算更新四元数的函数如下：

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时刻的状态量(欧拉法)，是否有更优的递推方法？

上一篇：无

下一篇：长时间静态测试(MEMS-KH50系列）