CN113124856B - 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法 - Google Patents

基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法 Download PDF

Info

Publication number
CN113124856B
CN113124856B CN202110558941.7A CN202110558941A CN113124856B CN 113124856 B CN113124856 B CN 113124856B CN 202110558941 A CN202110558941 A CN 202110558941A CN 113124856 B CN113124856 B CN 113124856B
Authority
CN
China
Prior art keywords
imu
uwb
frame
anchor point
unmanned aerial
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110558941.7A
Other languages
English (en)
Other versions
CN113124856A (zh
Inventor
田栢苓
李海松
谌宏鸣
宗群
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tianjin University
Original Assignee
Tianjin University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Tianjin University filed Critical Tianjin University
Priority to CN202110558941.7A priority Critical patent/CN113124856B/zh
Publication of CN113124856A publication Critical patent/CN113124856A/zh
Application granted granted Critical
Publication of CN113124856B publication Critical patent/CN113124856B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本发明属于四旋翼无人机导航与控制领域，为解决在GNSS拒止环境下四旋翼无人机的定位导航问题。在线得到较为准确地得到锚点在世界坐标系下的位置，本发明，基于UWB在线锚点的视觉惯性紧耦合里程计及计量方法，超宽带技术制成的电波测距模块UWB，根据TW‑ToF测量原理获得两个模块之间的距离，通过离群值检测单元以滤除部分噪声后，作为输入送入紧耦合优化单元；IMU惯性测量结果也作为输入送入紧耦合优化单元；嵌入式机载处理器利用所述的紧耦合优化单元，处理UWB模块之间的距离测量值、IMU测量值和双目摄像头测量值，用紧耦合优化的方式解析出无人机的位置与姿态信息。本发明主要应用于四旋翼无人机导航与控制场合。

Description

基于UWB在线锚点的视觉惯性紧耦合里程计及计量方法
技术领域
本发明属于四旋翼无人机导航与控制领域，涉及嵌入式***、传感器网络，尤其涉及GNSS(全球导航卫星***)拒止环境下四旋翼无人机定位***及其相关算法。具体涉及基于UWB在线锚点标定的视觉惯性紧耦合里程计。
背景技术
近年来，四旋翼无人机具有体积小、重量轻、隐蔽性好、适合多平台、多空间使用等特点，具备在地面、军舰上灵活垂直起降，不需要弹射器、发射架发射等优点，已经在军事和民用领域展现出巨大的应用价值。在民用领域，街景拍摄、电力巡检、交通监视、快递递送、灾后救援等方面均可由四旋翼无人机完成；在军用方面，情报侦察、军事打击、信息对抗、空中预警、通信中继等任务中四旋翼无人机发挥了重要的作用。然而在隧道、丛林和室内等场景下，由于GNSS的非视距测量，导致定位误差极大。因此在GNSS拒止环境下，无人机的定位与导航问题仍是一种挑战。
超宽带(ultra-wideband，UWB)是近年来一种新颖的电波测距技术，超宽带技术通过测量电波的到达时间、到达时间差或到达角计算出两个模块之间的距离。由于发射电波的波段在3.1GHz-4.8GHz之间，能够有效克服其他电波信号的干扰。此外其较高的带宽能够轻易克服多径效应、减弱非视距测量的影响。Time domain公司生产的P440 UWB模块采用双向飞行时间法(TW-ToF)测量距离和通信，由于其良好的性能和低廉的价格，可利用该模块搭建一套定位***，为无人机在GNSS拒止环境下提供定位信息。
使用UWB定位的步骤一般是先通过机载UWB标签依次测量与地面锚点之间的距离，再通过特定的算法估计无人机位置。UWB定位方法的优点是传感器的成本低，不受光照和天气的影响，但UWB定位方法也有显著的缺陷，比如容易受到外界电波干扰，定位精度不甚理想，这也无疑限制了无人机在GNSS拒止环境下的应用。
针对GNSS拒止环境下基于UWB增强的四旋翼无人机自主定位问题，国内外学者都进行了大量的研究。2017年，新加坡南洋理工大学Chen Wang等人提出了一套UWB增强的快速定位和建图***。他们意识到视觉SLAM***虽然在短期内拥有较高的精确度，却在长时间运行时存在累计误差，他们使UWB距离测量来修正累计误差，提升了***的鲁棒性。2018年，新加坡国立大学Jiaxin Li等人采用EKF(Extended Kalman filter)算法融合了IMU(Inertial measurement unit)和UWB测量数据，改善UWB传感器单独定位有滞后，估计精度较低等问题。2020年蒙特利尔工业大学Yanjun Cao等人提出了VIR-SLAM，他们使用UWB距离测量修正单目视觉惯性里程计的累计误差。由此可见，利用基于UWB增强的多传感器融合定位是一种富有前景的方案。然而，上述基于UWB增强的方法需要提前人工测量计算得到无人机初始时刻与锚点之间的相对位姿，因此每一次实验都需要标定一次，室内实验运行条件较为苛刻，室外实验更加受限于此，除此之外，上述方法并未将IMU信息与UWB测距信息进行融合，因此在精度方面本方法也有改进之处。
发明内容
为克服现有技术的不足，本发明旨在解决在GNSS拒止环境下四旋翼无人机的定位导航问题。本发明提出了一种基于UWB在线锚点标定的视觉惯性紧耦合里程计设计方法，使用本方法的在线锚点标定单元可以在线得到较为准确地得到锚点在世界坐标系下的位置，通过融合IMU惯性测量、双目视觉图像信息、UWB距离测量，利用紧耦合的方式解析出无人机的位姿信息，通过设计四旋翼无人机平台并运行该定位算法，实现无人机快速精确定位，具有极大的应用价值。为此，本发明采取的技术方案是，基于UWB在线锚点的视觉惯性紧耦合里程计，结构如下：超宽带技术制成的电波测距模块UWB，部署在两个位置，其一是作为锚点部署在环境四周，锚点自身的位置通过在线锚点标定单元计算出；其二是作为标签放置在无人机上，标签按照预设的顺序主动向锚点发送测距请求，根据TW-ToF测量原理获得两个模块之间的距离，通过离群值检测单元以滤除部分噪声后，作为输入送入紧耦合优化单元；IMU惯性测量单元用于高频测量无人机的加速度和角速度信息，IMU测量值经过低通滤波器模块处理后，同样作为输入送入紧耦合优化单元；双目摄像头的测量值经过视觉里程计的处理后，也作为输入送入紧耦合优化单元；在线锚点标定单元、离群值检测单元和紧耦合优化单元均为供嵌入式机载处理器调用的模块，嵌入式机载处理器利用所述的紧耦合优化单元，处理UWB模块之间的距离测量值、IMU测量值和双目摄像头测量值，用紧耦合优化的方式解析出无人机的位置与姿态信息。
基于UWB在线锚点的视觉惯性紧耦合里程计量方法，借助如下装置实现：超宽带技术制成的电波测距模块UWB，部署在两个位置，其一是作为锚点部署在环境四周，锚点自身的位置通过在线锚点标定计算出；其二是作为标签放置在无人机上；还包括机载双目摄像头、IMU单元；标签按照预设的顺序主动向锚点发送测距请求，根据TW-ToF测量原理获得两个模块之间的距离，通过离群值检测滤除部分噪声后，作为输入进行紧耦合优化；IMU惯性测量单元用于高频测量无人机的加速度和角速度信息，IMU测量值经过低通滤波器处理后，同样作为输入进行紧耦合优化；双目摄像头的测量值经过视觉里程计的处理后，也作为输入进行紧耦合优化；嵌入式机载处理器处理UWB模块之间的距离测量值、IMU测量值和双目摄像头测量值，用紧耦合优化的方式解析出无人机的位置与姿态信息。
IMU预积分：为了充分利用IMU数据，在未接收到UWB数据和图像数据时，需要对IMU数据进行处理，进行粗略的估计得到IMU的预测位姿；另一方面，为了避免重复计算，在每次更新加速度和角速度的零偏后更新预积分的值。
IMU预积分具体步骤如下：利用已知相机第k帧时刻的位置、速度和姿态，通过将相机第k帧和第k+1帧之间的所有IMU测量数据进行积分，得到第k+1帧对应的位置、速度及姿态状态，IMU积分连续形式为：
Figure BDA0003078362430000031
这里有：
Figure BDA0003078362430000032
其中，
Figure BDA0003078362430000033
和
Figure BDA0003078362430000034
表示世界坐标系w下第k帧时刻机体b的位置和速度以及旋转的四元数表示，
Figure BDA0003078362430000035
和
Figure BDA0003078362430000036
表示世界坐标系w下第k+1帧时刻机体b的位置和速度以及旋转的四元数表示，△tk为两个IMU测量数据的时间差，
Figure BDA0003078362430000037
和
Figure BDA0003078362430000038
分别表示IMU测得的t时刻的加速度和角速度，
Figure BDA0003078362430000039
为世界坐标系w下t时刻的四元数表示，
Figure BDA00030783624300000310
和
Figure BDA00030783624300000311
分别表示其对应的加速度及角速度测量偏差bias，gw表示世界坐标系下的重力加速度，ωx、ωy、ωz分别为ω的三个分量；
由于实际过程中IMU为离散采样，因此通过第i个时刻IMU状态求解第i+1时刻IMU状态的中值离散形式为：
Figure BDA00030783624300000312
其中，
Figure BDA00030783624300000313
和
Figure BDA00030783624300000314
分别为将IMU系下的测量加速度和角速度转换至世界系的机体加速度和角速度；
为避免重复计算，将优化变量从第k帧到第k+1帧的IMU预积分项中分离开来：
Figure BDA00030783624300000315
其中：
Figure BDA0003078362430000041
其中
Figure BDA0003078362430000042
为第k帧世界坐标系到机体坐标系的旋转矩阵，
Figure BDA0003078362430000043
将公式(3)化为对应离散形式，通过第i个时刻IMU状态求解第i+1时刻IMU状态关系为：
Figure BDA0003078362430000044
此处
Figure BDA0003078362430000045
为相对于bk帧时刻下
Figure BDA0003078362430000046
的IMU增量信息，其状态只与bk帧有关，通过对式(5)的迭代求解，预积分出两帧之间的相对位姿。
离群值检测滤除部分噪声的具体步骤如下：当程序处于在线锚点标定阶段时，离群值检测单元的作用就是对接受到的UWB测距信息进行滤波，假设UWB的测量值为d，在当前场景下，收集n个样本点di(i＝1,…,n)，如果满足以下条件，则第i次距离测量值di被判定为离群值：
Figure BDA0003078362430000047
Figure BDA0003078362430000048
其中
Figure BDA0003078362430000049
是n次测距值的均值，γ是可调阈值,vmax为无人机最大飞行速度，f是UWB模块测距更新速率，如果本次距离被判定为离群值，则直接舍弃，若为正常情况，则将距离di作为输入送入在线锚点标定单元；
当在线锚点标定阶段结束后，使用无人机当前位姿与标定的锚点位置之差的二范数作为判据，若该二范数满足以下条件，则第i次距离测量值di被判定为离群值：
Figure BDA00030783624300000410
其中Pk是当前无人机位姿，Pa是通过在线锚点标定单元得到的无人机初始时刻坐标系下的锚点位置坐标，γ是可调阈值,vmax为无人机最大飞行速度，f是UWB模块测距更新速率，如果本次距离被判定为离群值，则直接舍弃；若为正常情况，则将距离di作为输入送入紧耦合单元。
在线锚点标定步骤：使用UWB的定位时，利用输出的里程计信息以及UWB的测距信息，标定出在无人机起飞初始时刻的坐标系下锚点的坐标。
在线锚点标定具体步骤如下：
Figure BDA0003078362430000051
Figure BDA0003078362430000052
其中di是第i次距离测量值，dn是第n(i>n)次距离测量值，γ是可调阈值,f是UWB模块测距更新速率，
Figure BDA0003078362430000053
为当前无人机在世界坐标系下的位姿，vthr为预先设定的速度阈值，如果满足该条件，则第i次距离测量值将被抛弃，将第i+1次距离测量值代入上式进行判断，以此类推直到第m次测量值不满足该条件后，将n赋值为m；
于是，针对第i次距离测量，得到关于测距信息的残差
Figure BDA0003078362430000054
Figure BDA0003078362430000055
其中di是第i次距离测量值，
Figure BDA0003078362430000056
是由IMU预积分和视觉里程计得到的与UWB测距信息同一时间戳下的无人机世界坐标系下的位姿，
Figure BDA0003078362430000057
是待估计的锚点在世界坐标系下的三维坐标，于是优化的代价函数Er：
Figure BDA0003078362430000058
其中
Figure BDA0003078362430000059
ρ()是一个pseudo-Huber损失函数，使用Ceres的自动求导功能，编写代价函数后确定待优化变量
Figure BDA00030783624300000510
将参数di、
Figure BDA00030783624300000511
传入接口后即可进行优化，本方法判断优化终止的条件为前后两次优化得到的
Figure BDA00030783624300000512
之差的二范数小于0.1。
紧耦合优化包括IMU因子、UWB因子以及视觉因子，在划窗内，IMU因子是通过预积分分别得到相邻两帧数据之间的位姿残差、UWB因子得到每一帧时刻下无人机与锚点之间的距离残差、视觉因子通过重投影得到每一帧下的位姿残差，在得到各项因子的残差后，使用用于建模和解决优化问题的求解器，利用自动求导的功能对划窗内每一帧的参数进行优化得到位姿信息和加速度和角速度的零偏，假设tk为划窗内第k+1帧图像对应的时间戳，tk+1为划窗内第k+2帧图像对应的时间戳，tki为划窗内第k+1帧图像与第k+2帧图像之内IMU的第i个数据的时间戳，tku为划窗内第k+1帧图像与第k+2帧图像之内UWB的第u个数据的时间戳；
UWB因子
针对划窗内每一次的UWB测距信息di，得到关于测距信息的残差
Figure BDA00030783624300000513
和代价函数Γα：
Figure BDA0003078362430000061
△tuk＝tku-tk
Figure BDA0003078362430000062
其中
Figure BDA0003078362430000063
是tk时刻下无人机在世界坐标系下的位置坐标，
Figure BDA0003078362430000064
时刻下无人机在世界坐标系下的速度，△tuk是UWB数据时间戳与图像时间戳之差，gW是世界坐标系下重力加速度的三维表示，
Figure BDA0003078362430000065
是tk时刻下无人机在世界坐标系下的旋转矩阵，
Figure BDA0003078362430000066
是根据IMU信息得到的预积分位置分量，
Figure BDA0003078362430000067
是经过在线锚点标定单元后得到的世界坐标系下锚点的三维坐标，ρ()是一个pseudo-Huber损失函数；
IMU因子
针对前后两次IMU的测量信息，得到关于IMU测量信息的代价函数Γβ：
Figure BDA0003078362430000068
其中
Figure BDA0003078362430000069
分别为由里程计得到的关于第k帧到第k+1帧的位置、速度、姿态的增量，
Figure BDA00030783624300000610
分别为第k+1帧和第k帧时刻世界坐标系下的陀螺仪偏差，
Figure BDA00030783624300000611
分别为第k+1帧和第k帧时刻世界坐标系下的加速度计偏差；
视觉因子
视觉定义为被跟踪特征的重投影误差，将当前帧中所有特征的重投影与它们的第一次观察进行比较，得到关于重投影误差的代价函数Γγ：
Figure BDA00030783624300000612
其中
Figure BDA00030783624300000613
是相机第j帧图像下第lk个特征的坐标，π是将齐次坐标转换成图像坐标的投影函数，
Figure BDA00030783624300000614
分别表示从相机第i帧到第j的帧旋转矩阵和平移矩阵，
Figure BDA00030783624300000615
是在第一次观察帧i中第k个特征的3D位置，视觉因子在所有帧和在估计状态下的所有被跟踪的特征中进行不断的迭代。
还包括配合边缘化策略使用滑动窗口步骤，边缘化策略是将被移出滑窗的帧作为边缘化帧，与边缘化帧有关的约束称为边缘化约束，边缘化帧在之后不会进行更新，但是边缘化约束会变为正常待优化帧的先验信息，这相当于从求解一个联合概率分布变为求解有先验信息的条件概率分布。
其中，一个划窗选择十个关键帧，将紧耦合优化的结果作为输入，每一帧分别包含同一个时刻tk下相应的IMU因子、UWB因子以及视觉因子，这些因子之间相互独立，构成了紧耦合优化的约束项，确保划窗内最多只存在十个关键帧，若超过十个，首先会对即将被边缘化的关键帧内的各个因子进行修改，接下来直接丢弃被边缘化的关键帧内的所有数据，将修改后因子与最近接收的因子进行耦合，再次进行紧耦合优化，对待优化变量进行优化，得到无人机的位姿以及地图信息。
本发明的特点及有益效果是：
本发明对GNSS拒止环境下四旋翼无人机的定位与导航算法研究具有十分重要的意义。本发明稳定可靠，不受天气和光线等的影响，无需提前知道锚点的位置，通过在线的方法标定出锚点的位置，同时算法节省计算资源，对硬件要求较低。本发明具有很高的理论与工程价值，本发明的优点总结如下：
(1)本发明提出了IMU预积分和划窗的策略，可以节省计算资源，提高***的实时性。通过IMU预积分单元，可以避免了在每一次优化加速度和角速度的零偏后重复计算预测位姿的步骤，因此节省了在紧耦合优化单元内每一次优化的时间。无人机起飞后，随着时间的积累，如果使用优化的方法会使得待优化变量越来越多，划窗的策略使得优化的位姿数量减少的同时不会失去先前的信息，从而保障精度不会受到影响。
(2)本发明设计了在线标定锚点位置的算法，节省了人力的同时提高了UWB增强定位的实用性。传统的方法需要提前人工测量计算得到无人机初始时刻与锚点之间的相对位姿，因此每一次实验都需要标定一次，室内实验运行条件较为苛刻，室外实验更加受限于此。本发明设计的在线锚点标定算法通过使用IMU和相机数据得到的里程计信息与测距信息进行耦合，在无人机飞行一段时间后得到无人机初始时刻坐标系下的锚点位置坐标。标定完成后，UWB信息将作为约束进入紧耦合优化单元。
(3)本发明设计了一个基于紧耦合的优化算法。松耦合的优化方式是通过利用某种传感器的原始数据后先算出一个位姿作为初值，随后再与其他传感器融合，利用该初值继续优化。紧耦合的方法则是使用所有传感器的原生数据，分别利用每个传感器观测得到的数据得到残差，利用一个大的优化函数得到优化后的参数信息。对应起来，本方法即是使用紧耦合的优化算法对IMU、UWB和双目相机的信息处理得到无人机的位姿和加速度和角速度的零偏。
(4)本发明搭建的GNSS拒止环境下无人机定位平台可扩展性强，除了IMU、UWB和双目相机等传感器设备之外，还可根据开发者自行添加传感器设备，如激光雷达、定高雷达等，可进行二次开发。
附图说明：
附图1四旋翼无人机***硬件实物图。
附图2基于UWB在线锚点标定的视觉惯性紧耦合里程计设计结构框图。
附图3在线锚点标定单元示意图。
附图4室内场景下四次在线锚点标定结果示意图。
附图5走廊场景下四次在线锚点标定结果示意图。
附图6 Rviz下室内场景实验俯视图。
附图7 Rviz下室内场景实验侧视图。
附图8室内场景下四次实验误差对比分析表。
附图9 Rviz下走廊场景实验俯视图。
附图10 Rviz下走廊场景实验侧视图。
附图11走廊场景下四次实验误差对比分析表。
附图12 Euroc数据集(一个得到认可的公开数据集)下Vins(一个先进的开源视觉惯性里程计)方法的绝对误差曲线图。
附图13 Euroc数据集下本方法的绝对误差曲线图。
附图14 Euroc数据集下Vins、本方法、真值的轨迹图。
具体实施方式
本发明采取的技术方案包括硬件和软件两部分，硬件包括：包括UWB测距模块，IMU惯性测量传感器、双目摄像头、嵌入式机载处理器和无人机平台。软件包括：IMU预积分单元、离群值检测单元、在线锚点标定单元、滑动窗口单元、紧耦合优化单元。***硬件实物见附图1，整个算法流程见附图2。下面针对各部分做具体说明。
硬件部分：
UWB指采用超宽带技术制成的电波测距模块，它一般是通过发射纳秒级甚至亚纳秒级的脉冲信号来测量两个端点之间的距离，这种脉冲信号的时间分辨率极高从而可保证高精度的测距，理想情况下达到厘米级精度；它的信号的频谱范围比大多数电波测距技术的频谱要宽，同时拥有丰富的低频分量，导致UWB信号拥有很高的穿透力，可以轻松穿过木板、玻璃等障碍物，能在视距与非视距共存的场景下完成测距。UWB模块需要部署在两个位置，其一是作为锚点部署在环境四周，锚点需要知道自身的位置，其位置可通过在线锚点标定单元计算出；其二是作为标签放置在无人机上，标签按照预设的顺序主动向锚点发送测距请求，根据TW-ToF测量原理获得两个模块之间的距离。由于原生距离测量带有一定的误差，需通过离群值检测单元以滤除部分噪声后，作为输入送入紧耦合优化单元。IMU惯性测量单元用于高频测量无人机的加速度和角速度信息，IMU测量值经过低通滤波器模块处理后，同样作为输入送入紧耦合优化单元。双目摄像头的测量经过视觉里程计的处理后，也作为输入送入紧耦合优化单元。嵌入式机载处理器主要用于所述算法实时运行，通过处理UWB模块之间的距离测量值、IMU测量和双目摄像头测量，用紧耦合优化的方式解析出无人机的位置与姿态信息；无人机平台用于搭载以上设备进行飞行和验证。
软件部分：
(1)IMU预积分单元
IMU的测量频率往往在100Hz以上，相较于相机和UWB而言，其输出频率要高很多。IMU惯性测量单元在短的时间内可以得到较为准确的加速度和角速度信息，但是经过一段时间内会发生漂移现象。一方面为了充分利用IMU数据，在未接收到UWB数据和图像数据时，需要对IMU数据进行处理，进行粗略的估计得到IMU的预测位姿。另一方面由于需要估计的状态量除了无人机的位姿，还有加速度和角速度的零偏，这意味着每进行一次优化就要重新计算一遍IMU的预测位姿。为了避免重复计算，通过IMU预积分单元，在每次更新加速度和角速度的零偏后更新预积分的值即可。
(2)离群值检测单元
即便理想情况下UWB测量的距离能够达到厘米级，但实际上距离测量常常因为温度、场景等原因具有静差和误差。此外，在障碍物密集的场景下有可能引入离群值，离群值指明显有悖于事实的测量，如读数出现负数，突然发生很大跳变等情况，离群值的引入会导致错误的状态估计。这里采用添加阈值方法检测离群值，一旦认为本次距离测量为离群值，将直接被剔除。经过实验测试，离群值对状态估计有较为明显的影响，不仅会影响定位精度，甚至可能导致定位的完全错误，在引入离群值检测单元后可以改善此种情况。
(3)在线锚点标定单元
传统的使用UWB的定位方法需要提前知道布置在无人机上的锚点与布置在地面上的锚点之间的相对位姿。通常的方法是将地面上的锚点布置与无人机初始航向垂直或者平行的方法并人工测量出距离，或者通过离线的方法先得到相对位姿后再切换模式，将标定后的锚点位置作为已知信息。显然，这写方法存在缺陷，不仅不够方便，也无法保证精度，限制了基于UWB增强的定位实用性。如附图3所示，本发明通过设计一个在线锚点标定单元，假设无人机起飞的一段时间内，IMU和视觉的定位足够精确，利用输出的里程计信息以及UWB的测距信息，标定出在无人机起飞初始时刻的坐标系下锚点的坐标。
(4)滑动窗口单元
在基于优化的定位技术中，随着运行时间的加长，待优化状态与观测约束越来越多，海森矩阵Hl的规模会快速上升，使求解速度变得缓慢，无法满足实时性要求。为了解决这个问题，本文使用基于滑动窗口策略的后端优化。在构建联合优化问题时，只考虑滑窗内的帧与约束。在里程计运行时，不断会有新的帧加入到窗口中，同时不断有旧的帧从滑窗中剔除出去，这样就可以保持待优化的帧的规模，避免了求解速度下降。当旧的帧被移除出滑窗时，被移除滑窗的帧仍然与在滑窗内的帧存在约束，直接移除的话此部分的约束就会丢失，导致里程计精度下降，所以滑动窗口的使用通常配合边缘化策略。边缘化策略是将被移出滑窗的帧作为边缘化帧，与边缘化帧有关的约束称为边缘化约束。边缘化帧在之后不会进行更新，但是边缘化约束会变为正常待优化帧的先验信息，这相当于从求解一个联合概率分布变为求解有先验信息的条件概率分布。这样做即维持了优化问题的规模，也没有损失约束。
(5)紧耦合优化单元
紧耦合优化单元是该无人机定位技术的核心单元，包括IMU因子、UWB因子以及视觉因子。在划窗内，IMU因子是通过预积分分别得到相邻两帧数据之间的位姿残差、UWB因子得到每一帧时刻下无人机与锚点之间的距离残差、视觉因子通过重投影得到每一帧下的位姿残差。在得到各项因子的残差后，使用用于建模和解决大型，复杂的优化问题的求解器(Ceres)，利用自动求导的功能对划窗内每一帧的参数进行优化得到位姿信息和加速度和角速度的零偏。
本发明提出的一种基于UWB在线锚点标定的视觉惯性紧耦合里程计设计，主要由硬件部分和算法部分组成。
下面结合附图对本发明的GNSS拒止环境下一种基于UWB在线锚点标定的视觉惯性紧耦合里程计设计做进一步描述。
GNSS拒止环境下基于UWB在线锚点标定的视觉惯性紧耦合里程计设计四旋翼无人机***硬件实物如附图1所示。首先对***的硬件构成进行详细的介绍：该***的硬件组成包括四旋翼无人机、UWB超宽带测距传感器、双目相机、IMU惯性测量单元和嵌入式机载处理器。考虑到***的可靠性和定位精度，需要具有高精度的传感器设备，其中UWB超宽带测距传感器采用美国Time domain公司生产的P440模块。P440模块是一种波段在3.1G到4.8GHz之间的超宽带无线收发器，它是一种相干无线收发器，也就是说每一个发射脉冲的能量可以相加来增加接收信号的信号噪声比(SNR)。P440主要采用双向飞行时间方式在2个或者2个以上的模块之间进行测距，刷新率最高为125Hz，可以在两个或多个模块之间实现通信。它具备用优化双向飞行时间测距的组网功能，网络测距可以采用ALOHA(随机)或者TDMA(时分多址)协议，测量准确度可达10cm，发射功率极小，可同时执行四种功能(测距、数据传输、单基地雷达和多基地雷达)，测距包括单点测距和组网测距模式，本发明主要使用单点测距功能。
对于双目相机，本发明采用英特尔公司生产的RealSense D435i深度摄像头，该产品具有成像器、IR投影仪、左成像器、RGB模块、内置IMU等，但本方法仅使用左右成像器作为双目相机使用。RealSense D435i成本低、重量轻且功能强大，拍摄范围高达10米，可轻松集成到各种平台上，提供跨平台支持。
对于IMU惯性测量单元，本发明采用荷兰Xsense Technology公司生产的MTi-300惯性测量单元，这是一款基于微型惯性传感技术的工业级IMU，该传感器可以1000Hz频率发布原生的角速率与线加速度信息。采用USB3.0接口直接将传感器信息传给上位机进行处理。
对于嵌入式机载传感器的选择，考虑到处理大量数据且能够搭载于无人机上的需求。嵌入式开发平台需要具备性能优异、体积小、重量轻等优点。本发明采用英特尔第八代NUC作为机载处理器。NUC是英特尔公司推出的最轻量化的计算机，搭载第八代i7处理器，32G高内存，这使它成为适合嵌入式机载处理器的优良***平台。
四旋翼无人机平台，硬件上由碳纤维机架、Pixhawk开源飞行控制器、DJI E300电机电调和6S高压电池和上述传感器等组装而成。算法程序是在Linux操作***下基于ROS(Robot Operating System)机器人开源框架进行开发。在ROS框架下在ROS***环境下接受相机、IMU传感器和UWB超宽带测距模块采集的数据，通过本发明设计的一种基于UWB在线锚点标定的视觉惯性紧耦合里程计设计进行处理，以实现无人机实时自主定位与环境感知。其中，关键算法均采用C++语言编写。
(1)IMU预积分单元
IMU的测量频率往往在100Hz以上，相较于相机和UWB而言，其输出频率要高很多。IMU惯性测量单元在短的时间内可以得到较为准确的加速度和角速度信息，但是经过一段时间内会发生漂移现象。一方面为了充分利用IMU数据，在未接收到UWB数据和图像数据时，需要对IMU数据进行处理，进行粗略的估计得到IMU的预测位姿。另一方面由于需要估计的状态量除了无人机的位姿，还有加速度和角速度的零偏，这意味着每进行一次优化就要重新计算一遍IMU的预测位姿。为了避免重复计算，通过IMU预积分单元，在每次更新加速度和角速度的零偏后更新预积分的值即可。本方法不仅将IMU用于预测每一帧图像时刻下的位姿，还通过寻找UWB时间戳与IMU时间戳之间的关系将IMU预积分应用于UWB预处理部分，通过此方法可以预测得到接受UWB信息时无人机的位姿。
利用已知相机第k帧时刻的位置、速度和姿态，通过将相机第k帧和第k+1帧之间的所有IMU测量数据进行积分，得到第k+1帧对应的位置、速度及姿态状态。IMU积分连续形式为：
Figure BDA0003078362430000111
这里有：
Figure BDA0003078362430000112
其中，
Figure BDA0003078362430000113
和
Figure BDA0003078362430000114
表示世界坐标系w下第k帧时刻机体b的位置和速度以及旋转的四元数表示，
Figure BDA0003078362430000115
和
Figure BDA0003078362430000116
表示世界坐标系w下第k+1帧时刻机体b的位置和速度以及旋转的四元数表示，△tk为两个IMU测量数据的时间差，
Figure BDA0003078362430000117
和
Figure BDA0003078362430000118
分别表示IMU测得的t时刻的加速度和角速度，
Figure BDA0003078362430000119
为世界坐标系w下t时刻的四元数表示，
Figure BDA00030783624300001110
和
Figure BDA00030783624300001111
分别表示其对应的加速度及角速度测量偏差bias，gw表示世界坐标系下的重力加速度，ωx、ωy、ωz分别为ω的三个分量。
由于实际过程中IMU为离散采样，因此通过第i个时刻IMU状态求解第i+1时刻IMU状态的中值离散形式为：
Figure BDA00030783624300001112
其中，
Figure BDA00030783624300001113
和
Figure BDA00030783624300001114
分别为将IMU系下的测量加速度和角速度转换至世界系的机体加速度和角速度，其他符号解释同上。
通过IMU积分的连续形式可以得到，求解k+1时刻的状态需要依赖第k帧的姿态
Figure BDA0003078362430000121
在后端优化更新
Figure BDA0003078362430000122
后，需要重新对帧间的IMU重新进行积分，导致很多重复计算。针对该问题，将优化变量从第k帧到第k+1帧的IMU预积分项中分离开来：
Figure BDA0003078362430000123
其中：
Figure BDA0003078362430000124
其中
Figure BDA0003078362430000125
为第k帧世界坐标系到机体坐标系的旋转矩阵，
Figure BDA0003078362430000126
将公式(3)化为对应离散形式，通过第i个时刻IMU状态求解第i+1时刻IMU状态关系为：
Figure BDA0003078362430000127
此处
Figure BDA0003078362430000128
为相对于bk帧时刻下
Figure BDA0003078362430000129
的IMU增量信息，其状态只与bk帧有关。通过此式的迭代求解，可预积分出两帧之间的相对位姿。
(2)离群值检测单元
即便理想情况下UWB测量的距离能够达到厘米级，但实际上距离测量常常因为温度、场景等原因具有静差和误差。此外，在障碍物密集的场景下有可能引入离群值，离群值指明显有悖于事实的测量，如读数出现负数，突然发生很大跳变等情况，离群值的引入会导致错误的状态估计。这里采用添加阈值方法检测离群值，一旦认为本次距离测量为离群值，将直接被剔除。经过实验测试，离群值对状态估计有较为明显的影响，不仅会影响定位精度，甚至可能导致定位的完全错误，在引入离群值检测单元后可以改善此种情况。
当程序处于在线锚点标定阶段时，离群值检测单元的作用就是对接受到的UWB测距信息进行滤波。假设UWB的测量值为d，在当前场景下，我们收集n个样本点di(i＝1,…,n)，如果满足以下条件，则第i次距离测量值di被判定为离群值：
Figure BDA0003078362430000131
Figure BDA0003078362430000132
其中
Figure BDA0003078362430000133
是n次测距值的均值，γ是可调阈值,vmax为无人机最大飞行速度，f是UWB模块测距更新速率。如果本次距离被判定为离群值，则直接舍弃。若为正常情况，则将距离di作为输入送入在线锚点标定单元。
当在线锚点标定阶段结束后，由于每个UWB测距数据在接受后就要被处理，因此不能通过利用历史UWB数据求均值的方式来判断当前测量值是否为离群值，因此使用无人机当前位姿与标定的锚点位置之差的二范数作为判据，若该二范数满足以下条件，则第i次距离测量值di被判定为离群值：
Figure BDA0003078362430000134
其中Pk是当前无人机位姿，Pa是通过在线锚点标定单元得到的无人机初始时刻坐标系下的锚点位置坐标，γ是可调阈值,vmax为无人机最大飞行速度，f是UWB模块测距更新速率。如果本次距离被判定为离群值，则直接舍弃。若为正常情况，则将距离di作为输入送入紧耦合单元。
(3)在线锚点标定单元
传统的使用UWB的定位方法需要提前知道布置在无人机上的锚点与布置在地面上的锚点之间的相对位姿。通常的方法是将地面上的锚点布置与无人机初始航向垂直或者平行的方法并人工测量出距离，或者通过离线的方法先得到相对位姿后再切换模式，将标定后的锚点位置作为已知信息。显然，这写方法存在缺陷，不仅不够方便，也无法保证精度，限制了基于UWB增强的定位实用性。如附图3所示，本方法通过设计一个在线锚点标定单元，假设无人机起飞的一段时间内，IMU和视觉的定位足够精确，利用输出的里程计信息以及UWB的测距信息，标定出在无人机起飞初始时刻的坐标系下锚点的坐标。
自无人机运行时刻，在线锚点标定单元即开始工作，但是可能会出现无人机静止或移动速度较慢下，接受的UWB存在重复性较高的情况，一方面会出现数据冗余，浪费不必要的空间资源，另一方面重复的数据可能会导致这些数据占比过高，从而无法完成锚点标定的任务。
因此，本方法选择以下判据对UWB数据进行筛选：
Figure BDA0003078362430000135
Figure BDA0003078362430000136
其中di是第i次距离测量值，dn是第n(i>n)次距离测量值，γ是可调阈值,f是UWB模块测距更新速率，
Figure BDA0003078362430000141
为当前无人机在世界坐标系下的位姿，vthr为预先设定的速度阈值(本方法取0.2m/s)。如果满足该条件，则第i次距离测量值将被抛弃，将第i+1次距离测量值代入上式进行判断，以此类推直到第m次测量值不满足该条件后，将n赋值为m。
于是，针对第i次距离测量，可以得到关于测距信息的残差
Figure BDA0003078362430000142
Figure BDA0003078362430000143
其中di是第i次距离测量值，
Figure BDA0003078362430000144
是由IMU预积分和视觉里程计得到的与UWB测距信息同一时间戳下的无人机世界坐标系下的位姿，
Figure BDA0003078362430000145
是待估计的锚点在世界坐标系下的三维坐标。于是优化的代价函数Er：
Figure BDA0003078362430000146
其中
Figure BDA0003078362430000147
ρ()是一个pseudo-Huber损失函数。本方法使用Ceres的自动求导功能，编写代价函数后确定待优化变量
Figure BDA0003078362430000148
将参数di、
Figure BDA0003078362430000149
传入接口后即可进行优化，本方法判断优化终止的条件为前后两次优化得到的
Figure BDA00030783624300001410
之差的二范数小于0.1。
(4)滑动窗口单元
在基于优化的定位技术中，随着运行时间的加长，待优化状态与观测约束越来越多，矩阵Hl的规模会快速上升，使求解速度变得缓慢，无法满足实时性要求。为了解决这个问题，本文使用基于滑动窗口策略的后端优化。在构建联合优化问题时，只考虑滑窗内的帧与约束。在里程计运行时，不断会有新的帧加入到窗口中，同时不断有旧的帧从滑窗中剔除出去，这样就可以保持待优化的帧的规模，避免了求解速度下降。当旧的帧被移除出滑窗时，被移除滑窗的帧仍然与在滑窗内的帧存在约束，直接移除的话此部分的约束就会丢失，导致里程计精度下降，所以滑动窗口的使用通常配合边缘化策略。边缘化策略是将被移出滑窗的帧作为边缘化帧，与边缘化帧有关的约束称为边缘化约束。边缘化帧在之后不会进行更新，但是边缘化约束会变为正常待优化帧的先验信息，这相当于从求解一个联合概率分布变为求解有先验信息的条件概率分布。这样做即维持了优化问题的规模，也没有损失约束。
其中，一个划窗选择十个关键帧，将紧耦合优化单元模块的输出作为本模块的输入，每一帧分别包含同一个时刻tk下相应的IMU因子、UWB因子以及视觉因子，这些因子之间相互独立，构成了紧耦合优化单元模块中的约束项。本模块确保划窗内最多只存在十个关键帧，若超过十个，首先会对即将被边缘化的关键帧内的各个因子进行修改，这是为了不丢失上述因子之间的约束，接下来直接丢弃被边缘化的关键帧内的所有数据。经过本模块的处理后，将该模块修改后因子与最近接收的因子进行耦合，再次输入到紧耦合优化单元模块，对待优化变量进行优化，得到无人机的位姿以及地图信息。
(5)紧耦合优化单元
紧耦合优化单元是该无人机定位技术的核心单元，包括IMU因子、UWB因子以及视觉因子。在划窗内，IMU因子是通过预积分分别得到相邻两帧数据之间的位姿残差、UWB因子得到每一帧时刻下无人机与锚点之间的距离残差、视觉因子通过重投影得到每一帧下的位姿残差。在得到各项因子的残差后，使用用于建模和解决大型，复杂的优化问题的求解器(Ceres)，利用自动求导的功能对划窗内每一帧的参数进行优化得到位姿信息和加速度和角速度的零偏。假设tk为划窗内第k+1帧图像对应的时间戳，tk+1为划窗内第k+2帧图像对应的时间戳，tki为划窗内第k+1帧图像与第k+2帧图像之内IMU的第i个数据的时间戳，tku为划窗内第k+1帧图像与第k+2帧图像之内UWB的第u个数据的时间戳。
UWB因子
针对划窗内每一次的UWB测距信息di，可以得到关于测距信息的残差
Figure BDA0003078362430000151
和代价函数Γα：
Figure BDA0003078362430000152
△tuk＝tku-tk
Figure BDA0003078362430000153
其中
Figure BDA0003078362430000154
是tk时刻下无人机在世界坐标系下的位置坐标，
Figure BDA0003078362430000155
时刻下无人机在世界坐标系下的速度，△tuk是UWB数据时间戳与图像时间戳之差，gW是世界坐标系下重力加速度的三维表示，
Figure BDA0003078362430000156
是tk时刻下无人机在世界坐标系下的旋转矩阵，
Figure BDA0003078362430000157
是根据IMU信息得到的预积分位置分量，
Figure BDA0003078362430000158
是经过在线锚点标定单元后得到的世界坐标系下锚点的三维坐标，ρ()是一个pseudo-Huber损失函数。
1)IMU因子
针对前后两次IMU的测量信息，可以得到关于IMU测量信息的代价函数Γβ：
Figure BDA0003078362430000159
其中
Figure BDA00030783624300001510
分别为由里程计得到的关于第k帧到第k+1帧的位置、速度、姿态的增量，
Figure BDA00030783624300001511
分别为第k+1帧和第k帧时刻世界坐标系下的陀螺仪偏差，
Figure BDA00030783624300001512
分别为第k+1帧和第k帧时刻世界坐标系下的加速度计偏差。
2)视觉因子
视觉定义为被跟踪特征的重投影误差，本发明将当前帧中所有特征的重投影与它们的第一次观察进行比较，可以得到关于重投影误差的代价函数Γγ：
Figure BDA0003078362430000161
其中
Figure BDA0003078362430000162
是相机第j帧图像下第lk个特征的坐标，π是将齐次坐标转换成图像坐标的投影函数，
Figure BDA0003078362430000163
分别表示从相机第i帧到第j的帧旋转矩阵和平移矩阵，
Figure BDA0003078362430000164
是在第一次观察帧i中第k个特征的3D位置。视觉因子在所有帧和在估计状态下的所有被跟踪的特征中进行不断的迭代。
下面给出实验结果：
附图4为室内场景下四次在线锚点标定结果示意图，附图5为走廊场景下四次在线锚点标定结果示意图。从表中可以看出室内在线锚点标定最大相对误差为4.48％，走廊场景下最大相对误差为7.62％，该标定结果足以满足后续里程计使用。
如附图6和附图7所示，该实验的场景为室内，起点与终点为同一地点。图中轨迹①表示由Vins得到的轨迹，轨迹②表示本方法得到的轨迹，轨迹③表示由Motion Capture(运动捕捉***)得到的轨迹(可认为是真值)。可以看出曲线②夹在曲线①和曲线③之间，这表明相较于Vins，本方法对轨迹的估计更加趋近于真值。附图8为室内场景下四次实验误差对比分析表，从表中可以看出室内场景下本方法可以比Vins方法提升约15％的精度。
同理，如附图9和附图10所示，该实验场景为走廊，起点与终点为同一地点。由于本实验场景为走廊，在拐弯处会出现偏航变化较大、特征点突然减少的现象，因此Vins得到的轨迹会出现十分明显的毛刺并且可以发现终点与起点存在较大的偏差，虽然本方法得到的轨迹也有毛刺，但终点与起点偏差十分小。附图11为走廊场景下四次实验误差对比分析表，从表中可以看出走廊场景下本方法可以比Vins方法提升约20％的精度。
附图12与附图13展示了Euroc数据集下Vins和本方法的绝对误差曲线。由图可知，本方法不论是绝对误差的最大值和平均值相较于Vins都有改进，附图14为Euroc数据集下Vins、本方法、真值的轨迹图，由图。可见本方法得到的轨迹在真值和Vins之间，说明了本方法相较于Vins有一定的改进
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种基于UWB在线锚点的视觉惯性紧耦合里程计，其特征是，结构如下：超宽带技术制成的电波测距模块UWB，部署在两个位置，其一是作为锚点部署在环境四周，锚点自身的位置通过在线锚点标定单元计算出，在线锚点标定步骤：
使用UWB的定位时，利用输出的里程计信息以及UWB的测距信息，标定出在无人机起飞初始时刻的坐标系下锚点的坐标，具体步骤如下：
Figure FDA0004050946160000011
Figure FDA0004050946160000012
其中di是第i次距离测量值，dn是第n次距离测量值，其中：i＞n，γ是可调阈值,f是UWB模块测距更新速率，
Figure FDA0004050946160000013
为当前无人机在世界坐标系下的位姿，vthr为预先设定的速度阈值，如果满足该条件，则第i次距离测量值将被抛弃，将第i+1次距离测量值代入上式进行判断，以此类推直到第m次测量值不满足该条件后，将n赋值为m；
于是，针对第i次距离测量，得到关于测距信息的残差
Figure FDA0004050946160000014
Figure FDA0004050946160000015
其中di是第i次距离测量值，
Figure FDA0004050946160000016
是由IMU预积分和视觉里程计得到的与UWB测距信息同一时间戳下的无人机世界坐标系下的位姿，
Figure FDA0004050946160000017
是待估计的锚点在世界坐标系下的三维坐标，于是优化的代价函数Er：
Figure FDA0004050946160000018
其中
Figure FDA0004050946160000019
ρ()是一个pseudo-Huber损失函数，使用Ceres的自动求导功能，编写代价函数后确定待优化变量
Figure FDA00040509461600000110
将参数di、
Figure FDA00040509461600000111
传入接口后即可进行优化，判断优化终止的条件为前后两次优化得到的
Figure FDA00040509461600000112
之差的二范数小于0.1；
其二是作为标签放置在无人机上，标签按照预设的顺序主动向锚点发送测距请求，根据TW-ToF测量原理获得两个模块之间的距离，通过离群值检测单元以滤除部分噪声后，作为输入送入紧耦合优化单元；IMU惯性测量单元用于高频测量无人机的加速度和角速度信息，IMU测量值经过低通滤波器模块处理后，同样作为输入送入紧耦合优化单元；双目摄像头的测量值经过视觉里程计的处理后，也作为输入送入紧耦合优化单元；在线锚点标定单元、离群值检测单元和紧耦合优化单元均为供嵌入式机载处理器调用的模块，嵌入式机载处理器利用所述的紧耦合优化单元，处理UWB模块之间的距离测量值、IMU测量值和双目摄像头测量值，用紧耦合优化的方式解析出无人机的位置与姿态信息。
2.一种基于UWB在线锚点的视觉惯性紧耦合里程计量方法，其特征是，借助如下装置实现：超宽带技术制成的电波测距模块UWB，部署在两个位置，其一是作为锚点部署在环境四周，锚点自身的位置通过在线锚点标定计算出，在线锚点标定步骤：
使用UWB的定位时，利用输出的里程计信息以及UWB的测距信息，标定出在无人机起飞初始时刻的坐标系下锚点的坐标，具体步骤如下：
Figure FDA0004050946160000021
Figure FDA0004050946160000022
其中di是第i次距离测量值，dn是第n次距离测量值，其中：i＞n，γ是可调阈值,f是UWB模块测距更新速率，
Figure FDA0004050946160000023
为当前无人机在世界坐标系下的位姿，vthr为预先设定的速度阈值，如果满足该条件，则第i次距离测量值将被抛弃，将第i+1次距离测量值代入上式进行判断，以此类推直到第m次测量值不满足该条件后，将n赋值为m；
于是，针对第i次距离测量，得到关于测距信息的残差
Figure FDA0004050946160000024
Figure FDA0004050946160000025
其中di是第i次距离测量值，
Figure FDA0004050946160000026
是由IMU预积分和视觉里程计得到的与UWB测距信息同一时间戳下的无人机世界坐标系下的位姿，
Figure FDA0004050946160000027
是待估计的锚点在世界坐标系下的三维坐标，于是优化的代价函数Er：
Figure FDA0004050946160000028
其中
Figure FDA0004050946160000029
ρ()是一个pseudo-Huber损失函数，使用Ceres的自动求导功能，编写代价函数后确定待优化变量
Figure FDA00040509461600000210
将参数di、
Figure FDA00040509461600000211
传入接口后即可进行优化，判断优化终止的条件为前后两次优化得到的
Figure FDA00040509461600000212
之差的二范数小于0.1；
其二是作为标签放置在无人机上；还包括机载双目摄像头、IMU单元；标签按照预设的顺序主动向锚点发送测距请求，根据TW-ToF测量原理获得两个模块之间的距离，通过离群值检测滤除部分噪声后，作为输入进行紧耦合优化；IMU惯性测量单元用于高频测量无人机的加速度和角速度信息，IMU测量值经过低通滤波器处理后，同样作为输入进行紧耦合优化；双目摄像头的测量值经过视觉里程计的处理后，也作为输入进行紧耦合优化；嵌入式机载处理器处理UWB模块之间的距离测量值、IMU测量值和双目摄像头测量值，用紧耦合优化的方式解析出无人机的位置与姿态信息。
3.如权利要求2所述的基于UWB在线锚点的视觉惯性紧耦合里程计量方法，其特征是，IMU预积分：为了充分利用IMU数据，在未接收到UWB数据和图像数据时，需要对IMU数据进行处理，进行粗略的估计得到IMU的预测位姿；另一方面，为了避免重复计算，在每次更新加速度和角速度的零偏后更新预积分的值。
4.如权利要求3所述的基于UWB在线锚点的视觉惯性紧耦合里程计量方法，其特征是，IMU预积分具体步骤如下：利用已知相机第k帧时刻的位置、速度和姿态，通过将相机第k帧和第k+1帧之间的所有IMU测量数据进行积分，得到第k+1帧对应的位置、速度及姿态状态，IMU积分连续形式为：
Figure FDA0004050946160000031
这里有：
Figure FDA0004050946160000032
其中，
Figure FDA0004050946160000033
和
Figure FDA0004050946160000034
表示世界坐标系w下第k帧时刻机体b的位置和速度以及旋转的四元数表示，
Figure FDA0004050946160000035
和
Figure FDA0004050946160000036
表示世界坐标系w下第k+1帧时刻机体b的位置和速度以及旋转的四元数表示，Δtk为两个IMU测量数据的时间差，
Figure FDA0004050946160000037
和
Figure FDA0004050946160000038
分别表示IMU测得的t时刻的加速度和角速度，
Figure FDA0004050946160000039
为世界坐标系w下t时刻的四元数表示，
Figure FDA00040509461600000310
和
Figure FDA00040509461600000311
分别表示其对应的加速度及角速度测量偏差bias，gw表示世界坐标系下的重力加速度，ωx、ωy、ωz分别为ω的三个分量；
由于实际过程中IMU为离散采样，因此通过第i个时刻IMU状态求解第i+1时刻IMU状态的中值离散形式为：
Figure FDA00040509461600000312
其中，
Figure FDA00040509461600000313
和
Figure FDA00040509461600000314
分别为将IMU系下的测量加速度和角速度转换至世界系的机体加速度和角速度；
为避免重复计算，将优化变量从第k帧到第k+1帧的IMU预积分项中分离开来：
Figure FDA00040509461600000315
其中：
Figure FDA0004050946160000041
其中
Figure FDA0004050946160000042
为第k帧世界坐标系到机体坐标系的旋转矩阵，
Figure FDA0004050946160000043
将公式(3)化为对应离散形式，通过第i个时刻IMU状态求解第i+1时刻IMU状态关系为：
Figure FDA0004050946160000044
此处
Figure FDA0004050946160000045
为相对于bk帧时刻下
Figure FDA0004050946160000046
的IMU增量信息，其状态只与bk帧有关，通过对式(5)的迭代求解，预积分出两帧之间的相对位姿。
5.如权利要求2所述的基于UWB在线锚点的视觉惯性紧耦合里程计量方法，其特征是，离群值检测滤除部分噪声的具体步骤如下：当程序处于在线锚点标定阶段时，离群值检测单元的作用就是对接受到的UWB测距信息进行滤波，假设UWB的测量值为d，在当前场景下，收集n个样本点di(i＝1,…,n)，如果满足以下条件，则第i次距离测量值di被判定为离群值：
Figure FDA0004050946160000047
Figure FDA0004050946160000048
其中
Figure FDA0004050946160000049
是n次测距值的均值，γ是可调阈值,vmax为无人机最大飞行速度，f是UWB模块测距更新速率，如果本次距离被判定为离群值，则直接舍弃，若为正常情况，则将距离di作为输入送入在线锚点标定单元；
当在线锚点标定阶段结束后，使用无人机当前位姿与标定的锚点位置之差的二范数作为判据，若该二范数满足以下条件，则第i次距离测量值di被判定为离群值：
Figure FDA00040509461600000410
其中Pk是当前无人机位姿，Pa是通过在线锚点标定单元得到的无人机初始时刻坐标系下的锚点位置坐标，γ是可调阈值,vmax为无人机最大飞行速度，f是UWB模块测距更新速率，如果本次距离被判定为离群值，则直接舍弃；若为正常情况，则将距离di作为输入送入紧耦合单元。
6.如权利要求2所述的基于UWB在线锚点的视觉惯性紧耦合里程计量方法，其特征是，紧耦合优化包括IMU因子、UWB因子以及视觉因子，在划窗内，IMU因子是通过预积分分别得到相邻两帧数据之间的位姿残差、UWB因子得到每一帧时刻下无人机与锚点之间的距离残差、视觉因子通过重投影得到每一帧下的位姿残差，在得到各项因子的残差后，使用用于建模和解决优化问题的求解器，利用自动求导的功能对划窗内每一帧的参数进行优化得到位姿信息和加速度和角速度的零偏，假设tk为划窗内第k+1帧图像对应的时间戳，tk+1为划窗内第k+2帧图像对应的时间戳，tki为划窗内第k+1帧图像与第k+2帧图像之内IMU的第i个数据的时间戳，tku为划窗内第k+1帧图像与第k+2帧图像之内UWB的第u个数据的时间戳；
UWB因子
针对划窗内每一次的UWB测距信息di，得到关于测距信息的残差
Figure FDA0004050946160000051
和代价函数Γα：
Figure FDA0004050946160000052
Δtuk＝tku-tk
Figure FDA0004050946160000053
其中
Figure FDA0004050946160000054
是tk时刻下无人机在世界坐标系下的位置坐标，
Figure FDA0004050946160000055
时刻下无人机在世界坐标系下的速度，Δtuk是UWB数据时间戳与图像时间戳之差，gW是世界坐标系下重力加速度的三维表示，
Figure FDA0004050946160000056
是tk时刻下无人机在世界坐标系下的旋转矩阵，
Figure FDA0004050946160000057
是根据IMU信息得到的预积分位置分量，
Figure FDA0004050946160000058
是经过在线锚点标定单元后得到的世界坐标系下锚点的三维坐标，ρ()是一个pseudo-Huber损失函数；
IMU因子
针对前后两次IMU的测量信息，得到关于IMU测量信息的代价函数Γβ：
Figure FDA0004050946160000059
其中
Figure FDA00040509461600000510
分别为由里程计得到的关于第k帧到第k+1帧的位置、速度、姿态的增量，
Figure FDA00040509461600000511
分别为第k+1帧和第k帧时刻世界坐标系下的陀螺仪偏差，
Figure FDA00040509461600000512
Figure FDA00040509461600000513
分别为第k+1帧和第k帧时刻世界坐标系下的加速度计偏差；
视觉因子
视觉定义为被跟踪特征的重投影误差，将当前帧中所有特征的重投影与它们的第一次观察进行比较，得到关于重投影误差的代价函数Γγ：
Figure FDA0004050946160000061
其中
Figure FDA0004050946160000062
是相机第j帧图像下第lk个特征的坐标，π是将齐次坐标转换成图像坐标的投影函数，
Figure FDA0004050946160000063
分别表示从相机第i帧到第j的帧旋转矩阵和平移矩阵，
Figure FDA0004050946160000064
是在第一次观察帧i中第k个特征的3D位置，视觉因子在所有帧和在估计状态下的所有被跟踪的特征中进行不断的迭代。
7.如权利要求2所述的基于UWB在线锚点的视觉惯性紧耦合里程计量方法，其特征是，还包括配合边缘化策略使用滑动窗口步骤，边缘化策略是将被移出滑窗的帧作为边缘化帧，与边缘化帧有关的约束称为边缘化约束，边缘化帧在之后不会进行更新，但是边缘化约束会变为正常待优化帧的先验信息，这相当于从求解一个联合概率分布变为求解有先验信息的条件概率分布；其中，一个划窗选择十个关键帧，将紧耦合优化的结果作为输入，每一帧分别包含同一个时刻tk下相应的IMU因子、UWB因子以及视觉因子，这些因子之间相互独立，构成了紧耦合优化的约束项，确保划窗内最多只存在十个关键帧，若超过十个，首先会对即将被边缘化的关键帧内的各个因子进行修改，接下来直接丢弃被边缘化的关键帧内的所有数据，将修改后因子与最近接收的因子进行耦合，再次进行紧耦合优化，对待优化变量进行优化，得到无人机的位姿以及地图信息。
CN202110558941.7A 2021-05-21 2021-05-21 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法 Active CN113124856B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110558941.7A CN113124856B (zh) 2021-05-21 2021-05-21 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110558941.7A CN113124856B (zh) 2021-05-21 2021-05-21 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法

Publications (2)

Publication Number Publication Date
CN113124856A CN113124856A (zh) 2021-07-16
CN113124856B true CN113124856B (zh) 2023-03-14

Family

ID=76782363

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110558941.7A Active CN113124856B (zh) 2021-05-21 2021-05-21 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法

Country Status (1)

Country Link
CN (1) CN113124856B (zh)

Families Citing this family (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113589266B (zh) * 2021-07-21 2023-11-24 中国煤炭科工集团太原研究院有限公司 一种基于uwb技术的矿用多模式无线测距***
CN114222366B (zh) * 2021-08-06 2023-08-01 深圳技术大学 一种基于单基站的室内定位方法及装置
CN113865584B (zh) * 2021-08-24 2024-05-03 知微空间智能科技(苏州)有限公司 一种基于视觉惯性里程计的uwb三维寻物方法和装置
CN113625774B (zh) * 2021-09-10 2023-07-21 天津大学 局部地图匹配与端到端测距多无人机协同定位***和方法
CN114264297B (zh) * 2021-12-01 2022-10-18 清华大学 Uwb和视觉slam融合算法的定位建图方法及***
CN114323002B (zh) * 2021-12-27 2023-12-08 浙江大学 一种基于双目视觉、imu和uwb融合的agv定位导航方法
CN114485623B (zh) * 2022-02-16 2024-02-23 东南大学 一种聚焦距离的相机-imu-uwb融合精准定位方法
CN114459506B (zh) * 2022-02-28 2023-08-08 清华大学深圳国际研究生院 在线标定全球导航卫星***接收机与视觉惯性里程计之间外参的方法及***
CN115143959A (zh) * 2022-06-08 2022-10-04 北京眸星科技有限公司 测定室内无线信号发射锚点的位置坐标的方法及装置
CN115540854A (zh) * 2022-12-01 2022-12-30 成都信息工程大学 一种基于uwb辅助的主动定位方法、设备和介质
CN116242372A (zh) * 2023-01-03 2023-06-09 东南大学 一种gnss拒止环境下的uwb-激光雷达-惯导融合定位方法

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111091587A (zh) * 2019-11-25 2020-05-01 武汉大学 一种基于视觉标志物的低成本动作捕捉方法
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN111890373A (zh) * 2020-09-29 2020-11-06 常州唯实智能物联创新中心有限公司 车载机械臂的感知定位方法
CN111983660A (zh) * 2020-07-06 2020-11-24 天津大学 Gnss拒止环境下四旋翼无人机定位***和方法
CN112378396A (zh) * 2020-10-29 2021-02-19 江苏集萃未来城市应用技术研究所有限公司 基于抗差lm视觉惯性里程计与uwb混合高精度室内定位方法
WO2021048500A1 (fr) * 2019-09-12 2021-03-18 Dronisos Procédé et système de positionnement automatique de drones en essaim
CN112525197A (zh) * 2020-11-23 2021-03-19 中国科学院空天信息创新研究院 基于图优化算法超宽带惯性导航融合位姿估计方法
CN112762929A (zh) * 2020-12-24 2021-05-07 华中科技大学 一种智能导航方法、装置和设备

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10180734B2 (en) * 2015-03-05 2019-01-15 Magic Leap, Inc. Systems and methods for augmented reality
CN112484725B (zh) * 2020-11-23 2023-03-21 吉林大学 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
WO2021048500A1 (fr) * 2019-09-12 2021-03-18 Dronisos Procédé et système de positionnement automatique de drones en essaim
CN111091587A (zh) * 2019-11-25 2020-05-01 武汉大学 一种基于视觉标志物的低成本动作捕捉方法
CN111983660A (zh) * 2020-07-06 2020-11-24 天津大学 Gnss拒止环境下四旋翼无人机定位***和方法
CN111890373A (zh) * 2020-09-29 2020-11-06 常州唯实智能物联创新中心有限公司 车载机械臂的感知定位方法
CN112378396A (zh) * 2020-10-29 2021-02-19 江苏集萃未来城市应用技术研究所有限公司 基于抗差lm视觉惯性里程计与uwb混合高精度室内定位方法
CN112525197A (zh) * 2020-11-23 2021-03-19 中国科学院空天信息创新研究院 基于图优化算法超宽带惯性导航融合位姿估计方法
CN112762929A (zh) * 2020-12-24 2021-05-07 华中科技大学 一种智能导航方法、装置和设备

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
A New Quaternion Kalman Filter Based Foot-Mounted IMU and UWB Tightly-Coupled Method for Indoor Pedestrian Navigation;Kai Wen，等;《 IEEE Transactions on Vehicular Technology》;20200218;第69卷(第04期);全文 *
基于超宽带、里程计、ＲＧＢ－Ｄ融合的室内定位方法;王文博，等;《计算机科学》;20200131;第47卷(第S2期);全文 *

Also Published As

Publication number Publication date
CN113124856A (zh) 2021-07-16

Similar Documents

Publication Publication Date Title
CN113124856B (zh) 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法
CN112347840B (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
US11218689B2 (en) Methods and systems for selective sensor fusion
CN109911188B (zh) 非卫星导航定位环境的桥梁检测无人机***
CN108225302B (zh) 一种石化工厂巡检机器人定位***和方法
CN109887057B (zh) 生成高精度地图的方法和装置
Li et al. LIDAR/MEMS IMU integrated navigation (SLAM) method for a small UAV in indoor environments
CN113945206A (zh) 一种基于多传感器融合的定位方法及装置
CN112130579A (zh) 一种隧道无人机巡检方法及***
CN112505737B (zh) 一种gnss/ins组合导航方法
WO2016187758A1 (en) Sensor fusion using inertial and image sensors
CN106289235A (zh) 基于建筑结构图的自主计算精度可控室内定位导航方法
CN111983660A (zh) Gnss拒止环境下四旋翼无人机定位***和方法
Steiner et al. A vision-aided inertial navigation system for agile high-speed flight in unmapped environments: Distribution statement a: Approved for public release, distribution unlimited
Yun et al. IMU/Vision/Lidar integrated navigation system in GNSS denied environments
CN115574816B (zh) 仿生视觉多源信息智能感知无人平台
CN111694001A (zh) 一种用于无人机的实时测距定位***
CN115902930A (zh) 一种面向船舶检测的无人机室内建图与定位方法
CN117685953A (zh) 面向多无人机协同定位的uwb与视觉融合定位方法及***
Wang et al. Micro aerial vehicle navigation with visual-inertial integration aided by structured light
Liu et al. The altitude hold algorithm of UAV based on millimeter wave radar sensors
CN115542363A (zh) 一种适用于垂直下视航空吊舱的姿态测量方法
CN104792336B (zh) 一种飞行状态测量方法及装置
Sanna et al. A novel ego-motion compensation strategy for automatic target tracking in FLIR video sequences taken from UAVs
Deneault et al. Tracking ground targets with measurements obtained from a single monocular camera mounted on an unmanned aerial vehicle

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
CB03 Change of inventor or designer information

Inventor after: Tian Bailian

Inventor after: Li Haisong

Inventor after: Chen Hongming

Inventor after: Zong Qun

Inventor before: Tian Bailian

Inventor before: Li Haisong

Inventor before: Chen Hongming

Inventor before: Zong Qun

Inventor before: Wang Cong

Inventor before: He Lei

CB03 Change of inventor or designer information
GR01 Patent grant
GR01 Patent grant