CN104236548B - 一种微型无人机室内自主导航方法 - Google Patents
一种微型无人机室内自主导航方法 Download PDFInfo
- Publication number
- CN104236548B CN104236548B CN201410466305.1A CN201410466305A CN104236548B CN 104236548 B CN104236548 B CN 104236548B CN 201410466305 A CN201410466305 A CN 201410466305A CN 104236548 B CN104236548 B CN 104236548B
- Authority
- CN
- China
- Prior art keywords
- unmanned plane
- dimensional
- path
- point
- rgb
- 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.)
- Expired - Fee Related
Links
- 238000000034 method Methods 0.000 title claims abstract description 116
- 238000005259 measurement Methods 0.000 claims description 43
- 230000007613 environmental effect Effects 0.000 claims description 38
- 238000013519 translation Methods 0.000 claims description 36
- 230000004927 fusion Effects 0.000 claims description 27
- 238000005457 optimization Methods 0.000 claims description 15
- 238000001914 filtration Methods 0.000 claims description 14
- 238000013178 mathematical model Methods 0.000 claims description 13
- 239000013598 vector Substances 0.000 claims description 11
- 238000001514 detection method Methods 0.000 claims description 10
- 230000001133 acceleration Effects 0.000 claims description 7
- 230000009471 action Effects 0.000 claims description 6
- 238000009825 accumulation Methods 0.000 claims description 5
- 238000013459 approach Methods 0.000 claims description 4
- 238000013461 design Methods 0.000 claims description 4
- 238000005070 sampling Methods 0.000 claims description 4
- 238000012544 monitoring process Methods 0.000 claims description 3
- 238000012805 post-processing Methods 0.000 claims description 3
- 230000001960 triggered effect Effects 0.000 claims description 3
- 230000008859 change Effects 0.000 claims description 2
- 238000011156 evaluation Methods 0.000 claims 2
- 238000012937 correction Methods 0.000 claims 1
- 239000000155 melt Substances 0.000 claims 1
- 238000005516 engineering process Methods 0.000 abstract description 10
- 230000035899 viability Effects 0.000 abstract description 2
- 230000008569 process Effects 0.000 description 24
- 230000033001 locomotion Effects 0.000 description 18
- 239000011159 matrix material Substances 0.000 description 10
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical group C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 7
- 238000004364 calculation method Methods 0.000 description 7
- 238000012545 processing Methods 0.000 description 7
- 230000006870 function Effects 0.000 description 6
- 238000004387 environmental modeling Methods 0.000 description 5
- 230000000007 visual effect Effects 0.000 description 5
- 238000000605 extraction Methods 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 238000006243 chemical reaction Methods 0.000 description 3
- 230000004888 barrier function Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 241000208340 Araliaceae Species 0.000 description 1
- 235000005035 Panax pseudoginseng ssp. pseudoginseng Nutrition 0.000 description 1
- 235000003140 Panax quinquefolius Nutrition 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 235000012730 carminic acid Nutrition 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000001186 cumulative effect Effects 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000005265 energy consumption Methods 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 230000010006 flight Effects 0.000 description 1
- 239000000446 fuel Substances 0.000 description 1
- 235000008434 ginseng Nutrition 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 230000010287 polarization Effects 0.000 description 1
- 230000008439 repair process Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
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/18—Stabilised platforms, e.g. by gyroscope
-
- 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
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)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
本发明涉及一种微型无人机室内自主导航方法,属于微型无人机自主导航技术领域,该方法包括:基于RGB‑D相机和MEMS惯性传感器的微型无人机运动状态估计;基于RGB‑D相机和MEMS惯性传感器融合的三维环境实时建模;实时可信路径规划与路径跟踪控制,通过所述三个步骤实施位置控制和姿态控制,引导无人机跟踪预定路径飞行。本发明提高了无人机定位以及运动状态估计的精度;提高了微型无人机在室内的自主环境感知能力;生成的路径可在满足路径可行性的基础上有效保证定位精度;有效提高微型无人机的自主能力。
Description
技术领域
本发明属于微型无人机自主导航技术领域,特别涉及一种基于RGB-D相机和MEMS传感器的微型无人机室内导航方法及其系统。
背景技术
微型无人机具有体积小、机动性强、操作灵活、成本低廉等特点,可在危险及复杂环境下执行任务,因此近年来在侦察、救灾、环境探测等军用和民用领域得到了非常广泛的应用。如何在复杂室内环境(如高层建筑火灾现场、震后建筑物、塌方矿井等)下实现微型无人机的自主飞行是该领域的重要研究问题,与之相对应的自主导航、规划与控制是实现微型无人机自主飞行的关键技术所在。
室内环境是一类典型的复杂任务场景,与大尺度室外空间相比,结构复杂且非结构化,具有封闭三维空间、环境不确定性、无GPS定位信号辅助等特点。因此,在该类环境下实现自主飞行,要求微小无人机在无外部辅助导航设施、自身机载载荷和计算资源有限的情况下,仅采用机载传感器自主确定自身的位置、速度、姿态等运动状态,并利用传感器信息建立环境的三维模型,在此基础上自主规划最优飞行轨迹并控制无人机按照既定轨迹飞行,完成飞行任务。
这些关键技术在当前均有一定的局限性,难以完全满足自主室内飞行的要求。
由于室内环境下无法接收到GPS信号,或者信号的可靠性较低,因此微型无人机的状态估计与环境建模所依赖的机载传感器主要包括惯性测量器件和视觉传感器。其中典型的惯性测量器件主要包括陀螺和加速度计,可感测无人机运动过程中的角速度和加速度,基于惯性测量器件的测量数据可以确定无人机的姿态与速度。微机电器件(MEMS)具有重量轻、成本低、功耗低、集成度高等优点,可满足微型无人机的重量、体积和功耗限制,因此微型无人机系统一般采用MEMS惯性测量器件。但MEMS惯性测量器件的测量值往往存在较大的误差,且随时间漂移,因此必须采用视觉传感器的测量值与惯性测量值通过滤波技术进行信息融合,以修正误差,获得更高精度的状态估计值。目前的微型无人机采用的视觉辅助运动估计技术主要基于激光测距、单目视觉和双目视觉。
激光测距技术依赖激光测距仪所提供的无人机与环境的相对距离测量值,通过对前后两帧激光扫描值进行对比和匹配,解算出前后两帧数据之间的旋转、平移转换关系,从而得到无人机的运动状态参数。但激光测距技术存在很大的局限性。激光测距仪的测量范围有限,且本质上只能提供在测量平面内的二维相对距离信息,因此激光测距技术仅适用于具有特殊垂直结构的环境,当环境仅存在简单结构时(长廊、环形建筑等),激光测距算法将会失效。
视觉运动估计技术基于摄像头器件获取的环境图像序列,提取连续图像序列中的特征点,对相同的特征点进行匹配,进而计算特征点之间的平移和旋转关系,获得运动体的运动参数。根据所采用的摄像头器件数量,视觉运动估计技术主要包括单目视觉和双目视觉技术。与激光测距技术相比,视觉运动估计技术可以充分利用环境的三维信息。单目视觉技术利用单个摄像头器件提供的环境图像信息计算运动体的方向参数,但无法提供运动体与环境的相对距离信息(即环境深度信息)。与之相比,双目视觉技术可对两个摄像头器件所获取的图像中的相同环境特征点进行立体匹配,从而计算深度参数,并可通过不同帧图像的特征提取、特征匹配和运动估计过程获得运动体的六自由度运动参数。但双目视觉算法一般较为复杂,运算量大,且视觉运动估计技术通常要求环境中包括足够丰富的纹理特征,因此该类技术在微型无人机运动状态估计中的应用也具有较大的局限性。
RGB-D相机是近年来在机器人和计算机视觉领域逐渐得到应用的新型视觉设备,该类型的设备可同时提供外部环境的彩色图像和三维深度数据(即环境中的点相对于RGB-D相机的距离)。其中三维深度的测量基于结构光技术实现:RGB-D相机主动向环境空间发射红外光线,并对环境空间的被测方向进行编码并产生光斑,进而通过检测环境中的光斑测量空间的三维深度。与传统的摄像头器件相比,RGB-D相机成本较低,体积较小,重量较轻;可直接提供三维深度数据,并能够在光照不良或纹理较少的环境中有效检测三维深度;因此更适用于室内环境中的微型无人机的运动估计和环境建模。
另一方面,自主环境感知和三维建模也是微型无人机实现室内自主飞行的关键技术,其目的是为了建立微型无人机所处环境的三维模型,为实现路径规划和控制提供基础。微型无人机机载传感器(激光测距仪、双目视觉系统)通过测量其与环境中各点的三维相对距离获得环境的深度信息,通常以三维点云(一系列的三维点)形式表达。通过对连续不同帧的三维点云数据进行配准和融合,即可得到完整的三维环境模型。当前的三维环境建模技术一般单纯利用视觉传感器获取的三维点云数据进行匹配,以计算不同三维点云之间的旋转和平移关系,本质上与运动估计为同一过程。在计算过程中,每一步中两帧数据之间匹配的误差会随着计算过程累积,易导致最终的建模误差。目前的系统一般利用SLAM(即时定位与建模)的闭环检测进行优化,但该类方法计算量较大,难以满足实时性要求。
路径规划指的是依据环境感知信息,在无人机状态空间内规划出一条从初始点到目标点的、满足各方面约束的最优连续路径的过程。目前常用的路径规划算法包括概率路径图(PRM)、快速随机树(RRT)、A*算法、混合整数线性规划(MILP)等。而在室内复杂环境下的路径规划具有其特殊性,微型无人机的定位和运动状态估计、以及环境建模的精度高度依赖传感器能够检测到的环境中有效特征信息,因此要求算法能够结合机载传感器的特性以及环境特征,自主规划出能够确保定位和运动状态估计性能的路径。在实际应用中,微型无人机一般采用滤波(如Kalman滤波等)的方法融合机载各传感器的测量信息,从而实现定位和运动状态估计。滤波方法的基本原理是认为无人机位置和各运动状态服从某种类型的概率分布模型(如高斯分布等),进而根据机载各传感器的测量信息估计无人机的位置和各运动状态的后验概率分布(由概率论可知,概率分布以期望和协方差表示)。因此衡量定位和运动状态估计性能的主要指标为定位和运动状态估计的可信度(由概率论可知,通过滤波方法估计得到的后验概率分布的协方差的大小表征了定位和运动状态估计的可信度:若协方差较大,则定位和运动状态估计的可信度较低;反之则可信度较高)。而目前常用的方法在规划过程中均未考虑定位和状态估计可信度因素,因此难以完全适应室内自主飞行的要求。
因此,当前微型无人机系统的导航、环境建模以及路径规划方法均存在一定的局限性,难以满足室内环境下自主飞行的特殊需求。
发明内容
本发明的目的是为克服已有技术的不足之处,提出一种微型无人机室内自主导航方法及其导航系统,以实现在无外部定位和计算辅助的条件下,仅依赖机载传感器和设备的微型无人机自主定位、环境感知建模以及路径规划,提高微型无人机在复杂室内环境下的自主飞行能力。
本发明提出的一种微型无人机室内自主导航方法,其特征在于,该方法包括以下步骤:
S1:基于RGB-D相机和MEMS惯性传感器的微型无人机运动状态估计:利用RGB-D相机获取环境的二维彩色图像和三维深度数据,通过特征点检测、特征点匹配、运动状态估计获得微型无人机的姿态和位置参数估计值,利用MEMS传感器获取惯性测量值,将所述姿态和位置参数估计值与惯性测量值通过滤波方法进行信息融合,用以抑制惯性测量值误差积累,得到更为精确的无人机包括位置、速度、姿态在内的运动状态估计值;
S2:基于RGB-D相机和MEMS惯性传感器融合的三维环境实时建模:利用步骤S1中RGB-D相机获取的三维深度数据,以及融合后的运动状态估计值进行三维点云的精确配准与融合,构建三维环境模型,并触发式地对该模型进行全局优化,用以提高模型精度;
S3:实时可信路径规划与路径跟踪控制:设定规划周期,在每个规划周期中,以步骤S2得到的三维环境模型为基础,同时建立RGB-D相机的测量误差模型和定位可信度的评价函数,从而生成无人机从当前点到目标点的路径,在每个规划周期结束后,选择当前最优路径执行;根据所述当前最优路径,与步骤S1得到运动状态估计值进行比较,得到当前的位置偏差,从而根据当前位置偏差生成无人机的位置控制指令,根据位置控制指令解算为无人机的姿态控制指令,实施位置控制和姿态控制,引导无人机跟踪预定路径飞行。
本发明提出的微型无人机室内自主导航方法及其导航系统,其优点是:
1、本发明提供的方法中,利用机载RGB-D相机和MEMS惯性传感器进行集成,实现了基于多传感器融合的状态估计,有效修正低成本MEMS惯性测量器件的误差积累,提高了无人机定位以及运动状态估计的精度,可在室内无GPS及其它外部定位辅助的特殊情况下,完全依靠机载传感器自主为微型无人机提供高精度、可靠的运动状态估计;
2、本发明提供的方法实现了基于RGB-D相机和MEMS惯性传感器融合的实时三维环境建模,基于融合后的高精度运动参数实现三维点云的精确配准和融合,有效提高了三维环境建模的精度;采用并行的闭环检测及优化过程对三维环境模型进行全局优化,可在避免影响建模实时性的前提下有效提高整体建模精度,提高了微型无人机在室内的自主环境感知能力;
3、本发明基于RGB-D相机的工作特性,实现了一种可信路径实时规划和路径跟踪控制方法,可有效适应不确定和动态环境,以及无人机的复杂动力学和运动学约束,生成的路径可在满足路径可行性的基础上有效保证定位精度;
4、本发明实现的微型无人机自主飞行系统可在复杂室内环境以及无外部定位和计算辅助条件下,完全依赖机载传感器和处理单元实现微型无人机的自主导航、环境感知与建模以及路径规划,有效提高微型无人机的自主能力。
附图说明
图1为本发明提出的微型无人机室内自主导航方法的总体流程框图;
图2为根据本发明实施例步骤1基于RGB-D相机和MEMS惯性传感器的微型无人机运动状态估计流程图;
图3为根据本发明实施例步骤2基于RGB-D相机和MEMS惯性传感器融合的三维环境实时建模流程图;
图4为根据本发明实施例步骤3实时可信路径规划与路径跟踪控制流程图;
图5为根据本发明实施例步骤3可信路径规划框架示意图;
图6为根据本发明实施例的微型无人机室内自主导航系统结构示意图。
具体实施方式
下面结合附图和实施例对本发明的具体实施方式进行详细描述。
为达到所述目标,本发明的第一方面的目的在于提供一种微型无人机室内自主导航方法流程,如图1所示,该方法包括以下步骤:
S1:基于RGB-D相机和MEMS惯性传感器的微型无人机运动状态估计:利用RGB-D相机获取环境的二维彩色图像和三维深度数据,通过特征点检测、特征点匹配、运动状态估计获得微型无人机的姿态和位置参数估计值,利用MEMS传感器获取惯性测量值,将所述姿态和位置参数估计值与惯性测量值通过滤波方法进行信息融合,用以抑制惯性测量值误差积累,得到更为精确的无人机包括位置、速度、姿态在内的运动状态估计值;
S2:基于RGB-D相机和MEMS惯性传感器融合的三维环境实时建模:利用步骤S1中RGB-D相机获取的三维深度数据,以及融合后的运动状态估计值进行三维点云的精确配准与融合,构建三维环境模型,并触发式地对该模型进行全局优化,用以提高环境模型精度;
S3:实时可信路径规划与路径跟踪控制:设定规划周期,在每个规划周期中,以步骤S2得到的三维环境模型为基础,同时建立RGB-D相机的测量误差模型和定位可信度的评价函数,从而生成无人机从当前点到目标点的路径,在每个规划周期结束后,选择当前最优路径执行;根据所述当前最优路径,与步骤S1得到运动状态估计值进行比较,得到当前的位置偏差,从而根据当前位置偏差生成无人机的位置控制指令,根据位置控制指令解算为无人机的姿态控制指令,实施位置控制和姿态控制,引导无人机跟踪预定路径飞行。
进一步地,步骤S1中所述的基于RGB-D相机和MEMS惯性传感器的微型无人机运动状态估计流程,如图2所示,具体包括以下步骤:
S11:通过RGB-D相机获取当前环境的二维彩色图像与三维深度数据,并将该二维彩色图像进行预处理,转换为以灰度值表示的二维灰度图像;
S12:从步骤S11预处理后的每帧二维灰度图像中提取出一组特征点,并计算每一特征点的特征描述向量,并通过步骤S11中获取的三维深度数据对所述特征点进行筛选,剔除与三维深度数据无对应的特征点;
S13:对相邻两帧图像中的特征点进行特征匹配,通过对比特征点的对应描述向量获得两组匹配的特征点,并再剔除匹配有误的特征点;
S14:根据特征点对应的三维深度数据,将步骤S13匹配后的特征点映射到三维空间,从相邻两帧三维深度数据中获得具有匹配关系的相邻两组三维点云;
S15:求解步骤S14中获得的该两组匹配的三维点云之间的旋转和平移参数,利用所述旋转和平移参数反算出无人机的姿态和位置参数估计值;
S16:根据无人机的动力学特性建立状态空间模型(其中选择无人机的位置、速度、姿态等作为状态空间模型的状态量)以及RGB-D相机、MEMS惯性传感器的观测模型,将S15中得到的无人机的位置参数估计值作为系统的状态空间模型的观测量,与MEMS惯性传感器提供的由角速度、加速度、磁观测量信息组成的惯性测量值通过滤波方法进行融合,进一步修正惯性测量值误差,以有效抑制惯性测量值误差积累,获得包括无人机的位置、速度、姿态在内的更为精确的最终运动状态估计值。
进一步地,步骤S2中基于RGB-D相机和MEMS惯性传感器融合的三维环境实时建模流程,如图3所示,具体包括以下步骤:
S21:利用如步骤S16获得的RGB-D运动参数估计值与MEMS测量值融合后的运动状态估计值,以及步骤S14获得的两组匹配的三维点云,求解所述两组匹配的三维点云之间的初始旋转和平移关系;
S22:利用步骤S21中获得的两组三维点云及其初始旋转和平移关系,定义目标函数为所述两组三维点云中的每对三维点云之间旋转和平移误差之和,通过最小二乘优化迭代方法优化所述目标函数,得到更为精确的三维点云之间的旋转和平移关系;
S23:定义与环境固连的全局坐标系,根据步骤S22得到的不同帧三维点云之间的相对旋转、平移关系,以及各帧三维点云与初始帧三维点云之间的旋转和平移关系,将连续帧的三维点云组合至统一的全局坐标系中,得到三维环境模型;
S24:采用通用的SLAM(同时定位与地图构建)方法进行闭环监测,当监测到闭环时,触发全局优化过程,进一步校正不同帧三维点云之间的旋转和平移关系,对步骤S23已经建立的三维环境模型进行优化,得到更为精确的的三维环境模型。
进一步地,所述步骤3中所述的实时可信路径规划与路径跟踪控制流程,如图4所示,具体包括以下步骤:
S31:对步骤S2获得的三维环境模型进行后处理,将三维环境模型转换为体元形式(栅格等)存储,以便于利用三维环境模型进行路径规划;
S32:设定无人机的起始点和目标点,根据步骤S16建立的状态空间模型设计控制律,建立无人机的闭环系统数学模型(在自动控制理论中,通过反馈可获得被控系统的实际输出,控制律用于根据反馈计算控制量,从而改变被控系统的实际输出,使其趋近期望值。包含所述反馈、控制律和状态空间模型的数学模型称为闭环系统数学模型),将所述起始点、目标点和闭环系统数学模型作为无人机路径规划的输入条件,对路径进行初始化;
S33:设定单步路径规划周期为Δt(该周期根据无人机系统的实际计算能力、飞行速度等确定,一般Δt为秒的量级),在Δt周期内,通过步骤S1获得无人机包括位置、速度、姿态在内的运动状态估计值,记为x(t),并根据步骤S16中建立的系统状态空间模型预测Δt时刻后的无人机状态x(t+Δt);
S34:在同一个规划周期Δt的时间内,在S31获得的体元形式的三维环境模型中连续进行随机采样,获得一系列采样点,利用路径的代价以及定位和运动状态估计可信度作为启发式信息,根据该启发式信息在上一规划周期中生成的、以步骤S33预测得到的无人机状态x(t+Δt)为起点的路径集合中选择与每个采样点对应的最近点,利用步骤32中建立的无人机的闭环系统数学模型预测无人机的状态,生成由一系列可行路径组成的路径集合,并计算每条可行路径的代价以及定位和运动状态估计可信度,从而对路径连续进行增量式扩展;
所述路径代价代表无人机沿路径飞行所对应的代价,如该路径的总距离、无人机沿该路径飞行消耗的时间、燃料或电能等;
所述路径对应的定位和运动状态估计可信度的具体含义如下:由步骤1可知,无人机从某路径的起始点飞行到该路径终点的过程中,利用机载RGB-D相机可获取环境的图像和深度数据,在此基础上通过滤波方法可获得无人机的包括位置、速度、姿态在内的运动状态估计值,所述运动状态估计值的协方差阵的大小用于衡量无人机运动状态估计的可信度,因此路径终点对应的无人机运动状态估计值本质上为无人机沿该路径飞行过程中通过滤波方法获得的后验运动状态估计值,无人机运动状态估计值的协方差阵的大小即为该路径对应的定位和运动状态估计可信度。
S35:以路径的代价以及定位和运动状态估计可信度为选择指标,从步骤S34中生成的当前可行路径集合中选择最优路径,作为无人机的执行指令,引导无人机按照该最优路径飞行;循环执行步骤S33和S34,直至无人机飞行到目标点。
上述基于RGB-D相机和MEMS惯性传感器的微型无人机运动状态估计流程,基于机载RGB-D相机环境的二维彩色图像和三维深度数据,通过特征点检测、特征点匹配、运动状态估计等计算过程,可获得稳定、可靠的微型无人机的姿态和位置参数估计值;利用机载RGB-D相机的运动状态估计值和MEMS惯性传感器提供的惯性测量值,实现了基于多传感器融合的无人机定位和运动状态估计,可有效修正低成本MEMS器件的误差和漂移,提高无人机定位以及运动状态估计的精度,可在室内无GPS及外部定位辅助的特殊情况下,完全依靠机载传感器自主为微型无人机提供高精度、可靠的无人机运动状态估计参数;
上述基于RGB-D相机和MEMS惯性传感器融合的三维环境实时建模流程,一方面通过RGB-D运动状态估计和MEMS惯性测量值的融合获得更高精度的无人机位置和姿态估计参数,并用于确定两组三维点云之间的相对关系,实现三维点云的精确配准和融合,相比于单纯采用图像或三维深度数据进行三维环境建模的方法,本发明提出的三维环境实时建模流程可获得更高精度的三维环境模型;另一方面,采用闭环检测方法触发式地对三维环境模型进行优化,以抑制误差的积累,同时触发式的优化方式也减少了整体的运算量,避免了对三维环境建模实时性的影响。
上述实时可信路径规划与路径跟踪控制流程的优点如下:采用基于随机采样的路径规划架构,可以有效适应无人机的复杂动力学和运动学约束,在大规模环境空间下降低计算复杂度,提高计算效率,同时生成的路径可保证路径代价的最优性;在路径规划过程中引入包含控制;率的无人机闭环系统数学模型进行状态预测,可保证生成的路径更接近无人机实际物理特性,减少路径跟踪控制误差;在路径执行的同时连续扩展路径,保证路径规划的实时性,可有效应对不确定和动态环境;将传感器特性和定位和运动状态估计可信度因素结合到路径规划中,使生成的路径在满足路径代价约束的同时保证无人机的定位性能。
本发明的方法可利用无人机的机载计算处理单元,通过常规的编程技术实现实时执行上述方法中的各项运算和处理过程。上述方法中通过RGB-D相机,用于获取环境的RBG图像信息和三维深度参数;MEMS惯性测量单元,测量并输出无人机当前的角速度、加速度等惯性测量参数。
实施例:
S1基于RGB-D和MEMS传感器的微型无人机运动状态估计,如图2所示,具体包括以下步骤:
S11:通过RGB-D相机获取环境的图像和深度数据,包括一系列二维色彩域(RGB)像素点构成的二维彩色图像以及对应的三维深度域数据。
对于三维环境中的某点pi,RGB-D相机所获得的信息形式如下:pi=(xi,yi,zi,ri,gi,bi),其中xi,yi,zi为该点的三维深度数据,代表改点相对于RGB-D相机中心的三维位置,ri,gi,bi为该点对应的色彩域(RGB)信息。因此,某时刻RGB-D相机视场中的所有环境点的所述如pi形式的信息构成了RGB-D相机在该时刻产生的一帧二维彩色图像和三维深度数据。为进一步实现特征点提取,将所述二维彩色图像转换为二维灰度图像,转换公式如下:
Gi=λ1ri+λ2gi+λ3bi
其中λ1,λ2,λ3为转换系数,Gi为像素点转换后对应的灰度值。
S12:基于SURF(快速鲁棒图像特征法,已公开的通用方法)方法从步骤S11产生的二维灰度图像中通过灰度梯度极值识别并提取一组特征点,计算每个特征点邻域像素的灰度梯度方向,基于灰度梯度方向构建特征点的SURF特征描述向量,本实施例中采用128维的SURF特征描述向量。为减少错误特征点,利用步骤S11中RGB-D相机获取的三维深度数据对提取的特征点进行筛选,将无对应深度数据的特征点剔除。
S13:对比步骤S12提取的相邻两帧图像中特征点的特征向量,进行特征匹配。在本实施例中,对其中一帧图像中的特征点,以步骤S12中计算的两特征点的SURF特征向量的欧式距离来作为特征相似的测度,通过k-近邻搜索方法(通用方法)在相邻帧图像中寻找与某特征点最相近的k个对应特征点(本实施例中采用k=2)。令pi为所述一帧图像中的某个特征点,di1di2分别为特征点pi与其最优和次优匹配点qi1,qi2的欧式距离,为提高匹配的准确性,设定ε1,ε2为阈值,当di1di2满足约束di1<ε1,di1/di2<ε2时,确认qi1为pi对应的匹配特征点(本实施例中ε1=0.5,ε2=0.8)。最后,通过RANSAC(随机一致性采样,已公开的通用方法)方法再剔除匹配有误的特征点。
S14:根据步骤S11中获取的特征点的三维深度数据,将S13产生的匹配后的特征点映射到三维空间,获得具有匹配关系的相邻两组三维点云。如步骤S11中所述,RGB-D中获取的单个点的参数如下:pi=(xi,yi,zi,ri,gi,bi)。针对步骤S13中获得的二维图像匹配点,从步骤S11获得的三维深度数据中提取出相邻两组二维图像匹配点对应的三维深度数据(xi,yi,zi),并将提取出的三维深度数据组成两组三维点云Pi={pi},Qi={qi},即可将两组具有匹配关系的二维图像特征点映射到三维空间,获得具有匹配关系的相邻两组三维点云:Pi={pi},Qi={qi},i=1...n,pi,qi∈□3
S15:根据S14步骤获得的相邻两组具有匹配关系的三维点云之间的旋转和平移关系,求解无人机的姿态和位置参数估计值。具体过程如下:对于步骤S14获得的两组具有匹配关系的三维点云Pi={pi},Qi={qi},i=1...n,其中pi=(xi1,xi2,xi3)T,qi=(yi1,yi1,yi1)T为三维点云中具有匹配关系的某一对特征点,在特征点严格匹配且不存在传感器误差的理想情况下,二者之间的关系可表示为:
Qi=RPi+t
其中R∈□3×3代表三维点云在三维空间的旋转,t代表三维点云在三维空间的平移;以上关系从物理意义上即对应相机(及其无人机)在采集两帧三维点云数据时刻之间的运动。因此通过计算连续两帧三维点云之间的旋转和平移关系即可估计无人机的相对运动。
在本实施例中,为提高计算精度和对噪声的鲁棒性,通过基于RANSAC架构的迭代方法求解最小二乘意义上的最优R,t,使得以下误差函数取最小值:
无人机在采集两帧三维点云数据时刻之间的相对运动可通过下式确定:
ΔR=RT,Δt=-RTt
其中ΔR为无人机在采集两帧三维点云数据时刻之间的相对旋转,Δt为无人机在采集两帧三维点云数据时刻之间的相对平移。
因此,定义全局坐标系为固定在环境中三维坐标系,设无人机在全局坐标系的相对运动初始值为T0,其中:
无人机在时刻t相对于全局坐标系原点的刚体运动Tc关系可通过以下的连续变换确定:
Tc=T0·ΔTt-n+1·...·ΔTt-1·ΔTt
其中通过Tc即可反算出t时刻无人机在全局坐标系下的位置和姿态参数。
S16:根据无人机的动力学特性建立状态空间模型以及RGB-D相机、MEMS惯性传感器的观测模型,通过卡尔曼滤波(通用方法)将步骤S15中得到的RGB-D运动参数估计值与MEMS惯性传感器提供的测量值进行融合,修正惯性测量值误差,获得括无人机的位置、速度、姿态在内的更为精确的最终运动状态估计值。
在本实施例中,采用状态空间方式表示无人机的系统模型,建立无人机系统的状态空间模型如下:
F(X)=F(t)X+Gw
其中X为待估计的状态向量:
其中δs为位置误差,δv为速度误差,为姿态平台误差角,bω,bf分别为MEMS惯性传感器中陀螺和加速度计的漂移。系统模型的输入量包括陀螺提供的角速度测量值和加速度计提供的加速度测量值。其中状态转移矩阵为:
建立无人机系统RGB-D相机的观测模型:
其中为步骤S15提供的无人机位置估计值,观测矩阵为Hs(t)=[I3×3 03×12]
建立无人机系统MEMS惯性传感器的观测模型:
其中观测矩阵为且
其中为通过MEMS惯性传感器中的加速度计和磁传感器的测量值计算得到的姿态矩阵,为通过MEMS惯性传感器中的陀螺的测量值积分得到的姿态矩阵。
在本实施例中,通过卡尔曼滤波方法的时间更新和观测更新过程实现状态估计。
卡尔曼滤波方法的时间更新过程如下:
Xk,k-1=Φk,k-1Xk-1
其中P为卡尔曼滤波器的协方差阵,Q为系统噪声的协方差阵。
卡尔曼滤波方法的观测更新过程如下:
Xk=Xk,k-1+Kk[Zk-HXk,k-1]
Pk=[I-KkHk]Pk,k-1
其中K为卡尔曼增益矩阵,H为无人机系统观测模型相对于状态向量的Jacobian矩阵,R为观测噪声的协方差阵。
完成卡尔曼滤波的状态更新后,对状态向量进行修正,得到无人机的的位置、速度和姿态估计值:
1.位置:sk=sk-δsk
2.速度:vk=vk-δvk
3.姿态:
S2基于RGB-D相机和MEMS惯性传感器融合的三维环境实时建模,如图3所示,具体包括以下步骤:
S21:设当前时刻为i,利用如S16步骤获得的RGB-D运动参数估计值与MEMS测量值融合后的运动状态估计值,以及S14步骤获得的当前时刻两组匹配的三维点云,求解所述两组匹配的三维点云之间的初始旋转和平移关系;
其中一帧三维点云P为某时刻RGB-D相机获取的一组三维深度数据的集合,即P={pi},i=1...n,P中的单个数据pi∈P代表环境中对应点相对于相机中心的三维距离,即pi=(xi,yi,zi),RGB-D相机在不同时刻获得的三维点云表示为P0...Pn;通过步骤S14获得的具有匹配关系的三维点云可表示为Pi={pi},Qi={qi}。
利用步骤S16 RGB-D运动参数估计值和MEMS测量值融合后获得的运动状态估计值(主要利用融合后获得的无人机位置和姿态估计值),进一步转化为当前时刻两组匹配的三维点云之间的旋转Ri0和平移关系ti0,并整体表示为:
将该初始旋转和平移关系Ti0用于后续步骤的计算。
S22:基于步骤S21中获得的两组匹配的三维点云之间的初始旋转和平移关系,通过最小二乘迭代计算进一步求解更为精确的两组匹配的三维点云之间的旋转和平移变换关系。在本实施例中,采用基于RANSAC的方法迭代求解两组匹配的三维点云之间最小二乘意义上的旋转和平移关系,利用步骤S22得到的初始旋转和平移关系Ti0作为RANSAC方法的输入,通过重复的RANSAC迭代计算,获得两组匹配的三维点云之间的最优更为精确的旋转关系Ri和平移关系ti。
S23:定义与环境固连的全局坐标系,根据步骤S22计算得到的匹配的三维点云之间的相对旋转和平移关系,计算每一帧三维点云Pi相对于初始帧三维点云P0之间的旋转和平移关系Ti。根据以上旋转和平移关系,将每一帧三维点云进行组合:
通过以上过程将每一帧三维点云组合到全局坐标系下,最终得到三维环境模型P。
S24:为修正三维环境实时建模过程中的累积误差,通过SLAM闭环检测触发运行三维环境模型的全局优化过程,通过全局优化过程对步骤S23已经建立的进行三维环境模型进行优化,得到更为精确的三维环境模型。
在本实施例中,闭环指的是当RGB-D相机运动(即无人机的运动)到环境中之前曾经经过的位置时,RGB-D相机获取的环境特征点与以往某一帧图像中的特征点中有较大的公共部分,可根据所述公共部分的特征点对三维环境模型进行全局优化。
所述闭环检测的原理如下:在步骤S14的计算过程后,本实施例中以图(graph)的数据结构形式存储RGB-D相机获取的二维彩色图像、特征点和三维点云,并在步骤S15获得无人机的姿态和位置参数估计值后,进一步将姿态和位置参数估计值存储到已经建立的图中,其中图的节点Ci包括无人机相机当前的位置、姿态估计值以及在该点获取的二维彩色图像、特征点和三维点云,通过步骤S22得到不同帧三维点云之间的相对旋转和平移关系(Rij,tij)后,将(Rij,tij)作为图中相邻两节点Ci、Cj之间的边,同时也作为全局优化过程中的约束。在本发明的实例中,采用快速图像特征匹配方法(已公开的通用方法)检测闭环,全局优化过程采用触发式并行计算的机制。当检测到闭环时,触发全局优化过程,通过SBA(Spare Bundle Adjustment,通用方法)方法对已经建立的图进行全局优化求解。本实施例中首先在图中存储的连续帧二维彩色图像中以n帧为间隔,选择其中第k帧作为关键帧图像,并从图中获取当前帧二维彩色图像与关键帧图像之间的约束关系,当检测到闭环时,基于此触发Levenberg-Marquardt非线性优化计算过程(通用方法),对所述的二维彩色图像与关键帧图像之间的约束关系和三维点云之间的旋转和平移关系进行优化,从而生成更精确的三维环境模型。
S3实时可信路径规划与路径跟踪控制,如图4所示,具体包括以下步骤:
S31:对如步骤S2三维环境实时建模过程建立的三维点云形式的三维环境模型进行后处理,将三维点云形式的三维环境模型转化为体元(voxel)形式存储;
S32:执行初始化过程,根据任务要求,设定无人机的起始点xinit和目标点xgoal,根据步骤S16中建立的无人机状态空间模型设计控制律(本实施例中采用PID方法设计无人机的控制律,其中PID方法为控制律设计的通用方法),结合无人机状态空间模型和控制律建立无人机的闭环系统数学模型,并将起始点xinit、目标点xgoal、无人机的闭环系统数学模型与步骤S31生成的体元形式的三维环境模型共同作为路径规划的输入;(在本实施例中,以树(Tree)的数据结构存储和表达路径,其中无人机状态空间中的点代表树的节点,两节点之间的边代表无人机的路径,路径规划的目的在于寻找连接所述起始点xinit和xgoal的连续路径。)
S33:设定单步周期为Δt(本实施例中选择Δt为5秒),在Δt周期内,通过步骤1获得无人机包括位置、速度、姿态在内的当前运动状态估计值x(t),并根据步骤S16中建立的系统状态空间模型预测Δt时刻后的无人机状态x(t+Δt);
S34:在同一个规划周期Δt的时间内,对步骤S32所述的无人机路径树进行增量式扩展,具体过程如下:
S34-1:在步骤S32中建立的无人机闭环系统数学模型的参考空间Xr中进行随机采样,获得采样点xsample;(在本实施例中,设定参考空间Xr为4维,对于参考点xr∈Xr:xr=(x,y,z,ψ)T,其中x,y,z为无人机在全局坐标系下的三维位置,ψ代表无人机的航向)
S34-2:根据启发式目标函数在已有路径树中选择与所述采样点xsample最接近的点Nnear,在本实施例中,采用如下的混合式启发目标函数:
其中代表路径基本代价,为路径树中候选节点Ni到采样点xsample的代价(本实施例中采用平均时间代价,即两点间的欧式距离除以无人机的平均速度),τ(Ni|Nroot)为从当前路径树根节点Nroot到候选节点Ni的代价,ω为权值。
为路径的定位和运动状态估计可信度函数。其中Σ为无人机运动状态估计值的协方差阵,本实施例中以tr(Σ)代表定位和运动状态估计可信度。Σsamp为无人机在采样xsample的状态估计后验协方差阵。因此,通过可衡量从候选节点Ni到xsample的路径所对应的定位和运动状态估计可信度,根据在当前路径树中选择使得定位和运动状态估计可信度较高的候选节点。
S34-3:以所述Nnear节点对应的无人机最终状态为起始点,对比和xsample生成步骤22建立的无人机闭环系统控制律的参考输入,控制律进一步根据计算控制输入,无人机闭环系统数学模型根据控制输入预测下一时刻的无人机状态。循环执行以上过程,直到路径不可行或者状态已经到达xsample点。通过以上预测过程,生成一系列节点集合{Nj}。
S34-4:对于所述节点集合Nj∈{Nj},检查Nj是否与环境中的障碍物有碰撞,若Nj与障碍物无碰撞,计算节点对应的路径代价τ(Nj|Nroot)以及定位和运动状态估计可信度Σj,将节点Nj以及Nj和Nnear之间的可行路径添加至路径树中,从而生成由一系列可行路径组成的路径集合。
S35:从步骤S34生成的所有可行路径中根据选择指标选择最优路径,作为无人机的执行指令,控制无人机按照所述最优路径飞行。循环执行步骤S33和S34,直至无人机飞行到目标点。
在本实施例中,采用以下的选择指标选择最优路径:
其中τ(Ni|Nroot)为从根节点到Ni的路径代价,为从Ni到目标点xgoal的路径代价,tr(Σi)为Ni的定位和运动状态估计可信度。λ1,λ2和λ3为权重值。
本实施例中的实时可信路径规划与路径跟踪控制的基本框架如图5所示。
本发明实施例另一方面提供了一种实现所述基于RGB-D相机和MEMS惯性传感器的微型无人机室内自主导航方法的微型无人机室内自主导航系统装置。具体包括:
1.无人机机载计算处理单元,用于实时执行所述的各项运算和处理过程。本实施例中采用GB-BXi7H-4500 Brix型号的计算机系统作为机载计算处理单元,在此基础上运行Linux操作系统。本实施例中在无人机机载计算处理单元中执行的算法模块包括:
1)RGB-D运动状态估计模块,采用如本发明中所述的基于RGB-D的运动状态估计步骤,利用RGB-D传感器获取的图像数据和三维深度数据,通过特征提取、特征匹配,计算无人机的位置和姿态参数;
2)三维环境建模模块,采用如本发明中所述的基于RGB-D和MEMS惯性传感器融合的三维环境建模步骤,基于RGB-D传感器获取的三维深度信息以及二维图像特征匹配结果,利用RGB-D和MEMS惯性传感器融合得到的运动参数估计值,将三维点云进行配准和融合,构建精确的三维环境模型;
3)实时路径规划模块,采用如本发明中所述的实时可信路径规划方法,根据无人机飞行任务和三维环境模型,并结合RGB-D相机特性,生成从初始点到目标点的最优路径;
本实施例中所述算法模块均采用C++语言开发。
2.无人机机载飞行控制单元,用于执行RGB-D/MEMS传感器融合和飞行控制算法模块,本实施例中采用STM32F407型号的单片机作为无人机机载飞行控制单元。本实施例中在无人机机载飞行控制单元中执行的算法模块包括:
1)RGB-D/MEMS惯性传感器融合模块,采用如本发明中所述的基于RGB-D相机和MEMS惯性传感器的微型无人机运动状态估计步骤,通过滤波算法对RBG-D运动状态估计值和MEMS惯性测量结果进行融合,修正惯性测量误差,输出包括无人机位置、速度、姿态在内的运动状态估计值;
2)飞行控制模块,根据实时路径规划模块生成的参考路径和RGB-D/MEMS传感器融合模块提供的无人机运动状态估计值计算飞行控制指令,实施姿态控制和路径跟踪控制,引导无人机按照参考路径飞行。
本实施例中RGB-D/MEMS惯性传感器融合和飞行控制算法模块均采用C语言开发。
3.无人机机载传感器系统,包括:
1)RGB-D相机,获取并提供无人机周围环境的RGB图像和三维深度数据。本实施例中采用的RGB-D相机具体型号为PrimeSense Carmine 1.08,该型号的RGB-D相机可以30Hz的频率提供分辨率为640×480的二维彩色图像和三维深度数据;
2)MEMS惯性测量单元,包括三轴角速率陀螺,三轴加速度计和磁传感器,测量并输出无人机当前的角速度、加速度、姿态角等惯性测量数据和当地的磁测量值。本实施例中采用ADIS16405型号的集成式惯性测量模块,该模块集成了所述各惯性测量单元;
3)高度传感器,测量并输出无人机当前相对于地面的高度参数。本实施例中采用超声波测距模块作高度传感器,该模块的高度测量范围为2cm至450cm,分辨率为2mm,数据输出频率为25Hz;
以上仅为本发明的较佳实施例,仅用于解释本发明,而并非对本发明的限制。且本发明的保护范围并不局限于此,任何熟悉本领域的技术人员,在不脱离本发明的精神、原理和范围的情况下,还可以对这些实施例进行多种变化、变型、修改和替换,因此所有等同的技术方法都应属于本发明的范畴,本发明的保护范围应由所附权利要求及其等同限定。
Claims (4)
1.一种微型无人机室内自主导航方法,其特征在于,该方法包括以下步骤:
S1:基于RGB-D相机和MEMS惯性传感器的微型无人机运动状态估计:利用RGB-D相机获取环境的二维彩色图像和三维深度数据,通过特征点检测、特征点匹配、运动状态估计获得微型无人机的姿态和位置参数估计值,利用MEMS传感器获取惯性测量值,将所述姿态和位置参数估计值与惯性测量值通过滤波方法进行信息融合,用以抑制惯性测量值误差积累,得到更为精确的无人机包括位置、速度、姿态在内的运动状态估计值;
S2:基于RGB-D相机和MEMS惯性传感器融合的三维环境实时建模:利用步骤S1中RGB-D相机获取的三维深度数据,以及融合后的运动状态估计值进行三维点云的精确配准与融合,构建三维环境模型,并触发式地对该模型进行全局优化,用以提高模型精度;
S3:实时可信路径规划与路径跟踪控制:设定规划周期,在每个规划周期中,以步骤S2得到的三维环境模型为基础,同时建立RGB-D相机的测量误差模型和定位可信度的评价函数,从而生成无人机从当前点到目标点的路径,在每个规划周期结束后,选择当前最优路径执行;根据所述当前最优路径,与步骤S1得到运动状态估计值进行比较,得到当前的位置偏差,从而根据当前位置偏差生成无人机的位置控制指令,根据位置控制指令解算为无人机的姿态控制指令,实施位置控制和姿态控制,引导无人机跟踪预定路径飞行。
2.如权利要求1所述方法,其特征在于,所述步骤S1中的基于RGB-D相机和MEMS惯性传感器的微型无人机运动状态估计,具体包括以下步骤:
S11:通过RGB-D相机获取当前环境的二维彩色图像与三维深度数据,并将该二维彩色图像进行预处理,转换为以灰度值表示的二维灰度图像;
S12:从步骤S11预处理后的每帧二维灰度图像中提取出一组特征点,并计算每一特征点的特征描述向量,并通过步骤S11中获取的三维深度数据对所述特征点进行筛选,剔除与三维深度数据无对应的特征点;
S13:对相邻两帧图像中的特征点进行特征匹配,通过对比特征点的对应描述向量获得两组匹配的特征点,并再剔除匹配有误的特征点;
S14:根据特征点对应的三维深度数据,将步骤S13匹配后的特征点映射到三维空间,获得具有匹配关系的相邻两帧图像中的两组三维点云;
S15:求解步骤S14中获得的该两组匹配的三维点云之间的旋转和平移参数,利用所述旋转和平移参数反算出无人机的姿态和位置参数估计值;
S16:根据无人机的动力学特性建立状态空间模型以及RGB-D相机、MEMS惯性传感器的观测模型,将S15中得到的无人机的位置参数估计值作为系统的状态空间模型的观测量,与MEMS惯性传感器提供的由角速度、加速度、磁观测量信息组成的惯性测量值通过滤波方法进行融合,进一步修正惯性测量值误差,以有效抑制惯性测量值误差积累,获得包括无人机的位置、速度、姿态在内的更为精确的最终运动状态估计值。
3.如权利要求2所述方法,其特征在于,所述步骤S2中基于RGB-D相机和MEMS惯性传感器融合的三维环境实时建模,具体包括以下步骤:
S21:利用如步骤S15获得的RGB-D运动参数估计值与MEMS测量值融合后的运动状态估计值,以及步骤S14获得的相邻两帧图像中的两组匹配的三维点云,求解所述两组匹配的三维点云之间的初始旋转和平移关系;
S22:利用步骤S21中获得的两组三维点云及其初始旋转和平移关系,定义目标函数为所述两组三维点云中的每对三维点云之间旋转和平移误差之和,通过最小二乘优化迭代方法优化所述目标函数,得到更为精确的三维点云之间的旋转和平移关系;
S23:定义与环境固连的全局坐标系,根据步骤S22得到的不同帧三维点云之间的相对旋转、平移关系,以及各帧三维点云与初始帧三维点云之间的旋转和平移关系,将连续帧的三维点云组合至统一的全局坐标系中,得到三维环境模型;
S24:采用通用的SLAM方法进行闭环监测,当监测到闭环时,触发全局优化过程,进一步校正不同帧三维点云之间的旋转和平移关系,对步骤S23已经建立的三维环境模型进行优化,得到更为精确的的三维环境模型。
4.如权利要求3所述方法,其特征在于,所述步骤3中的实时可信路径规划与路径跟踪控制,具体包括以下步骤:
S31:对步骤S2获得的三维环境模型进行后处理,将三维环境模型转换为体元形式存储,以便于利用三维环境模型进行路径规划;
S32:设定无人机的起始点和目标点,根据步骤S16建立的状态空间模型设计控制律,建立无人机的闭环系统数学模型,将所述起始点、目标点和闭环系统数学模型作为无人机路径规划的输入条件,对路径进行初始化;
S33:设定单步路径规划周期为Δt,在Δt周期内,通过步骤S1获得无人机包括位置、速度、姿态在内的运动状态估计值,记为x(t),并根据步骤S16中建立的系统状态空间模型预测Δt时刻后的无人机状态x(t+Δt);
S34:在同一个规划周期Δt的时间内,在S31获得的体元形式的三维环境模型中连续进行随机采样,获得一系列采样点,利用路径的代价以及定位和运动状态估计可信度作为启发式信息,根据该启发式信息在上一规划周期中生成的、以步骤S33预测得到的无人机状态x(t+Δt)为起点的路径集合中选择与每个采样点对应的最近点,利用步骤32中建立的无人机的闭环系统数学模型预测无人机的状态,生成由一系列可行路径组成的路径集合,并计算每条可行路径的代价以及定位和运动状态估计可信度,从而对路径连续进行增量式扩展;
S35:以路径的代价以及定位和运动状态估计可信度为选择指标,从步骤S34中生成的当前可行路径集合中选择最优路径,作为无人机的执行指令,引导无人机按照该最优路径飞行;循环执行步骤S33和S34,直至无人机飞行到目标点。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410466305.1A CN104236548B (zh) | 2014-09-12 | 2014-09-12 | 一种微型无人机室内自主导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410466305.1A CN104236548B (zh) | 2014-09-12 | 2014-09-12 | 一种微型无人机室内自主导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN104236548A CN104236548A (zh) | 2014-12-24 |
CN104236548B true CN104236548B (zh) | 2017-04-05 |
Family
ID=52225094
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201410466305.1A Expired - Fee Related CN104236548B (zh) | 2014-09-12 | 2014-09-12 | 一种微型无人机室内自主导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN104236548B (zh) |
Families Citing this family (94)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104574386B (zh) * | 2014-12-26 | 2017-05-17 | 速感科技(北京)有限公司 | 一种基于三维环境模型匹配的室内定位方法 |
US10028102B2 (en) * | 2014-12-26 | 2018-07-17 | Here Global B.V. | Localization of a device using multilateration |
CN104639916A (zh) * | 2015-03-04 | 2015-05-20 | 合肥巨清信息科技有限公司 | 一种大场景多目标跟踪拍摄视频监控系统及其监控方法 |
EP3078935A1 (en) * | 2015-04-10 | 2016-10-12 | The European Atomic Energy Community (EURATOM), represented by the European Commission | Method and device for real-time mapping and localization |
CN104794213B (zh) * | 2015-04-27 | 2018-02-02 | 清华大学 | 基于飞行计划与导航数据库的航迹预测方法 |
CN104899918B (zh) * | 2015-05-14 | 2018-02-27 | 深圳大学 | 一种无人机的三维环境建模方法及系统 |
CN113093808A (zh) * | 2015-05-23 | 2021-07-09 | 深圳市大疆创新科技有限公司 | 使用惯性传感器和图像传感器的传感器融合 |
CN107850899B (zh) * | 2015-05-23 | 2022-04-19 | 深圳市大疆创新科技有限公司 | 使用惯性传感器和图像传感器的传感器融合 |
CN105045263B (zh) * | 2015-07-06 | 2016-05-18 | 杭州南江机器人股份有限公司 | 一种基于Kinect深度相机的机器人自定位方法 |
JP6304771B2 (ja) * | 2015-08-20 | 2018-04-04 | 株式会社サイバーウォーカー | 経路生成プログラム、経路生成方法及び経路生成装置 |
US11151654B2 (en) * | 2015-09-30 | 2021-10-19 | Johnson Controls Tyco IP Holdings LLP | System and method for determining risk profile, adjusting insurance premiums and automatically collecting premiums based on sensor data |
EP3360023A4 (en) * | 2015-10-09 | 2018-10-10 | SZ DJI Technology Co., Ltd. | Salient feature based vehicle positioning |
CN105352508A (zh) * | 2015-10-22 | 2016-02-24 | 深圳创想未来机器人有限公司 | 机器人定位导航方法及装置 |
KR101756650B1 (ko) * | 2015-10-27 | 2017-07-26 | 광주과학기술원 | 물품의 설치공간 탐색방법 및 이를 위한 프로그램이 기록된 컴퓨터로 판독 가능한 기록매체 |
CN105346706B (zh) * | 2015-11-13 | 2018-09-04 | 深圳市道通智能航空技术有限公司 | 飞行装置、飞行控制系统及方法 |
CN106705951A (zh) * | 2015-11-13 | 2017-05-24 | 恩斯迈电子(深圳)有限公司 | 移动装置 |
CN105353768B (zh) * | 2015-12-08 | 2017-12-26 | 清华大学 | 狭窄空间内基于随机采样的无人机轨迹规划方法 |
CN105509748B (zh) * | 2015-12-29 | 2019-03-01 | 深圳先进技术研究院 | 机器人的导航方法及装置 |
CN105759840A (zh) * | 2016-03-03 | 2016-07-13 | 陈健强 | 一种基于无人机的饭店占座实现方法及系统 |
CN107194984A (zh) * | 2016-03-14 | 2017-09-22 | 武汉小狮科技有限公司 | 移动端实时高精度三维建模方法 |
US10134293B2 (en) | 2016-03-21 | 2018-11-20 | Walmart Apollo, Llc | Systems and methods for autonomous drone navigation |
CN105717933A (zh) * | 2016-03-31 | 2016-06-29 | 深圳奥比中光科技有限公司 | 无人机以及无人机防撞方法 |
CN105847684A (zh) * | 2016-03-31 | 2016-08-10 | 深圳奥比中光科技有限公司 | 无人机 |
CN107272727B (zh) * | 2016-04-01 | 2022-02-01 | 松下电器(美国)知识产权公司 | 自主移动体 |
CN105759829A (zh) * | 2016-04-12 | 2016-07-13 | 深圳市龙云创新航空科技有限公司 | 基于激光雷达的微型无人机操控方法及系统 |
CN106441286B (zh) * | 2016-06-27 | 2019-11-19 | 上海大学 | 基于bim技术的无人机隧道巡检系统 |
CN106249755B (zh) * | 2016-09-14 | 2019-08-16 | 北京理工大学 | 一种无人机自主导航系统及导航方法 |
CN106527720A (zh) * | 2016-11-09 | 2017-03-22 | 福建工程学院 | 一种沉浸式交互控制方法与系统 |
WO2018086133A1 (en) | 2016-11-14 | 2018-05-17 | SZ DJI Technology Co., Ltd. | Methods and systems for selective sensor fusion |
CN106780608B (zh) * | 2016-11-23 | 2020-06-02 | 北京地平线机器人技术研发有限公司 | 位姿信息估计方法、装置和可移动设备 |
CN106767682A (zh) * | 2016-12-01 | 2017-05-31 | 腾讯科技(深圳)有限公司 | 一种获取飞行高度信息的方法及飞行器 |
CN106767817B (zh) * | 2016-12-01 | 2019-01-04 | 腾讯科技(深圳)有限公司 | 一种获取飞行定位信息的方法及飞行器 |
US11132816B2 (en) | 2016-12-21 | 2021-09-28 | Intel Corporation | Large scale CNN regression based localization via two-dimensional map |
CN106846485A (zh) * | 2016-12-30 | 2017-06-13 | Tcl集团股份有限公司 | 一种室内三维建模方法及装置 |
CN106705965A (zh) * | 2017-01-12 | 2017-05-24 | 苏州中德睿博智能科技有限公司 | 场景三维数据配准方法及导航系统误差校正方法 |
CN106940186B (zh) * | 2017-02-16 | 2019-09-24 | 华中科技大学 | 一种机器人自主定位与导航方法及系统 |
CN108628337A (zh) * | 2017-03-21 | 2018-10-09 | 株式会社东芝 | 路径生成装置、路径控制系统以及路径生成方法 |
CN107341442B (zh) * | 2017-05-22 | 2023-06-06 | 腾讯科技(上海)有限公司 | 运动控制方法、装置、计算机设备和服务机器人 |
CN107389968B (zh) * | 2017-07-04 | 2020-01-24 | 武汉视览科技有限公司 | 一种基于光流传感器和加速度传感器的无人机定点实现方法和装置 |
CN107479569B (zh) * | 2017-07-05 | 2020-11-06 | 南宁学院 | 一种可调螺旋翼姿态的无人机自动飞行控制系统 |
CN107479570B (zh) * | 2017-07-05 | 2021-04-09 | 南宁学院 | 一种可调螺旋翼姿态的无人机自动飞行控制方法 |
CN107270900A (zh) * | 2017-07-25 | 2017-10-20 | 广州阿路比电子科技有限公司 | 一种6自由度空间位置和姿态的检测系统和方法 |
CN107396051A (zh) * | 2017-08-09 | 2017-11-24 | 广州云从信息科技有限公司 | 一种采用无人机进行人脸识别监控的方法 |
CN107831515B (zh) * | 2017-10-10 | 2019-11-01 | 北京臻迪科技股份有限公司 | 水下定位方法和系统 |
CN109708636B (zh) * | 2017-10-26 | 2021-05-14 | 广州极飞科技股份有限公司 | 导航图配置方法、避障方法以及装置、终端、无人飞行器 |
CN111373336B (zh) * | 2017-11-25 | 2022-03-29 | 华为技术有限公司 | 状态感知方法以及相关设备 |
CN108152831B (zh) * | 2017-12-06 | 2020-02-07 | 中国农业大学 | 一种激光雷达障碍物识别方法及系统 |
CN108415453A (zh) * | 2018-01-24 | 2018-08-17 | 上海大学 | 基于bim技术的无人机隧道巡检方法 |
CN108319285A (zh) * | 2018-02-26 | 2018-07-24 | 厦门大学嘉庚学院 | 一种基于摄像头的四旋翼飞行器跟踪控制系统及方法 |
CN108399627B (zh) * | 2018-03-23 | 2020-09-29 | 云南大学 | 视频帧间目标运动估计方法、装置和实现装置 |
CN108917752B (zh) * | 2018-03-30 | 2022-11-11 | 深圳一清创新科技有限公司 | 无人船导航方法、装置、计算机设备和存储介质 |
CN108759822B (zh) * | 2018-04-12 | 2021-04-30 | 江南大学 | 一种移动机器人3d定位系统 |
KR102420568B1 (ko) * | 2018-04-27 | 2022-07-13 | 삼성전자주식회사 | 차량의 위치를 결정하는 방법 및 이를 위한 차량 |
CN108544494B (zh) * | 2018-05-31 | 2023-10-24 | 珠海一微半导体股份有限公司 | 一种基于惯性和视觉特征的定位装置、方法及机器人 |
CN108871311B (zh) * | 2018-05-31 | 2021-01-19 | 北京字节跳动网络技术有限公司 | 位姿确定方法和装置 |
CN108812599B (zh) * | 2018-06-13 | 2021-08-27 | 仲恺农业工程学院 | 一种用于手动操控的无人机植保监控系统及方法 |
CN108303099B (zh) * | 2018-06-14 | 2018-09-28 | 江苏中科院智能科学技术应用研究院 | 基于三维视觉slam的无人机室内自主导航方法 |
CN108804161B (zh) * | 2018-06-21 | 2022-03-04 | 北京字节跳动网络技术有限公司 | 应用的初始化方法、装置、终端和存储介质 |
CN108985193A (zh) * | 2018-06-28 | 2018-12-11 | 电子科技大学 | 一种基于图像检测的无人机航拍人像对准方法 |
CN110741625B (zh) * | 2018-07-23 | 2022-06-21 | 深圳市大疆创新科技有限公司 | 运动估计方法及摄影器材 |
CN109358638B (zh) * | 2018-09-10 | 2021-07-27 | 南京航空航天大学 | 基于分布式地图的无人机视觉避障方法 |
US10671064B2 (en) | 2018-09-17 | 2020-06-02 | Wing Aviation Llc | Unmanned aerial vehicle fleet management |
US10597157B1 (en) | 2018-09-17 | 2020-03-24 | Wing Aviation Llc | Unmanned aerial vehicle fleet management |
CN108829116B (zh) * | 2018-10-09 | 2019-01-01 | 上海岚豹智能科技有限公司 | 基于单目摄像头的避障方法及设备 |
CN111383324B (zh) * | 2018-12-29 | 2023-03-28 | 广州文远知行科技有限公司 | 点云地图的构建方法、装置、计算机设备和存储介质 |
WO2020142036A1 (en) * | 2018-12-31 | 2020-07-09 | Istanbul Sehir University | Indoor drone navigation support system based on machine learning |
CN109848988B (zh) * | 2019-01-24 | 2022-12-06 | 深圳市普森斯科技有限公司 | 一种基于历史多帧点云信息融合的扫描匹配方法及系统 |
CN109782806B (zh) * | 2019-02-19 | 2020-10-23 | 北京邮电大学 | 一种无人机室内路径跟踪方法及装置 |
CN109900266A (zh) * | 2019-03-27 | 2019-06-18 | 小驴机器人(武汉)有限公司 | 一种基于rgb-d和惯导的快速识别定位方式及系统 |
CN110118556A (zh) * | 2019-04-12 | 2019-08-13 | 浙江工业大学 | 一种基于协方差交叉融合slam的机器人定位方法及装置 |
CN110196602B (zh) * | 2019-05-08 | 2020-05-05 | 河海大学 | 目标导向集中优化的快速水下机器人三维路径规划方法 |
CN110632941B (zh) * | 2019-09-25 | 2020-12-15 | 北京理工大学 | 一种复杂环境下无人机目标跟踪的轨迹生成方法 |
CN110686687B (zh) * | 2019-10-31 | 2021-11-09 | 珠海市一微半导体有限公司 | 视觉机器人构建地图的方法、机器人和芯片 |
CN110986930B (zh) * | 2019-11-29 | 2022-07-15 | 北京三快在线科技有限公司 | 设备定位方法、装置、电子设备及存储介质 |
CN111080682B (zh) * | 2019-12-05 | 2023-09-01 | 北京京东乾石科技有限公司 | 点云数据的配准方法及装置 |
CN111198576A (zh) * | 2020-01-09 | 2020-05-26 | 哈尔滨工程大学 | 人工智能大数据下粒子化无人机组控制方法、介质及机组 |
CN111272172A (zh) * | 2020-02-12 | 2020-06-12 | 深圳壹账通智能科技有限公司 | 无人机室内导航方法、装置、设备和存储介质 |
CN111337021B (zh) * | 2020-03-19 | 2022-08-16 | 深圳国信泰富科技有限公司 | 一种基于复杂现场的机器人的定位系统及定位方法 |
CN111398522B (zh) * | 2020-03-24 | 2022-02-22 | 山东智翼航空科技有限公司 | 基于微型无人机的室内空气质量检测系统及检测方法 |
CN111580548B (zh) * | 2020-04-17 | 2021-10-19 | 中山大学 | 一种基于样条-rrt和速度障碍的无人机避障方法 |
CN111526478B (zh) * | 2020-04-29 | 2021-07-09 | 成都理工大学 | 一种山地景区复杂地形无人机协同搜救系统及其装置 |
CN111678511B (zh) * | 2020-04-29 | 2022-04-08 | 北京牡丹电子集团有限责任公司 | 一种机器人的多传感器融合定位方法和系统 |
CN114521836B (zh) * | 2020-08-26 | 2023-11-28 | 北京石头创新科技有限公司 | 一种自动清洁设备 |
WO2022077296A1 (zh) * | 2020-10-14 | 2022-04-21 | 深圳市大疆创新科技有限公司 | 三维重建方法、云台负载、可移动平台以及计算机可读存储介质 |
US11905014B2 (en) | 2020-10-21 | 2024-02-20 | Wing Aviation Llc | Terminal area operation of UAVs for package delivery system |
CN112434709B (zh) * | 2020-11-20 | 2024-04-12 | 西安视野慧图智能科技有限公司 | 基于无人机实时稠密三维点云和dsm的航测方法及系统 |
CN113720331B (zh) * | 2020-12-25 | 2023-12-19 | 北京理工大学 | 多相机融合的无人机建筑物内导航定位方法 |
CN112965531A (zh) * | 2021-03-04 | 2021-06-15 | 中国矿业大学 | 一种用于煤矿采空区无人探测的微小型飞行器及其方法 |
CN114445491A (zh) * | 2021-12-17 | 2022-05-06 | 广州欧科信息技术股份有限公司 | 室内定位方法、终端以及存储介质 |
CN114054302B (zh) * | 2022-01-17 | 2022-03-25 | 南通西屋智能科技有限公司 | 一种点胶机的精准定位控制方法及系统 |
CN114964265B (zh) * | 2022-07-19 | 2022-10-25 | 山东亿华天产业发展集团有限公司 | 一种用于微型无人机的室内自主导航系统及导航方法 |
CN116048120B (zh) * | 2023-01-10 | 2024-04-16 | 中国建筑一局(集团)有限公司 | 一种未知动态环境下小型四旋翼无人机自主导航系统及方法 |
CN117128861A (zh) * | 2023-10-23 | 2023-11-28 | 常州市建筑材料研究所有限公司 | 一种去测站化三维激光扫描桥梁监测系统及监测方法 |
CN117911829B (zh) * | 2024-03-15 | 2024-05-31 | 山东商业职业技术学院 | 车辆导航的点云图像融合方法及系统 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102707708A (zh) * | 2012-05-25 | 2012-10-03 | 清华大学 | 多模型飞行控制系统故障诊断方法及装置 |
US8296063B1 (en) * | 2009-05-04 | 2012-10-23 | Exelis Inc. | Emergency rescue system and method having video and IMU data synchronization |
CN103925920A (zh) * | 2014-04-10 | 2014-07-16 | 西北工业大学 | 一种基于图像透视的微型无人机室内自主导航方法 |
CN104035091A (zh) * | 2014-06-11 | 2014-09-10 | 江苏数字鹰科技发展有限公司 | 室内无人机定位系统 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2011149544A1 (en) * | 2010-05-26 | 2011-12-01 | Aerovironment Inc. | Reconfigurable battery-operated vehicle system |
-
2014
- 2014-09-12 CN CN201410466305.1A patent/CN104236548B/zh not_active Expired - Fee Related
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8296063B1 (en) * | 2009-05-04 | 2012-10-23 | Exelis Inc. | Emergency rescue system and method having video and IMU data synchronization |
CN102707708A (zh) * | 2012-05-25 | 2012-10-03 | 清华大学 | 多模型飞行控制系统故障诊断方法及装置 |
CN103925920A (zh) * | 2014-04-10 | 2014-07-16 | 西北工业大学 | 一种基于图像透视的微型无人机室内自主导航方法 |
CN104035091A (zh) * | 2014-06-11 | 2014-09-10 | 江苏数字鹰科技发展有限公司 | 室内无人机定位系统 |
Non-Patent Citations (2)
Title |
---|
依靠自身传感器的室内无人机自主导航引导技术综述;倪磊等;《计算机应用于软件》;20120831;第29卷(第8期);第160-163、166页 * |
单目视觉/惯性室内无人机自主导航算法研究;庄曈;《中国优秀硕士学位论文全文数据库工程科技Ⅱ辑》;20130225(第2期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN104236548A (zh) | 2014-12-24 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN104236548B (zh) | 一种微型无人机室内自主导航方法 | |
Panahandeh et al. | Vision-aided inertial navigation based on ground plane feature detection | |
CN111288989B (zh) | 一种小型无人机视觉定位方法 | |
CN109084732A (zh) | 定位与导航方法、装置及处理设备 | |
CN113781582A (zh) | 基于激光雷达和惯导联合标定的同步定位与地图创建方法 | |
CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
CN107966989A (zh) | 一种机器人自主导航系统 | |
CN111338383B (zh) | 基于gaas的自主飞行方法及系统、存储介质 | |
Bryson et al. | Airborne smoothing and mapping using vision and inertial sensors | |
Wulf et al. | Benchmarking urban six‐degree‐of‐freedom simultaneous localization and mapping | |
CN104280022A (zh) | 视觉辅助惯性测量单元的数字头盔显示设备跟踪系统 | |
Daftry et al. | Robust monocular flight in cluttered outdoor environments | |
CN111474932B (zh) | 一种集成情景经验的移动机器人建图与导航方法 | |
CN109781092A (zh) | 一种危险化工事故中移动机器人定位与建图方法 | |
CN109282808A (zh) | 用于桥梁三维巡航检测的无人机与多传感器融合定位方法 | |
CN115574816B (zh) | 仿生视觉多源信息智能感知无人平台 | |
CN114111818B (zh) | 一种通用视觉slam方法 | |
Ellekilde et al. | Dense 3D map construction for indoor search and rescue | |
Li et al. | Autonomous navigation and environment modeling for MAVs in 3-D enclosed industrial environments | |
Karam et al. | Integrating a low-cost mems imu into a laser-based slam for indoor mobile mapping | |
Rahman et al. | Contour based reconstruction of underwater structures using sonar, visual, inertial, and depth sensor | |
Mebarki et al. | Image moments-based velocity estimation of UAVs in GPS denied environments | |
Liu et al. | A multi-sensor fusion with automatic vision-LiDAR calibration based on Factor graph joint optimization for SLAM | |
Giubilato et al. | Minivo: Minimalistic range enhanced monocular system for scale correct pose estimation | |
Hu et al. | A small and lightweight autonomous laser mapping system without GPS |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20170405 |
|
CF01 | Termination of patent right due to non-payment of annual fee |