无人机状态估计

1.1组合导航从上章介绍飞行器需要获得自己的十五个状态信息，需要通过传感器测量并推算。单独的惯性测量元件由于误差累积，导致随时间增长发发散。因此需要通过额外的传感器信息来进行互补修正。组合导航技术结合GNSS惯性测量元件、磁力计、气压计和超声波等传感设备，利用各自的有点互补，采用数字信号处理技术融合多传感器的测量值，从而获得较为准确的飞行器的十五个状态空间量，通过一定的算法和规律尽可能的估计出与真实状态量接近的量。惯性测量元件容易发散，从而这个发散可以通过GNS轮球卫星定位系统来获取修正和抑制，然而GNSS有在室外无遮挡的环境下才能得到较好的信号强度和定位信息，当信号质量较差或者位于室内导致无法定位的时候，系统则需要由惯性元件来短时间抑制和修正。GNSS勺高度信息通常误差较大，这时候可以通过超声波传感器和气压传感器进行一定程度的修正。组合导航技术中传感器互补的思想原理是由克劳德-香农在1948年的信息论中提出。信息论的线性估计理论让数字处理工程师在系统设计时能够把多个具有误差的传感器通过数学方程组合矩阵运算融合起来，利用传感器信息估计特定的状态量，并且理论上提出了更多的传感器状态信息参与互补运算，就能够得到更好的状态估计（更接近真值，并且方差更小）。1.2飞行器的坐标系三维笛卡尔支教坐标系?三维笛卡尔直角坐标系是指三条相互垂直的坐标轴（正交）相交形成的坐标系，其中相交的点称为原点O（origin）。?每个轴都指向特定的方向，两个不同的坐标轴决定一个平面，也叫笛卡尔平面。?通常3个轴采用右手系标定，即右手的拇指方向指向X轴的正方向，食指则指向Y轴方向，中指指向的是Z轴方向。?此外对于围绕每个轴的正旋转方向，则是用右手大拇指对应每个轴的正方向，则其他四指自然弯曲的方向为围绕该轴旋转的正方向。本文所介绍的软件和算法均采用右手定则刚体刚体（RigidBody）的意思是指物体在任何外力的作用下，它的体积和形状都不会发生改变。这是力学范畴的一个理想抽象模型概念，实际情况任何物体在受外力作用的时候都会或多或少的发生形变，并不存在真正意义上的刚体。但如果形变本身相对丁物体的运动微乎其微，可以忽略，或者并不影响我们分析物体的运动结果，则可以在特定的情况下有条件的忽略这种形变，即将物体假设成刚体。其中质心运动的分析范畴届丁牛顿第二定律定义的质点运动与受力之间的关系，即主要研究飞行器质心的位置和速度信息。而转动的建模方程则届丁欧拉方程所分析的模型运动关系，即主要研究飞行器的姿态信息。要研究以上两种运动的模型和信息，需要定义两个三维笛卡尔直角坐标系：大地坐标系-（或惯性坐标系，用e或者G表示）用丁研究飞行器相对大地的运动状态以及空间位置坐标。通常飞控都是把地面起飞点选取为该坐标系的坐标原点，即初始化x,y,z坐标为0。通常大地坐标系的X-Y-Z方向选取为大地的北-东-地。机体坐标系-（用b或B表示）坐标原点取机体的重心，用丁研究飞行器相对丁重心的旋转运动。通常机体坐标系的X-Y-Z方向选取为机身的前-右-下。机体坐标系是固定在机体上随着机体的运动而运动。1.3方向余弦矩阵和欧拉角63.1方向余勰阵定义桐以采用对应三个位矢量来表示蝴蛭泓对于大地座盛.三蚀对成的城量邠提啊体蛔b三倾醐分腱聃蝴蝴娜啤敛1股地地晰I。上，毗剜式lilcos(XJ)|i|/|co$(/J)IIr=；=i<|co$(n)=IWIW)Xri0ig«<)Ki海朝牖一.体蛔防1i/Ikco$(l,i)co$(/,/)cos(/,4)/iJI/*=cos(J.i)cos(/)cos/.*)KiKJKkco$(K,i)co$(K./)cos(K,fc)63.1方向余湘地翊脚«缺1邮也可以敬I体蛎&4赫出来,MHliJiKiA)K*=ljIIKIIkIkKkcoXU)cosi/i)co$(lj)co«/J)co$(/,k)cos(/,k)cos"CO5(K,/)cos(KJt)这两饷定义了从也标系的单晚辗到X坐标系的瞰崩或都物式.这两馔阵即称为方向余魏阵（DirectionCosineMatrix）6.3.2姿态与艇角利用蜘翊胸牌s做a脚喇慨主邮激相跚糟魏.枷e瘢（或者敬）黝是飞行嬲涮）/皿同政WK了艇个当顺娜®3跚嗷计U晦蜓师麴-漩T.对于空曜BYIW系,薜跳瞰W觎何I烤任就晦,响射系剧止不9虬晌献逊贞螂受标系,做姓漩顺部咻谜娥碰说这三颁角分别蛔靳横渣M晰M，双俯仰角横滚角偏航角1.3.2姿态与欧拉角在定义欧拉角的时候，相同的姿态不同的取法会得到不同的欧拉角。本书采用的取法顺序为Z-Y-X，即绕三个轴的旋转顺序是先绕Z轴，再绕Y轴，再绕X轴，最终达到一定的姿态。而对丁同样的姿态，如果绕轴的顺序不同，则得到的三个欧拉角也不相同。因此按Z-Y-X的定义进一步来说，本书所定义的欧拉角（pM）e-推与好醐林碱正偏敬（方位知W-顷-似方向在地平面的膨与正北（M）Z间的夹制北骸为正镰角见港铜13-机假衅焊兢转眦.撤为正姿态解算(attitudealgorithm)也叫姿态分析或者姿态估计，即指飞控系统或者惯导系统根据惯性测量单元(IMU)陀螺仪，加速度计，磁力计等传感器的实时信息求解飞行器的欧拉角的算法过程，因此也叫IMU数据融合(IMUDatafusing)。633欧拉角的定轴转动表示矩阵融皿z电可却岫A&的定虬可朔tERffl跋BS岫*系韵三峋Wt*,峙到机体坐标系上施H三欢有琳窿痢S第-次围绕旋转心勘瓶转W变成ox1v1zl显然此次旋转新的坐标系与老坐标系之间的关系(投影)COS(psin(p)sin(也)cos(p)cos(ip)sin(W)0段=sin(p)cos(W)0-001第二次旋转围绕匕轴旋转仇旋OX2Y2Z2显然此次旋转新的坐标系与老坐标系之间的关系（投影）sin(0)0cos(8)cos(O)001sin(8)0rcos(0)0sin(9>Rf=010sin(8)0cos(G)一第三次旋转围绕汗2轴旋转巾，旋转到OX/bZb显然此次旋转新的坐标系与老坐标系之间的关系（投影）n100Rg=0cos(伊)sin(p0sin(p)cos(<p)虽然飞行器的姿态是随机转动，而并不是依次沿三轴旋转而成，我们仍然可以依靠欧拉角的三次旋转来还原出飞行器的姿态信息。而且如果旋转的三轴顺序不同，得到的旋转矩阵也不同，并且欧拉角也不相等，三次旋转先后组合在一起即可以形成从惯性大地坐标系e到机体坐标系b的旋转矩阵：-sui(tf)'Q1。阿£=踽擀=。娜仲）$附）0I。-sinffl啪仲毗）顽*康即)CU5(0)*血(河-血仞sta(*)*§iu(0)»o(f)-嫩。)嵋)如(。)sin(4)9n(f)+a»(*)uos(*)血物cos($，血8),娜闸+sin阑血珅印解”$仙(办和(*)-抽闸*cos®)cos由),仍的pl12财球9蛔挪圈0肚21hl23,li此顾问啪蜘伽r3132诳=arctanUMJ23=-arcsin(r13),$=arttan(r33)方向余弦麟的几椭§直跚防向斜拒晚就HIS丸欧柿斜E屈是日陵踪葡饪爵Hg斩黑布皿睡间上肪万都秫（GimtMlhrt）理，蝮93三个缺械冲间浙I睇游J成惭t第-烛籍阿煽瞻催合.瑚三烛醐站蚁641宙3甦军丑留膏(留.Ibr监西心畏圳亨iM-M/q.wgst目哉善其iis64富潜3段Q(qMgMTw+n+qJ+*.J!.*n-kc*j"kJ*kHL73j二"k-k*jnlL;k;j0X-10*D38'Ti*%。；gf<*<R8iBa«-n.-«矿«Q.q“u+php7a¥pj*pj3Q阡p"(<JO±P+(glI4-pJ+3阡P2)j+(g3iaPQ”(PHJ+sk)(qogJ+£+)HMIsP31p>+(PA+pa+2-15)Apow+PW。十P+-p一W)J(pow十gogqpx)knro.n-ryr*641四元数的赦敏形胡版=o17IJqqqq3I23-p;w弟四-P2n%Jpppp=FolnqlnI=Q.p或者11Jpppp3123wgg642四元数与旋转的关系四元敏是可以描还三间中林的旋转匿中三维空间(，)林佛上的任意一点Pk以)点要围绕通过原点的矢星晰旋整确度,转呼(丫以)点上.将p点用齐次坐标来衰示为四元数心0。定义Q为任意单位四元数，则W“rW2),co$(8/2)H馈的程皓可以将吹量困绕v矢量辅旋转确到尸近地首先.对于单位四元加如鼬行稣乘法的话，并不影响旋转本身.围境Q与$如为标)所对应的旋药是一样的，显然(sQ)%Q)yQPQ<f如：=QPQ:=QPQ欺.刷四元数的实数部分可以写成=(Q+QW2,因此QPQ的标实部(QPQ"四二竺-=竺平四=咛*1=QR(P)Q=R(P),因此对于四元数的实部在绶过旋转Q后不会改变,此外迥钵身不会议变虚部矢部分的大小.因此|P|=|P'|.6.4.2四元数与旋转的关系粘汗直为耋直p点与，点的共面且垂直于玲转蛔定义vo是四元数9矢脚分pH昭部上的投影矢乳是四元肿的矢量部分"在旋转平面上的投影知L因雌当于矢量刃围戴卑E质药痼度到这里假谕1是该雄湖海，城）矢量,这里侃设矢量均为单位矢.这律翱vOvl=e£由于炭膈垂直于醐投影面.旋转瑁位矢量加=告我现在我们定义用于8述该位转的四元放Q*ItWXVllvO*=(voxvl.vOvll=|vsuij.cos此外令y2=QvOQ只要证明"与叫的央角g,嫩用Oft从幅刃的I龄四元ftN到的网EtcRvzvr=Qvo(?r=(vivoj>r=vovo(vrvr),由于单ti5dtvM=Tvivi=-i,因ftv2*vr=叶血#保5抑，这表版与力的平面以加1与榆平面重合，并且夹角稠，均冷醐"与徇夹角为&因此这里定义的V2=伽。即是P知;传平面上的投影，是薄朋调还的!跄四元蛔裙的矢代(VOXV1)ilVOXvlj|既然四元敬可用来描述旋血那么就可以用它来插述从大地坐町系。到机体坐标系b之间的转虬岫胡它来描困.3.1所介绍的方向余弦炬阵.在H又赖2泰永元的板性导航）一文中922节培出了四元数与方向余弦炬岸的对应关系和详细的推导过秘这里简酗给出一下培论机体以心叩+W+心做运动，从QA*到oryU为垂直于可的单位嘲曲且广二经运何队”=（/+虫两+&/扣0尸3"从而得到0即为机体坐标熟到大地坐舔的方向余弦矩阵.郎：ggg=/+l/2sin-cos-+2sin*-(/y22200-nsln-0msln-(mi+nz)sin：/msin2y6fitsin2-000nsin-0-Isinj+2/msin2-(F+n2)sin2-mnsin-e-msin-fosin-0加sin%mnsm-2R2e-(m2+r)sin2-100010+2msy001Z92%二叫具初n0”；赳踽胞澳洒燃暨岷g广崛Q=酎+。山仙°砺*晦)血-=cosy/血;代椅孙痢"耕q同*(132(仙2-糊3)2(佃3+初2)=2(徊2+他3)必-q；+q；-2(初3-qWi)2(仙3-初2)2(仙3+QoQi)qf+q四元数与转蛔的对应关系.因岫四元敏对飞行剧姿态进行估计，值每倾采样嘛环送代远K出新的四元岫并疝就可以获舫向余勰阵,从砌螂周瞬过晰感器飓观新的咐信瞬这翻四元鼬行蜓，由于匹元数Q=cos捉(扁+m/0+nko)sin；=cos；+u(sin嘴翊边进行粉可以得到dQ10de1ededue0=-sin-r+ue-cos-+-sin-dt22dt22dtdt21/0B=-a)eIcos-+uesin-乙乙J11H=-a)e-Q=-Q-a)b乙乙1/、=5(qO+qli+q2j+q3k)(a)xi+a)yj+a)zk)=QbQIt中/0一3工F八10<i)y2<i)y0S0/口b=因此采用一阶龙格库塔法，q(t+T)=q(t)+丁*专=机上)+,展开后可以得到四元数每部分的更新(-wxQi一外如一3四3)7%=%2顿血一a)yq3+纯如)2(斜钓+仞叫0-3次1)丁如=+Qi=91+2(一皓+yQi+小血)丁的=如+卡尔曼滤波卡尔曼滤波是一种系统状态观测估计器，用来描述一个系统的输入、输出和内部递归迭代及重构的方式。卡尔曼滤波作为一个估计算法，主要基丁系统状态空间的方法，因此也届丁第七章所谈到的现代控制理论。卡尔曼滤波使用系统模型的迭代方程(或者微分方程)计算出理论输出模型在丁实际观测相比较，乘上比例因子形成状态反馈，将系统状态重构偏差迭代消除，滤波器不但对系统进行模型抽象，同时建立量测噪声和系统噪声模型，并通过测量与理论的比较结果估计出噪声能量。卡尔曼滤波器是一种递推线性最小方差估计的算法。对丁解决大部分的问题，是最优，效率最高，也是最有用的。卡尔曼滤波的广泛应用超过30年，包括机器人控制、导航，传感器数据融合，信号处理，计算机图像处理等等。它的特点主要是无偏性E(x)=X.即系统状态的估计值的数学期望等丁点值估计方差D(i)=E(iz)-E(x)7最小，即任何一个其他的估计，所对成的方差都大于滤波器的估计值的方差实时性，采用递推方式，减小运算量，实时输出运算滤波结果卡尔曼滤波公式:系统状态方程.也=叫籍7将-1+&卜iMt观测公式：虹上_】为某嫉状态空间传将粗阵.¥卜1为上时刻的纂统输入量,珞为泰统啪小贿牝冲卜为系统嵯声，既为僵荆距阵，无为观测嵯声-奏施噪声与量嗥声的统梢膊为高斯白噤声.=G.且喽声力至上阵为系统唉声方差畋：心=QAi=07/测是财方差矩解：龄=毒跖=枝；I；离散卡尔曼滤波器的目的就是要通过系统上一时刻的状态和输入，以及当前时刻的观测来估计出当前时刻的状态，并且它是最小方差的无偏估计。对丁线性系统来说，它也是这几部分的线性叠加，即式Xkk=KRk-lk-l+KkZk+KUk-i（3）初曼滤波拼kii«袖岫i,mb猊曲制响醐咐境ifi谶gw,育噎敬1沛和.下瓯aii城|屋岫蝴th川.社I醐蹴E空驱讯拂娴副的估刑要醐黜三个系献；，、耍款.是卜棉倒味此聊治*%叫）=&知二堀-知=%aN-1+"k-】+4,k-iw<-i-K*m-心佻仲耻_1+ui-l+vk）=（蝠-LT冉如TY4+敬JVi|fr-i）+（JW#阳.曲T+*-珈z-3要求E（而|k）二。.EP雎浦=昨以却4浙4小帆T）却T饱4）昨I）W"。因为£侃-111）=。,E（阳1）=0,E（vk）=0E（瞄I）莉所以必须满足由虹卜1*5花卜1-段二0l-KkHk-K=0任并顷T一出坂电aT即3F成将式，代回式中徉到才堆=SalKkH雄+KjfcZ*+(/-K/Quri=稣山-1版-1伏-1+Kk(Z/t-HkWkk-出-1诉-1+uk-l)+ufc-l你心-2邮-J(式子4)4中跖I二牝+岫它表小的.匕】叫曲割的系?锹态估计位迎探统建隼来也H用当前时郭的状态信i/L如_】=临心它应亦由当前时刻估计值通过观测模型炬阵彷计也来当前时刻的周测量，它。i-j到实际说测伯之间的是即为叫前时宛的财圳的息系统状态估计M预测状态+卡尔曼滤波系数观测新息这个卡尔曼渡镀系数也星每次迭代帜幡瘵蜕估计和噪声估计动态调整.检心地中TitK它，使得f堆是JT咖的最小方贫估计即使得*庸的方差矩阵P蛔的值最小将式（式子4）代入4|南黑到：£*诉Xt|k了刮*itR-iIK.（E|J=知-跖卜】-K*伊冉+十堆.J=U-U-上场虫阡I-上队二凡|L%*-J-临介=（J也H&）知t-女吨为了计耳并实现士I肪差扼阶邮的琉姓小.定义如=E（%MJ）=E（u-品舶林T-v*）r=U出卜）UT汛产+川映妃=u-u-+w/式子区申F*|k*l=E（Yk|kT?ijH）=.JkT斗-1|卜1孔1，威r财的遍（）bpgL（k血S-J+上饱4|Z叩+=-麻佻快-J）+打佻伉为卜W十乩"）K他为廿（外估）达到暖小值时曲临.虹便君它时JK*们柠数为即淑M%）二次何俱II）-2+"Mm+时矽）=0山土计（碱f）=24B土年（曲?）=尸并由如卜1是对幡矩阵.因此云H（构*）*口£”阪（为4卜述【+於）。即*睑=加1旋（叩如T町十诳广E式代入到（式了5）中,可式代到当前估计值的误差方差阵表示:p刘A二PkE（队伍4|1）-WhPk"+KkQ"邮-1叩+电）略=Pitu-KMPkk-i二（f-KM）%由以上可以得出卡尔曼滤波中的主要公式，其中如何使用这些公式在数字信号中进行循环迭代处理运算，其中有关卡尔曼滤波的迭代运算符如图所示卡尔曼滤波的运算流程可以分为以下五步骤，也是俗称的卡尔曼滤波的五个公式：第一步根据系统状态迁移方程和上一时刻的估计状态估计当前时刻的状态kk-l=ktk-lk-lk-l+ufc-l第二步由上一时刻估计误差协方差来预测当前估计误差的协方差P/cfc-l中k*k-lPfc-llk-l"kjc-1+Qk-1第三步由误差协方差和观测矩阵来计算卡尔曼增益Kk=PkZE（H加+&）T第四步使用卡尔曼滤波增益对第一步的估计值进行观测量的补偿校正-京&诉一1+Kk(Z-第五步对第二步的估计误差协方差进行校正Pkk=(，一KjgHQP虬卜1