基于视觉/惯导的无人机组合导航算法研究

2021-03-04黄剑雄刘小雄章卫国高鹏程

计算机测量与控制 2021年2期

关键词：位姿延时姿态

黄剑雄，刘小雄，章卫国，高鹏程

(西北工业大学自动化学院, 西安 710072)

0 引言

传统的无人机通常采用全球定位系统(GPS,global position system)与惯性测量单元(IMU,inertial measurement unit)进行融合[1]，为无人机提供所需的姿态、速度以及位置等导航信息。

当GPS受遮挡或完全无信号时，这将直接影响到使用GPS与IMU的传统组合导航算法，严重的情况下，甚至会使其完全失效，从而引发无人机坠机的事故。

针对上述问题，许多学者将研究重点转向了同时定位与地图构建(SLAM，simultaneous localization and mapping)的研究，并提出了许多优秀的开源算法。其中利用视觉传感器的SLAM算法，主要分为基于特征点法的SLAM算法，如PTAM[2]、ORB_SLAM[3]，与基于直接法或半直接法的SLAM算法，如DSO[4]、SVO[5]等算法。其中，ORB_SLAM是基于特征点法的SLAM算法中性能非常优越的算法，并且于2017年推出了同时支持单目、双目、RGBD的ORB_SLAM2[6]。

但由于无人机的机载处理器性能较弱，如在NVIDIA JETSON TX2平台上，ORB_SLAM2经过GPU加速等优化后，通常也只能输出10～20 Hz的位姿信息。这对于无人机的导航而言，确实有些捉襟见肘。因此，利用误差状态卡尔曼滤波(ErKF，error-state kalmanfilter)松耦合方法[7]对视觉位姿与惯导信息进行融合从而提升导航信息的输出频率是非常有效的手段之一。

在传统的使用扩展卡尔曼滤波(EKF,extended kalmanfilter)对GPS信息与惯性信息进行融合的流程中，建模的准确与否直接影响到EKF的精确性。为了解决系统建模不准确的问题，自适应渐消卡尔曼滤波(AFKF,adaptive fading kalman filter)应运而生。AFKF主要通过实时的计算遗忘因子，不断调整系统的协方差矩阵，从而削弱历史数据的权重，即限制了模型误差累积。国内外学者在传统AFKF方面做了许多研究工作，其中，AFKF直接对协方差阵进行加权渐消处理的合理性已得到了完备的证明[8]；学者也提出了非常合理的最佳遗忘因子的求取算法[9]。

基于上述文献的研究成果，本文提出了使用AFKF作为视觉位姿与惯导信息的融合算法，进一步提升滤波结果的精度。在此基础上，本文还讨论了一种由于视觉SLAM解算过程而引入的视觉位姿信息相对惯性信息滞后的问题，并提出了一种延时补偿方法以解决这类滞后问题，进一步改善导航解算精度。

1 ORB_SLAM2简介及其简化工作

ORB_SLAM2是当前基于特征点法的主流SLAM算法之一，前端是一个轻量级的定位模型，后端主要采用BA优化方法，可以实现未知环境中的定位。ORB_SLAM2在原ORB_SLAM的基础上，新增了对双目以及RGB-D摄像头的支持，并可以实现地图重用、回环检测以及重定位等功能。

ORB_SLAM2一共包含3个主要线程以及一个次要线程，分别是Tracking线程、Local Mapping线程、Loop Closure线程及全局BA线程，其中全局BA线程仅发生在回环检测成功后才执行。

本文为了保证实时性要求，对ORB_SLAM2框架进行了一定程度的简化，简化包括去除闭环检测以及回环修正的线程；同时，为了与惯导进行融合，增加了Scheduler线程，Scheduler中主要完成IMU数据的采集以及基于EKF框架的视觉惯导融合。如图1所示。

图1 简化ORB_SLAM2流程框图

本文暂且把修改后的框架称为简化ORB_SLAM2，简化ORB_SLAM2共包含3个线程，分别是Tracking线程、Local Mapping线程、Scheduler线程。

在Scheduler线程中，本文将ORB_SLAM2与IMU视为相互独立的传感器，按设定频率进行数据采集与处理，并基于EKF框架进行融合，得到融合后的无人机位置、速度以及姿态信息，作为无人机的导航信息。

2 基于EKF的视觉惯导组合导航算法

2.1 EKF预测更新

假设系统状态为[q,v,p,εr,▽r]T，其中q表示无人机姿态，v表示无人机速度，p表示无人机位置，εr表示角速度bias，▽r表示加速度bias。

将陀螺仪误差与加速度计误差建模为一阶马尔科夫随机过程，可以将陀螺仪误差模型建模如下：

(1)

加计误差模型建模如下：

(2)

根据运动学关系，式(1)以及式(2)，可得如下运动学状态方程：

(3)

其中：

(4)

其中：ωm表示陀螺仪量测值，am表示加速度计量测值。

上述运动状态方程中，含有噪声项，这是我们无法在实际测量中直接去除的，因此我们需要将这些噪声项通过EKF估计出来之后再进行消除。我们首先将上述运动方程中的噪声项去除，然后对噪声项进行建模，得到我们最终的误差状态方程：

(5)

由式(1)和式(2)描述的IMU误差模型可知，如果我们将噪声项消除，则IMU误差应该恒为0，则其倒数也为0，因此可以得到如上消除噪声项后的运动方程，我们对每一个状态量求取上述两组运动方程的误差，则可以得到我们最终的状态量为[δθ,δv,δp,εr,▽r]T的误差状态方程如下：

(6)

对各状态量求偏导，可得如下预测模型的状态空间方程：

(7)

其具体表达形式为：

(8)

因为机载处理器只能以离散化的计算形式对导航数据进行处理，因此需要将式(7)进行离散化处理。具体离散化流程可参考文献[10],本文采用泰勒展开的方式，并且仅取二阶项，如式(9)所示：

(9)

其中：Φk+1,k=Φ(tk+1,tk)，Fk=F(tk)，T=tk+1-tk。根据系统状态转移矩阵Φ以及噪声矩阵G，可以求得系统的噪声协方差矩阵Qk的离散化形式，如式(10)：

(10)

其中：Qc为系统噪声的协方差矩阵，利用式(9)，式(10)求得的状态转移矩阵与离散过程噪声协方差矩阵，则可计算k+1时刻的状态协方差矩阵：

(11)

以及完成状态量的预测过程：

(12)

2.2 EKF量测更新

量测更新采用视觉SLAM解算的位姿信息，由于视觉输出位姿信息是离散形式的，因此本文的量测方程可以直接离散化形式表示为：

Zk+1=Hk+1Xk+1+Vk+1

(13)

其中：

(14)

其中：[Xn,Yn,Zn]T为转换到本文所定义的导航坐标系下的轨迹对应的三维坐标值。

根据视觉SLAM的特性，量测噪声Vk+1可以表示为：

(15)

其中：np∈R3×1为位置量测值所含白噪声，nθ∈R3×1为姿态量测值所含白噪声。

不难知道：

(16)

则有：

(17)

(18)

同理可得：

(19)

根据上述量测更新模型，可以计算k+1时刻的增益矩阵：

(20)

进一步可以更新k+1时刻的状态协方差矩阵：

Pk+1=(I-Kk+1Hk+1)Pk+1,k(I-Kk+1Hk+1)T+Kk+1RKk+1

(21)

并完成对系统状态量的更新：

(22)

3 自适应渐消卡尔曼滤波

由于在建立EKF的系统模型的过程中，通常会对其在合理的范围内进行近似或简化，以提高解算效率，但这些简化把不可避免的会使EKF的估计误差不断增大，甚至严重影响滤波效果。因此，本文采用了自适应卡尔曼滤波的方式来进一步提高视觉惯导融合的精度。

3.1 基本原理

为了减小在建模时由于模型近似或简化带来的估计误差，可以在时间更新中求取当前时刻状态预测协方差矩阵时，乘上相应的遗忘因子[12]，即：

(23)

其余更新流程与式(12)，式(20)～(22)一致，通过分析可知，当估计误差增大时，会导致遗忘因子λk+1增大，从而状态预测协方差矩阵Pk+1,k也会相应增大，Pk+1,k增大会使增益Kk+1增大，因此在计算经过量测更新得到的当前状态预测值时，会更多的信任量测信息，从而抑制历史状态信息带来的影响，即可减小由于建模时模型近似或简化带来的估计误差。从λk+1的物理意义可知，通常有：

λk+1≥1

(24)

3.2 遗忘因子的求取方式

由于通过求取最优的遗忘因子，应当使自适应渐消卡尔曼滤波成为最优滤波器，因此遗忘因子在求取过程中，首先应该满足最优滤波器的性质。由文献[9]可知，最终可将最优滤波器的性质转化为如下等价条件：

(25)

其中：C0k为残差的协方差，如下：

(26)

其中：zk为残差，是一个白噪声序列：

zk=Zk-HkXk

(27)

通过联立式(25)与式(20)，可以将式(25)展开得到：

(28)

假设Qk，Rk，P0均正定，且Hk满秩，则将式(28)化简并将式(23)代入，可展开得到：

λkMk=Nk

(29)

其中：

(30)

(31)

其中：C0k由实际数据计算得到，而并非式(26)所示的理想化表达式得到，则最佳遗忘因子计算方法如下：

λk=max{1,tr(Nk)/tr(Mk)}

(32)

4 视觉信息滞后的补偿方法

4.1 问题描述

通过AFKF的方法，能够得到精确的无人机姿态、速度以及位置信息。但由于实时的视觉SLAM求解位姿的过程根据具体运算平台以及算法的不同，往往需要30～200 ms甚至更长的运算时间，这将会导致视觉位姿信息滞后于惯导信息。

图2 延时示意图

如图2所示，由于视觉SLAM求解位姿需要一定的处理时间，通常，在t1时刻获得的图像信息，要在t2时刻才能求解出相应的SLAM位姿结果，并传递到组合导航流程与惯性信息融合，t2时刻获得的图像信息同理，要在t3时刻才能够解算出相应的位姿结果，这不可避免的导致了视觉位姿滞后于惯导信息，即在t3时刻对视觉位姿以及惯性信息进行融合时，视觉位姿缺失了t2时刻至t3时刻的运动信息。因此，我们需要对视觉位姿进行补偿，以减缓甚至消除这类延时对系统带来的影响。

通常在简化ORB_SLAM2实时运行时：

t2-t1≠t3-t2

(33)

也可以理解为，通常连续两组位姿之间的惯导数据的数量并不相同，即：

m≠n

(34)

如图2所示，假设IMU采样周期为T，则有：

(35)

本文着重针对图2中，t2至t3时刻的延时处理做详细说明，其余时刻同理。

4.2 补偿姿态延时

由于视觉位姿的姿态信息包含了从初始参考帧一直到t2时刻的姿态信息，因此，只需要补偿t3时刻相对于t2时刻的旋转量即可。可令t2时刻为参考时刻，即t2时刻对应的旋转四元数为单位四元数[1,0,0,0]T。

Φt2 + T=ωt2 + TbT= [Δθx,Δθy,Δθz]T|t2 + T

(36)

t2+T时刻的四元数增量为：

qt2 + T(t) =

(37)

进而可得到，t2+T时刻对应的旋转四元数为：

Q(t2+T)=Q(t2)⊗qt2+T(t)

(38)

经过k次迭代后，可以得到表示由t2时刻到t3时刻，机体坐标系旋转量的四元数Q(t3)。

(39)

进而通过旋转矩阵与欧拉角间的转换关系[11]，可求出t3时刻对应的真实姿态角，从而完成姿态延时的补偿。

4.3 补偿位置延时

由于视觉位姿的姿态信息包含了从初始参考帧一直到t2时刻的位置信息，因此，只需要补偿t3时刻相对于t2时刻的平移量即可，也即是，认为t2时刻为参考时刻，即t2时刻对应的相对位置为：

pt2=[0,0,0]T

(40)

其对应的初始速度可以由组合导航算法在t2时刻输出的速度量得到，则t2时刻对应的速度为：

vt2= [vx,vy,vz]T|t2

(41)

假设t2+T时刻IMU采集到的机体坐标系下的原始加速度值为：

at2 + T= [ax,ay,az]T|t2 + T

(42)

则通过运动学公式，可以得到t2+T时刻对应的速度为：

vt2+T=vt2+at2+TT

(43)

同理，通过运动学公式，可以得到t2+T对应的相对位置为：

(44)

经过k次迭代，最终可以得到B_坐标系相对于B坐标系在导航坐标系下的相对位移：

pt3= [px,py,pz]T|t3

(45)

由于在时刻t3传入组合导航系统的视觉位置量测信息，表示的是B坐标系在导航坐标系下的位置信息Pt2，则t3时刻对应的真实位置信息应该为：

Pt3=Pt2+pt3

(46)

5 实验结果分析

本文将采用EuRoC数据集中的V2_02_medium序列来对本文算法进行验证，首先是基于该序列验证本文的AFKF算法，由此分析AFKF的相对于传统EKF在视觉惯导组合导航中的优越性；其次，本文将基于简化ORB_SLAM2从V2_02_medium序列解算得到的视觉位姿信息，人工模拟200 ms的延时，并采用本文的延时补偿方法对延时进行处理，并分析本文算法的有效性。