无人机PX4状态估计

1、1.1 组合导航从上章介绍飞行器需要获得自己的十五个状态信息，需要通过传感器测量并推算。单独的惯性测量元件由于误差累积，导致随时间增长发发散。因此需要通过额外的传感器信息来进行互补修正。组合导航技术结合GNSS惯性测量元件、磁力计、气压计和超声波等传感设备，利用各自的有点互补，采用数字信号处理技术融合多传感器的测量值，从而获得较为准确的飞行器的十五个状 态空间量，通过一定的算法和规律尽可能的估计出与真实状态量接近的量。惯性测量元件容易发散，从而这个发散可以通过GNS验球卫星定位系统来获取修正和抑制，然而 GNSSR有在室外无遮挡的环境下才能得到较好的信号强 度和定位信息，当信号质量较差或者位于

2、室内导致无法定位的时候，系统则需要由惯性元件来短时间抑制和修正。 GNSS勺高度信息通常误差较大，这时候可 以通过超声波传感器和气压传感器进行一定程度的修正。组合导航技术中传感器互补的思想原理是由克劳德-香农在1948年的信息论中提出。信息论的线性估计理论让数字处理工程师在系统设计时能够把多个具有误差的传感器通过数学方程组合矩阵运算融合起来，利用传感器信息估计特定的状态量，并且理论上提出了更多的传感器状态信息参与互补运算，就能 够得到更好的状态估计（更接近真值，并且方差更小）。1.2 飞行器的坐标系三维笛卡尔支教坐标系? 三维笛卡尔直角坐标系是指三条相互垂直的坐标轴（正交）相交形成的坐标系，其

3、中相交的点称为原点O（ origin ）。? 每个轴都指向特定的方向，两个不同的坐标轴决定一个平面，也叫笛卡 尔平面。? 通常 3 个轴采用右手系标定，即右手的拇指方向指向X 轴的正方向，食指则指向Y轴方向，中指指向的是Z轴方向。? 此外对于围绕每个轴的正旋转方向，则是用右手大拇指对应每个轴的正方向，则其他四指自然弯曲的方向为围绕该轴旋转的正方向。本文所介绍的软件和算法均采用右手定则刚体刚体（Rigid Body ）的意思是指物体在任何外力的作用下，它的体积和形 状都不会发生改变。这是力学范畴的一个理想抽象模型概念，实际情况任何物 体在受外力作用的时候都会或多或少的发生形变，并不存在真正意义上

4、的刚体。 但如果形变本身相对于物体的运动微乎其微，可以忽略，或者并不影响我们分 析物体的运动结果，则可以在特定的情况下有条件的忽略这种形变，即将物体 假设成刚体。其中质心运动的分析范畴属于牛顿第二定律定义的质点运动与受力之间的 关系，即主要研究飞行器质心的位置和速度信息。而转动的建模方程则属于欧 拉方程所分析的模型运动关系，即主要研究飞行器的姿态信息。要研究以上两种运动的模型和信息，需要定义两个三维笛卡尔直角坐标系:大地坐标系-（或惯性坐标系，用e或者G表示）用于研究飞行器相对大 地的运动状态以及空间位置坐标。通常飞控都是把地面起飞点选取为该坐标系 的坐标原点，即初始化x,y,z坐标为0o通常

5、大地坐标系的X-Y-Z方向选取为 大地的北-东-地。机体坐标系-（用b或B表示）坐标原点取机体的重心,用于研究飞行器 相对于重心的旋转运动。通常机体坐标系的X-Y-Z方向选取为机身的前-右-下。机体坐标系是固定在机体上随着机体的运动而运动。1.3方向余弦矩阵和欧拉角63.1方向余雄阵及啊般翻住个靴沃1就际蜥触耳对刊地嘛Me,三恤胞fi婵瞅1邠提,业也吗于机体金标系b,三个轴对应的单位矢1分别是|i|cos（A（川。呵匕01心。旧）CO$(/.i) cos(/，0 cowKi)co$(/J)co$U/)co$(K./)CQSil.k) cosy.*) cos(K, k)瞰蜗拗胭婵岐1瓢酸理树如t

6、,帆酝成|i|/|co$(/J), m,i)=川 IMcos(K“三融融脆-这侥献Ml体坐届收剧嘛歌班嫣标63.1方向余蜩大地坐赧的3邮位矢也可以初体坐标系冲标出来,MHcoXU) cosi/i) cw(L力 co«/J) co$(/,k) cosfj.k)cos(Ki) CO5(K,/) cos(KJt)I i Ji Ki K*= l j I I K I Ik Ik K k这两使阵定义了从Y坐标系的靴廨螺到另-世标系的转换法:,或都用防式.这两饯 阵即称为方向余魏阵（Direction Cosine Matrix）632姿态与欧拉角利用舱公式来研究洲WB统质情闹的时悦主即究的对象就

7、题体的戢.无人机fi啜杰（或者翻角）指的是飞行黜端仙 椒/!词的你见是用瀛斯黝固连机体坐标系与大地幽新系之间的角位1版飞谓需要了解这个当前的姿态角位1关系, 从而蝴鹿预娜搦恻睐计Utt®缴蒯L从而实观一定蹴作.对于三堆空间里的Y豺系，任碉体摘披魏颐用三个鼬角果标,鹿的射系是mt不动的，即前酬谜的大嬲歌 颗耐腕挫颁嘱翻懒喻镰,四确粉刷蒯陋ima岫询机偏航角1.3.2姿态与欧拉角在定义欧拉角的时候，相同的姿态不同的取法会得到不同的欧拉角。本书 采用的取法顺序为Z-Y-X,即绕三个轴的旋转顺序是先绕 Z轴，再绕Y轴，再 绕X轴，最终达到一定的姿态。而对于同样的姿态，如果绕轴的顺序不同，则得

8、到的三个欧拉角也不相同。 因此按Z-Y-X的定义进一步来说，本书所定义的欧拉角 舶角（触）0 -机的水平面般也抬头为正 犒航角（方位角耀仁f -机头方向在他平面的嬲与林M轴加缺配北偏东为正 源角（雕角-擀颗体斓觑树植姐 极为正姿态解算(attitude algorithm )也叫姿态分析或者姿态估计，即指飞控 系统或者惯导系统根据惯性测量单元(IMUU陀螺仪，加速度计，磁力计等传感 器的实时信息求解飞行器的欧拉角的算法过程，因此也叫 IMU数据融合(IMU Data fusing )。633欧拉南的定轴转动表示矩阵物物一切中.瞋欧检由的定此可中粗的分制就次性系的jmx三地旃转也*.明爵到机体坐

9、标率上但文三次腐辆的的第 为。儿几兀力JT津津i.小儿匕& 0片九时】.第一次国境旋转雹轴旋您5变成显然此次旋转新的坐标系与老坐标系之间的关系(投影)cos(W) sin(W)-sin(W) cos(W)00-cos(W) sin(W) 0庵=-sin(“)cos(W) 0-001第二次旋转围绕匕轴旋转仇旋转到OXzPzZz显然此次旋转新的坐标系与老坐标系之间的关系（投影）sin(6)0cos(0)匕zcos(8) 001sin(») 0Fcos(0) 0 sin(0)' 偌=010sin(J) 0 cos(6) 一第三次旋转围绕汗2轴旋转巾，旋转到OX/&Z

10、b显然此次旋转新的坐标系与老坐标系之间的关系（投影）1100Rg = 0 cos ） sing01 sin（p） cos（p）1d-疝间Q期阴虽然飞行器的姿态是随机转动，而并不是依次沿三轴旋转而成，我们仍然 可以依靠欧拉角的三次旋转来还原出飞行器的姿态信息。而且如果旋转的三轴 顺序不同，得到的旋转矩阵也不同，并且欧拉角也不相等，三次旋转先后组合 在一起即可以形成从惯性大地坐标系 e到机体坐标系b的旋转矩阵：1 Q 。眄。用"即牌=Q哪的$瞰）0 I。-$i晒 cm（啾-M*期仲）网0卜疝喇-疝劭sin®*如口）»o（f） - ch（旬州（昉 如仲）如砌+如仲）（财

11、同 珈刚 ）CO心”疝的* ODE砌+ 5X1（。卜疝响 酬（*）*酬砌疝砌网+词的 期明*侬伯T11 r12 r13, 房触阻哪魏隔D.R” %hl侬，裁沱喻旗砚傩IMMRI r31 r32 r33,巾=arctan p = - arcsin(r13)仲=arctan(rn/r33)方向余弦矩阵的几大缺陷抑S7阀，会注直蝴历向亲密电螭蝴算眦t因为朋余弦属愚个统取部个元爵腰酬漉新勺曲奇异点这眄庭间上萩为万向节滕佝曲皿叫随域是当三个雕中的中间这个雌达到641四元数的定义酝触XI用于顿缢8 (及邮图像分析)上标E蝴的踞引他糊于另懿啸 O,器诵，娥)，四元嬲靛鲂面的优虹 强更快，提供平滑雕、布姆免万

12、向蛹题、存网西畋小”.四无数(Quaternions)是由爱尔兰数学家汉韵强 (WilliamRowan Hamilton, 18052865)在1843年提出来的数学枇念它将复数所湎幽三鲤间拓展到间，可以郭蝗酸更睚断他然也可以以脚助.强尊捌螃!, < 聘运算酝fi主要包含四个元索qoqqq，其中%均为蝮四元数的表达网:Q(flo-?r?2»?3)= % + ?ii + + ?3fc四元数艇算题J:i*i = j*j = k*k = -li*j = k,j*k = t,k*i = jy*i = -k,k*/ = -i,i*k = -j6.4.1四元数的定义酝州眄OMf”.典也是

13、o3ToMUk到3B,财哂峥*酚，任中勿易的几何廉义可以3瞟成为一种直转.而四方由懒义为Qy,q 旗阵形式可以写为Q =此外定义四旅的+ q/+q/ + qj. aftCHta四元,这表示单位四元数.四酒的逆QT = Lt对于单位四元数Q-l = Q运n为相应部位的加派，例如假设Q = q0+qj+qj+q3kP = P0+Pj WP31fQ±P = (%±Po)+(Qt ± P” + (/ 土 P2)J + 83 土 p3 M而p Q =(Po+p j+p j+/)&”+q?" q同=OVb - PNi - PW J +(Po% + PN( +

14、 Pz/ -23奥)'(Pi/h +%q。+PW| -%)/ +(Po +PWo +P1% -P2*)k= r0 + rj r“ r5t641四元数的我研形确以=0 1 I J q q q q. 3 1 -P%; 2 3-P-P&A pl诙巴-P2 1 4 p p p p>=m 八 nlnI=Q p或者IB 4 1J p p p p 3 CS3 0 1 2 3 H q q qb642四元数与旋转的关系四元我是可以描述三间中腑的旋转医中三维空间(R3)中削咻上的任意一点Pk以)点要围境通过原点的矢用抽y旋舞帷度,转期(匕)点上.将P点用齐次坐标来表示为酝 数:®M

15、.定义Q为任意单位四元数，则(月"“6/2)o5(8/2)代表的旋转可以将喋期%y/M5w.首先，对于单位四元数Q如釉行标乘法的话，并不影响旋转本身.围境Q与$00为标却所对应的旋转是一样的，显然(sQ)P(sQ)XQPQ«4s 1QAQ :=QPQ :=QPQ耿,任何四元数的实数郃分可以写成% = (Q + Q.)/2,因此QPQ,的标实酬(QPQ，)="二等匚=也誓2二丝产=QR(P)Q=R(P).因此对于四元数P二p，M的实部在及过旋转Q后不会改变，此外旋转本身不会改变虚部矢部分的大小，因此|P| = |P'|.642四元数与旋转的关系旋转,为事直啮

16、与，点的共面目垂直于旋转轴V.定义砒四元数唯反量部分底旋相画上的投影矢曲0是四元即的矢量部分喈旋鼾面I影灯L 蝴当我量围颇电唱专渝度到2 谨饯设yl是该碓统姮能言献1.谴归蛛量均为单位 #矢艮这洋魏vOvl = es1由于应喻垂直于旋转投影面.低特柏0位矢量为v =窗孑现在我们定义用于描述该底喻四元BlQ = vl Z|EV1|vOa = (vOx vl,vO vl| = |vsuij.cos此外令v2 = QvOQ只要证明与“的夹角是JW袅用Oft从到，的俺裴四元ft ”到以的旋转EjcRvZ vl, =QvO(?r = (vl vOJ>r = vOvO(vrvr),由于单25dtvO

17、vO = T, vlvl= -1,因itv2*v=Q：卜叫叫M，vO二这a聊2与八的相椒匕与幽平面重合，并映角醐,均笔 网以与幽夹角为e.酬3雅义的v2 = (MQ ,即是P知;传平面上的投影，是VCS过Q所通述的货转四元数旋转后的矢！(vO X vl)II VO X vlj|既然四元数可用来描述旋转,那么就可以用它来描述从大地坐标系e到机体坐标系b之间的转换，也就是用它来描述6.3.1所介绍的方向余龙炬阵.在参考文献2臬永元的惯性导航）一文中922节给出了四元数与方向余弦矩阵的对应关系和详细的推导过也这里簿要的给出一下结论机体以卬=3J+叼+恤A,旋转运动.从QA/到（MW; u为垂直于臼的

18、单位眼时轴.自小二m经运算可胤r，= （/ + UsE& +而吗UU）d = DJ,从而得到网为机体坐标系b到大地坐标系的方向余弦矩阵,即:ggg=/+ l/2sin-cos- + 2sin*-47y2220 -nsln-msin j,， i0-(m2 +rr)sinx-0 1 0 + 2cos-0 Rsin-e-msin j/msinInsin/msin2 y-(f2 + n2)sin2 j* mnsm -Insin -一mnsm -» a 2°-(m2 + r)sin2 -RBUn n 0灯醐 勘Y叫 ybi* q产而叫 %=对Q 二 “ + 仇.。*.外心=3

19、；.(%.町.+ 成0)疝-二61. /血； 代A幅用会费钳瑞城+q”4一谙2值曲-命%)2®%+q。4)=28凶2+用3)2%-研1)2(如也-qoq2) 2(和外 + QoQi) % -d-d + 4酝数与懒划砌应关系,因此采用酝期飞行图姿态进行估计，值需要每个时钟采样硼腌环迭代运H出新的四元 数为444,就可以获得方向余弦矩阵，从祓得实时的欧拉角信义如何在每个时钟采样周期通过新的传感器信息送代出新的四元教信息呢？这就是采用K格库塔法对四元嬲行粉运鼠由于匹元数Q = cos g + (如 + m/0 +*)sin % cos ； + u( sin 鹏期边进行酚可以得到dQdti

20、9 de ,1 e de due e-sin - - + ue - - cos - - + sin 22dt22dtdt21/06= -6)e-l cos-4- ue-sin-j乙乙乙/11h= Q = 3Q 3b 乙乙1 , 、=5 (qO + qli + q2j + q3k) (a)xi + a)yj 4- a)zk)=。匕 Q 乙Jt'p因此采用一阶龙格库塔法,q(t + r)= q+ T *=q(E)+ 孙。Q展开后可以得到四元数每部分的更新(一山彳91 R尸 3芸93)丁2(3,9。一 qa + 破)丁2(q3 + a)yqa -/qj T03= 93 +卡尔曼滤波卡尔曼滤波

21、是一种系统状态观测估计器，用来描述一个系统的输入、输出 和内部递归迭代及重构的方式。卡尔曼滤波作为一个估计算法，主要基于系统 状态空间的方法，因此也属于第七章所谈到的现代控制理论。卡尔曼滤波使用 系统模型的迭代方程(或者微分方程)计算出理论输出模型在于实际观测相比 较，乘上比例因子形成状态反馈，将系统状态重构偏差迭代消除，滤波器不但 对系统进行模型抽象，同时建立量测噪声和系统噪声模型，并通过测量与理论 的比较结果估计出噪声能量。卡尔曼滤波器是一种递推线性最小方差估计的算法。对于解决大部分的问 题，是最优，效率最高，也是最有用的。卡尔曼滤波的广泛应用超过30年，包括机器人控制、导航，传感器数据融

22、合，信号处理，计算机图像处理等等。它 的特点主要是 无储性EG)二心 即系统状态的怙计值的数学期望等真徒 估计方差。(£) =以尸)一双为工最小，即任何一个其他的估计.所对应的 方差都大于滤波器的估计值的方差 实时性，采用递推方式，减小运尊量，实时输出运克滤波结果卡尔曼滤波公式:系统状态方程.X# =巾*#7*4 +限-1 + Wr-iWjcT观测公式：入="*见+/灯上7为索线状态空间转尊矩阵.为上时刻的凝统输入量*为索统哄自嘉ft力系统啜声，乩内说刈翅阵，也为堀网噬声-索统噪声与羯嚏声的统计将性为高斯白喉声.= G,且些声方壁姐 降为系统唤声节差短库： 丸 =QAi =

23、汽，测量嘤声方差矩阵：/ =网品产离散卡尔曼滤波器的目的就是要通过系统上一时刻的状态和输入，以及当 前时刻的观测来估计出当前时刻的状态，并且它是最小方差的无偏估计。对于 线性系统来说，它也是这几部分的线性叠加，即式父 = K泼kfk-i + K向t + Kk uk-i 杵曼源波式中曲幽,巾4崛魏独骑趟趟醐酎蝴1帆蛔B.跚嗨茄城1加田I.下夙aii端晶地蟠tX.小,社局切械章空肥计.掰工悯的倒耨靛导出三个孤蜥”.人 更嘉晟九战触捺比幽Mb 卸3 -“ = %An =加-加=穴.*-1麻=| + %-1 +- 6L-1|*-L & %(心卜“内T + Mt-1 + 口上-例1k-J + “

24、) -=隗上7 -此为中土b1 - £；%T + K；依=1 -八=斤1)+ Q -即%)%.1嗨=1 + 口 -儿加-端Uhl - &岭要求E(品，二0.EPE/G =*N*Th J- Mi 凡H也上T -3F7)+人闻h.滴帆t) +(»凡瓦- (；)的-)卜口因为时111)=。. EWi);。，E(Vjt) = 0 且£(*1)¥0, Ed-1)中 o所助须足除卜1 -及必轧山7 - K =。设二外/T 一&此,心】.ft即f -KkHk-Kk = 0K； = 1- KkHk 将式口泗代同式中.得到均上二(外/一1 一 K春见/虑_

25、)鼠_情_i + KkZk +。一 K/Quri=巾*品-1他-1 + *k 体1 %他/一国-ill + uk-l) + ufc-l= fc|fc-l +雁f -之桃7)(式子4 )其中几41 =中门-1心71 +见.它农小的占山日叶制的系统状态例通过条统建华来的汁3的勺前时刻向状 志化iHk %1 = &输1，它仍油当前时刻估计值脑过观澜鹿也矩阵估计出来当前时刻的双窝量，它号力 到实际也割哂之间的差即为当前时见的晚洲浙皂系统状态估计言预测状态+卡尔曼滤波系数*观测新息这个卡尔曼澹被系数均是年次法代根指系统咕计和煤由佑计动态调整.根心堪中 T计算它，使得岫*是打法的最小方趋估计，即使将先席的方差矩阵生夬的值最小将式（式子4） ftUu中.得到:=£埠即丫广备（小后十 口广册/* J=U- 4见&* -%或市一 i - v=i-小用 乂上中一 %n j -=（，一即跖）%tT - & ,为了计。口比现几出的方毡饰/加的