一种基于激光雷达和相机数据融合的无人机定点降落方法

1.本发明涉及无人机定位技术领域，具体涉及一种基于激光雷达和相机数据融合的无人机定点降落方法。


背景技术：

2.旋翼无人机具有体积较小、结构简单、控制比较灵活等特点，能够垂直起降、自由悬停，还能够适应各种自然环境，具备自主飞行和自主降落等优点，可以在一些不适合人类进入的复杂和危险环境中进行作业，近年来在科研机构、政府机构、广播媒体、个人应用和军事领域都有着越来越重要的应用。而无人机定点降落是指无人机飞行在高空时，通过配备下视相机采集地面图像，通过人工在地面图像上选择一个降落目标点，无人机自动飞行到目标点上空并精确降落到设定的降落点。无人机定点降落可以应用到抢险救援、自动化物流对接、紧急情况下的无人机迫降等。
3.传统无人机只搭载相机识别落点参照物，而相机近距离视野窄，强光下过曝，无法接收夜光，因而存在应用场景单一、方法容易失效、夜间无法应用等问题。近几年，激光雷达逐渐发展小型化，成为众多功能无人机需携带的传感器。激光雷达以主动发射及接收激光术原理对三维环境进行感知，具有更大的空间覆盖能力，不受白天、黑夜、光线强度等自然因素影响，为无人机识别落点参照物提供了可靠的选择。
4.随机抽样一致算法(random sample consensus，ransac)在1981年由fischler和bolles首次提出,它采用迭代的方式从一组包含离群的被观测数据中估算出数学模型的参数。随机抽样一致算法是一个非确定性算法，在某种意义上说，它会产生一个在一定概率下合理的结果，而更多次的迭代会使这一概率增加，因此该算法常用于处理激光雷达点云，以形成估计平面。随机抽样性算法包括以下步骤：(1)在数据中随机选择几个点设定为内群；(2)计算拟合内群的模型；(3)把其它刚才没选到的点带入刚才建立的模型中，计算是否为内群；(4)记下内群数量；(5)重复以上步骤多做几次，比较哪次计算中内群数量最多，内群最多的那次所建的模型就是所要求的估计平面。
5.但目前无人机定点降落仍存在识别落点参照物不具有真实可见光色彩、点云密度低、测距范围短等技术问题。如weimin wang等人提出的反射强度辅助三维激光雷达和全景相机的印刷棋盘自动精确外部校准的无人机定点降落方法，该方法采用velodyne激光雷达，但由于远距离点云密度过低，无法从中提取有效特征信息，所作实验的最远测距范围只有2.6米，限制了无人机的使用范围。为此，有必要研究能够提高落点参照物识别效果的无人机定点降落方法。
6.如中国专利cn113946157a，公开日2022年1月18日，一种基于多功能识别定位的定点降落无人机方法及系统。该方法通过无人机接收位于待降落平台上的控制端的自动返航指令，并根据gps导航返回待降落平台上空的第一预设距离高度处悬停；通过双摄像头实时获取下方图像数据信息，并传输至控制端，控制端通过轮廓提取方法识别锁定待降落平台上的标靶，以引导无人机逐步降低，当无人机位于标靶中心上方第二预设距离高度时，自动
降落完成定点降落，实现了落点参照物识别精度较高、降落耗时短的无人机定点降落。但其技术方案并没有解决无人机定点降落易受环境因素影响、测距范围短的技术问题。


技术实现要素：

7.本发明所要解决的技术问题：目前无人机在实现定点降落时，存在易受环境因素影响、测距范围短的技术问题。提出了一种基于激光雷达和相机数据融合的无人机定点降落方法，能够融合激光雷达与相机数据识别aruco码角点，提高无人机定点降落的环境适应性，扩大测距范围。
8.解决上述技术问题，本发明采用如下技术方案：一种基于激光雷达和相机数据融合的无人机定点降落方法，用于采用livox激光雷达的无人机的降落，包括：
9.相机拍摄无人机下方图像，识别图像中的aruco码，获得aruco码的二维点坐标，根据二维点坐标建立xoy平面；
10.激光雷达扫描无人机下方，获得三维点云及反射强度数据，根据反射强度数据识别aruco码，获得aruco码的三维点云坐标；
11.计算出相机二维点坐标和激光雷达三维点云坐标之间的变换矩阵m，将三维点云投影到xoy平面；
12.根据投影后的三维点云，计算aruco码的中心位置；
13.将中心位置作为坐标原点在xoy平面建立坐标系；
14.将投影后的三维点云数据映射到所述坐标系上；
15.识别aruco码角点，计算无人机与aruco码角点的相对位置，形成无人机移动数据；
16.重复上述步骤，直至无人机与aruco码角点位置对齐，完成定点降落。
17.作为优选，根据反射强度数据识别aruco码的方法包括：
18.r
l
为黑色图案反射强度的最大值，rh为白色图案反射强度的最小值；
19.定义一个中间区间[β1,β2]，β1＝((σ-1)r
l
+rh)/σ,β2＝(r
l
+(σ-1)rh)/σ,其中σ为预设常量值；
[0020]
若三维点的反射强度小于β1，则将三维点纳入黑色图案区域，若三维点的反射强度大于β2，则将三维点纳入白色图案区域；
[0021]
根据黑色图案区域和白色图案区域，识别出aruco码。
[0022]
作为优选，反射强度数据与aruco码的图案颜色之间的成本函数为：
[0023][0024]
其中，xi表示第i个三维点的反射强度，f1(xi)用于确定三维点反射强度是否在中间区间[β1,β2]的范围内，c表示aruco码的正方形格子，表示三维点，用于确定正方形格子c是否包含了三维点用于确定正方形格子c是否包含了三维点vi表示第i个点对应的格子的四个顶点，ci表示根据反射强度xi估计的颜色值，
表示三维点对应的图案颜色，0表示黑色，1表示白色，f3用以确定三维点云映射错误的三维点位置，f3＝min(δx1,δx2)+min(δy1,δy2)。
[0025]
作为优选，三维点云映射错误的三维点位置的确定方法为：
[0026]
当点云错误落入白格时，通过f3＝min(δx1,δx2)+min(δy1,δy2)确定错误落入白格的点云位置，其中δx1表示点云到vi确定的正方形格左边的距离，δx2表示点云到vi确定的正方形格右边的距离，δy1表示点云到vi确定的正方形格上边的距离，δy2表示点云到vi确定的正方形格下边的距离；
[0027]
当点云错误落入aruco码外时，通过f3＝min(δx1,δx2)+min(δy1,δy2)确定错误落入aruco码外的点云位置，其中δx1表示点云到正方形格子c左边的距离，δx2表示点云到正方形格子c右边的距离，δy1表示点云到正方形格子c上边的距离，δy2表示点云到正方形格子c下边的距离。
[0028]
作为优选，xoy平面点集合为：
[0029][0030]
其中，tr(θm,tm,p
im
)为激光雷达矩阵。
[0031]
作为优选，计算变换矩阵m的方法为：
[0032][0033]
其中，(u1,v1)表示相机检测的二维点坐标，(x，y，z)表示激光雷达捕捉的三维点云坐标，和均表示相机的焦距，r1为旋转矩阵，t1为平移矩阵。
[0034]
作为优选，计算aruco码中心位置的方法为：
[0035]
将aruco码的三维点云矩阵沿激光雷达的坐标系pb＝(a1,a2,a3)
t
中的三个基向量a1、a2及a3进行分解；
[0036]
两个三维点组合并连接成线段，计算线段与基向量a1、a2及a3的比值，分别记为δ1、δ2及δ3；
[0037]
通过随机抽样一致性算法处理点云，形成估计平面；
[0038]
将比值δ3小于0.01的线段视为在同一平面，并将同一平面的点投影到估计平面上；
[0039]
将投影到估计平面上所有点的坐标通过矩阵变换旋转到xoy平面，并将δ1、δ2与x轴和y轴通过旋转点对齐，其中，定义pn×
3,f
为拟合点，为拟合点pn×
3,f
的旋转矩阵；
[0040]
计算出旋转点pn×
3,fc
的旋转矩阵即为aruco码的中心位置。
[0041]
作为优选，将中心位置作为坐标原点在xoy平面建立坐标系的方法为：
[0042]
将中心位置与xoy坐标系的原点重合；
[0043]
将中心位置的值减去旋转点pn×
3,fc
的平均值，得到旋转后xoy坐标原点的坐标系。
[0044]
本发明的有益技术效果包括：采用一种基于激光雷达和相机数据融合的无人机定点降落方法，融合了相机与激光雷达对落点参照物的识别优势，提出建立真实物体色彩与激光点云反射强度数据之间的成本函数，从而提高了无人机对落点参照物的识别效果，进一步优化了无人机的定点降落；在激光雷达和相机数据融合的基础上，借助激光雷达的反射强度优势，无人机定点降落的最远测距范围可达27米以上，极大的扩充了无人机的使用范围。
[0045]
本发明的其他特点和优点将会在下面的具体实施方式、附图中详细的揭露。
附图说明
[0046]
下面结合附图对本发明做进一步的说明：
[0047]
图1为本发明实施例无人机定点降落方法流程示意图；
[0048]
图2为本发明实施例aruco码中心位置的计算方法流程示意图。
具体实施方式
[0049]
下面结合本发明实施例的附图对本发明实施例的技术方案进行解释和说明，但下述实施例仅为本发明的优选实施例，并非全部。基于实施方式中的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得其他实施例，都属于本发明的保护范围。
[0050]
在下文描述中，出现诸如术语“内”、“外”、“上”、“下”、“左”、“右”等指示方位或者位置关系仅是为了方便描述实施例和简化描述，而不是指示或暗示所指的装置或者元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
[0051]
一种基于激光雷达和相机数据融合的无人机定点降落方法，用于采用livox激光雷达的无人机的降落，请参阅附图1，包括：
[0052]
步骤a01)相机拍摄无人机下方图像，识别图像中的aruco码，获得aruco码的二维点坐标，根据二维点坐标建立xoy平面；
[0053]
步骤a02)激光雷达扫描无人机下方，获得三维点云及反射强度数据，根据反射强度数据识别aruco码，获得aruco码的三维点云坐标；
[0054]
根据反射强度数据识别aruco码的方法包括：
[0055]rl
为黑色图案反射强度的最大值，rh为白色图案反射强度的最小值；
[0056]
定义一个中间区间[β1,β2]，β1＝((σ-1)r
l
+rh)/σ,β2＝(r
l
+(σ-1)rh)/σ,其中σ为预设常量值；
[0057]
若三维点的反射强度小于β1，则将三维点纳入黑色图案区域，若三维点的反射强度大于β2，则将三维点纳入白色图案区域；
[0058]
根据黑色图案区域和白色图案区域，识别出aruco码。
[0059]
反射强度数据与aruco码的图案颜色之间的成本函数为：
[0060][0061]
其中，xi表示第i个三维点的反射强度，f1(xi)用于确定三维点反射强度是否在中间区间[β1,β2]的范围内，c表示aruco码的正方形格子，表示三
维点，用于确定正方形格子c是否包含了三维点用于确定正方形格子c是否包含了三维点vi表示第i个点对应的格子的四个顶点，ci表示根据反射强度xi估计的颜色值，估计的颜色值，表示三维点对应的图案颜色，0表示黑色，1表示白色，f3用以确定三维点云映射错误的三维点位置，f3＝min(δx1,δx2)+min(δy1,δy2)。
[0062]
根据反射强度数据与aruco码的图案颜色之间的成本函数可得出xoy平面点集合为：
[0063][0064]
其中，tr(θm,tm,p
im
)为激光雷达矩阵。
[0065]
三维点云映射错误的三维点位置的确定方法为：
[0066]
当点云错误落入白格时，通过f3＝min(δx1,δx2)+min(δy1,δy2)确定错误落入白格的点云位置，其中δx1表示点云到vi确定的正方形格左边的距离，δx2表示点云到vi确定的正方形格右边的距离，δy1表示点云到vi确定的正方形格上边的距离，δy2表示点云到vi确定的正方形格下边的距离；
[0067]
当点云错误落入aruco码外时，通过f3＝min(δx1,δx2)+min(δy1,δy2)确定错误落入aruco码外的点云位置，其中δx1表示点云到正方形格子c左边的距离，δx2表示点云到正方形格子c右边的距离，δy1表示点云到正方形格子c上边的距离，δy2表示点云到正方形格子c下边的距离。
[0068]
步骤a03)计算出相机二维点坐标和激光雷达三维点云坐标之间的变换矩阵m，将三维点云投影到xoy平面；
[0069]
计算变换矩阵m的方法为：
[0070][0071]
其中，(u1,v1)表示相机检测的二维点坐标，(x，y，z)表示激光雷达捕捉的三维点云坐标，和均表示相机的焦距，r1为旋转矩阵，t1为平移矩阵。
[0072]
步骤a04)根据投影后的三维点云，计算aruco码的中心位置；
[0073]
步骤a05)将中心位置作为坐标原点在xoy平面建立坐标系；
[0074]
其中，将中心位置作为坐标原点在xoy平面建立坐标系的方法为：
[0075]
将中心位置与xoy坐标系的原点重合；
[0076]
将中心位置的值减去旋转点pn×
3,fc
的平均值，得到旋转后xoy坐标原点的坐标系。
[0077]
步骤a06)将投影后的三维点云数据映射到所述坐标系上；
[0078]
步骤a07)识别aruco码角点，计算无人机与aruco码角点的相对位置，形成无人机移动数据；
[0079]
步骤a08)重复上述步骤，直至无人机与aruco码角点位置对齐，完成无人机定点降落。
[0080]
请参阅附图2，计算aruco码中心位置的方法为：
[0081]
步骤b01)将aruco码的三维点云矩阵沿激光雷达的坐标系pb＝(a1,a2,a3)
t
中的三个基向量a1、a2及a3进行分解；
[0082]
步骤b02)两个三维点组合并连接成线段，计算线段与基向量a1、a2及a3的比值，分别记为δ1、δ2及δ3；
[0083]
步骤b03)通过随机抽样一致性算法处理点云，形成估计平面；
[0084]
步骤b04)将比值δ3小于0.01的线段视为在同一平面，并将同一平面的点投影到估计平面上；
[0085]
步骤b05)将投影到估计平面上所有点的坐标通过矩阵变换旋转到xoy平面，并将δ1、δ2与x轴和y轴通过旋转点对齐，其中，定义pn×
3,f
为拟合点，为拟合点pn×
3,f
的旋转矩阵；
[0086]
步骤b06)计算出旋转点pn×
3,fc
的旋转矩阵即为aruco码的中心位置。
[0087]
本实施例的有益技术效果包括：采用一种基于激光雷达和相机数据融合的无人机定点降落方法，融合了相机与激光雷达对落点参照物的识别优势，提出建立真实物体色彩与激光点云反射强度数据之间的成本函数，从而提高了无人机对落点参照物的识别效果，进一步优化了无人机的定点降落；在激光雷达和相机数据融合的基础上，借助激光雷达的反射强度优势，无人机定点降落的最远测距范围可达27米以上，极大的扩充了无人机的使用范围。
[0088]
本实施例识别的落点参照物为aruco码，而融合激光雷达点云和相机图像数据识别h型标记、apriltags图案、自定义特殊图案等落点参照物都与本实施例相似，均应在本发明的保护范围内。
[0089]
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，熟悉该本领域的技术人员应该明白本发明包括但不限于附图和上面具体实施方式中描述的内容。任何不偏离本发明的功能和结构原理的修改都将包括在权利要求书的范围中。