INS/GNSS组合导航（二）-INS常用的坐标系统及转换

一、常用坐标系

由于INS是一种完全自主的导航系统，不与外界发生联系。但是我们又想知道物体的的位置，尤其是其与GNSS组合时需要知道其与GNSS在同一坐标系下的位置信息，这就牵扯到了INS的几个导航系统之间的转换。笼统的讲，INS输出的是载体坐标系下的比力和旋转角速度，我们需要将这种输出信息转换到我们常用的如惯性系或者地固系。因此本节主要介绍INS相关的几个常用坐标系及相互之间的转换关系。

1. 惯性系

惯性坐标系（用 i 表示），主要是指在空间固定或者常速运动的坐标下，它提供惯性传感器输出的参考基准。在GNSS中，我们也需要用到惯性坐标系，这里我们常用的惯性系是以地心为参考原点，即地心惯性系，其具体定义为

• 原点位于地球质心
• Z轴延地球自转轴指向协议地极；
• x轴位于赤道平面指向春分点；
• y轴位于赤道平面内，与x轴、z轴构成右手系。

2. 地固系

地固系（用 e 表示），其参考原点、Z轴的定义与惯性系一致，但坐标系随着地球旋转，旋转角速度为 ω i e \omega_{ie} ωie​，其具体定义为：

• 原点位于地球质心
• Z轴延地球自转轴指向协议地极；
• X轴只想赤道平面与格林尼治经线的交点
• y轴位于赤道平面内，与x轴、z轴构成右手系。
  其与惯性系的转换简单，即：
  C i e = [ c o s ω i e t s i n ω i e t 0 − s i n ω i e t c o s ω i e t 0 0 0 1 ] C^{e}_{i}=\begin{bmatrix} cos\omega_{ie}t & sin\omega_{ie}t & 0 \\ -sin\omega_{ie}t & cos\omega_{ie}t & 0 \\ 0 & 0 & 1 \end{bmatrix} Cie​=⎣⎡​cosωie​t−sinωie​t0​sinωie​tcosωie​t0​001​⎦⎤​

3.当地导航坐标系

导航坐标系（用 n 表示）是一种地理坐标系，即常说的NEU、NED坐标系。其常用的定义如下：

• 原点在惯性传感器中心
• x轴指向当地北方向
• y轴指向当地东方向
• z轴指向地向/天，与x、y构成右手系
  其与地固系的转换较为简答，只需知道地理坐标即可，转换矩阵如下：
  C e n = [ − c o s λ s i n L − s i n λ s i n L c o s L − s i n λ c o s λ 0 − c o s λ c o s L − s i n λ c o s L − s i n L ] C^{n}_{e}=\begin{bmatrix} -cos\lambda{sinL} & -sin\lambda{sinL} & cosL \\ -sin\lambda & cos\lambda& 0 \\ -cos\lambda{cosL} & -sin\lambda{cosL} & -sinL \end{bmatrix} Cen​=⎣⎡​−cosλsinL−sinλ−cosλcosL​−sinλsinLcosλ−sinλcosL​cosL0−sinL​⎦⎤​
  式中 λ \lambda λ和L分别为当地的大地经度和大地纬度。

4. 载体坐标系

载体坐标系（用 b 表示）用来描述载体的运动情况，有不同的定义方法，对大部分应用来说，在安装捷联系统的加速度计敏感轴时与载体的几何轴向一致。即常用的载体坐标系如下定义：

• 原点为运动载体的重心
• x轴沿载体的纵轴向前，表示横滚轴
• y轴沿载体横轴向右，表示俯仰轴
• z轴沿载体竖轴向下，表示偏航轴
  导航坐标系与载体坐标系的转换是惯性导航中的核心。因为惯性传感器测量输出的是载体坐标系的比力，需要将其转换到导航坐标系进行积分才能得出加速度信息。在已知欧拉角的情况下，两者的转换矩阵如下：
  C b n = [ c n b ] T = [ c o s θ c o s ψ − c o s ϕ s i n ψ + s i n ϕ s i n θ c o s ψ s i n ϕ s i n ψ + c o s ϕ s i n θ c o s ψ c o s θ s i n ψ c o s ϕ c o s ψ + s i n ϕ s i n θ s i n ψ − s i n ϕ c o s ψ + c o s ϕ s i n θ s i n ψ − s i n θ s i n ϕ c o s θ c o s ϕ c o s θ ] C^{n}_{b}=[c^{b}_{n}]^T=\begin{bmatrix} cos\theta{cos\psi} & -cos\phi{sin\psi}+sin\phi{sin\theta}{cos\psi} & sin\phi{sin\psi}+cos\phi{sin\theta}{cos\psi} \\ cos\theta{sin\psi} & cos\phi{cos\psi}+sin\phi{sin\theta}{sin\psi} & -sin\phi{cos\psi}+cos\phi{sin\theta}{sin\psi} \\ -sin\theta & sin\phi{cos\theta} & cos\phi{cos\theta} \end{bmatrix} Cbn​=[cnb​]T=⎣⎡​cosθcosψcosθsinψ−sinθ​−cosϕsinψ+sinϕsinθcosψcosϕcosψ+sinϕsinθsinψsinϕcosθ​sinϕsinψ+cosϕsinθcosψ−sinϕcosψ+cosϕsinθsinψcosϕcosθ​⎦⎤​
  式中 θ 、 ϕ 、 ψ \theta、\phi、\psi θ、ϕ、ψ为三个欧拉角。

二、姿态的表示

除此之外，介绍一下姿态的几种表示方法。由于INS输出的不仅仅有加速度信息，还会有姿态信息，对姿态的计算结果会影响到加速度的计算。因此姿态的精度决定了INS导航的精度。在SINS中，姿态的表示方法总分为欧拉角表示法、方向余弦矩阵表示法、四元数法。

1. 欧拉角

即对应于载体坐标系的三个参数。分别为俯仰角、横滚角和航向角。

2. 方向余弦矩阵

方向余弦矩阵（Direction Cosine Matrix， DCM）是一个3X3的九参数矩阵，表示两个坐标系之间的相对姿态。对应于上节不同坐标系之间的转换矩阵。如：
C b n = [ c n b ] T = [ C 11 C 12 C 13 C 21 C 22 C 23 C 31 C 32 C 33 ] = [ c o s θ c o s ψ − c o s ϕ s i n ψ + s i n ϕ s i n θ c o s ψ s i n ϕ s i n ψ + c o s ϕ s i n θ c o s ψ c o s θ s i n ψ c o s ϕ c o s ψ + s i n ϕ s i n θ s i n ψ − s i n ϕ c o s ψ + c o s ϕ s i n θ s i n ψ − s i n θ s i n ϕ c o s θ c o s ϕ c o s θ ] C^{n}_{b}=[c^{b}_{n}]^T=\begin{bmatrix} C_{11} & C_{12} & C_{13} \\ C_{21} & C_{22} & C_{23} \\ C_{31} & C_{32} & C_{33} \end{bmatrix}=\begin{bmatrix} cos\theta{cos\psi} & -cos\phi{sin\psi}+sin\phi{sin\theta}{cos\psi} & sin\phi{sin\psi}+cos\phi{sin\theta}{cos\psi} \\ cos\theta{sin\psi} & cos\phi{cos\psi}+sin\phi{sin\theta}{sin\psi} & -sin\phi{cos\psi}+cos\phi{sin\theta}{sin\psi} \\ -sin\theta & sin\phi{cos\theta} & cos\phi{cos\theta} \end{bmatrix} Cbn​=[cnb​]T=⎣⎡​C11​C21​C31​​C12​C22​C32​​C13​C23​C33​​⎦⎤​=⎣⎡​cosθcosψcosθsinψ−sinθ​−cosϕsinψ+sinϕsinθcosψcosϕcosψ+sinϕsinθsinψsinϕcosθ​sinϕsinψ+cosϕsinθcosψ−sinϕcosψ+cosϕsinθsinψcosϕcosθ​⎦⎤​
可以通过方向余弦矩阵计算欧拉角：
θ = a r c i s n ( C 32 ) \theta=arcisn(C_{32}) θ=arcisn(C32​)
ϕ = a r c t a n 2 ( − C 31 , C 33 ) \phi = arctan2(-C_{31},C_{33}) ϕ=arctan2(−C31​,C33​)
ψ = a r c t a n 2 ( C 12 , C 22 ) \psi = arctan2(C_{12},C_{22}) ψ=arctan2(C12​,C22​)
其中arctan2表示具有象限判断功能的反正切函数。

3.四元数

四元数（quaternion）是一个包含四个元素的矢量，可以用来描述刚体的转动和姿态的变换。
q = q 0 + q 1 i + q 2 j + q 3 k q=q_0+q_1i+q_2j+q_3k q=q0​+q1​i+q2​j+q3​k
其中，q表示四元数矢量， q 0 、 q 1 、 q 2 、 q 3 q_0、q_1、q_2、q_3 q0​、q1​、q2​、q3​表示实数部分，i、j、k是相互垂直的单位矢量。如果空间中一个矢量r在两个坐标系中的表示分别为 r b = x b i + y b j + z b k r^b=x^bi+y^bj+z^bk rb=xbi+ybj+zbk和 r n = x n i + y n j + z n k r^n=x^ni+y^nj+z^nk rn=xni+ynj+znk,采用四元数可以将其进行转换 r n 、 = q r n 、 q ∗ r^{n^、}=qr^{n^、}q^{*} rn、=qrn、q∗
其中， r n 、 = 0 + x n i + y n j + z n k r^{n^、}=0+x^ni+y^nj+z^nk rn、=0+xni+ynj+znk表示坐标的四元数形式， q ∗ q^{*} q∗表示为四元数的共轭。
四元数可以转换为方向余弦矩阵
C b n = [ ( q 0 2 + q 1 2 − q 2 2 − q 3 2 ) 2 ( q 1 q 2 − q 0 q 3 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 1 q 2 + q 0 q 3 ) ( q 0 2 − q 1 2 + q 2 2 − q 3 2 ) 2 ( q 2 q 3 − q 0 q 1 ) 2 ( q 1 q 3 − q 0 q 2 ) 2 ( q 2 q 3 + q 0 q 1 ) ( q 0 2 − q 1 2 − q 2 2 + q 3 2 ) ] C^n_{b}=\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} Cbn​=⎣⎡​(q0​2+q1​2−q2​2−q3​2)2(q1​q2​+q0​q3​)2(q1​q3​−q0​q2​)​2(q1​q2​−q0​q3​)(q0​2−q1​2+q2​2−q3​2)2(q2​q3​+q0​q1​)​2(q1​q3​+q0​q2​)2(q2​q3​−q0​q1​)(q0​2−q1​2−q2​2+q3​2)​⎦⎤​
方向余弦矩阵转化为四元数：
T 0 = 1 + t r ( C b n ) T_0=1+tr(C^n_b) T0​=1+tr(Cbn​)
T 1 = 1 + C 11 − t r ( C b n ) T_1 = 1+C_{11}-tr(C^n_b) T1​=1+C11​−tr(Cbn​)
T 2 = 1 + 2 C 22 − t r ( C b n ) T_2=1+2C_{22}-tr(C^n_b) T2​=1+2C22​−tr(Cbn​)
T 3 = 1 + C 33 − t r ( C b n ) T_3=1+C_{33}-tr(C^n_b) T3​=1+C33​−tr(Cbn​)
如果 T 0 = m a x T 0 , T 1 , T 2 , T 3 T_0=max{T_0,T_1,T_2,T_3} T0​=maxT0​,T1​,T2​,T3​，则
q 0 = T 0 2 , q 1 = C 32 − C 23 4 q 0 , q 2 = C 13 − C 31 4 q 0 , q 3 = C 21 − C 12 4 q 0 q_0=\frac{\sqrt{T_0}}{2},q_1=\frac{C_{32}-C_{23}}{4q_0},q_2=\frac{C_{13}-C{31}}{4q_0},q_3=\frac{C_{21}-C_{12}}{4q_0} q0​=2T0​ ​​,q1​=4q0​C32​−C23​​,q2​=4q0​C13​−C31​,q3​=4q0​C21​−C12​​
若 T 1 = m a x T 0 , T 1 , T 2 , T 3 T_1=max{T_0,T_1,T_2,T_3} T1​=maxT0​,T1​,T2​,T3​，则
q 1 = T 1 2 , q 2 = C 21 − C 12 4 q 0 , q 3 = C 13 − C 31 4 q 0 , q 0 = C 32 − C 23 4 q 0 q_1=\frac{\sqrt{T_1}}{2},q_2=\frac{C_{21}-C_{12}}{4q_0},q_3=\frac{C_{13}-C{31}}{4q_0},q_0=\frac{C_{32}-C_{23}}{4q_0} q1​=2T1​ ​​,q2​=4q0​C21​−C12​​,q3​=4q0​C13​−C31​,q0​=4q0​C32​−C23​​
若 T 2 = m a x T 0 , T 1 , T 2 , T 3 T_2=max{T_0,T_1,T_2,T_3} T2​=maxT0​,T1​,T2​,T3​，则
q 2 = T 2 2 , q 3 = C 32 + C 23 4 q 0 , q 0 = C 13 − C 31 4 q 0 , q 1 = C 21 + C 12 4 q 0 q_2=\frac{\sqrt{T_2}}{2},q_3=\frac{C_{32}+C_{23}}{4q_0},q_0=\frac{C_{13}-C{31}}{4q_0},q_1=\frac{C_{21}+C_{12}}{4q_0} q2​=2T2​ ​​,q3​=4q0​C32​+C23​​,q0​=4q0​C13​−C31​,q1​=4q0​C21​+C12​​
若 T 3 = m a x T 0 , T 1 , T 2 , T 3 T_3=max{T_0,T_1,T_2,T_3} T3​=maxT0​,T1​,T2​,T3​，则
q 3 = T 3 2 , q 0 = C 21 − C 12 4 q 0 , q 1 = C 13 + C 31 4 q 0 , q 2 = C 32 + C 23 4 q 0 q_3=\frac{\sqrt{T_3}}{2},q_0=\frac{C_{21}-C_{12}}{4q_0},q_1=\frac{C_{13}+C{31}}{4q_0},q_2=\frac{C_{32}+C_{23}}{4q_0} q3​=2T3​ ​​,q0​=4q0​C21​−C12​​,q1​=4q0​C13​+C31​,q2​=4q0​C32​+C23​​
四元数与欧拉角的转换关系为：
q 0 = c o s ϕ 2 c o s θ 2 c o s ψ 2 + s i n ϕ 2 s i n θ 2 s i n ψ 2 q_0=cos\frac{\phi}{2}cos\frac{\theta}{2}cos\frac{\psi}{2}+sin\frac{\phi}{2}sin\frac{\theta}{2}sin\frac{\psi}{2} q0​=cos2ϕ​cos2θ​cos2ψ​+sin2ϕ​sin2θ​sin2ψ​
q 1 = c o s ϕ 2 s i n θ 2 c o s ψ 2 + s i n ϕ 2 c o s θ 2 s i n ψ 2 q_1=cos\frac{\phi}{2}sin\frac{\theta}{2}cos\frac{\psi}{2}+sin\frac{\phi}{2}cos\frac{\theta}{2}sin\frac{\psi}{2} q1​=cos2ϕ​sin2θ​cos2ψ​+sin2ϕ​cos2θ​sin2ψ​
q 2 = s i n ϕ 2 c o s θ 2 c o s ψ 2 − c o s ϕ 2 s i n θ 2 s i n ψ 2 q_2=sin\frac{\phi}{2}cos\frac{\theta}{2}cos\frac{\psi}{2}-cos\frac{\phi}{2}sin\frac{\theta}{2}sin\frac{\psi}{2} q2​=sin2ϕ​cos2θ​cos2ψ​−cos2ϕ​sin2θ​sin2ψ​
q 3 = c o s ϕ 2 c o s θ 2 s i n ψ 2 − s i n ϕ 2 s i n θ 2 c o s ψ 2 q_3=cos\frac{\phi}{2}cos\frac{\theta}{2}sin\frac{\psi}{2}-sin\frac{\phi}{2}sin\frac{\theta}{2}cos\frac{\psi}{2} q3​=cos2ϕ​cos2θ​sin2ψ​−sin2ϕ​sin2θ​cos2ψ​