CN116295342A - 一种用于飞行器勘测的多传感状态估计器 - Google Patents
一种用于飞行器勘测的多传感状态估计器 Download PDFInfo
- Publication number
- CN116295342A CN116295342A CN202310243900.8A CN202310243900A CN116295342A CN 116295342 A CN116295342 A CN 116295342A CN 202310243900 A CN202310243900 A CN 202310243900A CN 116295342 A CN116295342 A CN 116295342A
- Authority
- CN
- China
- Prior art keywords
- unmanned aerial
- aerial vehicle
- state estimator
- imu
- optimization
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 claims abstract description 107
- 238000005457 optimization Methods 0.000 claims abstract description 49
- 230000000007 visual effect Effects 0.000 claims abstract description 43
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 30
- 238000010168 coupling process Methods 0.000 claims abstract description 24
- 230000010354 integration Effects 0.000 claims abstract description 23
- 230000008878 coupling Effects 0.000 claims abstract description 22
- 238000005859 coupling reaction Methods 0.000 claims abstract description 22
- 238000005259 measurement Methods 0.000 claims abstract description 18
- 238000013178 mathematical model Methods 0.000 claims abstract description 4
- 230000008569 process Effects 0.000 claims description 45
- 238000001514 detection method Methods 0.000 claims description 29
- 230000003287 optical effect Effects 0.000 claims description 11
- 239000011159 matrix material Substances 0.000 claims description 10
- 238000013461 design Methods 0.000 claims description 8
- 238000012545 processing Methods 0.000 claims description 7
- 238000004458 analytical method Methods 0.000 claims description 6
- 230000005484 gravity Effects 0.000 claims description 5
- 238000010276 construction Methods 0.000 claims description 4
- 238000011217 control strategy Methods 0.000 claims description 3
- 102100034282 Ankyrin repeat domain-containing protein 23 Human genes 0.000 claims 3
- 101000780120 Homo sapiens Ankyrin repeat domain-containing protein 23 Proteins 0.000 claims 3
- 238000000638 solvent extraction Methods 0.000 claims 1
- 238000005070 sampling Methods 0.000 abstract description 9
- 230000004927 fusion Effects 0.000 abstract description 5
- 230000001133 acceleration Effects 0.000 abstract description 4
- 238000010586 diagram Methods 0.000 description 15
- 230000006870 function Effects 0.000 description 7
- 238000004364 calculation method Methods 0.000 description 5
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 3
- 230000009471 action Effects 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 238000007500 overflow downdraw method Methods 0.000 description 2
- 238000010845 search algorithm Methods 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000015556 catabolic process Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000006731 degradation reaction Methods 0.000 description 1
- 230000000593 degrading effect Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005192 partition Methods 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 238000011084 recovery Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 238000012216 screening Methods 0.000 description 1
- 230000011218 segmentation Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000001629 suppression Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
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/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
-
- 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/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- 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)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种用于飞行器勘测的多传感状态估计器，包括对四旋翼无人机建立详细的数学模型，对基于优化的多传感器状态估计器进行模型描述，解决无人机对未知环境的探索问题，实现图像与IMU数据的紧耦合估计、视觉图像跟踪，完成惯性传感预积分，在完成上述步骤后，进入初始化，包含三个环节。本发明结合紧耦合和松耦合的优势，利用低成本的惯性测量单元(InertialMeasurementUnit,IMU)在图像采样间隔时间内的短时平稳性，从而实现更高的视觉***的采样频率，并使无人机在加速和急转的情况下都能进行稳定的位姿跟踪。最终结合无人机集群控制算法和策略，深入研究基于该融合框架理论的集群无人机同时定位与地图构建方法，从而设计出一款精度高、实时性好的多传感状态估计器。
Description
技术领域
本发明涉及飞控传感技术领域，具体为一种用于飞行器勘测的多传感状态估计器。
背景技术
无人机的定位方法从适用层面可分为室内定位方法和室外定位方法。GPS在室外空旷长距离场景下，是一种有效的定位方式，但是只能进行大范围粗略地估计。
状态估计是一种多传感器数据融合方法，利用两种以上的不准确信息融合来提高测量***的测量精度。
传统的室内定位算法主要分为里程计法和惯性导航法；惯性导航法利用加速度计和陀螺仪实现定位，由于积分累积误差的存在，惯性导航的定位精度一般很差；里程计法又依据所使用的传感器不同，又细分为光流法、视觉里程计法和视觉惯性里程计法；其中光流法因其原理简单、容易实现等特点，被广泛的应用在无人机室内定位过程中。
基于视觉里程计(VisualOdometry)的定位技术是目前较为新颖和研究较多的一种定位技术；VO通过刚性挂载在无人机上的摄像机采集图像数据，根据图像特征和运动约束进行位姿估计，不会产生积累误差；但当视觉传感器数据采样率和无人机运动速度比较快时，会造成特征点数目减少而引起估计精度降低。
发明内容
本发明的目的在于提供一种用于飞行器勘测的多传感状态估计器，以解决上述背景技术中提出的问题。
为实现上述目的，本发明提供如下技术方案：一种用于飞行器勘测的多传感状态估计器，包括对四旋翼无人机建立详细的数学模型，对基于优化的多传感器状态估计器进行模型描述，解决无人机对未知环境的探索问题，实现图像与IMU数据的紧耦合估计、视觉图像跟踪，完成惯性传感预积分，在完成上述步骤后，进入初始化，包含三个环节：求取相机与IMU之间的相对旋转、相机初始化、IMU与视觉信息的对齐；
同时进行回环检测，利用局部路径规划方法保障无人机能够在障碍物中飞行，当单架无人机具备了局部路径规划能力后，即可借助多无人机集群SLAM目标分配策略实现多无人机的协同区域搜索。
优选的，需要对四旋翼无人机建立运动模型，通过对机体受力分析和力矩分析，可以得到简化后集体的运动方程：
优选的，需要对基于优化的多传感器状态估计器进行模型描述，该状态估计器实现了无人机在复杂环境下的实时位姿估计，解决了传统无人机SLAM需搭载过多传感器的问题。最后针对四旋翼无人机集群对未知环境的探索和地图构建问题，研究适合多传感器状态估计器的在线无人机运动规划算法；提出基于动力学路径搜索的改进A*算法和B样条轨迹优化方法，解决了单一无人机的在线运动决策和规划方法；并设计无人机集群SLAM控制策略，完成无人机在集群状态下的目标点和路径点规划，从而完成方法框架的闭环。
优选的，解决无人机对未知环境的探索问题，并且重点解决三个问题。即无人机的状态估计问题、路径规划问题和运动控制问题。
优选的，需要实现图像与IMU数据的紧耦合估计，利用深度视觉图像获取相机运动和IMU数据获取载体的预积分轨迹，通过设计优化过程使图像和IMU计算的轨迹数据实现紧耦合估计；通过最小二乘优化方法，最小化视觉测量值与IMU估计值之间的差值，即“残差”，实现图像和IMU数据的紧耦合。
优选的，需要完成视觉图像跟踪设计，使用多组特征点的像素坐标及特征点深度，还原图像帧状态，状态包含：相机的空间位置(Position,P)、速度(velocity,V)和四元数表示的旋转(Quaternion,Q)；包含三个主要的处理步骤：稀疏光流法(KLT)跟踪，SFM三维运动重建和滑窗关键帧的选取。
优选的，需要完成惯性传感预积分，使用视觉观测值和惯性观测值进行耦合：需求解出视觉观测值并计算其残差，残差的雅各比矩阵是优化中下降的方向，协方差矩阵是观测值对应的权值；特别的，针对对连续时间积分存在的后期优化难度高的问题，提出预积分环节，通过对惯性传感器在世界坐标系下的连续时刻进行预积分推导来解决该问题。
优选的，完成上述权利要求部分后，需要进入初始化环节，特别的，视觉惯性紧耦合***需要通过初始化过程对***参数进行恢复和校准，恢复的参数包含相机尺度、重力、速度以及IMU的测量误差(Bias)；由于视觉三维运动重建(SFM)在初始化的过程中有着较好的表现，所以在初始化的过程中主要以SFM为主；通过将IMU的预积分结果与视觉三维运动重建结果对齐，可对IMU的测量误差进行进一步初始化，主要包括三个环节：求取相机与IMU之间的相对旋转、相机初始化、IMU与视觉信息的对齐。
优选的，需要进行回环检测在回环，检测过程中，当新的关键帧生成后，使用FAST特征点检测算法寻找处新的特征点，这些特征点与其在KTL光流跟踪中寻找的特征点不同；随后通过BRIEF方法提取描述子并与历史描述子匹配，使用DBow词袋字典库对描述子信息进行存储和检索；若存在本关键帧中的描述子对应的特征点与词袋中存储的历史特征点相同，则匹配对应的特征点***寻找到回点；若寻找到回环点，则需找出回环最早出现的关键帧，将此关键帧的位姿设为固定；在回环检测过程中，由于回环检测的速度总是慢于关键帧的生成速度，为了保持回环检测不落后于关键帧生成，通常采用跳帧法将部分关键帧剔除，从而保证检测效率；在完成回环检测后，多传感器状态估计器需通过快速重定位方式，将回环帧信息返回至后端联合优化过程以更新优化数据；需要确保无人机不与障碍物相撞，利用局部路径规划方法，使无人机利用已经生成的地图和传感器实时更新的障碍物信息，在线生成一条或多条无人机局部路径，保障无人机能够在障碍物中飞行，采用一种B样条优化方法对动力学搜索产生的轨迹进行优化，用以提高路径的平滑度，改进路径与障碍物间隙过小的问题。
优选的，通过借助多无人机集群SLAM目标分配策略实现多无人机的协同区域搜索，使用一种DARP算法对区域进行分区，其中DARP算法全名称是DivideAreasAlgorithmforOptimalMulti-RobotCoveragePathPlanning；DARP算法根据移动无人机的初始位置划分区域，确保每个区域的面积近似相等且联通，当完成无人机探索区域划分后，即可根据区域执行设定的搜索策略。
与现有技术相比，本发明的有益效果是：
通过一种基于优化的传感器融合框架，结合紧耦合和松耦合的优势，利用低成本的惯性测量单元(InertialMeasurementUnit,IMU)在图像采样间隔时间内的短时平稳性，从而实现更高的视觉***的采样频率，并使无人机在加速和急转的情况下都能进行稳定的位姿跟踪。最终结合无人机集群控制算法和策略，深入研究基于该融合框架理论的集群无人机同时定位与地图构建方法，从而设计出一款精度高、实时性好的多传感状态估计器。
附图说明
图1为未知环境探索的结构建模框图；
图2为位置控制框图；水平位置、高度控制、姿态控制PID框图；
图3为视觉图像跟踪***结构图；
图4为滑动窗口示意图；
图5为快速定位过程示意图；
图6为随机搜索和平行搜索示意图；
图7为图像与IMU紧耦合过程的示意图；
图8为预积分示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
请参阅图1-8，本发明提供一种技术方案：一种用于飞行器勘测的多传感状态估计器，来提供多场景下的无人机定位和环境构图功能，并将其应用于实际无人机集群SLAM***中，结合无人机控制算法和策略，完成集群***在未知环境条件中的自主飞行和地图构建。通过一种基于优化的传感器融合框架，结合紧耦合和松耦合的优势，利用低成本的惯性测量单元(InertialMeasurementUnit,IMU)在图像采样间隔时间内的短时平稳性，从而实现更高的视觉***的采样频率，并使无人机在加速和急转的情况下都能进行稳定的位姿跟踪。最终结合无人机集群控制算法和策略，深入研究基于该融合框架理论的集群无人机同时定位与地图构建方法，从而实现精度高、实时性好的无人机SLAM***。将该***装配于四旋翼无人机QAV250机体上，组成一种满足GPS室内高精度定位、能够进行大规模部署的封闭区域勘测飞行器。以下为多状态传感器估计***的研究方案：
首先对四旋翼无人机建立详细的数学模型，通过对机体受力分析和力矩分析，可以得到简化后集体的运动方程：
然后对基于优化的多传感器状态估计器进行模型描述；该状态估计器实现了无人机在复杂环境下的实时位姿估计，解决了传统无人机SLAM需搭载过多传感器的问题；最后针对四旋翼无人机集群对未知环境的探索和地图构建问题，研究适合多传感器状态估计器的在线无人机运动规划算法。提出基于动力学路径搜索的改进A*算法和B样条轨迹优化方法，解决了单一无人机的在线运动决策和规划方法。并设计无人机集群SLAM控制策略，完成无人机在集群状态下的目标点和路径点规划，从而完成方法框架的闭环。
特别的，若要完成无人机对未知环境的探索，需要重点解决三个问题。即无人机的状态估计问题、路径规划问题和运动控制问题。在实际工程中，这三类问题被分配给了三个不同的处理器进行分别处理，以减小***的运算和实时度压力，其作用和相互关系如图1所示。
小型四旋翼无人机飞控***一般以单片型微控制器作为飞行控制器的计算单元。且具备捷联惯性传感器，以及高度位置等传感器为四旋翼飞行提供必要的反馈数据。四旋翼无人机是一个非线性、欠驱动的被控对象，采用经典的比例积分微分控制算法设计无人机的姿态控制、位置控制和控制器。图2从上到下依次为位姿控制框图、水平位置控制PID结构图、高度控制PID结构图、姿态控制PID结构图。
控制分配器将期望拉力及力矩转换为期望的电机转速，旋翼无人机悬停时单个螺旋桨拉力可以表示为：
cT为常数且可通过试验测得。旋翼无人机悬停时单个螺旋桨在机身上产生的反扭力矩可以表示为/>
cM为常数且可通过试验测得。X字四旋翼的控制分配和多旋翼的控制效率模型如下：
cT,d,cM为未知参数，可以用控制器中的比例系数进行补偿，设油门、俯仰、滚转、偏航四个方向的输出分别为σ1,σ2,σ3,σ4则令:
状态估计是一种多传感器数据融合方法，利用两种以上的不准确信息融合来提高测量***的测量精度；基于优化的多传感器状态估计器***结构，主要分为四个部分：视觉特征识别和追踪、IMU预积分、针对残差的最小二乘优化、回环检测。
针对状态估计器本发明主要有以下几点创新：(1)引入RGBD相机作为图像传感器，解决了传统单目VIO***无法获得视觉特征尺度的问题；(2)提出了“视觉-IMU”残差概念，重新设计了优化目标函数，降低了***的复杂度；(3)重新设计了优化过程中的优化变量，降低了***优化所需的时间；(4)完成了状态估计器的程序***实现，通过设计实物***，验证其有效性。
本发明基于优化的多传感器状态估计器设计实现原理，利用深度视觉图像获取相机运动和IMU数据获取载体的预积分轨迹，通过设计优化过程使图像和IMU计算的轨迹数据实现紧耦合估计。通过最小二乘优化方法，最小化视觉测量值与IMU估计值之间的差值，即“残差”，实现图像和IMU数据的紧耦合；图7为图像与IMU紧耦合过程的示意图。
视觉图像跟踪是基于优化的多传感器状态估计器前端数据预处理中的图像与处理环节。使用多组特征点的像素坐标及特征点深度，还原图像帧状态，状态包含：相机的空间位置(Position,P)、速度(velocity,V)和四元数表示的旋转(Quaternion,Q)；视觉图像跟踪部分的***结构图如图3所示，包含三个主要的处理步骤：稀疏光流法(KLT)跟踪，SFM三维运动重建和滑窗关键帧的选取。
利用KLT稀疏光流法对上一帧提取的特征点在当前帧的位置进行跟踪。为了获取相机在空间中的位置，我们使用SFM法对相机运动进行重建，在寻找特征点的过程中，为了避免所寻找的特征点过于密集，需要设置非极大值抑制半径，该半径描述的是在寻找特征点过程中，每个特征点间距的最小距离。当完成了当前图像上的特征点获取后，使用KLT光流跟踪法跟踪下一帧图像中的上一帧图像寻要找的特征点，并给予相同的特征点ID。在完成上述步骤后，***将进行关键帧的筛选和无用帧的剔除，从而保证图像以一定速率进行发布，减小后端优化过程中的计算压力。使用滑动窗口对当前需要处理的关键帧进行存储和更新，在关键帧的判断过程中，关键帧存储的窗口的大小默认为10；滑窗法中的关键帧如图4所示。
惯性传感器预积分是基于优化的多传感器状态估计器前端处理中最为重要的一项处理，由于惯性传感器的采样频率通常远高于视觉传感器的采样频率，而IMU获得的是每
一时刻的加速度和角速度，通过积分获得两帧之间的由IMU测出的位移和旋转变换。在本发明中，使用视觉观测值和惯性观测值进行耦合：需求解出视觉观测值并计算其残差，残差的雅各比矩阵是优化中下降的方向，协方差矩阵是观测值对应的权值。
首先，对惯性传感器在世界坐标系下的连续时刻进行积分推导，公式如下：
由于传感器采样获得的IMU数据是离散的，因此需要对上式使用中值积分进行离散化：
根据上述对连续时间积分存在的后期优化难度高的问题，提出预积分环节。通过对惯性传感器在世界坐标系下的连续时刻进行预积分推导来解决该问题；预积分示意图如图8。
本发明在完成前端视觉图像跟踪和前端惯性传感器预积分后，将进入初始化环节。初始化环节是本发明运行过程中最为重要的环节。特别的，视觉惯性紧耦合***需要通过初始化过程对***参数进行恢复和校准，恢复的参数包含相机尺度、重力、速度以及IMU的测量误差(Bias)。由于视觉三维运动重建(SFM)在初始化的过程中有着较好的表现，所以在初始化的过程中主要以SFM为主。通过将IMU的预积分结果与视觉三维运动重建结果对齐，可对IMU的测量误差进行进一步初始化。***的初始化主要包括三个环节：求取相机与IMU之间的相对旋转、相机初始化、IMU与视觉信息的对齐。
由于多传感器状态估计器采用视觉元件和惯性元件作为紧耦合的数据来源，而传感器间存在空间和时间上的不匹配，因此相机与IMU之间的旋转标定十分重要，通常当***存在1-2°的标定误差时，***的精度就会变的极低。通过计算可得权重：
对N测量值有
其中threshold为阈值，一般取为5，QN的左奇异向量中最小奇异值对应的特征向量。由此，即可通过求解权重方程得到相对旋转。同时我们还需要注意求解的终止条件，即校准完成的终止条件。当存在足够多的旋转运动时，***可以很好的估计出相对旋转/>
这时QN对应一个准确解且其零空间的秩为1。但是在实际校准的过程中，某些轴向上可能存在退化运动，如匀速运动。此时QN的零空间的秩会大于1，其判断条件是判断QN的第二小的奇异值是否大于某个阈值，若大于则其零空间的秩为1，反之秩大于1。当秩大于1时，表示初始化过程中的相对旋转/>
精度不足或存在过多的退化运动，***则不能完成初始化。
视觉与IMU的对齐主要解决三个问题，修正陀螺仪的偏移bias，初始化速度、重力向量g和尺度因子(Metricscale)，改进重力向量g的量值，在上一段落中已经根据连续图像的相对旋转算出相机和IMU间的外参旋转，接下来使用SFM计算出来的各帧图像的相对旋转来计算出陀螺仪的偏移。通过IMU量模型，利用旋转矩阵最终求解最小二乘问题：
后端优化是基于优化的多传感器状态估计器的核心部分。后端优化的核心思想是使由边缘化的先验信息、IMU测量残差以及视觉的观测残差组成的代价函数最小：
在代价函数中，两个残差项依次是IMU测量残差
以及“视觉-IMU”的观测残差/>
其中残差大小以马氏距离来表示。在优化计算过程中使用高斯迭代法，将代价函数进行线性化。在后端的优化过程中IMU部分依赖IMU残差以及雅克比矩阵进行优化。在使用“Ceres函数库”计算非线性优化的过程中，使用雅可比矩阵对代价函数进行高斯迭代以求最优解。
在回环检测过程中，当新的关键帧生成后，使用FAST特征点检测算法寻找处新的特征点，这些特征点与其在KTL光流跟踪中寻找的特征点不同。随后通过BRIEF方法提取描述子并与历史描述子匹配，使用DBow词袋字典库对描述子信息进行存储和检索。若存在本关键帧中的描述子对应的特征点与词袋中存储的历史特征点相同，则匹配对应的特征点***寻找到回点。若寻找到回环点，则需找出回环最早出现的关键帧，将此关键帧的位姿设为固定。在回环检测过程中，由于回环检测的速度总是慢于关键帧的生成速度，为了保持回环检测不落后于关键帧生成，通常采用跳帧法将部分关键帧剔除，从而保证检测效率。在完成回环检测后，多传感器状态估计器需通过快速重定位方式，将回环帧信息返回至后端联合优化过程以更新优化数据，其过程示意图如图5所示，将当前回环帧记为Framecur,将与Framecur互为回环的关键帧记为Frameold。记录Frameold的位姿、关键帧序号、匹配点对、Framecur的关键帧序号、时间戳并标记为回环。
通过重投影计算，可以计算Framecur位姿移动到Frameold位姿的变化量，即当前帧和回环帧的相对位姿。使用KLT光流跟踪，可以得到当前帧与回环帧的匹配的点对。回环检测进程将回环帧信息打包，发送至联合优化过程。状态估计器将Framecur位姿计作Frameold位姿，将与Framecur匹配的特征点的坐标用Frameold对应点坐标代替，并重新进行优化计算。
无人机在自主运行过程中，需要确保无人机不与障碍物相撞。利用局部路径规划方法，使无人机利用已经生成的地图和传感器实时更新的障碍物信息，在线生成一条或多条无人机局部路径，保障无人机能够在障碍物中飞行，这对局部路径规划算法提出了一定的要求。首先，该算法必须是一种在线路径规划算法，能够保证无人机实时运行，且路径可根据更新的障碍物实时改变。其次，为保障无人机能够平稳飞行，局部路径规划算法所规划出的路径需要满足无人机的动力学约束。最后，局部路径规划算法所规划出的路径需是局部最优解或次优解。综合上述需求，本发明采用一种基于B样条轨迹优化的动力学路径搜索方法，使每架无人机能够在局部范围内进行实时路径规划。
单纯使用动力学路径搜索产生的路径可能不理想。同时由于在动力学搜索算法中，自由空间中的距离信息被忽略，因此搜索得到的路径通常接近障碍物。为了解决上述问题，采用一种B样条优化方法对动力学搜索产生的轨迹进行优化，用以提高路径的平滑度，改进路径与障碍物间隙过小的问题。
当单架无人机具备了局部路径规划能力后，即可借助多无人机集群SLAM目标分配策略实现多无人机的协同区域搜索。区别于无人机点对点的路径规划，协同路径规划的目标是多无人机协同覆盖扫描一个区域。这种协同搜索即针对特定的面积任务区域设计一种控制方式，使得多无人机能够以最小的代价快速高效地覆盖整个已知环境的任务区域，或者搜索任务区域，发现未知环境里价值较高的目标，降低环境的不确定性。多无人机在单个区域的协同搜索可从功能上分为两个部分。其一，每架无人机的搜索区域分配，即将一个简单的单连通区域分配成多个区域。其二，每架无人机在各自区域内的搜索策略。为了将一个简单的单连通区域分配成多个区域，本发明使用一种DARP(DivideAreasAlgorithmforOptimalMulti-RobotCoveragePathPlanning)算法对区域进行分区。DARP算法根据移动无人机的初始位置划分区域，确保每个区域的面积近似相等且联通。
当完成无人机探索区域划分后，即可根据区域执行设定的搜索策略。常见的无人机搜索策略有随机搜索、平行搜索(“Z”字形或之字形)、网格搜索、内螺旋式搜索等。随机搜索是指无人机以恒定的航迹角在搜索区域内飞行到达区域边界后，再以最小转弯半径转弯进入搜索区域，此时无人机以该航迹角继续飞行，如此往复，如图6(a)所示。平行搜索指无人机在搜需区域按照垂直或水平方向进行搜索，由于无人机性能的约束，从能量、路程、时间角度表明转弯过程比直线飞行过程的效率要低，因此，目前大部分的研究都是基于平行线式的搜索，如图6(b)所示。
在使用时，可以通过QAV250型微型四旋翼无人机搭载多传感器***和微型计算机，采用拥有Intel-i7-10700k1处理器；Nvidia-GTX2060图形计算卡；32GB运行内存的仿真计算机来说明；传感器有IntelRealsenseD435i，IntelRealsenseD435i，IntelRealsenseT265；IntelRealsenseD435i采集深度图像及RGBD图像；IntelRealsenseD435i采集惯性信息；IntelRealsenseT265进行位置跟踪。利用来自于EuRoC的数据集，在Ubuntu1***下使用C++语言进行飞行控制及数据处理。同时需要调用开源的第三方库作为***计算支持。如Eigen矩阵库、Ceres非线性优化库、PCL点云支持库、OpenCV图像处理库等。
为了获取相机在空间中的位置，我们使用SFM法对相机运动进行重建，在此过程中需要标记图像中的特征点，并获得特征点不同图像中的位置。本发明使用Opencv计算机视觉库中的特征点采集(GoodFeatures_To_Track)方法，在***采集到的第一帧图像内寻找特征最明显的多个特征点。在VIO***中范围通常在150至500个之间，每个特征点都具有其对应的特征ID。
由于基于优化的多传感器状态估计器使用相同的传感器数据同时进行位姿和深度的估计，因此视觉图像跟踪过程与SLAM地图点云的创建过程被设计为同步进行以避免***存在深度数据的时间延迟，对于还没有深度的特征点，如果它被之前2帧以上的关键帧观察到过，用奇异值分解计算出一个它的坐标，使得特征点在其被观察到的每帧图像上的重投影误差最小，以满足后端优化需求。但在点云更新过程中，特征点的坐标应当使用传感器测量的深度。同时在使用RGBD相机而非点云相机进行SLAM的过程中，需进行以下步骤以保证数据的可靠性和地图的无偏性：
1)信任深度相机，对于每个特征点设定其被观察到的第一次的位置即为准确值，可直接加入地图点。
2)对深度相误差所以加判断，对于每个特征点在器被观察到的连续两帧中，在世界坐标系中的三维坐标不能相差太大，如果相差太大，就把它的第一帧的观察记录删掉。如果相差不大，就取两个三维坐标的平均值，作为该点的三维位置，加入地图点。
3)对地图点进行筛选，该点需要在连续几帧中被观察到，并且该连续图像帧中所观察到的特征点其在世界坐标系中的三维坐标相差不大，则认为它是一个准确的特征点并将其加入地图点。
在进行会回环检测时，采用FAST特征点检测法和BRIEF描述子这两个实时性较好的检测和描述方法，这样的组合使***对处理器和内存的要求降低，从而保证实时性要求，同时使用DBow词袋字典库对描述子信息进行存储和检索。
在进行局部路径规划时，无人机借助基于优化的多传感器状态估计器实时估计其在空间中的位姿状态，借助RGBD传感器提供的深度信息和自身位置进而计算点云(PointCloud)信息并更新地图。同时由于在动力学搜索算法中，自由空间中的距离信息被忽略，因此搜索得到的路径通常接近障碍物。为了解决上述问题，采用一种B样条优化方法对动力学搜索产生的轨迹进行优化。针对多无人机室内调度问题，本发明采用一种基于DARP区域分割法的无人机调度算法，实现无人机集群在室内的多连通区域如房间、走廊等环境下的搜索调度。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。
Claims (10)
1.一种用于飞行器勘测的多传感状态估计器，其特征在于：包括对四旋翼无人机建立详细的数学模型，对基于优化的多传感器状态估计器进行模型描述，解决无人机对未知环境的探索问题，实现图像与IMU数据的紧耦合估计、视觉图像跟踪，完成惯性传感预积分，在完成上述步骤后，进入初始化，包含三个环节：求取相机与IMU之间的相对旋转、相机初始化、IMU与视觉信息的对齐；
同时进行回环检测，利用局部路径规划方法保障无人机能够在障碍物中飞行，当单架无人机具备了局部路径规划能力后，即可借助多无人机集群SLAM目标分配策略实现多无人机的协同区域搜索。
2.根据权利要求1所述的一种用于飞行器勘测的多传感状态估计器，其特征在于：需要对四旋翼无人机建立运动模型，通过对机体受力分析和力矩分析，可以得到简化后集体的运动方程：
3.根据权利要求1所述的一种用于飞行器勘测的多传感状态估计器，其特征在于：需要对基于优化的多传感器状态估计器进行模型描述，该状态估计器实现了无人机在复杂环境下的实时位姿估计，解决了传统无人机SLAM需搭载过多传感器的问题；最后针对四旋翼无人机集群对未知环境的探索和地图构建问题，研究适合多传感器状态估计器的在线无人机运动规划算法；提出基于动力学路径搜索的改进A*算法和B样条轨迹优化方法，解决了单一无人机的在线运动决策和规划方法；并设计无人机集群SLAM控制策略，完成无人机在集群状态下的目标点和路径点规划，从而完成方法框架的闭环。
4.根据权利要求1所述的一种用于飞行器勘测的多传感状态估计器，其特征在于：解决无人机对未知环境的探索问题，并且重点解决三个问题；即无人机的状态估计问题、路径规划问题和运动控制问题。
5.根据权利要求1所述的一种用于飞行器勘测的多传感状态估计器，其特征在于：需要实现图像与IMU数据的紧耦合估计，利用深度视觉图像获取相机运动和IMU数据获取载体的预积分轨迹，通过设计优化过程使图像和IMU计算的轨迹数据实现紧耦合估计；通过最小二乘优化方法，最小化视觉测量值与IMU估计值之间的差值，即“残差”，实现图像和IMU数据的紧耦合。
6.根据权利要求1所述的一种用于飞行器勘测的多传感状态估计器，其特征在于：需要完成视觉图像跟踪设计，使用多组特征点的像素坐标及特征点深度，还原图像帧状态，状态包含：相机的空间位置(Position,P)、速度(velocity,V)和四元数表示的旋转(Quaternion,Q)。包含三个主要的处理步骤：稀疏光流法(KLT)跟踪，SFM三维运动重建和滑窗关键帧的选取。
7.根据权利要求1所述的一种用于飞行器勘测的多传感状态估计器，其特征在于：需要完成惯性传感预积分，使用视觉观测值和惯性观测值进行耦合：需求解出视觉观测值并计算其残差，残差的雅各比矩阵是优化中下降的方向，协方差矩阵是观测值对应的权值；特别的，针对对连续时间积分存在的后期优化难度高的问题，提出预积分环节，通过对惯性传感器在世界坐标系下的连续时刻进行预积分推导来解决该问题。
8.根据权利要求1所述的一种用于飞行器勘测的多传感状态估计器，其特征在于：完成上述权利要求部分后，需要进入初始化环节，特别的，视觉惯性紧耦合***需要通过初始化过程对***参数进行恢复和校准，恢复的参数包含相机尺度、重力、速度以及IMU的测量误差(Bias)；由于视觉三维运动重建(SFM)在初始化的过程中有着较好的表现，所以在初始化的过程中主要以SFM为主；通过将IMU的预积分结果与视觉三维运动重建结果对齐，可对IMU的测量误差进行进一步初始化，主要包括三个环节：求取相机与IMU之间的相对旋转、相机初始化、IMU与视觉信息的对齐。
9.根据权利要求1所述的一种用于飞行器勘测的多传感状态估计器，其特征在于：需要进行回环检测在回环，检测过程中，当新的关键帧生成后，使用FAST特征点检测算法寻找处新的特征点，这些特征点与其在KTL光流跟踪中寻找的特征点不同；随后通过BRIEF方法提取描述子并与历史描述子匹配，使用DBow词袋字典库对描述子信息进行存储和检索；若存在本关键帧中的描述子对应的特征点与词袋中存储的历史特征点相同，则匹配对应的特征点***寻找到回点；若寻找到回环点，则需找出回环最早出现的关键帧，将此关键帧的位姿设为固定；在回环检测过程中，由于回环检测的速度总是慢于关键帧的生成速度，为了保持回环检测不落后于关键帧生成，通常采用跳帧法将部分关键帧剔除，从而保证检测效率；在完成回环检测后，多传感器状态估计器需通过快速重定位方式，将回环帧信息返回至后端联合优化过程以更新优化数据；需要确保无人机不与障碍物相撞，利用局部路径规划方法，使无人机利用已经生成的地图和传感器实时更新的障碍物信息，在线生成一条或多条无人机局部路径，保障无人机能够在障碍物中飞行，采用一种B样条优化方法对动力学搜索产生的轨迹进行优化，用以提高路径的平滑度，改进路径与障碍物间隙过小的问题。
10.根据权利要求1所述的一种用于飞行器勘测的多传感状态估计器，其特征在于：通过借助多无人机集群SLAM目标分配策略实现多无人机的协同区域搜索，使用一种DARP算法对区域进行分区，其中DARP算法全名称是DivideAreasAlgorithmforOptimalMulti-RobotCoveragePathPlanning；DARP算法根据移动无人机的初始位置划分区域，确保每个区域的面积近似相等且联通，当完成无人机探索区域划分后，即可根据区域执行设定的搜索策略。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310243900.8A CN116295342A (zh) | 2023-03-15 | 2023-03-15 | 一种用于飞行器勘测的多传感状态估计器 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310243900.8A CN116295342A (zh) | 2023-03-15 | 2023-03-15 | 一种用于飞行器勘测的多传感状态估计器 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116295342A true CN116295342A (zh) | 2023-06-23 |
Family
ID=86837447
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310243900.8A Pending CN116295342A (zh) | 2023-03-15 | 2023-03-15 | 一种用于飞行器勘测的多传感状态估计器 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116295342A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117830879A (zh) * | 2024-01-02 | 2024-04-05 | 广东工业大学 | 面向室内的分布式无人机集群定位与建图的方法 |
-
2023
- 2023-03-15 CN CN202310243900.8A patent/CN116295342A/zh active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117830879A (zh) * | 2024-01-02 | 2024-04-05 | 广东工业大学 | 面向室内的分布式无人机集群定位与建图的方法 |
| CN117830879B (zh) * | 2024-01-02 | 2024-06-14 | 广东工业大学 | 面向室内的分布式无人机集群定位与建图的方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112347840B (zh) | 视觉传感器激光雷达融合无人机定位与建图装置和方法 | |
| Mansouri et al. | Cooperative coverage path planning for visual inspection | |
| US10732647B2 (en) | Multi-sensor fusion for robust autonomous flight in indoor and outdoor environments with a rotorcraft micro-aerial vehicle (MAV) | |
| Achtelik et al. | Autonomous navigation and exploration of a quadrotor helicopter in GPS-denied indoor environments | |
| CN113625774B (zh) | 局部地图匹配与端到端测距多无人机协同定位***和方法 | |
| CN113485441A (zh) | 结合无人机高精度定位和视觉跟踪技术的配网巡检方法 | |
| JP2020507072A (ja) | 実時間オンライン自己運動推定を備えたレーザスキャナ | |
| CN110068335A (zh) | 一种gps拒止环境下无人机集群实时定位方法及*** | |
| CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
| 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 | |
| CN110018691A (zh) | 小型多旋翼无人机飞行状态估计***和方法 | |
| Sanfourche et al. | Perception for UAV: Vision-Based Navigation and Environment Modeling. | |
| CN116182837A (zh) | 基于视觉激光雷达惯性紧耦合的定位建图方法 | |
| CN115574816B (zh) | 仿生视觉多源信息智能感知无人平台 | |
| Abeywardena et al. | Fast, on-board, model-aided visual-inertial odometry system for quadrotor micro aerial vehicles | |
| Magree et al. | Monocular visual mapping for obstacle avoidance on UAVs | |
| CN111238469A (zh) | 一种基于惯性/数据链的无人机编队相对导航方法 | |
| CN116295342A (zh) | 一种用于飞行器勘测的多传感状态估计器 | |
| CN117685953A (zh) | 面向多无人机协同定位的uwb与视觉融合定位方法及*** | |
| Kehoe et al. | State estimation using optical flow from parallax-weighted feature tracking | |
| Liu et al. | On terrain-aided navigation for unmanned aerial vehicle using B-spline neural network and extended Kalman filter | |
| Guan et al. | A new integrated navigation system for the indoor unmanned aerial vehicles (UAVs) based on the neural network predictive compensation | |
| Cao et al. | Visual-Inertial-Laser SLAM Based on ORB-SLAM3 | |
| CN114459474A (zh) | 一种基于因子图的惯性/偏振/雷达/光流紧组合导航的方法 | |
| Hosen et al. | A vision-aided nonlinear observer for fixed-wing UAV navigation |
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 |