CN111199578B - 基于视觉辅助激光雷达的无人机三维环境建模方法 - Google Patents

基于视觉辅助激光雷达的无人机三维环境建模方法 Download PDF

Info

Publication number
CN111199578B
CN111199578B CN201911425909.0A CN201911425909A CN111199578B CN 111199578 B CN111199578 B CN 111199578B CN 201911425909 A CN201911425909 A CN 201911425909A CN 111199578 B CN111199578 B CN 111199578B
Authority
CN
China
Prior art keywords
laser radar
odometer
relative pose
unmanned aerial
binocular camera
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
CN201911425909.0A
Other languages
English (en)
Other versions
CN111199578A (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.)
Nanjing Changkong Technology Co ltd
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing Changkong Technology Co ltd
Nanjing University of Aeronautics and Astronautics
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 Nanjing Changkong Technology Co ltd, Nanjing University of Aeronautics and Astronautics filed Critical Nanjing Changkong Technology Co ltd
Priority to CN201911425909.0A priority Critical patent/CN111199578B/zh
Publication of CN111199578A publication Critical patent/CN111199578A/zh
Application granted granted Critical
Publication of CN111199578B publication Critical patent/CN111199578B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/10—Segmentation; Edge detection
    • G06T7/13—Edge detection
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration

Abstract

本发明公开了一种基于视觉辅助激光雷达的无人机三维环境建模方法，包括基于张正友标定法对双目相机进行标定，然后采用基于3D marker方法对相机/激光雷达进行外参标定方法，最后将双目视觉里程计作为激光雷达里程计的高频辅助，为激光雷达里程计提供局外点剔除和相对位姿初值，激光雷达里程计低频运行，利用双目视觉里程计提供的相对位姿初值对运动进行进一步的优化。本发明实现了无人机飞行环境的高精度建模，既能为无人机实现自主避障奠定基础，从而保障无人机安全飞行，又为情报和资源信息的获取提供可靠渠道，提高作战效能。

Description

基于视觉辅助激光雷达的无人机三维环境建模方法
技术领域
本发明涉及无人机图像处理与控制领域，具体涉及基于视觉辅助激光雷达的无人机三维环境建模方法。
背景技术
多无人机编队协同作战模式能够充分发挥作战效能，提升战斗力，逐渐成为当今研究的热点。据称，美国国防高级研究计划局已拟定于2025年推出可应用于实战的高智能化无人机。智能化无人机编队要求无人机能够对于飞行路径上可能遇到的障碍物进行实时分辨和监测，从而制定出合理的避让策略，并最终完成既定任务，如运送物资、探测地形和打击目标等。因而，无人机必须具备对周围环境的三维建模的能力。为了在未来战争中抢占先机，研究具有我国自主知识产权的无人机三维环境建模系统具有很强的必要性以及重大意义。
国内外多家研究机构针对无人机的三维环境重构进行了大量研究，并取得了一定成果。国外有机构在无人直升机上安装了机器人用二维激光扫描仪实现场景重构；也有人提出一种利用基于光流场的FOE(Focus Of Expansion)方法实现无人机在类似山谷地形地带的飞行。国内有机构采用基于惯导和单目摄像机的进行无人机自动着陆环境的构建。但是不管是视觉传感器还是激光传感器，都有其自身的优势和局限性。与激光传感器相比，双目视觉传感器存在以下优点：1)图像特征点的选取策略更加严格，ORB(Oriented Fast andRotated Brief，快速特征点提取和描述算法)特征的尺度不变性以及旋转不变性使得视觉特征点对场景的适应性和鲁棒性更好；2)采用RANSAC(Random Sample Consensus)算法使得错误匹配点的数量大大降低，可以为非线性优化部分提供更接近真值的运动估计初值；3)非线性优化中同时考虑了相对位姿和特征点的优化，相较于激光传感器只考虑相对位姿优化的模型更优。但双目视觉传感器也存在图像精度有限、深度方向误差随距离逐渐增大导致深度感知距离有限、在特征点缺失的场景下无法工作等缺点。而激光传感器的图像特征点选取策略较简单，可能存在较多误匹配点，但其测量精度高、深度观测范围广，与视觉传感器存在很强的互补性。
综上所述，通过对激光雷达、视觉与惯性器件的信息进行有效融合，将双目视觉传感器作为激光传感器的高频辅助，为激光传感器提供局外点剔除和相对位姿初值，激光传感器低频运行，利用双目视觉传感器提供的相对位姿初值对运动进行进一步的优化，可以改善环境建模的精度和可靠性。
发明内容
本发明针对现有技术中的不足，提供一种基于视觉辅助激光雷达的无人机三维环境建模方法。
为实现上述目的，本发明采用以下技术方案：
基于视觉辅助激光雷达的无人机三维环境建模方法，其特征在于：无人机上搭载有激光雷达、双目相机和计算机，激光雷达提供环境的激光点云信息，双目相机提供环境的双目图像信息；计算机通过对激光雷达、双目相机提供的信息进行有效融合，进行基于视觉辅助的激光雷达里程计设计，将双目相机作为激光雷达的高频辅助，为激光雷达提供局外点剔除和相对位姿初值，激光雷达低频运行，利用双目相机提供的相对位姿初值对运动进行进一步的优化，实现无人机环境三维模型构建。
为优化上述技术方案，采取的具体措施还包括：
进一步地，所述基于视觉辅助的激光雷达里程计设计之前，首先进行双目相机标定，并且对双目相机与激光雷达之间的外参进行标定。
进一步地，所述双目相机标定，具体包括如下步骤：
1)采用基于张正友标定法对左相机和右相机各自内参矩阵K进行标定；
2)对左右相机径向畸变系数k1，k2，k3和切向畸变系数p1，p2进行标定；
3)对左右相机的相对位姿关系进行标定，确定双目基线的长度；
4)得到相机归一化平面上的点坐标的畸变矫正公式如下：
Figure BDA0002351086640000021
其中，
Figure BDA0002351086640000022
为畸变矫正前的坐标，x，y为矫正后的坐标，r为点与坐标原点的距离。
进一步地，步骤1)具体如下：
a)使用一张7×9的棋盘格作为标定板，正方形格子的边长为108mm；
b)对不同角度、不同位置下的标定板进行图像采集；
c)通过提取标定板中的角点并利用投影关系构造约束方程；
d)利用最小二乘方法估计相机的内参矩阵K。
进一步地，所述对双目相机与激光雷达之间的外参进行标定，是采用基于3Dmarker的方法对双目相机与激光雷达之间的存在位置和角度差异
Figure BDA0002351086640000023
和
Figure BDA0002351086640000024
进行标定。
进一步地，所述对双目相机与激光雷达之间的外参进行标定，具体包括如下步骤：
1)选取一块具有镂空的四个大小相同圆形的硬纸板，其中四个圆作为3D marker，四个圆心连线构成正方形，正方形的边长和圆形的半径皆为已知参数；
2)激光雷达和双目相机分别对硬纸板进行检测，利用激光雷达得到的点云深度信息的不连续对圆形边缘进行提取，双目相机得到的图像则使用基于sobel算子的边缘检测和基于Hough变换的圆检测方法，标定算法利用3D marker已知的尺寸信息和RANSAC算法保证激光雷达和双目相机边缘提取的正确性；
3)计算
Figure BDA0002351086640000031
具体如下：
a)双目相机和激光雷达分别检测出3D marker后，先假设
Figure BDA0002351086640000032
为单位矩阵；
b)通过边缘匹配粗略地估计出双目相机与激光雷达的平移参数
Figure BDA0002351086640000033
c)通过重投影误差进一步优化
Figure BDA0002351086640000034
并计算
Figure BDA0002351086640000035
4)将激光雷达系下的点pL转换到左相机像素坐标系下的点pp，如下式：
Figure BDA0002351086640000036
其中，K为左相机内参矩阵，
Figure BDA0002351086640000037
为常值矩阵，u为x轴像素坐标的齐次形式，v为y轴像素坐标的齐次形式，u0为x轴像素坐标的齐次形式的初值，v0为y轴像素坐标的齐次形式的初值，(u0，v0)构成了像素点齐次坐标原点；
Figure BDA0002351086640000038
其中k为像素点的长，l为像素点的宽，(k，l)构成了像素点的物理尺寸。
进一步地，所述基于视觉辅助的激光雷达里程计设计，具体包括如下步骤：
1)设定双目相机视觉里程计的运行频率为40Hz，激光雷达里程计的运行频率为10Hz；
2)同步激光雷达与视觉里程计的到达时间，具体如下：
a)记录下激光雷达一帧扫描的开始时刻tk，结束时刻tk+1，ti～ti+5为视觉里程计连续的六个时刻，其中，tk对应时刻ti～ti+1之间，tk+1对应时刻ti+4～ti+5之间；
b)激光雷达与视觉里程计到达时间同步处理：利用视觉里程计ti～ti+1输出的相对位姿
Figure BDA0002351086640000039
和线性插值法求出视觉里程计tk～ti+1输出的相对位姿
Figure BDA00023510866400000310
以及视觉里程计ti+4～tk+1输出的相对位姿
Figure BDA00023510866400000311
3)将激光雷达里程计一个运行周期内的五帧相关的视觉里程计输出的相对位姿累积起来；
4)利用外参标定得到的外参矩阵
Figure BDA00023510866400000312
与
Figure BDA00023510866400000313
将视觉里程计输出的相对位姿转换到激光雷达系下，如下式所示：
Figure BDA0002351086640000041
式中，
Figure BDA0002351086640000042
表示激光雷达里程计的相对位姿估计初值，
Figure BDA0002351086640000043
表示激光雷达里程计输出的相对位姿，
Figure BDA0002351086640000044
表示视觉里程计输出的相对位姿，
Figure BDA0002351086640000045
分别表示视觉里程计在ti+4～tk+1、ti+3～ti+4、ti+2～ti+3、ti+1～ti+2、tk～ti+1输出的相对位姿；
5)将
Figure BDA0002351086640000046
作为激光雷达里程计的相对位姿估计初值，用非线性优化方法直接估计
Figure BDA0002351086640000047
与相对位姿真值
Figure BDA0002351086640000048
的差值
Figure BDA0002351086640000049
如下式所示：
Figure BDA00023510866400000410
式中，
Figure BDA00023510866400000411
对应优化初值的欧拉角和平移量均设置为零向量；
6)用
Figure BDA00023510866400000412
辅助激光雷达局外点的剔除，剔除策略以匹配误差为衡量标准；假设一对匹配点为(pk，pi)，分别为激光雷达在tk和ti时刻扫描到的点，ti∈[tk，tk+1)，tk+1是激光雷达一帧扫描的结束时刻，若该匹配点满足下式，则认为通过筛选：
Figure BDA00023510866400000413
其中，
Figure BDA00023510866400000414
为tk到ti的相对位姿转换矩阵，利用
Figure BDA00023510866400000415
通过线性插值给出，η为设定的匹配误差阈值；
7)利用通过局外点筛选的激光雷达匹配点对进行非线性优化估计
Figure BDA00023510866400000416
再根据式(4)进行位姿融合，得到最终的相对位姿真值
Figure BDA00023510866400000417
用于后续无人机环境三维模型构建。
本发明的有益效果是：本发明的基于视觉辅助激光雷达的无人机三维环境建模方法，首先采用基于张正友标定法对双目相机进行标定，然后采用基于3D marker方法对相机/激光雷达进行外参标定方法，最后将双目视觉里程计作为激光雷达里程计的高频辅助，为激光雷达里程计提供局外点剔除和相对位姿初值，激光雷达里程计低频运行，利用双目视觉里程计提供的相对位姿初值对运动进行进一步的优化。本发明实现了无人机飞行环境的高精度建模，既能为无人机实现自主避障奠定基础，从而保障无人机安全飞行，又为情报和资源信息的获取提供可靠渠道，提高作战效能。
附图说明
图1是本发明具体实施例基于视觉辅助激光雷达的无人机三维环境建模系统的设计框图。
图2是本发明具体实施例双目相机标定重投影误差直方图。
图3是本发明具体实施例双目相机3D marker检测结果图。
图4是本发明具体实施例激光雷达3D marker检测结果图。
图5是本发明具体实施例视觉辅助激光雷达里程计方案图。
图6是本发明具体实施例激光雷达与视觉里程计到达时间示意图。
图7是本发明具体实施例试验场景示意图。
图8是本发明具体实施例基于视觉辅助的激光雷达环境建模轨迹对比图。
具体实施方式
现在结合附图对本发明作进一步详细的说明。
基于视觉辅助激光雷达的无人机三维环境建模系统的设计思想：如图1所示，设计的无人机三维环境建模系统通过传感器采集相应数据，传至机载计算机，在机载计算机上进行相应传感器数据的同步采集和处理，运行相关算法软件程序，从而实现三维环境建模、自主避障及自主飞行等功能。其中激光雷达提供的激光点云信息，双目相机提供的双目图像信息，分别对双目相机和激光雷达进行标定；机载计算机通过对激光雷达、双目相机提供的信息进行有效融合，进行基于视觉辅助的激光雷达里程计设计，将双目相机作为激光雷达的高频辅助，为激光雷达提供局外点剔除和相对位姿初值，激光雷达低频运行，利用双目相机提供的相对位姿初值对运动进行进一步的优化，实现无人机环境三维模型构建；惯性传感器提供无人机姿态和位置信息，在此辅助下实现无人机的自主避障飞行。
所选取的激光雷达、惯性传感器、双目相机、机载计算机具体型号和性能说明如下：
(1)激光雷达传感器
所选取的激光雷达传感器为Velodyne公司生产的小型三维激光雷达VLP-16。其最远测量距离可达100米，重量为830g，输出频率范围为5～20Hz，每秒可输出多达30万个点，利用100Mbps以太网连接实现点云数据传输。VLP-16具有360°水平视场，并通过16个垂直均匀分布的激光发射器实现±15°的垂直视场。此外，VLP-16的环境防护等级为IP67，可抗2Hz～2000Hz 3Grms的振动，可适应飞行器的机身振动。
(2)惯性传感器
所选取的惯性传感器为Xsens Technologies公司生产的MTi-G-710惯性GPS/INS组合航姿系统。MTi-G-710集成了陀螺仪、加速度计、磁力计、气压计和多GNSS接收器，并配有GNSS接收天线。数据最大输出频率可以达到400Hz，环境防护等级为IP67。
(3)视觉传感器
所选取的视觉传感器为StereoLabs生产的ZED双目相机，其输出图像分辨率可高达2.2K，其他可选输出分辨率为1080P、720P、WVGA，其中WVGA分辨率下的输出频率可高达100Hz。
(4)机载计算机
所选取的机载计算机为Nvidia生产的Jetson TX2开发板。TX2的CPU部分由两个ARM v864位CPU集群组成，包括danver两核CPU集群和ARM Cortex-A57四核CPU，GPU由基于Pascal架构的256个CUDA核组成，运行内存为8G。内置操作系统为基于Linux的Ubuntu16.04。在实时图像处理中，可以通过CUDA实现GPU对图像加工、视觉里程计解算的加速。
基于视觉辅助激光雷达的无人机三维环境建模方法，其特征在于，包括如下步骤：
步骤S1，双目相机标定；
1)采用基于张正友标定法对左相机和右相机各自内参矩阵K也即相机系与像素坐标系的转换矩阵进行标定；
a)使用一张7×9的棋盘格作为标定板，正方形格子的边长为108mm；
b)对不同角度、不同位置下的标定板进行图像采集；
c)通过提取标定板中的角点并利用投影关系构造约束方程；
d)无线电高度表测得的相对高度小于等于截获高度利用最小二乘方法估计相机的内参矩阵K。
2)对左右相机径向畸变系数k1，k2，k3和切向畸变系数p1，p2进行标定；
3)对左右相机的相对位姿关系进行标定，确定双目基线的长度也即两相机光心之间的距离
4)得到相机归一化平面上的点坐标的畸变矫正公式如下：
Figure BDA0002351086640000061
其中，
Figure BDA0002351086640000062
为畸变矫正前的坐标，x，y为矫正后的坐标，r为点与坐标原点即光心的距离。
表1给出来双目相机的标定结果，图2给出了标定图像的重投影误差统计直方图，标定平均误差为0.33像素。
表1 ZED双目相机标定结果
Figure BDA0002351086640000063
Figure BDA0002351086640000071
步骤S2，采用基于3D marker的方法对双目相机与激光雷达之间的外参即双目相机和激光雷达之间存在位置和角度差异
Figure BDA0002351086640000072
和
Figure BDA0002351086640000073
进行标定；
1)选取一块具有镂空的四个大小相同圆形的硬纸板，其中四个圆的圆心构成正方形，正方形的边长和圆形的半径皆为已知参数；
2)利用点云深度信息的不连续对圆形边缘进行提取，图像则使用基于sobel算子的边缘检测和基于Hough变换的圆检测方法，标定算法利用3D marker已知的尺寸信息和RANSAC算法保证激光雷达和相机边缘提取的正确性。
相机和激光雷达的检测图像分别如如图3、图4所示。
3)计算
Figure BDA0002351086640000074
a)相机和激光雷达分别检测出3D marker后，先假设
Figure BDA0002351086640000075
为单位矩阵；
b)通过边缘匹配粗略地估计出相机与激光雷达的平移参数
Figure BDA0002351086640000076
c)通过重投影误差进一步优化
Figure BDA0002351086640000077
并计算
Figure BDA0002351086640000078
4)将激光雷达系下的点pL转换到左相机像素坐标系下的点pp，如下式：
Figure BDA0002351086640000079
其中，K为左相机内参矩阵，
Figure BDA00023510866400000710
为常值矩阵，u为x轴像素坐标的齐次形式，v为y轴像素坐标的齐次形式，u0为x轴像素坐标的齐次形式的初值，v0为y轴像素坐标的齐次形式的初值，(u0，v0)构成了像素点齐次坐标原点；
Figure BDA0002351086640000081
其中k为像素点的长，l为像素点的宽，(k，l)构成了像素点的物理尺寸。
标定结果如表2所示，其中
Figure BDA0002351086640000082
用欧拉角表示。通过人工测量z轴平移量验证了标定结果的正确性。
表2 相机/激光雷达外参标定结果
Figure BDA0002351086640000083
步骤S3，基于视觉辅助的激光雷达里程计设计：
将双目相机的视觉里程计作为激光雷达里程计的高频辅助，为激光雷达里程计提供局外点剔除和相对位姿初值，激光雷达里程计低频运行，利用双目视觉里程计提供的相对位姿初值对运动进行进一步的优化，如图5所示。
1)设定视觉里程计的运行频率为40Hz，激光雷达里程计的运行频率为10Hz；
由于传感器采集时间难以精确同步和视觉解算延时等因素的影响，激光雷达信息与视觉里程计信息到达时刻往往不同步，如图6所示。其中k，k+1对应激光雷达一帧的开始和结束时刻，i、i+1、i+2、i+3、i+4、i+5为各帧视觉里程计信息到达的时间节点。
2)同步激光雷达与视觉里程计的到达时间：
a)记录下激光雷达一帧扫描的开始时刻tk，结束时刻tk+1，ti～ti+5为视觉里程计连续的六个时刻，其中，tk对应时刻ti～ti+1之间，tk+1对应时刻ti+4～ti+5之间；
b)激光雷达与视觉里程计到达时间同步处理：利用视觉里程计ti～ti+1输出的相对位姿
Figure BDA0002351086640000084
和线性插值法求出视觉里程计tk～ti+1输出的相对位姿
Figure BDA0002351086640000085
以及视觉里程计ti+4～tk+1输出的相对位姿
Figure BDA0002351086640000086
3)将激光雷达里程计一个运行周期内的5帧相关的视觉里程计解算位姿累积起来；
4)利用外参标定得到的外参矩阵将相对位姿转换到激光雷达系下，如下式所示：
Figure BDA0002351086640000087
式中，
Figure BDA0002351086640000088
表示激光雷达里程计的相对位姿估计初值，
Figure BDA0002351086640000089
表示激光雷达里程计输出的相对位姿，
Figure BDA00023510866400000810
表示视觉里程计输出的相对位姿，
Figure BDA00023510866400000811
分别表示视觉里程计在ti+4～tk+1、ti+3～ti+4、ti+2～ti+3、ti+1～ti+2、tk～ti+1输出的相对位姿。
5)将
Figure BDA0002351086640000091
作为激光雷达里程计的相对位姿估计初值，用非线性优化方法直接估计
Figure BDA0002351086640000092
与相对位姿真值
Figure BDA0002351086640000093
的差值
Figure BDA0002351086640000094
如下式所示：
Figure BDA0002351086640000095
其中，由于视觉里程计较为严格的特征提取策略以及RANSAC局外点剔除算法，
Figure BDA0002351086640000096
已经较接近真值
Figure BDA0002351086640000097
因此
Figure BDA0002351086640000098
对应优化初值的欧拉角和平移量均可设置为零向量。
6)用视觉里程计输出的
Figure BDA0002351086640000099
辅助激光雷达局外点的剔除，剔除策略以匹配误差为衡量标准。假设一对匹配点为(pk，pi)，分别为激光雷达在tk和ti时刻扫描到的点，ti∈[tk，tk+1)，若该匹配点满足下式，则认为通过筛选。
Figure BDA00023510866400000910
其中，
Figure BDA00023510866400000911
为tk到ti的相对位姿转换矩阵，可以利用
Figure BDA00023510866400000912
通过线性插值给出，η为设定的匹配误差阈值。
7)利用通过局外点筛选的激光雷达匹配点对进行非线性优化估计
Figure BDA00023510866400000913
再根据式(4)进行位姿融合，得到最终的相对位姿真值
Figure BDA00023510866400000914
本发明实施动态试验对基于视觉辅助的激光雷达实时环境建模算法进行验证。动态试验在室外篮球场进行，篮球场有较多活动人员，试验场景如图7所示。运动轨迹为一个闭合的四边形，通过曲线的闭合判断误差。图8显示了使用激光雷达、基于视觉辅助的激光雷达以及视觉里程计生成的二维轨迹，图中标注了起终点以及运动的方向，并给出起终点的局部放大图。
图8中，三条线分别代表无视觉辅助的激光雷达环境建模结果，视觉辅助下的激光雷达环境建模结果，视觉里程计解算结果。由图8可知，视觉里程计由于存在不可避免的累积误差，整体轨迹歪斜，起终点不重合。激光雷达环境建模由于受到场景中大量活动人员的干扰，也出现了起终点不重合的情况，而视觉辅助的激光雷达环境建模由于利用了视觉里程计的相对位姿辅助进行运动估计和局外点剔除，不受视觉里程计长期的误差累积影响，又结合了激光雷达后端优化进一步优化位姿的优势，起终点几乎完全重合。由于准备原因，起终点均停留了一段时间，因此将开始时5s与最后5s的平均位置差绝对值作为起终点的位置误差，统计结果如表3所示。
表3 基于视觉辅助的激光雷达环境建模起终点位置误差统计
项目 x轴(m) y轴(m) z轴(m)
VO 2.0499 0.1038 0.3619
无VO辅助 0.7725 1.0466 0.0669
VO辅助 0.0944 0.0346 0.0418
由表3可知，基于视觉辅助的激光雷达环境建模位置精度有较大的提升，三轴位置的起终点误差均在10cm以下。
需要注意的是，发明中所引用的如“上”、“下”、“左”、“右”、“前”、“后”等的用语，亦仅为便于叙述的明了，而非用以限定本发明可实施的范围，其相对关系的改变或调整，在无实质变更技术内容下，当亦视为本发明可实施的范畴。
以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims (5)

1.基于视觉辅助激光雷达的无人机三维环境建模方法，其特征在于：无人机上搭载有激光雷达、双目相机和计算机，激光雷达提供环境的激光点云信息，双目相机提供环境的双目图像信息；计算机通过对激光雷达、双目相机提供的信息进行有效融合，进行基于视觉辅助的激光雷达里程计设计，将双目相机作为激光雷达的高频辅助，为激光雷达提供局外点剔除和相对位姿初值，激光雷达低频运行，利用双目相机提供的相对位姿初值对运动进行进一步的优化，实现无人机环境三维模型构建；
所述基于视觉辅助的激光雷达里程计设计之前，首先进行双目相机标定，并且对双目相机与激光雷达之间的外参进行标定；所述对双目相机与激光雷达之间的外参进行标定，是采用基于3D marker的方法对双目相机与激光雷达之间的存在位置和角度差异
Figure FDA0003434553370000011
和
Figure FDA0003434553370000012
进行标定；
所述基于视觉辅助的激光雷达里程计设计，具体包括如下步骤：
S31：设定双目相机视觉里程计的运行频率为40Hz，激光雷达里程计的运行频率为10Hz；
S32：同步激光雷达与视觉里程计的到达时间，具体如下：
a)记录下激光雷达一帧扫描的开始时刻tk，结束时刻tk+1，ti～ti+5为视觉里程计连续的六个时刻，其中，tk对应时刻ti～ti+1之间，tk+1对应时刻ti+4～ti+5之间；
b)激光雷达与视觉里程计到达时间同步处理：利用视觉里程计ti～ti+1输出的相对位姿
Figure FDA0003434553370000013
和线性插值法求出视觉里程计tk～ti+1输出的相对位姿
Figure FDA0003434553370000014
以及视觉里程计ti+4～tk+1输出的相对位姿
Figure FDA0003434553370000015
S33：将激光雷达里程计一个运行周期内的五帧相关的视觉里程计输出的相对位姿累积起来；
S34：利用外参标定得到的外参矩阵
Figure FDA0003434553370000016
与
Figure FDA0003434553370000017
将视觉里程计输出的相对位姿转换到激光雷达系下，如下式所示：
Figure FDA0003434553370000018
式中，
Figure FDA0003434553370000019
表示激光雷达里程计的相对位姿估计初值，
Figure FDA00034345533700000110
表示激光雷达里程计输出的相对位姿，
Figure FDA00034345533700000111
表示视觉里程计输出的相对位姿，
Figure FDA00034345533700000112
分别表示视觉里程计在ti+4～tk+1、ti+3～ti+4、ti+2～ti+3、ti+1～ti+2、tk～ti+1输出的相对位姿；
S35：将
Figure FDA00034345533700000113
作为激光雷达里程计的相对位姿估计初值，用非线性优化方法直接估计
Figure FDA00034345533700000114
与相对位姿真值
Figure FDA00034345533700000115
的差值
Figure FDA00034345533700000116
如下式所示：
Figure FDA0003434553370000021
式中，
Figure FDA0003434553370000022
对应优化初值的欧拉角和平移量均设置为零向量；
S36：用
Figure FDA0003434553370000023
辅助激光雷达局外点的剔除，剔除策略以匹配误差为衡量标准；假设一对匹配点为(pk,pi)，分别为激光雷达在tk和ti时刻扫描到的点，ti∈[tk,tk+1)，tk+1是激光雷达一帧扫描的结束时刻，若该匹配点满足下式，则认为通过筛选：
Figure FDA0003434553370000024
其中，
Figure FDA0003434553370000025
为tk到ti的相对位姿转换矩阵，利用
Figure FDA0003434553370000026
通过线性插值给出，η为设定的匹配误差阈值；
S37：利用通过局外点筛选的激光雷达匹配点对进行非线性优化估计
Figure FDA0003434553370000027
再根据式(4)进行位姿融合，得到最终的相对位姿真值
Figure FDA0003434553370000028
用于后续无人机环境三维模型构建。
2.如权利要求1所述的基于视觉辅助激光雷达的无人机三维环境建模方法，其特征在于：所述双目相机标定，具体包括如下步骤：
S11：采用基于张正友标定法对左相机和右相机各自内参矩阵K进行标定；
S12：对左右相机径向畸变系数k1，k2，k3和切向畸变系数p1,p2进行标定；
S13：对左右相机的相对位姿关系进行标定，确定双目基线的长度；
S14：得到相机归一化平面上的点坐标的畸变矫正公式如下：
Figure FDA0003434553370000029
其中，
Figure FDA00034345533700000210
为畸变矫正前的坐标，x，y为矫正后的坐标，r为点与坐标原点的距离。
3.如权利要求2所述的基于视觉辅助激光雷达的无人机三维环境建模方法，其特征在于：步骤S11具体如下：
a)使用一张7×9的棋盘格作为标定板，正方形格子的边长为108mm；
b)对不同角度、不同位置下的标定板进行图像采集；
c)通过提取标定板中的角点并利用投影关系构造约束方程；
d)利用最小二乘方法估计相机的内参矩阵K。
4.如权利要求1所述的基于视觉辅助激光雷达的无人机三维环境建模方法，其特征在于：所述对双目相机与激光雷达之间的外参进行标定，是采用基于3D marker的方法对双目相机与激光雷达之间的存在位置和角度差异
Figure FDA0003434553370000031
和
Figure FDA0003434553370000032
进行标定。
5.如权利要求4所述的基于视觉辅助激光雷达的无人机三维环境建模方法，其特征在于：所述对双目相机与激光雷达之间的外参进行标定，具体包括如下步骤：
S21：选取一块具有镂空的四个大小相同圆形的硬纸板，其中四个圆作为3D marker，四个圆心连线构成正方形，正方形的边长和圆形的半径皆为已知参数；
S22：激光雷达和双目相机分别对硬纸板进行检测，利用激光雷达得到的点云深度信息的不连续对圆形边缘进行提取，双目相机得到的图像则使用基于sobel算子的边缘检测和基于Hough变换的圆检测方法，标定算法利用3D marker已知的尺寸信息和RANSAC算法保证激光雷达和双目相机边缘提取的正确性；
S23：计算
Figure FDA0003434553370000033
具体如下：
a)双目相机和激光雷达分别检测出3D marker后，先假设
Figure FDA0003434553370000034
为单位矩阵；
b)通过边缘匹配粗略地估计出双目相机与激光雷达的平移参数
Figure FDA0003434553370000035
c)通过重投影误差进一步优化
Figure FDA0003434553370000036
并计算
Figure FDA0003434553370000037
S24：将激光雷达系下的点pL转换到左相机像素坐标系下的点pp，如下式：
Figure FDA0003434553370000038
其中，K为左相机内参矩阵，
Figure FDA0003434553370000039
为常值矩阵，u为x轴像素坐标的齐次形式，v为y轴像素坐标的齐次形式，u0为x轴像素坐标的齐次形式的初值，v0为y轴像素坐标的齐次形式的初值，(u0,v0)构成了像素点齐次坐标原点；
Figure FDA00034345533700000310
其中k为像素点的长，l为像素点的宽，(k,l)构成了像素点的物理尺寸。
CN201911425909.0A 2019-12-31 2019-12-31 基于视觉辅助激光雷达的无人机三维环境建模方法 Active CN111199578B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911425909.0A CN111199578B (zh) 2019-12-31 2019-12-31 基于视觉辅助激光雷达的无人机三维环境建模方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911425909.0A CN111199578B (zh) 2019-12-31 2019-12-31 基于视觉辅助激光雷达的无人机三维环境建模方法

Publications (2)

Publication Number Publication Date
CN111199578A CN111199578A (zh) 2020-05-26
CN111199578B true CN111199578B (zh) 2022-03-15

Family

ID=70746663

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911425909.0A Active CN111199578B (zh) 2019-12-31 2019-12-31 基于视觉辅助激光雷达的无人机三维环境建模方法

Country Status (1)

Country Link
CN (1) CN111199578B (zh)

Families Citing this family (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111784778B (zh) * 2020-06-04 2022-04-12 华中科技大学 基于线性求解非线性优化的双目相机外参标定方法和系统
CN112083433B (zh) * 2020-07-21 2023-06-13 浙江工业大学 一种应用于两轮移动机器人的激光雷达除畸变方法
CN114088131A (zh) * 2020-08-24 2022-02-25 北京市安全生产科学技术研究院 监测建模装置及森林应急消防监测系统
CN112434709B (zh) * 2020-11-20 2024-04-12 西安视野慧图智能科技有限公司 基于无人机实时稠密三维点云和dsm的航测方法及系统
CN114529603B (zh) * 2020-11-23 2023-05-19 新疆大学 一种基于激光slam与单目视觉slam融合的里程计方法
CN112484746B (zh) * 2020-11-26 2023-04-28 上海电力大学 一种基于地平面的单目视觉辅助激光雷达里程计方法
CN112562405A (zh) * 2020-11-27 2021-03-26 山东高速建设管理集团有限公司 一种雷达视频智能融合与预警方法及系统
CN112881999B (zh) * 2021-01-25 2024-02-02 上海西虹桥导航技术有限公司 一种用于多线激光雷达与视觉传感器的半自动标定方法
CN113050074B (zh) * 2021-03-16 2023-08-25 成都信息工程大学 无人驾驶环境感知中相机与激光雷达标定系统及标定方法
CN113093759A (zh) * 2021-04-08 2021-07-09 中国科学技术大学 基于多传感器信息融合的机器人编队构造方法及系统
CN114612348B (zh) * 2022-05-11 2022-07-15 季华实验室 激光点云运动畸变校正方法、装置、电子设备及存储介质
CN116299383B (zh) * 2023-02-14 2024-02-02 北京建筑大学 多传感器融合综合管廊巡检小车定位方法

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107608371A (zh) * 2016-07-12 2018-01-19 何守印 城市社区环境下四旋翼自主避障无人机
CN108401461A (zh) * 2017-12-29 2018-08-14 深圳前海达闼云端智能科技有限公司 三维建图方法、装置、系统、云端平台、电子设备和计算机程序产品
CN109697753A (zh) * 2018-12-10 2019-04-30 智灵飞（北京）科技有限公司 一种基于rgb-d slam的无人机三维重建方法、无人机
CN109816774A (zh) * 2018-12-31 2019-05-28 天合光能股份有限公司 一种基于无人机的三维重建系统及三维重建方法
CN109947103A (zh) * 2019-03-18 2019-06-28 深圳一清创新科技有限公司 无人驾驶控制方法、装置、系统和承载设备
CN109976379A (zh) * 2019-03-15 2019-07-05 上海工程技术大学 一种激光雷达和深度相机融合的自主导航及避障无人机
CN110261870A (zh) * 2019-04-15 2019-09-20 浙江工业大学 一种用于视觉-惯性-激光融合的同步定位与建图方法
CN110349221A (zh) * 2019-07-16 2019-10-18 北京航空航天大学 一种三维激光雷达与双目可见光传感器的融合标定方法
CN110389348A (zh) * 2019-07-30 2019-10-29 四川大学 基于激光雷达与双目相机的定位与导航方法及装置

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB201303076D0 (en) * 2013-02-21 2013-04-10 Isis Innovation Generation of 3D models of an environment

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107608371A (zh) * 2016-07-12 2018-01-19 何守印 城市社区环境下四旋翼自主避障无人机
CN108401461A (zh) * 2017-12-29 2018-08-14 深圳前海达闼云端智能科技有限公司 三维建图方法、装置、系统、云端平台、电子设备和计算机程序产品
CN109697753A (zh) * 2018-12-10 2019-04-30 智灵飞（北京）科技有限公司 一种基于rgb-d slam的无人机三维重建方法、无人机
CN109816774A (zh) * 2018-12-31 2019-05-28 天合光能股份有限公司 一种基于无人机的三维重建系统及三维重建方法
CN109976379A (zh) * 2019-03-15 2019-07-05 上海工程技术大学 一种激光雷达和深度相机融合的自主导航及避障无人机
CN109947103A (zh) * 2019-03-18 2019-06-28 深圳一清创新科技有限公司 无人驾驶控制方法、装置、系统和承载设备
CN110261870A (zh) * 2019-04-15 2019-09-20 浙江工业大学 一种用于视觉-惯性-激光融合的同步定位与建图方法
CN110349221A (zh) * 2019-07-16 2019-10-18 北京航空航天大学 一种三维激光雷达与双目可见光传感器的融合标定方法
CN110389348A (zh) * 2019-07-30 2019-10-29 四川大学 基于激光雷达与双目相机的定位与导航方法及装置

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
Calibration of RGB Camera With Velodyne LiDAR;Martin Velas, et al.;《WSCG2014 Conference on Computer Graphics, Visualization and Computer Vision》;20141231;第135-144页 *

Also Published As

Publication number Publication date
CN111199578A (zh) 2020-05-26

Similar Documents

Publication Publication Date Title
CN111199578B (zh) 基于视觉辅助激光雷达的无人机三维环境建模方法
CN110859044B (zh) 自然场景中的集成传感器校准
KR102001728B1 (ko) 스테레오 카메라 드론을 활용한 무기준점 3차원 위치좌표 취득 방법 및 시스템
Chiabrando et al. UAV and RPV systems for photogrammetric surveys in archaelogical areas: two tests in the Piedmont region (Italy)
Li et al. NRLI-UAV: Non-rigid registration of sequential raw laser scans and images for low-cost UAV LiDAR point cloud quality improvement
CN105424006B (zh) 基于双目视觉的无人机悬停精度测量方法
JP5992184B2 (ja) 画像データ処理装置、画像データ処理方法および画像データ処理用のプログラム
Rumpler et al. Evaluations on multi-scale camera networks for precise and geo-accurate reconstructions from aerial and terrestrial images with user guidance
CN102322859B (zh) 一种航空惯性导航测量系统及姿态校正方法
CN112270713A (zh) 标定方法以及装置、存储介质、电子装置
TW201832185A (zh) 利用陀螺儀的相機自動校準
CN104076817A (zh) 一种高清视频航拍多模传感器自外感知智能导航系统及其方法
CN111862214A (zh) 计算机设备定位方法、装置、计算机设备和存储介质
Hinzmann et al. Flexible stereo: constrained, non-rigid, wide-baseline stereo vision for fixed-wing aerial platforms
Nedevschi Online cross-calibration of camera and lidar
CN114037762A (zh) 基于图像与高精度地图配准的实时高精度定位方法
Moore et al. A stereo vision system for uav guidance
Ivancsits et al. Visual navigation system for small unmanned aerial vehicles
JP6155091B2 (ja) モザイク画像生成装置及び生成方法並びにモザイク画像生成プログラム
Fan et al. High-precision external parameter calibration method for camera and LiDAR based on a calibration device
CN110136168B (zh) 一种基于特征点匹配和光流法的多旋翼速度测量方法
CN116184430B (zh) 一种激光雷达、可见光相机、惯性测量单元融合的位姿估计算法
CN116952229A (zh) 无人机定位方法、装置、系统和存储介质
Wang et al. Micro aerial vehicle navigation with visual-inertial integration aided by structured light
CN114419259B (zh) 一种基于物理模型成像仿真的视觉定位方法及系统

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
GR01 Patent grant
GR01 Patent grant