PX4 的 ECL EKF 公式推导及代码解析.pdf

发布时间：2022-06-14 发布人：admin 分类：说明书资料大小：0.14M 资料格式：pdf 举报版权申诉

e89c59b1-38c6-41b5-aae3-023b8fd69b46.pdf-第1页.png

第1页 / 共18页

e89c59b1-38c6-41b5-aae3-023b8fd69b46.pdf-第2页.png

第2页 / 共18页

e89c59b1-38c6-41b5-aae3-023b8fd69b46.pdf-第3页.png

第3页 / 共18页

e89c59b1-38c6-41b5-aae3-023b8fd69b46.pdf-第4页.png

第4页 / 共18页

e89c59b1-38c6-41b5-aae3-023b8fd69b46.pdf-第5页.png

第5页 / 共18页

e89c59b1-38c6-41b5-aae3-023b8fd69b46.pdf-第6页.png

第6页 / 共18页

e89c59b1-38c6-41b5-aae3-023b8fd69b46.pdf-第7页.png

第7页 / 共18页

e89c59b1-38c6-41b5-aae3-023b8fd69b46.pdf-第8页.png

第8页 / 共18页

文本预览

PX4 的 ECL EKF 公式推导及代码解析 CUHK 赵祯俊 August 31, 2019 Contents 1 引言 2 状态向量 3 可用的传感器 4 IMU 传播 4.1 状态向量的传播 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.1.1 四元数状态的传播 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.1.2 速度状态的传播 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.1.3 位置状态的传播 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.1.4 其他状态的传播 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2 协方差的传播 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5 更新 5.1 GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.2 气压计 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.3 磁力计 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.3.1 三轴磁场的更新 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.3.2 真航向的更新 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.4 光流 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.5 ZED 相机 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6 附录 6.1 PX4 中四元数的传播方程 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.2 PX4 中速度的传播方程 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.3 PX4 中位置的传播方程 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.4 协方差传播中的 F 和 G 计算 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.4.1 F 的计算 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.4.2 G 的计算 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.5 更新过程的 Jacobian 推导 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.5.1 磁力计更新的观测矩阵 Jacobian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.5.2 光流更新的观测矩阵 Jacobian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.5.3 ZED 相机更新的观测矩阵 Jacobian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1 2 2 2 2 2 3 3 3 3 4 5 5 5 6 6 7 8 9 9 9 11 11 12 12 13 14 14 16 17

1 引言 PX4 采用 ECL(Estimation and Control Library，估计与控制库）通过 EKF 来进行多传感器信息融合，ECL 的官网为 https : //dev.px4.io/zh/tutorials/tuning_the_ecl_ek f .html。 2 状态向量 PX4 的状态向量如下所示： x241 = [ q v p ∆qb ∆vb mNED mb vwind ]T (1) 其中，q 表示 NED 系（北东地 North、East、Down 的世界坐标系）到 Body 系（IMU 的机体坐标系）的旋转向量的四元数，v 表示在 NED 系的机体速度，p 表示在 NED 系的机体位置，mNED 为 earth 在 NED 系下的磁场向量，简称地磁，vwind 为在北东 (NE) 方向上的风速。另外，∆qb 为角度增量的偏移误差，单位为 rad，注意并非角速度 w 的偏移误差，两者的关系为： ∆vb 为速度增量的偏移误差，单位为 m/s，注意并非加速度计 a 的偏移误差，两者的关系为: ∆qb = wb∆t ∆vb = ab∆t (2) (3) mb 为磁力计的偏移误差, 称为罗差，即飞机上磁力计和磁北极的一个差角，磁力计的读数修正了罗差之后所得到的才是磁航向。值得说明的是，q = qB N 是表示旋转向量的四元数，即表示将同一个系中的一个向量或者点，旋转到另一个地方。若我们希望得到从 B 系到 N 系的旋转矩阵，则可使用下面公式，详细可参考附录??。 264q2 RN B (vecq) = 3 可用的传感器 2 q2 q2 0 + q2 1 2q0q3 + 2q1q2 2q1q3 2q0q2 3 2q1q2 2q0q3 q2 q2 q2 0 2q0q1 + 2q2q3 1 + q2 2 3 2q0q2 + 2q1q3 2q2q3 2q0q1 q2 q2 2 + q2 3 0 q2 1 (4) 375 1. IMU：包含 Body 系下的陀螺仪角度增量和加速度计速度增量； 2. GPS：包含 NED 系下的速度和位置； 3. 气压计：提供 NED 系下的高度； 4. 磁力计：提供 NED 系下的偏航角。 IMU 传播 4 4.1 状态向量的传播下面来分析每来一个 IMU 数据 ∆qm 和 ∆vm，状态向量是如何从第 k 时刻传播到第 k+1 时刻的。对于三个 bias、earth 的磁场向量和风速是不随 IMU 变化的，因此我们只需讨论旋转、速度和位置。 2

4.1.1 四元数状态的传播下面我们详细讨论在 PX4 中旋转四元数随着陀螺仪数据的传播过程。飞行器在旋转过程中存在的陀螺仪圆锥运动和地球自转运动，将会对陀螺仪的测量数据产生误差，可以采用梯度近似积分法去除这些误差，在 PX4 中忽略了陀螺仪圆锥运动，考虑了地球自转运动的影响，所以真实角增量计算公式如下所示： ∆q = ∆qm ∆qb ∆qearth ∆qn (5) 其中，∆q 为真实的角增量，∆qm 为陀螺仪测量得到的角增量，∆qb 为角增量的偏移误差，∆qn 为角增量的噪声，即 ∆qn = nw ∆t，nw 为陀螺仪的角速度噪声，∆t 为 IMU 测量数据的帧间间隔。将修正之后的角度增量转化为四元数增量 ∆qk，再使用下列公式，即可得到从 k 时刻到 k+1 时刻旋转向量的四元数传播，详细推导可参考附录 6.1。 qk+1 = qk ∆qk (6) 4.1.2 速度状态的传播速度增量解算时含有旋转效应和划桨效应误差，需要对旋转效应和划桨效应进行补偿，一般采用的方法是梯度近似积分法。在低精度的小型无人机系统中也可以忽略这些影响。在 PX4 中忽略以上误差得到真实速度增量计算公式如下所示： ∆v = ∆vm ∆vb ∆vn (7) 其中，∆v 为真实的速度增量，∆vm ∆vn 为加速度计测量得到的速度增量，∆vb 为速度增量的偏移误差，∆vn 为速度增量的噪声，∆t 为 IMU 测量数据的帧间间隔。那么，可以得到从 k 时刻到 k+1 时刻的速度传播，详细推导可参考附录 6.2。 vk+1 = vk + RN B ∆v + gN ∆t (8) 其中，RN B 为机体坐标系到 NED 坐标系的旋转矩阵，gN = [0, 0, g]T 为 NED 坐标系下的重力加速度。 4.1.3 位置状态的传播位置增量计算的过程中同样存在旋转效应和划桨效应误差，也需要通过梯度近似积分法进行处理。在 PX4 中同样忽略以上误差，得到从 k 时刻到 k+1 时刻的位置传播，详细推导可参考附录 6.3。 pk+1 = pk + (vk + vk+1) ∆t 1 2 4.1.4 其他状态的传播如前所述，三个 bias、earth 的磁场向量和风速是不随 IMU 变化的。 ∆qk+1 b = ∆qk b ∆vk+1 b = ∆vk b mk+1 N = mk N mk+1 B = mk B vk+1 wind = vk wind 3 (9) (10) (11) (12) (13) (14)

4.2 协方差的传播下面我们来讨论状态向量的不确定度传播，即协方差传播，首先我们需要写出状态向量的转移矩阵，如下形式，为了简洁，我们省略了部分上下标： xk+1 = F xk + G nk 3777777777777775 2666666666666664 q v p ∆qb ∆vb mNED mB vwind = F k+1 2666666666666664 q v p ∆qb ∆vb mNED mB vwind 3777777777777775 k [ ] k ∆qn ∆vn + G (15) (16) 根据前面推导的第 k+1 时刻的状态向量公式 (6)、(8)、(9) 和 (10)-(14)，分别对第 k 时刻的状态 xk 求 Jacobian 可得 F 和 G，详细推导可参考附录 6.4： 26666666666666664 F2424 = 37777777777777775 Fqk+1 qk Fvk+1 qk 034 034 034 034 034 024 043 I33 Fpk+1 vk 033 033 033 033 023 043 Fqk+1 ∆qk b 033 033 033 I33 033 I33 033 033 033 033 033 033 023 023 043 Fvk+1 ∆vk b 033 033 I33 033 033 023 043 033 033 033 033 I33 033 023 043 033 033 033 033 033 I33 023 042 032 032 032 032 032 032 I22 2666666666666664 Gqk+1 043 ∆qk n 033 Gvk+1 ∆vk n 033 033 033 033 033 033 033 033 033 033 023 023 3777777777777775 G246 = (17) (18) (19) 根据公式 (15) 和 (18)，可以得到状态误差矩阵： 26666666664 Q = G 37777777775 GT ∆q2 nx 0 0 0 0 0 0 ∆q2 ny 0 0 0 0 0 0 ∆q2 nz 0 0 0 0 0 0 ∆v2 nx 0 0 0 0 0 0 ∆v2 ny 0 0 0 0 0 0 ∆v2 nz 4

所以根据公式 (17) 和 (19)，状态向量的协方差传播公式为： Pk+1jk = F Pkjk FT + Q + Nprocess (20) 其中，Nprocess 为除 IMU 噪声以外的滤波器状态过程噪声，这些噪声可以直接进入到推导的协方差预测方程中。该过程噪声决定了 IMU 偏差误差的估计率。 2666666666666664 N2424 process = 044 034 034 034 034 034 034 024 043 033 033 033 033 033 033 023 043 033 033 033 033 033 033 023 043 043 033 033 033 033 033 s2 ∆qb 033 s2 ∆vb 033 033 033 033 023 023 043 033 033 033 033 s2 mN 033 023 043 033 033 033 033 033 s2 mB 023 042 032 032 032 032 032 032 022 3777777777777775 其中，s2 过程噪声协方差，磁力计偏差的过程噪声协方差。，s2 mB ，s2 ∆qb ，s2 mN ∆vb 分别为陀螺仪偏差的过程噪声协方差，加速度计偏差的过程噪声协方差，地球磁场的 5 更新 5.1 GPS GPS 更新分为两部分，分别是 NED 方向速度和 NE 方向位置的更新。 GPS 的观测方程为： zGPS = HGPS x + RGPS 其中， GPS 的观测相对于状态向量 x 的观测状态转移矩阵，即对应的观测矩阵 Jacobian 为： z51 GPS = [vN, vE, vD, pN, pE]T [ H524 GPS = ¶zGPS ¶x = 034 024 I33 023 032 I22 0 033 0 023 033 023 033 023 033 023 032 022 RGPS 为当前时刻 GPS 观测的不确定度，即 GPS 的协方差。得到 GPS 的观测和 Jacobian 后，结合公式 (20)，可以直接套用 EKF 的更新公式： S = HGPS Pk+1jk HT GPS + RGPS K = Pk+1jk HT S 1 xk+1jk+1 = xk+1jk + K (zGPS HGPS xk+1jk) Pk+1jk+1 = [I K HGPS] Pk+1jk GPS 其中，zGPS 为 GPS 速度和位置的观测值，HGPS xk+1jk 为速度和位置的预测值。 5.2 气压计气压计更新是进行高度的更新。 5 ] (21) (22) (23) (24) (25)

气压计的观测方程为：其中， zBaro = HBaro x + RBaro z11 Baro = h 气压计的观测相对于状态向量 x 的观测状态转移矩阵，即对应的观测矩阵 Jacobian 为： H124 Baro = ¶zBaro ¶x = [014, 013, 012, 1, 013, 013, 013, 013, 012] (26) RBaro 为当前时刻气压计观测的不确定度，即气压计的协方差。得到气压计的观测和 Jacobian 后，结合公式 (20)，可以直接套用 EKF 的更新公式（注意：气压计的高度和 D 方向位置状态的符号相反）： S = HBaro Pk+1jk HT Baro + RBaro K = Pk+1jk HT S 1 xk+1jk+1 = xk+1jk + K (zBaro HBaro xk+1jk) Pk+1jk+1 = [I K HBaro] Pk+1jk Baro (27) (28) (29) (30) 其中，zBaro 为气压计高度的观测值，HBaro xk+1jk 为高度的预测值。 5.3 磁力计磁力计更新分为两种情况，第一种方法是用三轴磁力计的数据作为三维观测，第二种方法是直接将磁力计的数据转换为磁偏角作为一维观测。 5.3.1 三轴磁场的更新三轴磁场的更新根据是否有磁偏角的约束分为两个阶段。 1. 无磁偏角约束的三轴磁场的融合如果不考虑磁偏角，也就是理论上假设地球的地理南北极与地磁南北极重合。磁力计的计算公式为：所以，磁力计的观测方程为： m = RB N mNED + mb zMag = HMag x + RMag 其中，将三轴磁力计的数据作为三维观测，即： z31 Mag = m (31) (32) 磁力计的观测相对于状态向量 x 的观测状态转移矩阵，即对应的观测矩阵 Jacobian 为（详细推导可参考附录 6.5.1）： H324 Mag = ¶zMag ¶x = [Hm q , 033, 033, 033, 033, Hm mNED, Hm mb, 032] RMag 为当前时刻磁力计磁场观测值的不确定度，即磁力计磁场的协方差。 6

得到磁力计的观测和 Jacobian 后，结合公式 (20)，可以直接套用 EKF 的更新公式： S = HMag Pk+1jk HT Mag + RMag K = Pk+1jk HT S 1 xk+1jk+1 = xk+1jk + K (zMag mpred) Pk+1jk+1 = [I K HMag] Pk+1jk Mag (33) (34) (35) (36) 其中，zMag 为磁力计磁场的观测值，mpred = RB N k+1jk mNED k+1jk + mb k+1jk 为磁场的预测值。 2. 磁偏角约束的融合事实上，地球的地理南北极与地磁南北极并不重合，也就是真北和磁北是有一定夹角的，这个夹角称为磁偏角。因此如果增加了磁偏角的约束，则需要在融合完三轴磁力计测量值的基础上进行此部分的融合。这部分融合是在没有绝对位置或速度测量的情况下用于保持正确的航向，例如使用光流。磁偏角的计算公式为： ydeclination = arctan 所以，磁偏角的观测方程为： zydeclination = Hydeclination 其中， z11 ydeclination = ydeclination = arctan ( ) mE mN x + Rydeclination ) ( mE mN 磁偏角的观测相对于状态向量 x 的观测状态转移矩阵，即对应的观测矩阵 Jacobian 为（详细推导可参考附录 6.5.1）： H124 ydeclination = ¶zydeclination ¶x = [014, 013, 013, 013, 013, Hydeclination mNED , 013, 012] Rydeclination 为当前时刻磁偏角的不确定度，即磁偏角的协方差。得到磁偏角的观测和 Jacobian 后，结合公式 (20)，可以直接套用 EKF 的更新公式： S = Hydeclination K = Pk+1jk HT xk+1jk+1 = xk+1jk + K (zydeclination ) Pk+1jk+1 = [I K Hydeclination ] Pk+1jk Pk+1jk HT ydeclination S 1 ( ydeclination + Rydeclination ydeclination pred ) 其中，zydeclination 为当地磁偏角的值，ydeclination pred = arctan 5.3.2 真航向的更新 mE k+1jk mN k+1jk 为磁偏角的预测值。首先需要将磁力计的数据转换到 NED 坐标系下，等价于转换为了真航向，即：其中，m = [mX, mY, mZ] 为磁力计的测量值。磁偏角的计算公式为： mNED = RN B m ) ( mE mN y = arctan 7 (37) (38) (39) (40) (41) (42) (43) (44)

所以，磁偏角的观测方程为： (45) 其中，根据公式 (43)，将磁力计在 NED 坐标系下的输出转换为磁偏角作为一维观测，即， zy = Hy x + Ry ( z11 y = y = arctan ) mE mN 磁偏角的观测相对于状态向量 x 的观测状态转移矩阵，即对应的观测矩阵 Jacobian 为（详细推导可参考附录 6.5.1）： H124 y = ¶zy ¶x = [Hy q , 013, 013, 013, 013, 013, 013, 012] Ry 为当前时刻真航向观测值的不确定度，即真航向的协方差。得到磁偏角的观测和 Jacobian 后，结合公式 (20)，可以直接套用 EKF 的更新公式： S = Hy Pk+1jk HT y + Ry K = Pk+1jk HT y S 1 xk+1jk+1 = xk+1jk + K (zy ypred) [( Pk+1jk+1 = [I K Hy] Pk+1jk ( (46) (47) (48) (49) ] E 为磁偏角的预测值，m = ) ) m m N RN B k+1jk RN B k+1jk 其中，zy 为当地磁偏角的值，这里 zy = zydeclination ，ypred = arctan [mX, mY, mZ] 为磁力计的测量值。 5.4 光流光流传感器的更新主要是进行速度的更新。视线角速率是自主导航中一个非常重要的参数，它在光流中的计算公式为： [ [ ]T = ]T w = wx, wy ]T 为机体速度，range 为沿光流传感器视场中心测量的透镜到地面的范围，是 range (50) vy , vx range [ vx, vy, vz 其中，vB = RB N v = 一个定值。所以，光流的观测方程为：其中， z f low = H f low x + R f low z21 f low = w (51) (52) 光流的观测相对于状态向量 x 的观测状态转移矩阵，即对应的观测矩阵 Jacobian 为（详细推导可参考附录??）： H224 f low = ¶z f low ¶x = [Hw q , Hw v , 023, 023, 023, 023, 023, 022] R f low 为当前时刻光流传感器观测的不确定度，即视线角速率的协方差。得到光流的观测和 Jacobian 后，结合公式 8

分享到：

赞收藏

资料库

PX4 的 ECL EKF 公式推导及代码解析.pdf

相关推荐

开发技术

热门标签

最新资料