浙江大学学报(工学版), 2022, 56(4): 711-717 doi: 10.3785/j.issn.1008-973X.2022.04.010

计算机技术、信息工程

基于改进MSCKF的无人机室内定位方法

王思鹏,, 杜昌平,, 宋广华, 郑耀

浙江大学 航空航天学院,浙江 杭州 310027

Indoor positioning method of UAV based on improved MSCKF algorithm

WANG Si-peng,, DU Chang-ping,, SONG Guang-hua, ZHENG Yao

School of Aeronautics and Astronautics, Zhejiang University, Hangzhou 310027, China

通讯作者: 杜昌平,男,副教授. orcid.org/0000-0003-0552-2492. E-mail: duchangping@zju.edu.cn

收稿日期: 2021-05-20  

基金资助: 装备预研教育部联合基金资助项目(6141A02011803)

Received: 2021-05-20  

Fund supported: 装备预研教育部联合基金资助项目(6141A02011803)

作者简介 About authors

王思鹏(1997—),男,硕士生,从事飞行器路径规划与定位的研究.orcid.org/0000-0003-4387-0279.E-mail:wangsipeng@zju.edu.cn , E-mail:wangsipeng@zju.edu.cn

摘要

针对无人机室内定位容易出现漂移的问题,提出基于改进多状态约束卡尔曼滤波器(MSCKF)的无人机(UAV)室内定位方法. 该方法在MSCKF的框架下,提出高鲁棒性、低时延的标志点检测方法. 利用在世界坐标系下坐标已知的标志点计算得到无人机位姿,实现惯性测量单元(IMU)信息与单目视觉信息融合以及无人机位姿修正. 对提出的定位方法进行测试. 测试结果表明,该方法的定位误差小于0.266 m,与OpenVins和LARVIO开源算法相比,定位精度提高了54.6%以上.

关键词: 无人机(UAV) ; 室内定位 ; 卡尔曼滤波器 ; 标志点检测 ; 位姿修正

Abstract

An indoor positioning method of unmanned aerial vehicle (UAV) based on improved multi-state constraint Kalman filter (MSCKF) was proposed aiming at the problem that the indoor positioning of UAV is prone to drift. A high robustness and low delay detection method was proposed under the framework of MSCKF. The pose of UAV was calculated with the help of the known positions of the mark points in world coordinate system. Then inertial measurement unit (IMU) data and monocular vision data fusion and UAV pose correction were realized. The proposed positioning method was tested. The simulation results show that the positioning error of the proposed method was within 0.266 m, and the positioning accuracy was improved by more than 54.6% compared to OpenVins and LARVIO.

Keywords: unmanned aerial vehicle (UAV) ; indoor positioning ; Kalman filter ; mark detection ; pose correction

PDF (992KB) 元数据 多维度评价 相关文章 导出 EndNote| Ris| Bibtex  收藏本文

本文引用格式

王思鹏, 杜昌平, 宋广华, 郑耀. 基于改进MSCKF的无人机室内定位方法. 浙江大学学报(工学版)[J], 2022, 56(4): 711-717 doi:10.3785/j.issn.1008-973X.2022.04.010

WANG Si-peng, DU Chang-ping, SONG Guang-hua, ZHENG Yao. Indoor positioning method of UAV based on improved MSCKF algorithm. Journal of Zhejiang University(Engineering Science)[J], 2022, 56(4): 711-717 doi:10.3785/j.issn.1008-973X.2022.04.010

随着科技的不断进步,无人机在各领域得到越来越广泛的应用. 精确的定位是无人机完成任务的关键之一. 尤其在室内环境下,无人机只能依靠自身传感器进行定位,容易受到干扰,定位精度较低.

针对室内环境定位的问题,在过去的几十年中,Usenko等[1-2]开发出了多种算法,以提高室内定位的精度. Mur-Artal等[3-4]提出基于视觉的机器人室内定位方法,该方法只需要单目相机即可进行定位,对传感器的要求较低,但无法恢复尺度信息. 研究人员提出融合相机信息与IMU信息的视觉惯性里程计方法(visual-inertial odometry, VIO),主要分为基于滤波与基于优化的方法. 基于滤波的方法中,Castellanos等[5]提出基于EKF的室内定位方法,但将图像中的每个特征点都加入滤波器状态量中,计算负荷较大,定位精度较低. Mourikis等[6]提出基于MSCKF框架的定位方法,维护了相机位姿的滑动窗口,减小了滤波器状态量的维度. Sun等[7]提出MSCKF框架下基于双目视觉的定位方法,提高了定位的鲁棒性. Geneva等[8]在MSCKF的基础上提出通用框架,提供多种算法接口,便于算法选择与参数调优. 此外还有文献[9~12]的改进算法. 这些方法的计算速度快,定位精度较高,但在受到扰动时容易产生误差累积与漂移. Bavle等[13]提出基于粒子滤波与语义的定位方法,通过对环境中位置已知的物体的检测进行无人机定位的外环修正.

另一种定位方式为基于优化的定位方法. Levinson等[14-15]提出基于因子图优化的方法,建立高精度地图并进行多传感器融合与定位. Qin等[16-17]提出基于优化的定位方法,将IMU与视觉感知图像通过非线性优化方法进行融合. 徐晓苏等[18]采用聚类的方法对ORB特征点进行跟踪,提高了定位精度. 这些基于优化的方法能够利用历史信息进行全局优化,但对计算储存的能力要求较高.

针对无人机在室内定位中容易产生漂移的问题,本文提出基于改进MSCKF的无人机室内定位方法. 在室内场景中设置位置已知的标志点,在MSCKF框架下提出高鲁棒性、低时延的标志点检测方法,利用标志点信息计算无人机位姿,实现了惯性单元信息与视觉信息的融合与修正,减小了算法的漂移,提高了定位精度与鲁棒性.

1. 基于MSCKF的定位方法

采用基于改进MSCKF的算法框架,如图1所示. 将单目摄像头的感知信息与IMU得到的测量信息融合,开展无人机的内环位姿估计,主要分为状态量预测与更新2个阶段.

图 1

图 1   定位算法的整体框图

Fig.1   Block diagram of positioning algorithm


1.1. 状态量与运动模型

系统状态量 ${\boldsymbol{X}}$分为2个部分,分别为惯性单元有关状态量 ${{\boldsymbol{X}}_{\text{I}}}$N个相机历史位姿 ${{\boldsymbol{X}}_{\text{C}}}$

$ {\boldsymbol{X}} = {\left[ {\begin{array}{*{20}{l}} {{\boldsymbol{X}}_{\text{I}}^{\text{T}}}&{{\boldsymbol{X}}_{\text{C}}^{\text{T}}} \end{array}} \right]^{\text{T}}}, $

$ {{\boldsymbol{X}}_{\text{I}}} = {\left[ {\begin{array}{*{20}{l}} {_{\text{G}}^{\text{I}}{{{\boldsymbol{\bar q}}}^{\text{T}}}}&{^{\text{G}}{{\boldsymbol{p}}_{\text{I}}}^{\text{T}}}&{^{\text{G}}{{\boldsymbol{v}}_{\text{I}}}^{\text{T}}}&{{{\boldsymbol{b}}_{\text{g}}}^{\text{T}}}&{{\boldsymbol{b}}_{\text{a}}^{\text{T}}} \end{array}} \right]^{\text{T}}}. $

式中: $_{\text{G}}^{\text{I}}{{\boldsymbol{\bar q}}^{\text{T}}}$为IMU坐标系相对于世界坐标系的姿态转换四元数, $^{\text{G}}{{\boldsymbol{p}}_{\text{I}}}^{\text{T}}$为IMU坐标系相对于世界坐标系的位置, $^{\text{G}}{{\boldsymbol{v}}_{\text{I}}}^{\text{T}}$为IMU坐标系相对于世界坐标系的速度, ${{\boldsymbol{b}}_{\text{g}}}^{\text{T}}$${\boldsymbol{b}}_{\text{a}}^{\text{T}}$分别为陀螺仪与加速度计的三轴零偏.

为了便于状态量的计算并保证滤波器的稳定性,将状态量表示为误差形式:

$ {\boldsymbol{\tilde X}} = {\boldsymbol{X}} \ominus {\boldsymbol{\bar X}}. $

式中: ${\boldsymbol{X}}$为状态量的真实值, ${\boldsymbol{\bar X}}$为状态量的估计值, $ \ominus $表示广义减法. 对于四元数状态量,误差为

$ {\text{δ}} {\boldsymbol{\bar q}} = _{\text{G}}^{\text{I}}{\boldsymbol{\bar q}} \ominus _{\text{G}}^{\text{I}}{\boldsymbol{\hat q}} . $

式中: $ _{\text{G}}^{\text{I}}{\boldsymbol{\bar q}} $为无人机姿态四元数的真实值, $ _{\text{G}}^{\text{I}}{\boldsymbol{\hat q}} $为姿态四元数的估计值.

在四元数误差较小的情况下,四元数误差量可以近似表示为

$ {\text{δ}} {\boldsymbol{\bar q}} \simeq \left[ {\begin{array}{*{20}{l}} {{{{\boldsymbol{\tilde \theta }}}}/{2}}\\ 1\end{array}} \right]. $

式中: ${\boldsymbol{\tilde \theta }} $为三轴姿态角的误差. 

其他IMU相关状态量的误差量可以直接表示为欧式空间中真实值与估计值的减法IMU有关误差状态量表示为

$ {{\boldsymbol{\tilde X}}_{\text{I}}} = {\left[ {\begin{array}{*{20}{c}} {_{\text{G}}^{\text{I}}{{{\boldsymbol{\tilde \theta }}}^{\text{T}}}}&{^{\text{G}}{\boldsymbol{\tilde p}}_{\text{I}}^{\text{T}}}&{^{\text{G}}{\boldsymbol{\tilde v}}_{\text{I}}^{\text{T}}}&{{\boldsymbol{\tilde b}}_{\text{g}}^{\text{T}}}&{{\boldsymbol{\tilde b}}_{\text{a}}^{\text{T}}} \end{array}} \right]^{\text{T}}}. $

滤波器状态量可以表示为

$ {\boldsymbol{\tilde X}} = {\left[ {\begin{array}{*{20}{l}} {{\boldsymbol{\tilde X}}_{\text{I}}^{\text{T}}}&{{\boldsymbol{\tilde X}}_{{{\text{C}}_1}}^{\text{T}}}& \cdots &{{\boldsymbol{\tilde X}}_{{{\text{C}}_N}}^{\text{T}}} \end{array}} \right]^{\text{T}}}. $

式中: ${{\boldsymbol{\tilde X}}_{{{\text{C}}_i}}}$为状态量中第i个相机位姿的误差,

$ {{\boldsymbol{\tilde X}}_{{{\text{C}}_i}}} = {\left[ {\begin{array}{*{20}{l}} {_{\text{G}}^{{{\text{C}}_i}}{{{\boldsymbol{\tilde \theta }}}^{\text{T}}}}&{^{\text{G}}{\boldsymbol{\tilde p}}_{{{\text{C}}_i}}^{\text{T}}} \end{array}} \right]^{\text{T}}}, $

其中 $_{\text{G}}^{{{\text{C}}_i}}{{\boldsymbol{\tilde \theta }}^{\text{T}}}$$^{\rm{{\text{G}}}}{\boldsymbol{\tilde p}}_{{{\rm{{\text{C}}}}_i}}^{\text{T}}$分别为相对于世界坐标系的相机姿态与位置的误差.

1.2. 状态预测

在连续系统中,惯性单元状态量的运动更新模型如下:

$_{\rm{G}}^{\rm{I}}{\boldsymbol{\dot {\bar q}}} = \frac{1}{2}{\rm{\varOmega}} {\rm{(}}w{\rm{)}}_{\rm{G}}^{\rm{I}}{\boldsymbol{\bar q}},$

$ ^{\text{G}}{{\boldsymbol{\dot p}}_{\text{I}}}{ = ^{\text{G}}}{{\boldsymbol{v}}_{\text{I}}}, $

$ ^{\text{G}}{{\boldsymbol{\dot v}}_{\text{I}}} = _{\text{G}}^{\text{I}}{{\boldsymbol{R}}^{\text{T}}}{\boldsymbol{a}}, $

$ {{\boldsymbol{\dot b}}_{\text{g}}} = {{\boldsymbol{n}}_{{\text{wg}}}}, $

$ {{\boldsymbol{\dot b}}_{\text{a}}} = {{\boldsymbol{n}}_{{\text{wa}}}}. $

式中:R为世界坐标系到惯性坐标系的姿态转换矩阵,a为世界坐标系下的三轴加速度, ${{\boldsymbol{n}}_{{\text{wg}}}}$${{\boldsymbol{n}}_{{\text{wa}}}} $分别为三轴陀螺仪、三轴加速度计的噪声向量. 在连续时间下,惯性单元的误差状态方程为

$ {{\boldsymbol{\dot {\tilde X}}}_{\text{I}}} = {\boldsymbol{F}}{{\boldsymbol{\tilde X}}_{\text{I}}} + {\boldsymbol{G}}{{\boldsymbol{n}}_{\text{I}}}. $

式中: ${{\boldsymbol{n}}_{\text{I}}}$为状态量各项的系统噪声向量, ${\boldsymbol{F}}$${\boldsymbol{G}}$分别为状态转移矩阵与噪声转移矩阵.

滤波器的协方差矩阵可以表示为

$ {{\boldsymbol{P}}_{k\mid k}} = \left[ {\begin{array}{*{20}{l}} {{{\boldsymbol{P}}_{{\text{I}}{{\text{I}}_{k\mid k}}}}}&{{{\boldsymbol{P}}_{{\text{I}}{{\text{C}}_{k\mid k}}}}} \\ {{\boldsymbol{P}}_{{\text{I}}{{\text{C}}_{k\mid k}}}^{\text{T}}}&{{{\boldsymbol{P}}_{{\text{C}}{{\text{C}}_{k\mid k}}}}} \end{array}} \right]. $

式中: ${{\boldsymbol{P}}_{{\text{I}}{{\text{I}}_{k\mid k}}}}$$15 \times 15$的惯性单元协方差矩阵, ${{\boldsymbol{P}}_{{\text{C}}{{\text{C}}_{k\mid k}}}}$$6N \times 6N$的相机位姿协方差矩阵, ${{\boldsymbol{P}}_{{\text{I}}{{\text{C}}_{k\mid k}}}}$为惯性单元与相机位姿的协方差相关矩阵.

由于IMU测量值为离散量,使用龙格库塔法对离散量进行积分,近似连续状态量的积分值.

1.3. 状态量增广

在滤波器状态量中维护了一个长度为N的相机状态量滑动窗口. 当接收到最新的相机位姿后,滤波器对相机状态量与协方差矩阵进行相应的更新. 新的相机位姿可以通过该时刻惯性单元的位姿计算得到:

$ _{\text{G}}^{\text{C}}{\boldsymbol{\hat {\bar q}}} = _{\text{I}}^{\text{C}}{\boldsymbol{\bar q}} \otimes _{\text{G}}^{\text{I}}{\boldsymbol{\hat {\bar q}}} , $

$ ^{\text{G}}{{\boldsymbol{\hat p}}_{\text{C}}}{ = ^{\text{G}}}{{\boldsymbol{\hat p}}_{\text{I}}} + \left(C{\left( {_{\text{G}}^{\text{I}}{\boldsymbol{\hat {\bar q}}}} \right)}\right)^{\rm{T}}{{\mkern 1mu} ^{\text{I}}}{{\boldsymbol{\hat p}}_{\text{C}}}. $

式中: $_{\text{I}}^{\text{C}}{\boldsymbol{\bar q}}$表示相机坐标系相对于IMU坐标系的旋转四元数, $^{\text{I}}{{\boldsymbol{\hat p}}_{\text{C}}}$表示相机坐标系相对于IMU坐标系的位置变换,这两者均由离线标定获得;C表示将四元数转换成对应的姿态转换矩阵. 相应得到状态量增广之后的协方差矩阵为

$ {{\boldsymbol{P}}_{k\mid k}} = \left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{I}}_{15 + 6N}}} \\ {\boldsymbol{J}} \end{array}} \right]{{\boldsymbol{P}}_{k\mid k}}{\left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{I}}_{15 + 6N}}} \\ {\boldsymbol{J}} \end{array}} \right]^{\text{T}}}. $

式中: ${\boldsymbol{J}}$为新增加的相机位姿状态量的雅克比矩阵,

$ {\boldsymbol{J}} = \left[ {\begin{array}{*{20}{c}} {C\left( {_{\text{G}}^{\text{I}}{\boldsymbol{\hat {\bar q}}}} \right)}&{{{\boldsymbol{0}}_{3 \times 3}}}&{{{\boldsymbol{0}}_{3 \times 9}}}&{{{\boldsymbol{0}}_{3 \times 6N}}} \\ { - \left(C{{\left( {_{\text{G}}^{\rm{I}}{\boldsymbol{\hat {\bar q}}}} \right)}}\right)^{\rm{T}}\left[ {^{\text{I}}{{{\boldsymbol{\hat p}}}_{\text{C}}} \times } \right] }&{{{\boldsymbol{I}}_{3 \times 3}}}&{{{\boldsymbol{0}}_{3 \times 9}}}&{{{\boldsymbol{0}}_{3 \times 6N}}} \end{array}} \right]. $

1.4. 滤波器更新

设空间中的某一个特征点 ${f_j}$,在连续 ${M_j}$个相机位姿中观测到了该特征点. 某一时刻i的相机位姿为 $\left[ {\begin{array}{*{20}{l}} {_{\text{G}}^{{{\text{C}}_i}}{\boldsymbol{\bar q}}}&{^{\text{G}}{{\boldsymbol{p}}_{{{\text{C}}_i}}}} \end{array}} \right]$. 在每个相机位姿下,对于该特征点的观测方程为

$ {\boldsymbol{z}}_{j,i}=\frac{1}{{z}_{j}^{i}}\left[\begin{array}{l}{x}_{j}^{i}\hfill \\ {y}_{j}^{i}\hfill \end{array}\right]+{\boldsymbol{n}}_{j,i}\text{;}i\in {S}_{j} . $

$ ^{{{\text{C}}_i}}{{\boldsymbol{p}}_j} = \left[ {\begin{array}{*{20}{l}} {x_j^i} \\ {y_j^i} \\ {z_j^i} \end{array}} \right] = C\left( {_{\text{G}}^{{{\text{C}}_{i}}}{\boldsymbol{\bar q}}} \right)\left( {^{\text{G}}{{\boldsymbol{p}}_j}{ - ^{\text{G}}}{{\boldsymbol{p}}_{{{\rm{C}}_i}}}} \right). $

$ {{\boldsymbol{r}}_{j,i}} = {{{\boldsymbol{z}}}_{j,i}} - {{{\boldsymbol{\hat z}}}_{j,i}}. $

式中: ${{\boldsymbol{n}}}_{j,i}$ i时刻相机位姿观察第j个特征点时的观测噪声, ${{\boldsymbol{z}}}_{j,i}$${{\boldsymbol{\hat z}}}_{j,i}$分别为第i时刻相机位姿观察第j个特征点时的观测量和估计量. 将式(22)进行线性化,得到观测方程:

$ {{{{\boldsymbol{r}}}}_{j,i}} \simeq {{{{\boldsymbol{H}}}}_{{{{X}}_{j,i}}}}{{{\boldsymbol{\tilde X}}}} + {{{{\boldsymbol{H}}}}_{j,i}}{{\mkern 1mu} ^{\text{G}}}{{{{\boldsymbol{\tilde p}}}}_j} + {{{{\boldsymbol{n}}}}_{j,i}}. $

式中: ${{\boldsymbol{H}}_{{{{{X}}}}_{j,i}}}$为观测量相对于状态量的雅克比矩阵, ${{\boldsymbol{H}}_{j,i}}$为观测量相对于特征点位置的雅克比矩阵.

对于特征点 ${f_j}$,将所有观测到该特征点的相机位姿下的观测量进行组合,得到特征点 ${f_j}$的观测方程为

$ {{{{{\boldsymbol{r}}}}}_j} \simeq {{{{{\boldsymbol{H}}}}}_{{{X}}_j}}{{{{\boldsymbol{\tilde X}}}}} + {{{{{\boldsymbol{H}}}}}_j}{{\mkern 1mu} ^{\text{G}}}{{{{{\boldsymbol{\tilde p}}}}}_j} + {{{{{\boldsymbol{n}}}}}_j}. $

化简式(24),投影到 ${{\boldsymbol{H}}_j}$的左零空间中,即有

$ {{\boldsymbol{V}}^{\text{T}}}{{\boldsymbol{H}}_j} = {\boldsymbol{0}}\text{,}{{\boldsymbol{V}}^{\text{T}}}{\boldsymbol{V}} = {\boldsymbol{I}}. $

$ {{\boldsymbol{\bar r}}_j} = {{\boldsymbol{V}}^{\text{T}}}{{\boldsymbol{r}}_j} \simeq {{\boldsymbol{V}}^{\text{T}}}{{\boldsymbol{H}}_{{{{{X}}}}_j}}{\boldsymbol{\tilde X}} + {{\boldsymbol{V}}^{\text{T}}}{{\boldsymbol{n}}_j} = {{\boldsymbol{\bar H}}_{{{{{X}}}}_j}}{{\boldsymbol{\tilde X}}_j} + {{\boldsymbol{\bar n}}_j}. $

设相机共观测到M个地图特征点,此时的观测方程可由 ${{{{{\boldsymbol{\bar r}}}}}_j}\left( {j = 1,2, \cdots ,M} \right)$扩增得到:

$ {\boldsymbol{\bar r}} = {{\boldsymbol{H}}_{{{{X}}}}}{\boldsymbol{\tilde X}} + {\boldsymbol{\bar n}}. $

为了降低计算量,对观测方程通过QR分解进行降维:

$ {{\boldsymbol{H}}_{\text{X}}} = \left[ {\begin{array}{*{20}{l}} {{{\boldsymbol{Q}}_1}}&{{{\boldsymbol{Q}}_2}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{T}}_{\text{H}}}} \\ {\boldsymbol{0}} \end{array}} \right], $

将式(28)代入式(27)的观测方程中,得到

$ {\boldsymbol{\bar r}} = \left[ {\begin{array}{*{20}{l}} {{{\boldsymbol{Q}}_1}}&{{{\boldsymbol{Q}}_2}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{T}}_{\text{H}}}} \\ {\boldsymbol{0}} \end{array}} \right]{\boldsymbol{\tilde X}} + {\boldsymbol{\bar n}}, $

$ \left[ {\begin{array}{*{20}{c}} {{\boldsymbol{Q}}_1^{\text{T}}} \\ {{\boldsymbol{Q}}_2^{\text{T}}} \end{array}} \right]{\boldsymbol{\bar r}} = \left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{T}}_{\text{H}}}} \\ {\boldsymbol{0}} \end{array}} \right]{\boldsymbol{\tilde X}} + \left[ {\begin{array}{*{20}{l}} {{\boldsymbol{Q}}_1^{\text{T}}} \\ {{\boldsymbol{Q}}_2^{\text{T}}} \end{array}} \right]{\boldsymbol{\bar n}}. $

由于式(30)的下半部分中, ${\boldsymbol{Q}}_2^{\text{T}}{\boldsymbol{\bar r}}$只与噪声相关,可以忽略,式(30)可以简化为

$ {{\boldsymbol{r}}_{\text{n}}} = {\boldsymbol{Q}}_1^{\text{T}}{\boldsymbol{\bar r}} = {{\boldsymbol{T}}_{\text{H}}}{\boldsymbol{\tilde X}} + {\boldsymbol{Q}}_1^{\text{T}}{\boldsymbol{\bar n}} = {{\boldsymbol{T}}_{\text{H}}}{\boldsymbol{\tilde X}} + {{\boldsymbol{n}}_{\text{n}}}. $

式中: ${{\boldsymbol{n}}_{\text{n}}}$为系统噪声向量,协方差矩阵为 ${{\boldsymbol{R}}_{\text{n}}} = $ $ {\boldsymbol{Q}}_1^{\text{T}}{\boldsymbol{\bar R}}{{\boldsymbol{Q}}_1} = \sigma _{{\rm{im}}}^2{{\boldsymbol{I}}_{\text{r}}}$.

滤波器的卡尔曼增益矩阵、状态量与协方差的更新方程为

$ {\boldsymbol{K}} = {\boldsymbol{PT}}_{\text{H}}^{\text{T}}{\left( {{{\boldsymbol{T}}_{\text{H}}}{\boldsymbol{PT}}_{\text{H}}^{\text{T}} + {{\boldsymbol{R}}_{\text{n}}}} \right)^{ - 1}}, $

$ {{\Delta }}{\boldsymbol{X}} = {\boldsymbol{K}}{{\boldsymbol{r}}_{\text{n}}}, $

$ {{\boldsymbol{P}}_{k + 1\mid k + 1}} = \left( {{\boldsymbol{I}} - {\boldsymbol{K}}{{\boldsymbol{T}}_{\text{H}}}} \right){{\boldsymbol{P}}_{k + 1\mid k}}{\left( {{\boldsymbol{I}} - {\boldsymbol{K}}{{\boldsymbol{T}}_{\text{H}}}} \right)^{\text{T}}} + {\boldsymbol{K}}{{\boldsymbol{R}}_{\text{n}}}{{\boldsymbol{K}}^{\text{T}}}. $

2. 基于已知标志点的位姿修正

基于MSCKF的紧耦合定位方法能够将惯性单元的测量信息与单目相机的感知信息进行融合,得到无人机当前时刻的位姿. 该方法本质上是开环的定位方法. 由于没有加入闭环修正的环节,若无人机在定位过程中受到了较大的扰动,则滤波器会产生误差累积,导致定位精度降低乃至发散. 基于以上问题,提出基于环境中已知标志点的位姿外环修正算法. 在内环MSCKF滤波器的基础上,通过相机感知环境中位置已知的标志点信息,对无人机自身的位姿进行外环修正,避免误差累积的情况,提高定位精度.

2.1. 标志点检测方法

无人机机载相机像素有限,画面清晰度较低,机载处理器的计算能力有限. 提出适应于无人机特性的高鲁棒性低时延标志点检测方法. 检测方法的流程如下.

输入:原始图像

输出:标志区域角点像素坐标

1)将标志区域与背景二值化处理

2)提取二值化后的标志区域轮廓

3)提取二值化后的图像角点信息

4)对角点进行非极大值抑制,提取标志区域的真实角点坐标

为了降低对处理器计算能力的要求,提高算法的鲁棒性,标志区域采用矩形区域,如图2(a)所示,将矩形区域的4个角点作为需要提取的目标标志点. 处理器得到相机采集的图像后,通过颜色域的差别,将标志物与背景区分,对标志物与背景进行二值化处理,如图2(b)所示. 提取二值化后图像中矩形区域的轮廓,得到轮廓最小外接矩形的角点信息,如图2(c)所示. 在无人机运动过程中,标志物在相平面上会产生仿射变换,因此最小外接矩形的角点通常不是标志物的角点. 采用Harris角点检测法[19],对二值化图像进行角点检测,得到角点信息,如图2(d)所示. 由于角点通常出现聚集的情况,采用非极大值抑制的方法. 选取所有Harris角点中,距离图2(c)的外接轮廓的4个角点最近的4个Harris角点,如图2(d)所示. 将得到的4个角点作为标志物的4个特征点,如图2(e)所示.

图 2

图 2   标志点检测流程

Fig.2   Marks detection procedure


2.2. 无人机位姿修正

通过2.1节的标志点检测,可以得到4个标志点在相平面中的像素坐标,同时这4个标志点在世界坐标系下的坐标已知,因此可以将问题转化为PnP问题. 通过标志点的像素坐标与世界坐标之间的转换关系,得到相机坐标系在世界坐标系下的位姿,进而得到惯性单元在世界坐标系下的位姿,对内环估计的状态量进行修正.

设由标志点得到的当前时刻相机坐标系在世界坐标系下的位姿为 $\left[{\begin{array}{*{20}{l}} {_{\text{G}}^{\text{C}}{{{\boldsymbol{\bar q}}}^{\left( {\rm m}\right)}}}&{^{\text{G}}{\boldsymbol{p}}_{\text{C}}^{\left({\rm m} \right)}} \end{array}} \right]$,在相机坐标系相对于惯性单元坐标系位姿已知的情况下,可得当前时刻惯性单元相对于世界坐标系下的位姿为

$ _{\text{G}}^{\text{I}}{{\boldsymbol{\hat {\bar q}}}^{{\rm{(m)}}}} = _{\text{C}}^{\text{I}}{\boldsymbol{\bar q}} \otimes _{\text{G}}^{\text{C}}{{\boldsymbol{\hat {\bar q}}}^{{\rm{(m)}}}}, $

$ ^{\text{G}}{\boldsymbol{\hat p}}_{\text{I}}^{{\rm{(m)}}}{ = ^{\text{G}}}{\boldsymbol{\hat p}}_{\text{C}}^{{\rm{(m)}}} + \left(C\left( {_{\text{G}}^{\text{C}}{{{\boldsymbol{\hat {\bar q}}}}^{{\rm{(m)}}}}} \right)\right)^{\rm{T}}{{\mkern 1mu} ^{\text{C}}}{{\boldsymbol{\hat p}}_{\text{I}}}, $

${\boldsymbol{z}}_{\text{I}}^{{\rm{(m)}}} = {\left[ {\begin{array}{*{20}{c}} {_{\text{G}}^{\text{I}}{{{{\hat {\bar {\boldsymbol{q}}}}}}^{{\rm{(m)}}}}^{\text{T}}}&{^{\text{G}}{{\hat {\boldsymbol{p}}}}_{\text{I}}^{{\rm{(m)}}{\text{T}}}} \end{array}} \right]^{\text{T}}} . $

基于特征点观测量的惯性单元状态量位姿残差观测量为

$ {{\boldsymbol{r}}^{{\rm{(m)}}}} = {\boldsymbol{z}}_{\text{I}}^{{\rm{(m)}}} - {{\boldsymbol{z}}_{\text{I}}} = {\left[ {\begin{array}{*{20}{c}} {_{\text{G}}^{\text{I}}{{{\boldsymbol{\tilde \theta }}}^{{\rm{(m)}}{\text{T}}}}}&{^{\text{G}}{\boldsymbol{\tilde p}}_{\text{I}}^{{\rm{(m)}}{\text{T}}}} \end{array}} \right]^{\text{T}}} . $

构造有关外环残差观测量的观测方程:

$ {{\boldsymbol{r}}^{{\rm{(m)}}}} = {{\boldsymbol{H}}^{{\rm{(m)}}}}{\boldsymbol{\tilde X}} + {{\boldsymbol{n}}^{{\rm{(m)}}}}. $

式中:观测矩阵 ${{\boldsymbol{H}}^{{\rm}{\rm{(m)}}}} = \left[{\begin{array}{*{20}{c}} {{{\boldsymbol{I}}_6}}\ \;\;{{{\boldsymbol{0}}_{6 \times (6N + 9)}}} \end{array}} \right]$.

由于观测矩阵具有稀疏性,计算速度较快,直接将观测矩阵用于滤波器增益矩阵的计算. 增益矩阵及状态量和协方差矩阵的修正方程为

$ {{\boldsymbol{K}}^{{\rm{(m)}}}} = {\boldsymbol{P}}{{\boldsymbol{H}}^{{\rm{(m)}}{\text{T}}}}{\left( {{{\boldsymbol{H}}^{{\rm{(m)}}}}{\boldsymbol{P}}{{\boldsymbol{H}}^{{\rm{(m)}}{\text{T}}}} + {{\boldsymbol{R}}_{\rm{n}}}} \right)^{ - 1}}, $

$ \Delta {\boldsymbol{X}} = {{\boldsymbol{K}}^{{\rm{(m)}}}}{{\boldsymbol{r}}^{{\rm{(m)}}}}, $

$ \begin{split} {\boldsymbol{P}}_{k + 1\mid k + 1}^{{\rm{(m)}}} = & \left( {{\boldsymbol{I}} - {{\boldsymbol{K}}^{{\rm{(m)}}}}{{\boldsymbol{H}}^{{\rm{(m)}}}}} \right){{\boldsymbol{P}}_{k + 1\mid k + 1}}{\left( {{\boldsymbol{I}} - {{\boldsymbol{K}}^{{\rm{(m)}}}}{{\boldsymbol{H}}^{{\rm{(m)}}}}} \right)^{\text{T}}} + \\ & {{\boldsymbol{K}}^{{\rm{(m)}}}}{{\boldsymbol{R}}_{\rm{n}}}{{\boldsymbol{K}}^{{\rm{(m)}}{\text{T}}}}. \end{split} $

MSCKF对于相关参数初始值的选取比较敏感,容易出现由于初始值选取导致滤波器发散的情况. 为了减小标志点修正对滤波器收敛带来的影响,在滤波器运行的初始阶段不引入外环修正流程,仅采用内环的MSCKF进行定位. 待到内环滤波器稳定收敛后,引入外环修正流程,抑制内环滤波器的漂移.

3. 实验与结果分析

为了验证提出算法的有效性与稳定性,将提出的算法与OpenVins[8]与LARVIO[9]2种开源VIO算法进行对比,采用Gazebo仿真引擎[20]搭建仿真测试环境. 采用单目摄像头进行环境感知,所采集的RGB图像分辨率为 $500 \times 320$像素,采样频率约为20 Hz;惯性单元的采样频率为200 Hz. 搭建的测试环境如图3所示. 测试环境周围的楼房主要为算法前端提供丰富的纹理信息. 环境中的矩形为本文设置的标志物,提供外环位姿修正信息. 在仿真环境下,共设置2种飞行轨迹,对3种算法进行比较与评估.

图 3

图 3   定位算法的性能测试环境

Fig.3   Performance test environment of positioning algorithm


第1条轨迹下3种算法的定位效果如图4(a)、(b)所示,无人机的平均飞行速度为0.7 m/s,总飞行轨迹长度为22.03 m. 3种算法的定位误差如图5(a)所示,轨迹每一阶段的平均误差与标准差如图5(b)所示. 图中,Eabs为绝对位置误差,Em为平均误差,轨迹段i表示图4(a)、(b)中i号标志点与i+1号标志点之间的轨迹. 从图5可以看出,在1至2号标志点之间,由于加速度变化较小,3种算法的误差均较小. 在2号标志点处,由于加速度与角速度产生较大的变化,3种算法均有一定程度的定位误差.

图 4

图 4   轨迹1与轨迹2的定位图

Fig.4   Positioning results of trajectory 1 and trajectory 2


图 5

图 5   路径1的误差比较

Fig.5   Error comparision of trajectory 1


对于OpenVins与LARVIO,由于没有外环修正机制,从2号标志点开始这2种算法均逐渐产生误差累积,且误差逐渐增大;对于本文算法来说,虽然在2号标志点处产生了定位误差,但是在2号标志点之后,逐渐通过外环修正机制,将无人机定位修正至真实轨迹附近,提高了定位精度. 3种算法的平均定位误差如表1的Traj1所示,与OpenVins与LARVIO相比,本文算法的定位精度分别提升了54.6%与60.7%.

表 1   2条轨迹下3种算法的定位误差

Tab.1  Positioning error of three algorithms for two trajectories

算法 误差/m
轨迹1 轨迹2
本文算法 0.226 0.266
OpenVins 0.498 0.644
LARVIO 0.575

新窗口打开| 下载CSV


第2条轨迹下3种算法的定位效果如图4(c)、(d)所示. 由于LARVIO算法在该轨迹下未收敛,在图4(c)、(d)中未画出. 无人机的平均飞行速度为0.7 m/s,总飞行轨迹长度为44.83 m. 2种算法的定位误差如图6 (a)所示,轨迹中每一阶段的平均误差与定位误差的标准差如图6(b)所示. 图中,轨迹段i表示图4(c)、(d)中轨迹的第i圈轨迹. 从图6可以看出,在初始阶段,由于加速度与角速度的变化幅度较小,无人机定位误差较小. 随着飞行时间的增加,OpenVins算法的定位漂移量逐渐增加,本文算法通过外环修正对定位漂移进行抑制,提高了定位精度. 2种算法的定位误差平均值如表1的轨迹2所示. 与OpenVins相比,本文算法的定位精度提升了58.7%.

图 6

图 6   路径2的误差比较

Fig.6   Error comparision of trajectory 2


算法的实时性分析如表2所示. 表中,td为时延.所选择的3种算法分别为本文算法、OpenVins及文献[13]算法. 可以看出,与没有外环校正的OpenVins算法相比,本文算法在牺牲了一定的实时性基础上提高了定位的精度,且算法实时性符合无人机的计算要求. 与使用基于YOLO网络的目标检测进行外环校正[13]相比,本文算法使用了传统的图像处理算法,因此算法的实时性大幅提升.

表 2   各定位算法的实时性分析

Tab.2  Real-time analysis of positioning algorithms

算法 td/ms
本文算法 7.92
OpenVins 2.67
文献[13]算法 >22.22

新窗口打开| 下载CSV


4. 结 语

针对基于MSCKF的室内定位方法存在的容易产生漂移的问题,本文提出基于改进MSCKF的室内定位方法. 在友好的室内环境中设定位置已知的标志点,在MSCKF框架下得到无人机内环位姿. 通过前端图像处理进行标志点检测,得到无人机自身的位姿,对内环位姿估计进行修正. 实验结果表明,利用本文算法有效地抑制了定位漂移情况,提高了定位精度. 在未来工作中,将在真实环境中测试提出方法的定位效果.

参考文献

USENKO V, ENGEL J, STÜCKLER J, et al. Direct visual-inertial odometry with stereo cameras [C]// 2016 IEEE International Conference on Robotics and Automation. Stockholm: IEEE, 2016: 1885-1892.

[本文引用: 1]

MUR-ARTAL R, TARDÓS J D

Visual-inertial monocular SLAM with map reuse

[J]. IEEE Robotics and Automation Letters, 2017, 2 (2): 796- 803

DOI:10.1109/LRA.2017.2653359      [本文引用: 1]

MUR-ARTAL R, MONTIEL J M M, TARDOS J D

ORB-SLAM: a versatile and accurate monocular SLAM system

[J]. IEEE Transactions on Robotics, 2015, 31 (5): 1147- 1163

DOI:10.1109/TRO.2015.2463671      [本文引用: 1]

MUR-ARTAL R, TARDÓS J D

Orb-slam2: an open-source slam system for monocular, stereo, and RGB-D cameras

[J]. IEEE Transactions on Robotics, 2017, 33 (5): 1255- 1262

DOI:10.1109/TRO.2017.2705103      [本文引用: 1]

CASTELLANOS J A, NEIRA J, JD T

Limits to the consistency of EKF-based SLAM

[J]. IFAC Proceedings Volumes, 2004, 37 (8): 716- 721

DOI:10.1016/S1474-6670(17)32063-3      [本文引用: 1]

MOURIKIS A I, ROUMELIOTIS S I. A multi-state constraint Kalman filter for vision-aided inertial navigation [C]// Proceedings of 2007 IEEE International Conference on Robotics and Automation. Roma: IEEE, 2007: 3565-3572.

[本文引用: 1]

SUN K, MOHTA K, PFROMMER B, et al

Robust stereo visual inertial odometry for fast autonomous flight

[J]. IEEE Robotics and Automation Letters, 2018, 3 (2): 965- 972

DOI:10.1109/LRA.2018.2793349      [本文引用: 1]

GENEVA P, ECKENHOFF K, LEE W, et al. Openvins: a research platform for visual-inertial estimation [C]// 2020 IEEE International Conference on Robotics and Automation. Paris: IEEE, 2020: 4666-4672.

[本文引用: 2]

QIU X, ZHANG H, FU W

Lightweight hybrid visual-inertial odometry with closed-form zero velocity update

[J]. Chinese Journal of Aeronautics, 2020, 33 (12): 3344- 3359

DOI:10.1016/j.cja.2020.03.008      [本文引用: 2]

HUAI Z, HUANG G. Robocentric visual-inertial odometry [C]// 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems. Madrid: IEEE, 2018: 6319-6326.

MA F, SHI J, YANG Y, et al

ACK-MSCKF: tightly-coupled Ackermann multi-state constraint Kalman filter for autonomous vehicle localization

[J]. Sensors, 2019, 19 (21): 4816

DOI:10.3390/s19214816     

ZHENG F, TSAI G, ZHANG Z, et al. Trifo-VIO: robust and efficient stereo visual inertial odometry using points and lines [C]// 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems. Madrid: IEEE, 2018: 3686-3693.

[本文引用: 1]

BAVLE H, MANTHE S, DE L P P, et al. Stereo visual odometry and semantics based localization of aerial robots in indoor environments [C]// 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems. Madrid: IEEE, 2018: 1018-1023.

[本文引用: 4]

LEVINSON J, MONTEMERLO M, THRUN S. Map-based precision vehicle localization in urban environments [C]// Robotics: Science and Systems. Atlanta: [s. n.], 2007: 1.

[本文引用: 1]

LEVINSON J, THRUN S. Robust vehicle localization in urban environments using probabilistic maps [C]// 2010 IEEE International Conference on Robotics and Automation. Anchorage: IEEE, 2010: 4372-4378.

[本文引用: 1]

QIN T, LI P, SHEN S

Vins-mono: a robust and versatile monocular visual-inertial state estimator

[J]. IEEE Transactions on Robotics, 2018, 34 (4): 1004- 1020

DOI:10.1109/TRO.2018.2853729      [本文引用: 1]

QIN T, SHEN S. Online temporal calibration for monocular visual-inertial systems [C]// 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems. Madrid: IEEE, 2018: 3662-3669.

[本文引用: 1]

徐晓苏, 代维, 杨博, 等

室内环境下基于图优化的视觉惯性SLAM方法

[J]. 中国惯性技术学报, 2017, 25 (3): 313- 319

[本文引用: 1]

XU Xiao-su, DAI Wei, YANG Bo, et al

Visual-aid inertial SLAM method based on graph optimization in indoor

[J]. Journal of Chinese Inertial Technology, 2017, 25 (3): 313- 319

[本文引用: 1]

HARRIS C G, STEPHENS M. A combined corner and edge detector [C]// Alvey Vision Conference. Manchester: [s. n.], 1988: 10-5244.

[本文引用: 1]

KOENIG N, HOWARD A. Design and use paradigms for gazebo, an open-source multi-robot simulator [C]// 2004 IEEE/RSJ International Conference on Intelligent Robots and Systems. Sendai: IEEE, 2004: 2149-2154.

[本文引用: 1]

/