一种面向无人自主着降的相对位姿测量方法

阅读：评论：0

1.本发明涉及位姿测量领域，具体涉及一种面向无人自主着降的相对位姿测量方法。

背景技术：

2.无人机自主着降是实现无人机自主飞行的关键技术之一。
3.在该研究领域中，基于视觉的无人机自主着降引导技术是一个新兴的研究方向。传统有人机着降时，飞行员依靠视觉所提供信息定位飞机空间位置和飞机相对着陆平台的旋转关系。而无人机视觉自主着降引导主要依赖于预设的地面合作目标，通过对地面合作目标的检测、识别、跟踪和相对位姿测量实现无人机的定位与自主导航，并根据合作目标在实时图像中的尺度、形状等信息估计无人机降落过程中的飞行姿态。
4.国内外学者针对无人机视觉自主着降引导开展了一系列研究。南加州大学设计了“h”形标识着陆目标，采用透视投影不变特征技术提取“h”形特征，依据“h”不变矩特性对大量目标图像上“h”形的区域矩进行统计，得到一个统计量作为该“h”形目标正确的区域矩。加利福尼亚大学设计的视觉辅助着陆目标采用已知的正方形着陆目标图识，视觉算法是用机载摄像机采集目标视频图像，然后对其目标图像进行分割和提取目标角点，继而得到基于视觉状态估计运动信息引导无人机着陆化。
5.斯坦福大学航空机器人实验室设计的视觉着陆目标为多个圆环形着陆目标图识。
6.南京航空航天大学设计了一种新型着陆目标图识进行辅助视觉着陆，该图识包含两个同心彩圆。
7.上述相对位姿测量基于可见光成像，成像过程受气候条件、环境照度、自然光背景等条件影响较大，信标图案的识别以及特征信息提取算法复杂、稳健性差，严重影响算法的稳定性，从而导致无人机自主着降失败。另外，高分辨率的可见光图像带来巨大的运算量，尤其是在算力有限的机载平台上，容易导致实时性不足。本发明设计了一种基于“日盲区”波段的无人机相对位姿测量方法，通过对光谱的过滤，从物理层面上大幅度削除了无效信息，凸显出合作靶标上感兴趣目标点，从而提升系统在复杂环境下的适应性、稳定性以及实时性。
8.perspective-n-points (pnp)是一种常见的3d-2d的位姿求解方法，该类算法需要已知匹配的3d点和图像2d点。
9.此发明可应用于无人机灾情救援、物流运输、智能巡线等领域，可带来较大社会效益和经济效益。

技术实现要素：

10.本发明设计一种基于“日盲区”波段的无人机相对位姿测量方法，能够大幅度提升系统在多种环境下的适应性和稳定性；本发明旨在提供一种基于视觉的无人机与地面靶标相对位姿测量方法，适用于无人机自主着降领域。
11.在“日盲区”波段图像中，合作靶标上的多个led均体现为亮度较高的光斑，具有较大的相似性，难以辨识不同的led像点，导致无法直接使用pnp位姿求解方法。
12.本发明提出了一种基于组合遍历思想的led的 2d像点和3d物点匹配方法，遍历led像点和物点所有的匹配组合的候选位姿，使用led重投影的方法验证匹配组合的可能性；考虑到系统实施的实时性，进一步提出一种基于位姿预测的led 2d-3d匹配方法，采用线性预测的方法推导下一时刻led 2d像点的坐标，通过与真实像点坐标比对，判断2d-3d匹配是否有效。
13.本发明通过组合遍历法实现匹配初始化和重匹配，通过位姿预测法实现匹配关系持续跟踪，两种方法的切换使用，既保证了2d-3d匹配的精度，也提升了匹配速度。稳定、高速的led 2d-3d匹配方法，为下一步基于p3p的无人机相对位姿优化求解提供了技术基础。
14.一种面向无人自主着降的相对位姿测量方法，其特征在于，包括以下步骤：步骤（1）在无人机上安装固定垂直下视摄像机，以当前时刻采集合作靶标的图像、无人机前一时刻与靶标的相对位姿、当前时刻合作靶标中多个led的物理坐标作为测量系统的输入；步骤（2）当输入为当前时刻摄像机采集的合作靶标图像时，检测出图像中多个led的像素坐标；使用无人机前一时刻相对位姿，进行基于位姿预测的led灯2d-3d匹配；如果匹配失败，则用组合遍历法重新确定图像中led与合作靶标中led的2d-3d匹配关系；步骤（3）使用匹配后的多组led点对，以p3p算法的一个解作为位姿初值，迭代计算led的重投影误差最小值，求解输出位姿优化估计值。
15.其中，所述步骤（1）中的在无人机上安装固定垂直下视摄像机为：无人机搭载垂直朝下的摄像机，为了避免到日光干扰，成像器件配合窄带滤光片，将工作谱段限制在日盲区紫外波段，以消除自然背景光的干扰。
16.其中，所述步骤（1）中的合作靶标中多个led的工作波段限制在200nm-280nm。
17.其中，所述步骤（2）中包括以下步骤：步骤
①
当前时刻图像中多个led的检测；步骤
②
基于组合遍历法进行当前时刻图像中多个led的匹配。
18.其中，在所述步骤
①
之前，还包括一个定义的过程：定义led在靶标上的三维坐标为，单位为米；led的数量为，则led的坐标为；led在图像中的检测出的二维坐标为，单位为像素；检测的数量为，则检测的集合为；一个led的坐标和其对应检测图像坐标的对应关系表示为，位姿表示为；定义灰度图像，其中u和v分别为图像的像素坐标。
19.其中，所述步骤
①
具体为：
led为紫外led，其波长与摄像机中的滤镜相匹配；因此，led在图像中显得非常明亮，通过下述阈值函数检测出led灯；
ꢀꢀꢀꢀꢀꢀꢀꢀ
(1)其中阈值参数threshold根据经验设置为125；使用图像一阶矩来计算led光斑中心，其定义为
ꢀꢀꢀꢀꢀꢀꢀ
(2)其中，p、q分别取0或1；图像中的led检测的坐标为
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
ꢀꢀꢀꢀꢀꢀ
(4)其中，和分别为led质心的x轴坐标和y轴坐标；和分别为图像关于x轴和y轴的矩，表征led区域的面积。
20.其中，所述步骤
②
具体为：由于图像的led均体现为亮度较高的光斑，具有较大的相似性，因此无法直接区分不同的led；首先，在图像中选取3个检测到的led像点，然后在靶标上选取3个led物点，这样会有多种组合方式；然后，利用p3p算法来计算每种组合；根据p3p算法的特点，每种组合会产生的四个候选位姿；对于每个候选位姿，将靶标上未参与p3p计算的led再投影到摄像机图像中；如果该led重投影像点与检测到的像点距离小于阈值，则认为此组led匹配成功；对于重投影距离阈值，通常使用像素；为了对异常值具有鲁棒性，为每个组合的led配对生成一个计数直方图；当一个配对被认为是正确匹配，直方图就会加1；从直方图中可以到最终对应关系；靶标上的led和图像中检测到的led配对组合，用p3p算法来计算每种组合，将获得n个候选位姿，
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)其中，c、p分别表示组合、排列；上式中，对于较大的或，增长很快；然而，一般靶标中只使用了几个led，所以并不是很大；匹配关系直方图中的数字表示靶标中led的的重投影与检测到的led的的距离小于阈值的频数；在实际工程中，迭代搜索直方图中最大的数字，并取对应的靶标led和图像检测led作为配对，然后在后续的迭代中忽略这一列。
21.其中，所述步骤（2）中的基于位姿预测的led 2d-3d匹配，具体为：首先，假设靶标的位姿是从固定参考坐标系出发，以一个恒定的速度twist积分指定的时间得到，这种运动类似于螺旋运动；因此可以采用线性预测来估计下一时刻靶标的位姿，
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
ꢀꢀꢀꢀꢀ
(7)其中系数k=1,2,3,...，代表第k步靶标的位姿，是相邻两步之间的时间间隔，是第k步的时刻，是先前估计的位姿的数量；然后，使用预测出位姿将所有的led重投影到摄像机图像中；如果某个led重投影坐标与其最接近的检测出的led坐标的距离阈值小于，通常使用5像素作为阈值，则将二者进行匹配；最后检查基于位姿预测的led匹配关系是否正确；选取3个匹配好的点对，使用p3p算法计算出4个候选位姿；分别利用这4个候选位姿，计算剩余led的重投影，检查它们中是否至少有75%低于重投影阈值；如果是，则认为基于位姿预测的led匹配结果是正确的；如果不是，则使用基于组合遍历法的led匹配重新初始化。
22.其中，所述步骤（3）具体为：使用匹配后的多组led点对，以p3p算法的一个解作为位姿初值，迭代计算led的重投影误差最小值，求解输出位姿优化估计值，即
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)其中，是将一个led重投影到图像中，l为led在靶标上的三维物理坐标，为实际检测出的led像素坐标。
23.本发明的技术效果：本发明设计一种基于“日盲区”波段的无人机相对位姿测量方法，能够有效克服气候条件、环境照度、自然光背景等条件影响，大幅度提升系统在多种环境下的适应性和稳定性，为无人机在复杂环境下的自主着降提供了一种新技术方案。
附图说明
24.附图大体上通过举例而不是限制的方式示出各种实施例，并且与说明书以及权利要求书一起用于对所发明的实施例进行说明。在适当的时候，在所有附图中使用相同的附图标记指代同一或相似的部分。这样的实施例是例证性的，而并非旨在作为本装置或方法的穷尽或排他实施例。
25.图1示出了本发明的系统工作模式示意图；图2示出了本发明的相对位姿测量流程示意图。
具体实施方式
26.本发明提供的一种面向无人自主着降的相对位姿测量方法，包括以下步骤：步骤（1）在无人机上安装固定垂直下视摄像机，以当前时刻采集合作靶标的图像、无人机前一时刻与靶标的相对位姿、当前时刻合作靶标中多个led的物理坐标作为测量系统的输入；步骤（2）当输入为当前时刻摄像机采集的合作靶标图像时，检测出图像中多个led的像素坐标；使用无人机前一时刻相对位姿，进行基于位姿预测的led灯2d-3d匹配；如果匹配失败，则用组合遍历法重新确定图像中led与合作靶标中led的2d-3d匹配关系；当输入为当前时刻合作靶标中多个led的物理坐标时，直接进入下一步的处理；步骤（3）使用匹配后的多组led点对，以p3p算法的一个解作为位姿初值，迭代计算led的重投影误差最小值，求解输出位姿优化估计值。
27.其中，所述步骤（1）中的在无人机上安装固定垂直下视摄像机为：无人机搭载垂直朝下的摄像机，为了避免到日光干扰，成像器件配合窄带滤光片，将工作谱段限制在日盲区紫外波段，以消除自然背景光的干扰。
28.其中，所述步骤（1）中的合作靶标中多个led的工作波段限制在200nm-280nm。
29.其中，所述步骤（2）中包括以下步骤：步骤
①
当前时刻图像中多个led的检测；步骤
②
基于组合遍历法进行当前时刻图像中多个led的匹配。
30.其中，在所述步骤
①
之前，还包括一个定义的过程：定义led在靶标上的三维坐标为，单位为米；led的数量为，则led的坐标为；led在图像中的检测出的二维坐标为，单位为像素；检测的数量为，则检测的集合为；一个led的坐标和其对应检测图像坐标的对应关系表示为，位姿表示为；定义灰度图像，其中u和v分别为图像的像素坐标。
31.其中，所述步骤
①
具体为：led为紫外led，其波长与摄像机中的滤镜相匹配；因此，led在图像中显得非常明亮，通过下述阈值函数检测出led灯；
ꢀꢀ
(1)其中阈值参数threshold根据经验设置为125；使用图像一阶矩来计算led光斑中心，其定义为
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)其中，p、q分别取0或1；图像中的led检测的坐标为
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)其中，和分别为led质心的x轴坐标和y轴坐标；和分别为图像关于x轴和y轴的矩，表征led区域的面积。
32.其中，所述步骤
②
具体为：由于图像的led均体现为亮度较高的光斑，具有较大的相似性，因此无法直接区分不同的led；首先，在图像中选取3个检测到的led像点，然后在靶标上选取3个led物点，这样会有多种组合方式；然后，利用p3p算法来计算每种组合；根据p3p算法的特点，每种组合会产生的四个候选位姿；对于每个候选位姿，将靶标上未参与p3p计算的led再投影到摄像机图像中；如果该led重投影像点与检测到的像点距离小于阈值，则认为此组led匹配成功；对于重投影距离阈值，通常使用像素；为了对异常值具有鲁棒性，为每个组合的led配对生成一个计数直方图；当一个配对被认为是正确匹配，直方图就会加1；从直方图中可以到最终对应关系，如表1的匹配关系直方图统计表格所示；表1 匹配关系直方图统计表格靶标上的led和图像中检测到的led配对组合，用p3p算法来计算每种组合，将获得n个候选位姿，
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)其中，c、p分别表示组合、排列；上式中，对于较大的或，增长很快；然而，一般靶标中只使用了几个led，所
以并不很大；匹配关系直方图中的数字表示靶标中led的的重投影与检测到的led的的距离小于阈值的频数；在实际工程中，迭代搜索直方图中最大的数字，并取对应的靶标led和图像检测led作为配对，然后在后续的迭代中忽略这一列。
33.其中，所述步骤（2）中的基于位姿预测的led灯2d-3d匹配，具体为：首先，假设靶标的位姿是从固定参考坐标系出发，以一个恒定的速度twist积分指定的时间得到，这种运动类似于螺旋运动；因此可以采用线性预测来估计下一时刻靶标的位姿，
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
ꢀꢀꢀ
(7)其中系数k=1,2,3,...，代表第k步靶标的位姿，是相邻两步之间的时间间隔，是第k步的时刻，是先前估计的位姿的数量；然后，使用预测出位姿将所有的led重投影到摄像机图像中；如果某个led重投影坐标与其最接近的检测出的led坐标的距离阈值小于，通常使用5像素作为阈值，则将二者进行匹配；最后检查基于位姿预测的led匹配关系是否正确；选取3个匹配好的点对，使用p3p算法计算出4个候选位姿；分别利用这4个候选位姿，计算剩余led的重投影，检查它们中是否至少有75%低于重投影阈值；如果是，则认为基于位姿预测的led匹配结果是正确的；如果不是，则使用基于组合遍历法的led匹配重新初始化。
34.其中，所述步骤（3）具体为：使用匹配后的多组led点对，以p3p算法的一个解作为位姿初值，迭代计算led的重投影误差最小值，求解输出位姿优化估计值，即
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)其中，是将一个led重投影到图像中，l为led在靶标上的三维物理坐标，为实际检测出的led像素坐标。
35.请参阅图1、图2。
36.以上所述，仅为本发明优选的具体实施方式，但本发明的保护范围不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。

技术特征：

1.一种面向无人自主着降的相对位姿测量方法，其特征在于，包括以下步骤：步骤（1）在无人机上安装固定垂直下视摄像机，以当前时刻采集合作靶标的图像、无人机前一时刻与靶标的相对位姿、当前时刻合作靶标中多个led的物理坐标作为测量系统的输入；步骤（2）当输入为当前时刻摄像机采集的合作靶标图像时，检测出图像中多个led的像素坐标；使用无人机前一时刻相对位姿，进行基于位姿预测的led灯2d-3d匹配；如果匹配失败，则用组合遍历法重新确定图像中led与合作靶标中led的2d-3d匹配关系；步骤（3）使用匹配后的多组led点对，以p3p算法的一个解作为位姿初值，迭代计算led的重投影误差最小值，求解输出位姿优化估计值。2.根据权利要求1所述的方法，其特征在于，所述步骤（1）中的在无人机上安装固定垂直下视摄像机为：无人机搭载垂直朝下的摄像机，为了避免到日光干扰，成像器件配合窄带滤光片，将工作谱段限制在日盲区紫外波段，以消除自然背景光的干扰。3.根据权利要求1所述的方法，其特征在于，所述步骤（1）中的合作靶标中多个led的工作波段限制在200nm-280nm。4.根据权利要求1所述的方法，其特征在于，所述步骤（2）中包括以下步骤：步骤
①
当前时刻图像中多个led的检测；步骤
②
基于组合遍历法进行当前时刻图像中多个led的匹配。5.根据权利要求4所述的方法，其特征在于，在所述步骤
①
之前，还包括一个定义的过程：定义led在靶标上的三维坐标为，单位为米；led的数量为，则led的坐标为；led在图像中的检测出的二维坐标为，单位为像素；检测的数量为，则检测的集合为；一个led的坐标和其对应检测图像坐标的对应关系表示为，位姿表示为；定义灰度图像，其中u和v分别为图像的像素坐标。6.根据权利要求4所述的方法，其特征在于，所述步骤
①
具体为：led为紫外led，其波长与摄像机中的滤镜相匹配；因此，led在图像中显得非常明亮，通过下述阈值函数检测出led灯；
ꢀꢀꢀꢀꢀꢀ
(1)其中阈值参数threshold根据经验设置为125；使用图像一阶矩来计算led光斑中心，其定义为
ꢀꢀꢀꢀꢀꢀꢀ
(2)其中，p、q分别取0或1；图像中的led检测的坐标为
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)其中，和分别为led质心的x轴坐标和y轴坐标；和分别为图像关于x轴和y轴的矩，表征led区域的面积。7.根据权利要求4所述的方法，其特征在于，所述步骤
②
具体为：由于图像的led均体现为亮度较高的光斑，具有较大的相似性，因此无法直接区分不同的led；首先，在图像中选取3个检测到的led像点，然后在靶标上选取3个led物点，这样会有多种组合方式；然后，利用p3p算法来计算每种组合；根据p3p算法的特点，每种组合会产生的四个候选位姿；对于每个候选位姿，将靶标上未参与p3p计算的led再投影到摄像机图像中；如果该led重投影像点与检测到的像点距离小于阈值，则认为此组led匹配成功；对于重投影距离阈值，通常使用像素；为了对异常值具有鲁棒性，为每个组合的led配对生成一个计数直方图；当一个配对被认为是正确匹配，直方图就会加1；从直方图中可以到最终对应关系；靶标上的led和图像中检测到的led配对组合，用p3p算法来计算每种组合，将获得n个候选位姿，
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)其中，c、p分别表示组合、排列计算；上式中，对于较大的或，增长很快；然而，一般靶标中只使用了几个led，所以并不是很大；匹配关系直方图中的数字表示靶标中led的的重投影与检测到的led的的距离小于阈值的频数；在实际工程中，迭代搜索直方图中最大的数字，并取对应的靶标led和图像检测led作为配对，然后在后续的迭代中忽略这一列。8.根据权利要求1所述的方法，其特征在于，所述步骤（2）中的基于位姿预测的led灯2d-3d匹配，具体为：首先，假设靶标的位姿是从固定参考坐标系出发，以一个恒定的速度twist积分指定的时间得到，这种运动类似于螺旋运动；因此可以采用线性预测来估计下一时刻靶标的位姿，
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
ꢀꢀ
(7)其中系数k=1,2,3,...，代表第k步靶标的位姿，是相邻两步之间的时间间隔，是第k步的时刻，是先前估计的位姿的数量；然后，使用预测出位姿将所有的led重投影到摄像机图像中；如果某个led重投影坐标与其最接近的检测出的led坐标的距离阈值小于，通常使用5像素作为阈值，则将二者进行匹配；最后检查基于位姿预测的led匹配关系是否正确；选取3个匹配好的点对，使用p3p算法计算出4个候选位姿；分别利用这4个候选位姿，计算剩余led的重投影，检查它们中是否至少有75%低于重投影阈值；如果是，则认为基于位姿预测的led匹配结果是正确的；如果不是，则使用基于组合遍历法的led匹配重新初始化。9.根据权利要求1所述的方法，其特征在于，所述步骤（3）具体为：使用匹配后的多组led点对，以p3p算法的一个解作为位姿初值，迭代计算led的重投影误差最小值，求解输出位姿优化估计值，即
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)其中，是将一个led重投影到图像中，l为led在靶标上的三维物理坐标，为实际检测出的led像素坐标。

技术总结

本发明提供一种面向无人自主着降的相对位姿测量方法，它的步骤：（1）在无人机上安装垂直下视的日盲区紫外波段摄像机并采集合作靶标图像；（2）检测当前时刻摄像机采集合作靶标图像中的LED的像素坐标；使用前一时刻无人机位姿，进行基于位姿预测的LED的2D-3D匹配；如果匹配失败，则用组合遍历法重新确定图像中LED与合作靶标中LED的2D-3D匹配关系；（3）使用匹配后的多组LED点对，以P3P算法的一个解作为位姿初值，迭代计算LED的重投影误差最小值，求解输出位姿优化估计值。本发明能够大幅度提升无人机相对位姿测量系统在多种环境下的适应性和稳定性。性和稳定性。性和稳定性。