[发明专利]一种面向无人自主着降的相对位姿测量方法

权利要求书

1.一种面向无人自主着降的相对位姿测量方法，其特征在于，包括以下步骤：

步骤(1)在无人机上安装固定垂直下视摄像机，以当前时刻采集合作靶标的图像、无人机前一时刻与靶标的相对位姿、当前时刻合作靶标中多个LED的物理坐标作为测量系统的输入；

步骤(2)当输入为当前时刻摄像机采集的合作靶标图像时，检测出图像中多个LED的像素坐标；

使用无人机前一时刻相对位姿，进行基于位姿预测的LED灯2D-3D匹配；如果匹配失败，则用组合遍历法重新确定图像中LED与合作靶标中LED的2D-3D匹配关系；

步骤(3)使用匹配后的多组LED点对，以P3P算法的一个解作为位姿初值，迭代计算LED的重投影误差最小值，求解输出位姿优化估计值；

所述步骤(2)中的基于位姿预测的LED灯2D-3D匹配，具体为：

首先，假设靶标的位姿是从固定参考坐标系出发，以一个恒定的速度twist积分指定的时间得到，这种运动类似于螺旋运动；因此可以采用线性预测来估计下一时刻靶标的位姿，

其中系数k＝1,2,3,...，ξ_k代表第k步靶标的位姿，ΔT是相邻两步之间的时间间隔，T_k是第k步的时刻，n_P是先前估计的位姿的数量；

然后，使用预测出位姿将所有的LED重投影到摄像机图像中；如果某个LED重投影坐标与其最接近的检测出的LED坐标的距离阈值小于λ_m，通常使用5像素作为阈值，则将二者进行匹配；

最后检查基于位姿预测的LED匹配关系是否正确；选取3个匹配好的点对，使用P3P算法计算出4个候选位姿；分别利用这4个候选位姿，计算剩余LED的重投影，检查它们中是否至少有75％低于重投影阈值λ_r；如果是，则认为基于位姿预测的LED匹配结果是正确的；如果不是，则使用基于组合遍历法的LED匹配重新初始化；

所述步骤(2)中包括以下步骤：

步骤①当前时刻图像中多个LED的检测；

步骤②基于组合遍历法进行当前时刻图像中多个LED的匹配；

所述步骤②具体为：

由于图像的LED均体现为亮度较高的光斑，具有较大的相似性，因此无法直接区分不同的LED；

首先，在图像中选取3个检测到的LED像点，然后在靶标上选取3个LED物点，这样会有多种组合方式；

然后，利用P3P算法来计算每种组合；根据P3P算法的特点，每种组合会产生的四个候选位姿；对于每个候选位姿，将靶标上未参与P3P计算的LED再投影到摄像机图像中；如果该LED重投影像点与检测到的像点距离小于阈值λ_r，则认为此组LED匹配成功；

对于重投影距离阈值，通常使用λ_r＝5像素；为了对异常值具有鲁棒性，为每个组合的LED配对生成一个计数直方图；当一个配对被认为是正确匹配，直方图就会加1；从直方图中可以找到最终对应关系；

靶标上的LED和图像中检测到的LED配对组合，用P3P算法来计算每种组合，将获得N个候选位姿，

其中，C、P分别表示组合、排列计算；

上式中，对于较大的n_D或n_L，N增长很快；然而，一般靶标中只使用了几个LED，所以N并不是很大；

匹配关系直方图中的数字表示靶标中LED的l_i的重投影与检测到的LED的d_j的距离小于阈值λ_r的频数；

在实际工程中，迭代搜索直方图中最大的数字，并取对应的靶标LED和图像检测LED作为配对，然后在后续的迭代中忽略这一列。

2.根据权利要求1所述的方法，其特征在于，所述步骤(1)中的在无人机上安装固定垂直下视摄像机为：

无人机搭载垂直朝下的摄像机，为了避免到日光干扰，成像器件配合窄带滤光片，将工作谱段限制在日盲区紫外波段，以消除自然背景光的干扰。

3.根据权利要求1所述的方法，其特征在于，所述步骤(1)中的合作靶标中多个LED的工作波段限制在200nm-280nm。