无人机(PX4)状态估计

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

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

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

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

5、y,z坐标为0。通常大地坐标系的X-Y-Z方向选取为 大地的北-东-地。机体坐标系-（用b或B表示）坐标原点取机体的重心，用于研究飞行器相对于重心的旋转运动。通常机体坐标系的X-Y-Z方向选取为机身的前-右-下。机体坐标系是固定在机体上随着机体的运动而运动。% n1.3方向余弦矩阵和欧拉角631方向余弦矩阵0r 0 fI r01000 /I40001蝴ttxitt剤妖1&B如麻e上 网荷式MIIMM/J)IIr?S i(if(ro3帅网f)=n|f|co5(r)gKIcostlU),Ki*合起果形J个般.这侄阵加I体则応大JS坐戚上空投擁示I i / I kw$(rf) $(fj) co$(f

6、4)r. r. VI =)1 Ji /*=co5(/,f) cosy 7)esij.lt)Ki Ki K kcoHKJ) cos(lf./l cos(K.ft)6.3.1方向余3潮疋MM旳涟匚矢協塚以WX坐砾沖舫岀来,MU1 i 1 i Kico$(M) co/7) cos(K0f, K*| =/ 丿/ Ki=fo$Ct/) /) co$(K/)Ik Ik K kw( co$(/i) nsK, t)这两i制锭义了从f坐标系89单位耕投瀏另一个坐縣蹦换方式,或者称表达方式.这两链 阵瞞为方向鑼阵(Direction Cosine Matrix)632姿态抽:礁计 ndSHSKissr)AffiS

7、3hem利用曲岱醐删硕删洲州r主fi稅朋1劇細懿xAvo$ (輻泗 ism宦啲O 卿働se磁糕坍为穷睚I約乳QWtSB咖阿1观E他A師.赫D址不讷1 ail厕啟她颐I而郦姓标硕制刪獅族.这三个购岭测抑k帕飢WM札俯仰角横滚角偏航角1.3.2姿态与欧拉角在定义欧拉角的时候，相同的姿态不同的取法会得到不同的欧拉角。本书 采用的取法顺序为Z-Y-X，即绕三个轴的旋转顺序是先绕 Z轴，再绕丫轴，再 绕X轴，最终达到一定的姿态。而对于同样的姿态，如果绕轴的顺序不同，则得到的三个欧拉角也不相同。 因此按Z-Y-X的定义进一步来说，本书所定义的欧拉角 訓皿-M与水忡1敵飢抬头师脯触加触叩F - m方向任时翻

8、疏北d啲）Z间楸躺北偏东W翩鹹伽1皿-W翔m轴鞠就帕叽右勅正姿态解算(attitude algorithm )也叫姿态分析或者姿态估计，即指飞控 系统或者惯导系统根据惯性测量单元(IMU陀螺仪，加速度计，磁力计等传感 器的实时信息求解飞行器的欧拉角的算法过程，因此也叫IMU数据融合(IMUData fusing )。633欧拉S的定轴转动表示矩阵(&讎工2中，可亂醇应礙也.可恣联甘姿雄火甬运粧苦时讣msE即；.r V碟用曲笙41察上傅丈m閒將速蚌濤为忍 0野叫町 0.1,?,J第一次围疑就转柚腌转4变成釦XeV1#*显然此次旋转新的坐标系与老坐标系之间的关系(投影)內zj COS(0)sin(

9、仞0sin(0)cos(ip)05in(0)cos(0)000 :L cos(i/) 心=一 sin(0) 0第二次旋转围绕匕轴旋转优旋转到OX2F2Z2Xi显然此次旋转新的坐标系与老坐标系之间的关系（投影）卩2cos(0) 0-sin(&)cos(0)0.sin(e)显然此次旋转新的坐标系与老坐标系之间的关系（投影）sm(&y0cos(0).-sin(0)0cos 一第三次旋转围绕*2轴旋转e,旋转到OXMbZb%丫24ElkJ=0cos)_sin)0sin(爭)COS(0)r2 =0sinCOS(J9)0cos(卩)_sin(爭)而并不是依次沿三轴旋转而成，我们仍然 可以依靠欧拉角的三次旋

10、转来还原出飞行器的姿态信息。而且如果旋转的三轴 顺序不同，得到的旋转矩阵也不同，并且欧拉角也不相等，三次旋转先后组合 在一起即可以形成从惯性大地坐标系虽然飞行器的姿态是随机转动,e到机体坐标系b的旋转矩阵:1 0 0时=JifRifi； = 0 怀侧Sin(*)0 -sin(0)加斬血Wfl$砒)0 -sin(a)| casMJ 0 -sfnWID涮申)costp)0-sinffl)师3) * sinjiff) *血仲)-険仲)*血他)加(0)叮m斶*前(*)+C05仲伽啊SR(4)-血 co砂)sin(fl)*CO5.(r) + sin 少卜 5训0) cos(#) * $in(B) * s

11、in悼卜 $iii何 cd5 (0) m聃)tcos(fl(ni“222.13湎曲可I馮！懈腿酬SS0 = arctan U = - arcsintr) ,* = arc凶丿方向余髓阵的几趣 輕礁苗販餓課挾丸 肋錨般是33俪.&竹!#瀏邃祈 即盒變勵咖异点” i2詢期純t厭万肺臘伽砌耐）08,醺8壬t（删沖昵个辙asoSBW, IK玻輛幽汝1附附詩殛合.弱0解*i蚁641四元般定义05册测卿嵯9限越曲9分析)1你争脚輙池额于另JUn翻诜(辭0O,摘. (顼備Hom,mi 銅颂.胡St (OMtemions) 1轴馭空播(WliarntowanHamiltOf., 1805*1865在 184嘯

12、ttj瞬牌陰第Kfi晰播 mt删翻晦间，可以他晦圏Lh爼，也可以射fi. 05遗的馴法9谢淞 !.q?此外定义23元咖5臥B四元imWI = Jqj + qjqjBW日TtWB野aS为壽.这豪示单位四越 eraWOiSEQj鲁,对于单位E呵如=叭0?00算沁2&9) + PJ+P2/ + P*Q + P (%Po)+(91 土 Pl + (务 土 旳+(务 内WP Q = (Po+P2P2Mp/)(+wq“r*)=伉% - PWl 臥2 - P W J + (Mj + PW(+P 用 PW)*(P|.也Q为任広单位atfil lHQ=（v$m（e/2）e2）1代&的fi潇可以将咲珈轴S转确鳞Q

13、Q首先.对于单位四元如如臬进行标乗法的话，并不总响庚转本身 a軸与$叶为标所对应的8带是一显侬$Q）pwjF好QMT01 =QPQ 1=QPQ其次.即四硕9?a螂分可以写成 = （Q + Q）/2 BtoPQ瞒实腳iQPQ）丫加jyQJ =答旦（?R（W = （/）, B此对于0元数片血创的实都在经过旋转碼不会改变.此外筋本势不合改变JttB矢都分的大小，EJttlPI = n.642四元数与旋转的关系和为董即点与FQSS重a于S除鹹毂刈碾ts分阳眉評矢2量酰期eis附维養 斛而删彭矢期殆于EBB颔瞒册血M.设V1是iStX）瞞歸磴转掀！彦極均为单位2MvOv24由于加垂亶于捷转投甌 度删草剜

14、加二辎 SHWi定义肪船6翩酰KQ =小vOFWxvlM叫= |vsln*g% Jlfc?h2 = QM)Q；只夏证 19说与vlffHltM略澜#20转EtcRv! vf =QvOQr =9v0rl v03*yf = QvOv(Hvfvr),由于单位矢v(M) = h rlvl= -I, Qttv2vr =Q屮州g*小心 诂斓说与加评fiwsbd与如面合,相夹觥執均粕0W说与朋痢牺Bjtta販则 v2 = MT即肿知血平面上BW.是能过朝HS述繊餐酰I刚g的粗朗四预可用来描扳转.都么可以用它JIU魁从大地处輙昨堂尿和之间的转换.g处用它皿631而沟的方同他 阵.在阿文系21鼻水元的05性称）

15、一文中922节给出了四元敷与方向余弦矩阵09对应关累和详细的捲过私这劉卿幽岀一下 结论机体以血+的卄妬媲转运动，从04到U为垂于知的单位剛辄且/二程丽亀宀（/ + m我+岔”訓片“门Mif5妙为机体對蘇网大地竖議栃向爺膜庫H3： 酣=00& -n sln-0 mstn-（* +内码0Imsin-0(RSln-0 Rsin-0-hin|+ 2/msin-(F + *)引1?mnsin -8-mn-,.0 *5in-0/nsin -mmin*-(iz + F)$id-1 0 00 1 0 + 2cos 丁0 0 1 2e2其中U.:；-m ( I勺h吋91= Sin-Q=臥+Ianj代貓向兪荻鳞R

16、$2(加3 +牺2)2(蚀-碱H+gqhQi-QhQa 2(9-牺3) =2?2+舸3) U-qf+q；-苗 2(593-9092)2(923+ Q091)酰酬飞行B咖时 fBmWS样测环四元地.%就帧得楠極竄从ii55flSHffl035沁姗脈个附采徉周IH过诉的传mis息迭代出釉四硕眺7这ita采用划测湖四顽狮呀虬由于匹元数Q = cos + (% +叫+ nko) sin * cos + i? sin 膽钢翅行微分可以得到dQdi1 d de Q 1 e dd du* e-sin - + - cos + ； - sin 2 2dt22dtdt21/00= I cos+- sin-厶厶厶=

17、扣e Q =扣 3“=d0 + qli + q2j + Q3k) -(3/ + 3yj + 3zk) = 0乃 Q idy叫U估il方羞D（左）=E（护）E（f2最小，即任何一个其他的估汁，所对应的0-畋因此采用一阶龙格库塔法* q（t + T） = q（t） + T *譽=曲）+ 丁心曲）， 展开后町以得到四尤数毎部分的更新,（一Ml 一卩他一加9d 二耳02（3的0 一督巧+ 3畫血）丁Q1 二如 +2（-心2 +切1 +叫）丁卡尔曼滤波卡尔曼滤波是一种系统状态观测估计器，用来描述一个系统的输入、输出 和内部递归迭代及重构的方式。卡尔曼滤波作为一个估计算法，主要基于系统 状态空间的方法，因

18、此也属于第七章所谈到的现代控制理论。卡尔曼滤波使用 系统模型的迭代方程（或者微分方程）计算出理论输出模型在于实际观测相比 较，乘上比例因子形成状态反馈，将系统状态重构偏差迭代消除，滤波器不但 对系统进行模型抽象，同时建立量测噪声和系统噪声模型，并通过测量与理论 的比较结果估计出噪声能量。卡尔曼滤波器是一种递推线性最小方差估计的算法。对于解决大部分的问 题，是最优，效率最高，也是最有用的。卡尔曼滤波的广泛应用超过30年，包括机器人控制、导航，传感器数据融合，信号处理，计算机图像处理等等。它 的特点主要是无偏性EOt） = JC即系统状态的怙计值的數学期里等丁贞值方总都大于滤波器的估计值的方筮实时

19、性，采用递推方式*减小运as.实时输出运算滤波鉛果卡尔曼滤波公式:蹄!yi态方程：X* = g-Xx-i +44-1 + Ehi呛7规测公式：坯=HffXg + Vit%-为系绞找査唱榔甬阵，沟上 側闸的娠绒轸人叭 扎上“対燄纸戕2慕暫.W4山報汕胪.股为 跟制辿闻、观制樓類絢秦声U划血型出的址计將tv届閒DRfe飙I和W讨= AW = u.赵臥陷力土3 昨対砂沪方差矩斧： 陰=Q点/ = 代；/测砂出方差矩阵；% = R述肝二 比离散卡尔曼滤波器的目的就是要通过系统上一时刻的状态和输入，以及当 前时刻的观测来估计出当前时刻的状态，并且它是最小方差的无偏估计。对于 线性系统来说，它也是这几部分

20、的线性叠加，即式k|k - K岛7出7 + K血 + Kfl/k-i 卡尔曼滤波式辄憐弼搦的恒川讨農而!过卜谢删酬时鈕的醐觀运札顶掃嗫示逊为fe刑！下亂18此JI囲割齢计. 九忙圖吗期橄烝空酯讯彌嵋册洁计趕翻导出2?飆曲，陰K 麟b是賂植踽讯职环）= %&W昙柿1肪*二怙-1&7 +叽】+ &杓,昭-1 -KJh -治佩艸昭山_】+业-L +心-卅-）+ %）-K；iz=W皿L - K川曲卜厂；“卜】+ K；（g -hz） + U-知心耳叽】+ U-V.-弗町I -K声*鉴求甲綁）7即E& J+ 他I -#曲2 -畤叫）3 - 曲加卜同科+ Q - KiA-K疋仙U因为E仇i|l) = S E

21、觥-L)詁 f(Vjt) = 0E(hjm)羊 0献幽満足-歸血 *- - K；二0f-KMk-Kf = 0用屠】*戚1 （1） 即畤“-K戚将冗代冋式国中.得到（式子心九慣=（眛41 N歸枷圮1）九屮1 + K血+ - K何）町1 =U-lfc-l|k-l + Kr（2氏-撫触七_偲_屮一1 + % J）+ 叫一1 =+ 心& -张-1）真中鼬I =咿*1卩71 +吐小 宣&】的足由口时刻询系缩状态闹骨通过条紀离老和古汁出的W时刻的披 态倍iHiL 11 = Fh人它血示山M前时刻佔讣恆他过观测極t!砸阵i占讣出来与li曲刻的现憾臥它屮4洞时 刻宪酿电利（A 的疑lit为 片前跡刻的世罚閒总系统状态俗计-预测状态+卡尔曼滤波系数观範新息这个M履滤波累散陷是辐忙迭代枢盅系址牯1|和噱声佔i1効态鳴整.檢心聘卩Tim它使得爲出是K屮的爪小方星仙计.即吏得心血方总爪阵叫出的値恥卜将式（式子4）代”烯中.到：f*|F = X屮 f|* 二屁* -户屮7 -氐仏-2*|k-J=応-际厂心佃儿+廿町如J=（-丘檢口和r - “问卜,-心山= （/-耳鬥川匸屮-屮一 -SS U - K何风问-心%为了计貝卩域现砧方愼疇聯的壬血小远d加=f陰|風J）話（W弋叫凤m -跆jW -畑風|-恥J）=（卜店叽M（电业JW -KiAF卜氐F趴