多旋翼飞行器设计与控制理论 -第09讲_状态估计V2.pptx

2024/5/281如何对多传感器信息进行融合？前言

大纲1.姿态估计2.位置估计3.速度估计4.障碍估计5.本讲小结2024/5/282

1.姿态估计2024/5/283测量原理（1）俯仰角和滚转角测量原理根据第六讲气动阻力模型，忽略速度与角速度的交叉项，比力满足其中是加速度计测量值。当系统处于稳定状态，比如匀速运动或者静止，即

1.姿态估计2024/5/284测量原理（1）俯仰角和滚转角测量原理因此，低频的俯仰角和滚转角信号观测量可以由加速度计读数近似获得，表示如下其中是加速度计测量值。注：如果机体振动很大，那么将被噪声严重污染，这样将进一步影响角度的估计。因此，机体的减振很重要。

1.姿态估计2024/5/285测量原理（2）偏航角测量原理图9.1地磁场示意图（感谢爱丁堡大学PeterReid提供的图片）磁轴和地轴不是重合的。磁场在地球表面的水平投影不是严格指向地轴。磁偏角是磁场强度矢量的水平投影与正北方向之间的夹角，即磁子午线与地理子午线之间的夹角。如果磁场强度矢量的指向偏向正北方向以东称东偏，偏向正北方向以西称西偏。各个地方的磁偏角不同，而且由于磁极也处在运动之中，某一地点磁偏角会随时间而改变。因此，要精确找到正北方向，需要经过两步。

1.姿态估计2024/5/286测量原理（2）偏航角测量原理1）第一步，确定磁场方向在水平面的向量，求出方位角。磁力计的读数为。考虑到磁力计可能不是水平放置，所以需要利用两轴倾角传感器测量的角度将磁力计的测量值投影到水平面。因此，先做如下变换[1]其中表示磁力计读数在水平面的投影。[1]CarusoMJ.Applicationsofmagnetoresistivesensorsinnavigationsystems[R].SAETechnicalPaper,1997.

1.姿态估计2024/5/287测量原理（2）偏航角测量原理1）第一步，确定磁场方向在水平面的向量，求出方位角。定义，那么可以表示为：定义，那么可以表示为：机头顺时针方向转动，偏航角为正。

1.姿态估计2024/5/288测量原理（2）偏航角测量原理2）第二步，加上或减去磁偏角修正到正北方向。北京磁偏角约为6°偏西。因此，在北京磁场方向上加上6°的磁偏角，能找到正北方。图9.22015年世界磁偏角分布图（图片源自/geomag/WMM/image.shtml）

1.姿态估计2024/5/289测量原理（2）偏航角测量原理2）第二步，加上或减去磁偏角修正到正北方向。半自主飞行时，我们可以将地球固连坐标系轴指向本地磁场方向，如左图所示。从图中看出，本地磁场偏西。当作全自主飞行时，因为需要与地球的经纬度一致，我们可以将地球固连坐标系轴指向正北方向。图9.3本地磁场和正北方向

1.姿态估计2024/5/2810测量原理（2）偏航角测量原理对于体型较大的飞行器，可以分别在机头和机尾安装GPS天线。通过测量它们的地理位置，来确定飞行器的偏航角。然而，对于小型多旋翼来说，安装空间很小，多个GPS之间基线很短，较难准确测定偏航角，除非利用高精度差分GPS。如右图为差分GPS测向的原理。在多旋翼机头机尾装上两个差分GPS移动站（或双天线GPS），可以获取它们在地球固连坐标系下厘米级的定位精度，根据两点确定一条直线的方法，可以较简单地获取多旋翼的机头朝向。注：两移动站分的越开，精度越高，距离越短精度越低，一般需要大于30cm。差分GPS测向原理

1.姿态估计2024/5/2811线性互补滤波器姿态角变化率和陀螺仪的角速度有如下关系因为认为多旋翼工作过程中，所以上式近似为姿态角可以由加速度计和电子罗盘测量得到，漂移小，但噪声大。另一方面，姿态角也可以通过陀螺仪测得的角速度积分得到，该方法噪声小，但漂移大。互补滤波器的基本思想是利用它们各自的优势，在频域上特征互补，得到更精确的姿态角。

1.姿态估计2024/5/2812线性互补滤波器下面我们着重以俯仰角为例，详细推导线性互补滤波器。俯仰角的拉氏变换可以表示为为了估计俯仰角，以上式子的需要用传感器信息替代。低通滤波器，表示时间常数高通滤波器1）加速度计测量的俯仰角无漂移但噪声大，为了简便，俯仰角测量值建模为2）陀螺仪的角速度测量会有漂移但噪声小，可以建模为其中表示高频噪声，表示真值。角速率积分的拉氏变换常值