惯导姿态解算中的一下实际问题1（附姿态解算相关的C、matlab代码）

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

前言

导航解算中离不开载体姿态的计算，这个里面就涉及到姿态角、姿态矩阵、四元数等概念。本节通过对飞行器姿态解算进行介绍，理解好这几个概念之间的关系，再有面向其他载体的实际问题就能一通百通了。

一、姿态角与姿态矩阵的概念

首先，得明确姿态矩阵的作用是什么？姿态矩阵，是由姿态角的正余弦组成的矩阵，该矩阵的作用是把一个坐标系下的矢量通过左乘姿态矩阵，变换到另一个坐标系下。
R b = C n b R n {R_b} = C_n^b{R_n}Rb​=Cnb​Rn​
假设R n {R_n}Rn​为地理系下的一个矢量，通过左乘姿态矩阵C n b C_n^bCnb​，这个矢量在机体系下的表示为R b {R_b}Rb​。而C n b C_n^bCnb​是由地理坐标系开始，分别转动航向角、俯仰角、横滚角三个角度后得到机体坐标系，的转动矩阵。当然，这个转动顺序是由人来定义的。
1）第一步：绕Z轴旋转一个角度：


2）第二步：绕X轴旋转一个角度：

3）第三步：绕Y轴旋转一个角度：

不同行业中的坐标系定义差异很大，而载体的姿态角是基于坐标系定义来定义的，这就造成了姿态角描述必须以坐标系定义和轴系旋转顺序为基础，不然单纯说某个载体的姿态是XXX，其实是不严谨的。姿态角定义的不唯一，是根据坐标系绕其轴的旋转顺序不同，一共存在12种可能：首先绕三个坐标轴种的任意一轴转动，有3种情形；接着绕除第一次转轴外的任意一轴转动，有2种情形；最后绕除第二次转轴外的任意一轴转动，又有2种情形，因此总计3 * 2 * 2=12种。
我们以飞机作为载体，以导航地理系（站心坐标系）作为参考系，来分析姿态角（欧拉角）、姿态矩阵（旋转矩阵）以及四元数等概念。就这已经都限制载体了，还是因为某些历史原因，坐标系并没有统一：
1） 由于我国的军工建设初期得到了苏联的援助，因此惯性导航领域飞机的机体坐标系大都定义为xyz-右前上，导航坐标定义为xzy-东北天。业内有时会成为苏俄坐标系；
2） 飞行控制领域的发展离不开欧美国家早期的一些理论，因此他们在飞控领域飞机的机体坐标系大都定义为xzy-前右下，导航坐标系定义为xyz-北东地。我习惯称它为欧美坐标系。

二、飞机用两种常用坐标系分析

2.1 惯导常用坐标系（苏俄坐标系）

（飞机坐标为xyz-右前上，导航坐标为xzy-东北天），姿态角都以右手定则为正。
因此，在飞机腹部朝地北部朝天状态下，横滚角φ \varphiφ为右翼向下，左翼向上为正；俯仰角θ \thetaθ为机头向上为正；航向角ψ \psiψ为机头向左偏转为正（这里航向角定义为逆时针为正，是为了遵守右手定则。但与一般“北偏东为正”的思维惯式不同。不过没关系，如果想让航向角 顺时针为正，只要在航向角计算的值上加一个负号即可）。


姿态角计算公式如下：

2.2 飞控常用坐标系（欧美坐标系）

（飞机坐标为xyz-前右下，导航坐标为xzy-北东地）
在飞机腹部朝地北部朝天状态下，横滚角φ \varphiφ为右翼向下，左翼向上为正；俯仰角θ \thetaθ为机头向上为正；航向角ψ \psiψ为机头向右偏转为正（与上面提到的不同。因为此时Z轴指地，因此右手定则为航向角顺时针为正）。

姿态角计算公式如下：

可以看出来，无论是哪种坐标系体系，旋转顺序都是从导航坐标系通过旋转航向角（立轴）、俯仰角（横轴）、横滚角（纵轴）到机体坐标系。只不过因为坐标系定义的不同，引起旋转矩阵的不同。这就要求如果拿到旋转矩阵，计算姿态角时还是需要提前明确好坐标系定义。

三、计算机进行姿态计算时要考虑的一些问题

3.1 姿态角主值的问题

对于飞机来讲，姿态角定义并非在[ 0 ∘ ∼ 36 0 ∘ ] \left[ {{{\rm{0}}^ \circ } \sim {\rm{36}}{{\rm{0}}^ \circ }} \right][0∘∼360∘]整个范围内。一般固定翼飞机俯仰角定义范围为[ − 9 0 ∘ ∼ + 9 0 ∘ ] \left[ { - {\rm{9}}{{\rm{0}}^ \circ } \sim {\rm{ + 9}}{{\rm{0}}^ \circ }} \right][−90∘∼+90∘]，航向角定义为[ 0 ∘ ∼ 36 0 ∘ ] \left[ {{{\rm{0}}^ \circ } \sim {\rm{36}}{{\rm{0}}^ \circ }} \right][0∘∼360∘]或[ − 18 0 ∘ ∼ + 18 0 ∘ ] \left[ { - 18{{\rm{0}}^ \circ } \sim {\rm{ + 18}}{{\rm{0}}^ \circ }} \right][−180∘∼+180∘]，横滚角定义为[ − 9 0 ∘ ∼ + 9 0 ∘ ] \left[ { - {\rm{9}}{{\rm{0}}^ \circ } \sim {\rm{ + 9}}{{\rm{0}}^ \circ }} \right][−90∘∼+90∘]。以苏俄坐标系为例：
1）俯仰角计算时，asin的真值范围就是[ − 9 0 ∘ ∼ + 9 0 ∘ ] \left[ { - {\rm{9}}{{\rm{0}}^ \circ } \sim {\rm{ + 9}}{{\rm{0}}^ \circ }} \right][−90∘∼+90∘]，因此不用做调整；
2）横滚角计算时，atan的真值范围是[ − 18 0 ∘ ∼ + 18 0 ∘ ] \left[ { - 18{{\rm{0}}^ \circ } \sim {\rm{ + 18}}{{\rm{0}}^ \circ }} \right][−180∘∼+180∘]，因此需要通过判断C n b ( 1 , 3 ) = − sin ⁡ φ cos ⁡ θ C_n^b(1,3) = - \sin \varphi \cos \thetaCnb​(1,3)=−sinφcosθ的正负值，以判断atan的值是落在了第一四象限，还是二三象限；
3）偏航角计算时，同横滚角计算方法。需要注意的是，偏航角很容易跨 ±90°两个值，所以当C n b ( 2 , 2 ) C_n^b(2,2)Cnb​(2,2)接近于0时，需要根据C n b ( 2 , 1 ) C_n^b(2,1)Cnb​(2,1)的正负号判断偏航角是-90°还是＋90°。
当然，现在C语言或者matlab语言一般会提供atan2函数，已经帮你考虑到横滚和偏航计算的上述问题了。

3.2 姿态角计算时的锁死问题

设想一下，如果一个飞机的俯仰角在＋90°附近，即飞机机头朝上。此时旋转飞机纵轴（原本的横滚运动），或者旋转飞机立轴（原来的偏航运动），对于飞机来讲其实是没有意义的，即航向角和横滚角的定义存在锁死的情况。这能从姿态矩阵中看出来：

1）当C n b ( 2 , 3 ) > 0.999999 C_n^b(2,3) > 0.999999Cnb​(2,3)>0.999999，此时θ → 9 0 0 \theta \to {90^0}θ→900，近似的sin ⁡ θ ≈ 1 \sin \theta \approx 1sinθ≈1，cos ⁡ θ ≈ 0 \cos \theta \approx 0cosθ≈0， 可近似为：

2） 当C n b ( 2 , 3 ) < − 0.999999 C_n^b(2,3) < -0.999999Cnb​(2,3)<−0.999999，此时θ → − 9 0 0 \theta \to {-90^0}θ→−900，近似的sin ⁡ θ ≈ − 1 \sin \theta \approx -1sinθ≈−1，cos ⁡ θ ≈ 0 \cos \theta \approx 0cosθ≈0， 可近似为：

上两式说明了当俯仰角在±90°附近时，横滚角和偏航角之间是无法单独分离的，或者说两者存在多值性，只有在某一个值确定之后另一个值才能确定。一般，令偏航角ψ = 0 ∘ \psi = {0^ \circ }ψ=0∘，保证两个姿态角的唯一性。

四、一些代码

4.1 姿态矩阵计算姿态角（与公式的区别在于用的时Cbn，公式里用的时Cnb）

// 右前上 输出PRY Cbn: b-flame to n-flamevoidRotation_matrix2euler(constdoubleCbn[3*3],double*euler){euler[0]=asin(Cbn[2*3+1]);euler[1]=atan2f(-Cbn[2*3+0],Cbn[2*3+2]);euler[2]=atan2f(Cbn[0*3+1],Cbn[1*3+1]);if(Cbn[2*3+1]>0.999999){euler[1]=atan2f(Cbn[0*3+2],Cbn[0*3+0]);euler[2]=0;}if(Cbn[2*3+1]<-0.999999){euler[1]=-atan2f(Cbn[0*3+2],Cbn[0*3+0]);euler[2]=0;}// heading 0~2PIif(euler[2]<0){euler[2]=PI*2+euler[2];}}

4.2 姿态矩阵计算姿态角（matlab)

%Cnb:由n系到b系的转换矩阵 function[Att1,Att2]=Cnb_to_Att(Cnb)%Prototype:[att,attr]=m2att(Cnb)%Input:Cnb-DCM from navigation-frame(n)to body-frame(b)%Outputs:att-att=[pitch;roll;yaw]in radians,in yaw->pitch->roll%(3-1-2)rotation sequence%attr-in yaw->roll->pitch(3-2-1)rotation sequence theta=asin(Cnb(2,3));gama=atan2(-Cnb(1,3),Cnb(3,3));pesai=atan2(Cnb(2,1),Cnb(2,2));%Cnb(2,1)为+，顺时针为正；Cnb(2,1)为-，逆时针为正ifCnb(2,3)>0.999999gama=atan2(Cnb(3,1),Cnb(1,1));pesai=0;elseifCnb(2,3)<-0.999999gama=-atan2(Cnb(3,1),Cnb(1,1));pesai=0;endif(pesai<0)pesai=2*pi+pesai;end Att1=[theta;gama;pesai];ifnargout==2%dual Euler angles Att2=[atan2(Cnb(2,3),Cnb(3,3));asin(-Cnb(1,3));-atan2(Cnb(1,2),Cnb(1,1))];end end

总结

本节只介绍了姿态角和姿态矩阵的情况，由于常用算法里会有下一节里四元数的接入，因此算法中未进行姿态矩阵正交化的检测。如果导航算法里仅用姿态矩阵更新，需要在每次更新完姿态矩阵后进行正交化。