无人机（PX4）状态估计.pdf

⽆⽆⼈⼈机机（（PX4））状状态态估估计计

1.1组合导航

从上章介绍飞⾏器需要获得⾃⼰的⼗五个状态息，需要通过传感器测量并推算。单独的惯性测量元件由于误差累积，导致随

时间增长发发散。因此需要通过额外的传感器息来进⾏互补修正。组合导航技术结合GNSS、惯性测量元件、磁⼒计、⽓压

计和超声波等传感设备，利⽤各⾃的有点互补，采⽤数字号处理技术融合多传感器的测量值，从⽽获得较为准确的飞⾏器的

⼗五个状态空间量，通过⼀定的算法和规律尽可能的估计出与真实状态量接近的量。

惯性测量元件容易发散，从⽽这个发散可以通过GNSS全球卫星定位系统来获取修正和抑制，然⽽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⽅向余弦矩阵和欧拉⾓

俯仰⾓

横滚⾓

偏航⾓

1.3.2姿态与欧拉⾓

在定义欧拉⾓的时候，相同的姿态不同的取法会得到不同的欧拉⾓。本书

采⽤的取法顺序为Z-Y-X，即绕三个轴的旋转顺序是先绕Z轴，再绕Y轴，再

绕X轴，最终达到⼀定的姿态。

⽽对于同样的姿态，如果绕轴的顺序不同，则得到的三个欧拉⾓也不相同。因此按Z-Y-X的定义进⼀步来说，本书所定义的欧

拉⾓

姿态解算（attitudealgorithm）也叫姿态分析或者姿态估计，即指飞控系统或者惯导系统根据惯性测量单元（IMU）陀螺仪，

加速度计，磁⼒计等传感器的实时息求解飞⾏器的欧拉⾓的算法过程，因此也叫IMU数据融合（IMUDatafusing）。

显然此次旋转新的坐标系与⽼坐标系之间的关系（投影）

显然此次旋转新的坐标系与⽼坐标系之间的关系（投影）

显然此次旋转新的坐标系与⽼坐标系之间的关系（投影）

虽然飞⾏器的姿态是随机转动，⽽并不是依次沿三轴旋转⽽成，我们仍然可以依靠欧拉⾓的三次旋转来还原出飞⾏器的姿态

息。⽽且如果旋转的三轴顺序不同，得到的旋转矩阵也不同，并且欧拉⾓也不相等，三次旋转先后组合在⼀起即可以形成从惯

性⼤地坐标系e到机体坐标系b的旋转矩阵：

卡尔曼滤波

卡尔曼滤波是⼀种系统状态观测估计器，⽤来描述⼀