惯性导航基础实验

标签：rv 7D% 惯性导航基础实验 qnb Cnb n2 math

前言：

这是山科测院的某次实验设计，由于当时种种原因导致最后的捷联解算误差很大，故打算利用这次博客进行复盘，也可供日后学弟参考，叠甲（本人水平有限，可能错误很多，欢迎一起探讨！）

这部分实验课主要参考严恭敏老师的《捷联惯导算法与组合导航原理》，严老师的网站为：https://www.psins.org.cn/zlxz#:~:text=2020-1

实验要求：

使用matlab、python、C#及C++均可；代码要使用Git进行代码版本管理，通过pull request进行作业提交，需要用到gitee.com账号；使用markdown编写程序说明文档；给出详细的测试用例。

实验内容：

实验一：编程实现欧拉角、方向余弦阵、四元数以及等效旋转矢量之间的相互转换

实验二：编程实现大地坐标与地心直角坐标的相互转换

实验三：编程实现IMR格式惯导数据的读取与解析

实验四：编程实现地理坐标系下的速度和位置更新

实验五：编程实现地理坐标系下的速度和位置更新

实验六：编程实现解析粗对准

具体实现及代码：

实验一：姿态的四种表示方法之间的相互转换

欧拉角转方向余弦阵：

在“东—北—天312”欧拉角定义下，可得从地理坐标系（选为导航系，n系）到载体坐标系（b系）的方向余弦阵：

$\begin{gathered}C_{b}^{n}=C_{\psi}C_{\theta}C_{\gamma}=\begin{bmatrix}c_{\psi}&-s_{\psi}&0\\s_{\psi}&c_{\psi}&0\\0&0&1\end{bmatrix}\begin{bmatrix}1&0&0\\0&c_{\theta}&-s_{\theta}\\0&s_{\theta}&c_{\theta}\end{bmatrix}\begin{bmatrix}c_{\gamma}&0&s_{\gamma}\\0&1&0\\-s_{\gamma}&0&c_{\gamma}\end{bmatrix}=\\\begin{bmatrix}c_{\psi}c_{\gamma}-s_{\psi}s_{\theta}s_{\gamma}&-s_{\psi}c_{\theta}&c_{\psi}s_{\gamma}+s_{\psi}s_{\theta}c_{\gamma}\\s_{\psi}c_{\gamma}+c_{\psi}s_{\theta}s_{\gamma}&c_{\psi}c_{\theta}&s_{\psi}s_{\gamma}-c_{\psi}s_{\beta}c_{\gamma}\\-c_{\theta}s_{\gamma}&s_{\theta}&c_{\theta}c_{\gamma}\end{bmatrix}=\begin{bmatrix}C_{11}&C_{12}&C_{13}\\C_{21}&C_{22}&C_{23}\\C_{31}&C_{32}&C_{33}\end{bmatrix}\end{gathered}$

def a2mat(att):
    '''

    :param att: [pitch; roll; yaw] in radians
    :return:Cnb:n系到b系的方向余弦阵
    '''
    s = np.sin(att)
    c = np.cos(att)
    si = s[0]
    sj = s[1]
    sk = s[2]
    ci = c[0]
    cj = c[1]
    ck = c[2]
    # print(ck)
    # 从世界坐标系到载体坐标系的变换矩阵
    Cnb = np.array([[cj * ck - si * sj * sk, -ci * sk, sj * ck + si * cj * sk],
                    [cj * sk + si * sj * ck, ci * ck, sj * sk - si * cj * ck],
                    [-ci * sj, si, ci * cj]])
    return Cnb

欧拉角转四元数：

根据单位四元数的定义式：

$Q=q_0+q_v=\cos\frac\phi2+u\sin\frac\phi2$

在“东—北—天312”欧拉角定义下，由欧拉角求解四元数的公式为：

$Q_{b}^{n}=Q_{\psi}\circ Q_{\theta}\circ Q_{\gamma}=(c_{\psi/2}+ks_{\psi/2})\circ(c_{\theta/2}+is_{\theta/2})\circ(c_{\gamma/2}+js_{\gamma/2})=\\(c_{\psi/2}c_{\theta/2}+ic_{\psi/2}s_{\theta/2}+ks_{\psi/2}c_{\theta/2}+k\cdot is_{\psi/2}s_{\theta/2})\circ(c_{\gamma/2}+js_{\gamma/2})=\\(c_{\psi/2}c_{\theta/2}+ic_{\psi/2}s_{\theta/2}+ks_{\psi/2}c_{\theta/2}+js_{\psi/2}s_{\theta/2})\circ(c_{\gamma/2}+js_{\gamma/2})=$

$\begin{bmatrix}c_{\psi/2} c_{\theta/2} c_{\gamma/2}-s_{\psi/2} s_{\theta/2} s_{\gamma/2}\\\\c_{\psi/2} s_{\theta/2} c_{\gamma/2}-s_{\psi/2} c_{\theta/2} s_{\gamma/2}\\\\s_{\psi/2} s_{\theta/2} c_{\gamma/2}+c_{\psi/2} c_{\theta/2} s_{\gamma/2}\\\\s_{\psi/2} c_{\theta/2} c_{\gamma/2}+c_{\psi/2} s_{\theta/2} s_{\gamma/2}\end{bmatrix}$

def a2qua(delta_omega_nin):
    '''

    :param delta_omega_nin:att=[pitch; roll; yaw] 弧度制表达
    :return:qnb:姿态四元数
    '''
    att2 = delta_omega_nin / 2
    s = np.sin(att2)
    c = np.cos(att2)

    sp = s[0]
    sr = s[1]
    sy = s[2]
    cp = c[0]
    cr = c[1]
    cy = c[2]
    q = np.array([cp * cr * cy - sp * sr * sy,
                  sp * cr * cy - cp * sr * sy,
                  cp * sr * cy + sp * cr * sy,
                  cp * cr * sy + sp * sr * cy])
    return q

姿态阵转欧拉角：

由姿态阵求解欧拉角的完整算法如下：

$\begin{aligned}&\theta=\arcsin(C_{32})\\&\begin{cases}\gamma=-\arctan2(C_{31},C_{33})\\\psi=-\arctan2(C_{12},C_{22})\end{cases}\quad\mid C_{32}\mid\leqslant0.99999\\&\begin{cases}\gamma=\arctan2(C_{13},C_{11})\\\psi=0\end{cases}\quad\mid C_{32}\mid>0.99999\end{aligned}$

def m2att(Cnb):
    '''

    :param Cnb: n系到b系的方向余弦阵
    :return:att - att=[pitch; roll; yaw] in radians, in yaw->pitch->roll
               东北天(3-1-2)欧拉角
    '''
    att = [math.asin(Cnb[2, 1]),
           math.atan2(-Cnb[2, 0], Cnb[2, 2]),
           math.atan2(-Cnb[0, 1], Cnb[1, 1])]
    if Cnb[2, 1] > 0.999999:
        att[1] = 0
        att[2] = np.atan2(Cnb[0, 2], Cnb[0, 0])
    elif Cnb[2, 1] < -0.999999:
        att[1] = 0
        att[2] = -np.atan2(Cnb[0, 2], Cnb[0, 0])
    return att

姿态阵转四元数：

由下面四元数转方向余弦阵中的方向余弦阵的对角线元素可得：

$q_0^2+q_1^2-q_2^2-q_3^2=C_{11}\\q_0^2-q_1^2+q_2^2-q_3^2=C_{22}\\q_0^2-q_1^2-q_2^2+q_3^2=C_{33}\\q_0^2+q_1^2+q_2^2+q_3^2=1$

然后可得：

$\mid q_{0}\mid=0.5\sqrt{1+C_{11}+C_{22}+C_{33}}\\\mid q_{1}\mid=0.5\sqrt{1+C_{11}-C_{22}-C_{33}}\\\mid q_{2}\mid=0.5\sqrt{1-C_{11}+C_{22}-C_{33}}\\\mid q_{3}\mid=0.5\sqrt{1-C_{11}-C_{22}+C_{33}}$

再由非对角线元素可得：

$\begin{gathered} 2(q_{1}q_{2}-q_{0}q_{3}) = C_{12} \\ 2(q_{1}q_{2}+q_{0}q_{3}) = C_{21} \\ 2(q_{1}q_{3}+q_{0}q_{2}) = C_{13} \\ 2(q_{1}q_{3}-q_{0}q_{2}) = C_{31} \\ 2(q_{2}q_{3}-q_{0}q_{1}) = C_{23} \\ 2(q_{2}q_{3}+q_{0}q_{1}) = C_{32} \end{gathered}$

进而可得：

$\begin{gathered} 4 q_{0} q_{1}= C_{32}-C_{23} \\ 4 q_{0} q_{2}= C_{13}-C_{31} \\ 4q_{0} q_{3}= C_{21}-C_{12} \\ 4 q_{1} q_{2}= C_{12}+C_{21} \\ 4 q_{1} q_{3}= C_{13}+C_{31} \\ 4 q_{2} q_{3}= C_{23}+C_{32} \end{gathered}$

若仅根据第二组公式将难以确定四元数各元素的正负符号。如果已知四元数的某一个元素，则根据第四组公式可求解其他元素，但须避免该已知元素为 0。由四元数归一化条件： $q_{0}^{2}+q_{1}^{2}+q_{2}^{2}+q_{3}^{2}=1$ 可知，必然有 $\max\bigl(q_{i}^{2}\bigr)\geq1/4$ 成立，也就是说，四个元素中必然存在某个 $|q_{i}|\geq 1/2$ ，实际应用时，可先根据第二组公式计算获得某一个较大的元素(不妨取为正值),再根据第四组公署计算剩余的其他三个元素。

def m2qua(Cnb):
    '''

    :param Cnb:n系到b系的方向余弦阵
    :return:qnb：姿态四元数
    '''
    C11 = Cnb[0, 0]
    C12 = Cnb[0, 1]
    C13 = Cnb[0, 2]
    C21 = Cnb[1, 0]
    C22 = Cnb[1, 1]
    C23 = Cnb[1, 2]
    C31 = Cnb[2, 0]
    C32 = Cnb[2, 1]
    C33 = Cnb[2, 2]
    if C11 >= C22 + C33:
        q1 = 0.5 * math.sqrt(1 + C11 - C22 - C33)
        qq4 = 4 * q1
        q0 = (C32 - C23) / qq4
        q2 = (C12 + C21) / qq4
        q3 = (C13 + C31) / qq4
    elif C22 >= C11 + C33:
        q2 = 0.5 * math.sqrt(1 - C11 + C22 - C33)
        qq4 = 4 * q2
        q0 = (C13 - C31) / qq4
        q1 = (C12 + C21) / qq4
        q3 = (C23 + C32) / qq4
    elif C33 >= C11 + C22:
        q3 = 0.5 * math.sqrt(1 - C11 - C22 + C33)
        qq4 = 4 * q3
        q0 = (C21 - C12) / qq4
        q1 = (C13 + C31) / qq4
        q2 = (C23 + C32) / qq4
    else:
        q0 = 0.5 * math.sqrt(1 + C11 + C22 + C33)
        qq4 = 4 * q0
        q1 = (C32 - C23) / qq4
        q2 = (C13 - C31) / qq4
        q3 = (C21 - C12) / qq4
    qnb = np.array([q0, q1, q2, q3])
    return qnb

姿态阵转等效旋转矢量：

$\boldsymbol{\phi}=\frac\phi{2\sin\phi}\begin{bmatrix}C_{32}-C_{23}\\C_{13}-C_{31}\\C_{21}-C_{12}\end{bmatrix}$

$\phi=\arccos\frac{C_{11}+C_{22}+C_{33}-1}2$

$u=\frac{1}{2\sin\phi}\begin{bmatrix}C_{32}-C_{23}\\C_{13}-C_{31}\\C_{21}-C_{12}\end{bmatrix}=\frac{1}{\mu}\begin{bmatrix}C_{32}-C_{23}\\C_{13}-C_{31}\\C_{21}-C_{12}\end{bmatrix}$

其中： $\mu=2\sin\phi=\sqrt{(C_{32}-C_{23})^2+(C_{13}-C_{31})^2+(C_{21}-C_{12})^2}$

def m2rv(m, rv0):
    '''

    :param m:方向余弦阵
    :param rv0:确保输出 rv 的方向与 rv0 相同
    :return:rv:相应的等效旋转矢量，如：m = I + sin(|rv|)/|rv|*(rvx) + [1-cos(|rv|)]/|rv|^2*(rvx)^2
    '''
    rv = np.array([m[2, 1] - m[1, 2],
                   m[0, 2] - m[2, 0],
                   m[1, 0] - m[0, 1]])
    phi = math.acos((m[0, 0] + m[1, 1] + m[2, 2] - 1.0) / 2.0)
    if phi > math.pi - 1.0 * math.pow(10, -3):
        rv = q2rv(m2qua(m))
    elif phi < 1.0 * math.pow(10, -10):
        rv = 1 / 2 * rv
    else:
        rv = rv * phi / (2 * math.sin(phi))
    if len(sys.argv) > 1:
        nrv = np.transpose(rv) * rv0
        if nrv < 0:
            nrv = np.norm(rv)
            rv = -rv / nrv * (2 * math.pi - nrv)
    return rv

四元数转欧拉角：

可通过姿态阵作为中间过渡量，先由四元数计算姿态阵，再由姿态阵转欧拉角，分别如上式，综合可得结果为：

$\theta=\arcsin\left(2(q_2q_3+q_0q_1)\right)$

$\begin{gathered}\theta=\arcsin\left(2(q_2q_3+q_0q_1)\right)\\ \begin{cases}\gamma=-\mathrm{atan2}(2(q_1q_3-q_0q_2),q_0^2-q_1^2-q_2^2+q_3^2)\\ \psi=-\mathrm{atan2}(2(q_1q_2-q_0q_3),q_0^2-q_1^2+q_2^2-q_3^2)\end{cases}\\ |2(q_2q_3+q_0q_1)|\leqslant0.999999\\ \begin{cases}\gamma=\arctan2(2(q_1q_3+q_0q_2),q_0^2+q_1^2-q_2^2-q_3^2)\\ \psi=0\end{cases}\\ |2(q_2q_3+q_0q_1)|>0.999999\end{gathered}$

def q2att(qnb):
    '''

    :param qnb: 姿态四元数
    :return: att：欧拉角 att=[pitch; roll; yaw] 弧度制表达
    '''
    q11 = qnb[0] * qnb[0]
    q12 = qnb[0] * qnb[1]
    q13 = qnb[0] * qnb[2]
    q14 = qnb[0] * qnb[3]
    q22 = qnb[1] * qnb[1]
    q23 = qnb[1] * qnb[2]
    q24 = qnb[1] * qnb[3]
    q33 = qnb[2] * qnb[2]
    q34 = qnb[2] * qnb[3]
    q44 = qnb[3] * qnb[3]
    C12 = 2 * (q23 - q14)
    C22 = q11 - q22 + q33 - q44
    C31 = 2 * (q24 - q13)
    C32 = 2 * (q34 + q12)
    C33 = q11 - q22 - q33 + q44
    att = np.array([math.asin(C32),
                    math.atan2(-C31, C33),
                    math.atan2(-C12, C22)])
    return att

四元数转方向余弦阵：

$C_b^n=\begin{bmatrix}q_0^2+q_1^2-q_2^2-q_3^2&2(q_1q_2-q_0q_3)&2(q_1q_3+q_0q_2)\\2(q_1q_2+q_0q_3)&q_0^2-q_1^2+q_2^2-q_3^2&2(q_2q_3-q_0q_1)\\2(q_1q_3-q_0q_2)&2(q_2q_3+q_0q_1)&q_0^2-q_1^2-q_2^2+q_3^2\end{bmatrix}$

def q2mat(qnb):
    '''

    :param qnb:姿态四元数
    :return:Cnb：n系到b系的方向余弦阵
    '''
    q11 = qnb[0] * qnb[0]
    q12 = qnb[0] * qnb[1]
    q13 = qnb[0] * qnb[2]
    q14 = qnb[0] * qnb[3]
    q22 = qnb[1] * qnb[1]
    q23 = qnb[1] * qnb[2]
    q24 = qnb[1] * qnb[3]
    q33 = qnb[2] * qnb[2]
    q34 = qnb[2] * qnb[3]
    q44 = qnb[3] * qnb[3]
    Cnb = np.array([[q11 + q22 - q33 - q44, 2 * (q23 - q14), 2 * (q24 + q13)],
                    [2 * (q23 + q14), q11 - q22 + q33 - q44, 2 * (q34 - q12)],
                    [2 * (q24 - q13), 2 * (q34 + q12), q11 - q22 - q33 + q44]])
    return Cnb

四元数转等效旋转矢量：

$Q_{b(m)}^{b(m-1)}=\begin{bmatrix}\cos\frac{\phi_m}{2}\\\\\frac{\phi_m}{\phi_m}\sin\frac{\phi_m}{2}\end{bmatrix}$

def q2rv(q):
    '''

    :param q:变换四元数
    :return:相应的等效旋转矢量，如：q = [ cos(|rv|/2); sin(|rv|/2)/|rv|*rv ]
    '''
    if q[0] < 0:
        q = -q
    n2 = math.acos(q[0])
    if n2 > 1.0 * math.pow(10, -40):
        k = 2 * n2 / math.sin(n2)
    else:
        k = 2
    rv = k * q[1:4]
    return rv

等效旋转矢量转方向余弦阵：

$\mathbf{C}_b^i=\mathbf{I}+\frac{\sin\phi}\phi(\boldsymbol{\phi}\times)+\frac{1-\cos\phi}{\phi^2}(\boldsymbol{\phi}\times)^2$

def rv2m(rv):
    '''

    :param rv: 等效旋转矢量（弧度）
    :return: 相应的方向余弦阵DCM，如m = I + sin(|rv|)/|rv|*(rvx) + [1-cos(|rv|)]/|rv|^2*(rvx)^2
    '''
    xx = rv[0] * rv[0]
    yy = rv[1] * rv[1]
    zz = rv[2] * rv[2]
    n2 = xx + yy + zz
    if n2 < 1.0 * math.pow(10, -8):
        # if (n2 < 1.0 * math.pow(10, -8)).all():
        a = 1 - n2 * (1 / 6 - n2 / 120)
        b = 0.5 - n2 * (1 / 24 - n2 / 720)
    else:
        n = np.sqrt(n2)
        a = np.sin(n) / n
        b = (1 - np.cos(n)) / n2
    arvx = a * rv[0]
    arvy = a * rv[1]
    arvz = a * rv[2]
    bxx = b * xx
    bxy = b * rv[0] * rv[1]
    bxz = b * rv[0] * rv[2]
    byy = b * yy
    byz = b * rv[1] * rv[2]
    bzz = b * zz
    m = np.zeros((3, 3))
    # m = I + a * (rvx) + b * (rvx) ^ 2;
    m[0, 0] = 1 - byy - bzz
    m[0, 1] = -arvz + bxy
    m[0, 2] = arvy + bxz
    m[1, 0] = arvz + bxy
    m[1, 1] = 1 - bxx - bzz
    m[1, 2] = -arvx + byz
    m[2, 0] = -arvy + bxz
    m[2, 1] = arvx + byz
    m[2, 2] = 1 - bxx - byy
    return m

等效旋转矢量转四元数：

见上面等效旋转矢量与四元数相互转换

def rv2q(rv):
    '''

    :param rv:等效旋转矢量
    :return:q:相应的变换四元数，如：q = [ cos(|rv|/2); sin(|rv|/2)/|rv|*rv ]
    '''
    q = np.zeros(4)
    n2 = rv[0] * rv[0] + rv[1] * rv[1] + rv[2] * rv[2]
    if n2 < 1.0 * math.pow(10, -8):
        # if np.any(n2 < 1.0 * math.pow(10, -8)):
        q[0] = 1 - n2 * (1 / 8 - n2 / 384)
        s = 1 / 2 - n2 * (1 / 48 - n2 / 3840)
        # q[0] = 1 - n2 * (1 / 8 - n2 / 384)
        # s = 1 / 2 - n2 * (1 / 48 - n2 / 3840)
    else:
        n = math.sqrt(n2)
        n_2 = n / 2
        q[0] = math.cos(n_2)
        s = math.sin(n_2) / n
        q[1] = s * rv[0]
        q[2] = s * rv[1]
        q[3] = s * rv[2]
    return q

小结：

实验一相对来说较为基础，可以参考严老师的书进行编程实现也可以直接参考严老师的psins工具箱里面的代码进行实现。

标签：rv,7D%,惯性导航,基础,实验,qnb,Cnb,n2,math
From： https://blog.csdn.net/whlad/article/details/142679595

前言：

实验要求：

实验内容：

具体实现及代码：

小结：

相关文章

赞助商

阅读排行