辅助信息与随机抽样一致算法融合的无人机位姿估计方法专利检索-迭代重建图形技术专利检索查询-专利查询网

辅助信息与随机抽样一致 算法融合的无人机位姿估计方法

阅读：1025发布：2020-09-25

专利汇可以提供辅助信息与随机抽样一致算法融合的无人机位姿估计方法专利检索，专利查询，专利分析的服务。并且本发明公开了一种辅助信息与随机抽样一致算法融合的无人机位姿估计方法，包括以下步骤：S1：利用GPS和IMU系统提供的无人机的辅助信息计算出本质矩阵；辅助信息包括位置和姿态信息；S2：无人机在不同角度对同一目标区域拍摄，利用SURF算法对相邻时刻拍摄的两幅航拍图像进行特征点提取与匹配，同时获得对应的匹配点集D，并对匹配点集D中的各个坐标点进行归一化处理；S3：得到满足精度要求的本质矩阵；S4：对获得的本质矩阵进行SVD分解得到各相邻时刻航拍图像间摄像机的平移向量和旋转矩阵，利用旋转矩阵求解无人机的三个姿态角，最终实现无人机的位姿估计。本发明的位姿估计具有较高的精度和鲁棒性。，下面是辅助信息与随机抽样一致算法融合的无人机位姿估计方法专利的具体信息内容。

权利要求

1.辅助信息与随机抽样一致算法融合的无人机位姿估计方法，其特征在于：包括以下步骤：
S1：利用GPS和IMU系统提供的无人机的辅助信息计算出本质矩阵；辅助信息包括位置和姿态信息；
S2：无人机在不同角度对同一目标区域拍摄，利用SURF算法对相邻时刻拍摄的两幅航拍图像进行特征点提取与匹配，同时获得对应的匹配点集D，并对匹配点集D中的各个坐标点进行归一化处理；
S3：将步骤S1获得的本质矩阵作为辅助信息与随机抽样一致算法相融合，初步剔除匹配点集D中存在的误匹配点，再利用随机抽样一致算法结合五点算法对本质矩阵进行迭代求解，最终得到满足精度要求的本质矩阵；
S4：对获得的本质矩阵进行SVD分解得到各相邻时刻航拍图像间摄像机的平移向量和旋转矩阵，利用旋转矩阵求解无人机的三个姿态角，最终实现无人机的位姿估计。
2.根据权利要求1所述的辅助信息与随机抽样一致算法融合的无人机位姿估计方法，其特征在于：所述本质矩阵通过平移向量T和旋转矩阵R求得，平移向量T和旋转矩阵R分别由式(1)、(2)得到：
T＝[x1-x0 y1-y0 z1-z0]T＝[Tx Ty Tz]T (1)
式(1)中，(x0 y0 z0)为GPS系统输出的无人机首次拍摄获取图片I0时对应的无人机位置点坐标，(x1 y1 z1)为GPS系统输出的无人机首次拍摄获取图片I1时对应的无人机位置点坐标；Tx表示无人机在两相邻时刻位置点之间沿X轴方向的平移向量，Ty表示无人机在两相邻时刻位置点之间沿Y轴方向的平移向量，Tz表示无人机在两相邻时刻位置点之间沿Z轴方向的平移向量；
式(2)中，Rψ表示无人机在相邻时刻两位置点间绕X轴旋转的旋转矩阵，Rθ表示无人机在相邻时刻两位置点间绕Y轴旋转的旋转矩阵，表示无人机在相邻时刻两位置点间绕Z轴旋转的旋转矩阵，ψ为偏航角，θ为俯仰角，为横滚角。
3.根据权利要求1所述的辅助信息与随机抽样一致算法融合的无人机位姿估计方法，其特征在于：所述步骤S3具体包括以下步骤：
S3.1：对步骤S2获得的匹配点集D中的所有匹配点进行归一化处理；
i
S3.2：根据式(3)计算出第n次迭代第i对匹配点r与经本质矩阵En变化后的对应的匹配点ri'之间的距离
将与预先设置好的阈值L比较：时，记此点为内点；否则，记此点为外点；从而获得内点集合Dn；若内点数大于前次循环获得的内点数，则更新本质矩阵E；
S3.3：根据式(4)利用随机抽样一致算法计算满足置信率P的本质矩阵的最小迭代次数N：
式(4)中，ε为外点概率，S为每个样本中的匹配点的对数；
S3.4：利用五点算法计算本质矩阵，也即随机抽取S＝5对匹配点计算出本质矩阵E；
S3.5：重复步骤S3.2-S3.4，迭代N次后获得最优本质矩阵。
4.根据权利要求3所述的辅助信息与随机抽样一致算法融合的无人机位姿估计方法，其特征在于：所述步骤S3.2中，本质矩阵E更新过程中的初值由GPS和IMU系统提供的无人机位姿信息来提供。
5.根据权利要求1所述的辅助信息与随机抽样一致算法融合的无人机位姿估计方法，其特征在于：所述步骤S4具体包括以下步骤：
T
S4.1：对获得的本质矩阵进行SVD分解，得到旋转矩阵R和平移向量T的初步解：R＝UWV或者R＝UWTVT；T＝u3或者T＝-u3；U和V为3×3的正交矩阵；U里面的向量是正交的，称为左奇异向量，U的列为EET的正交特征向量；V里面的向量是正交的，称为右奇异向量，V的列为ETE的正交特征向量；E＝Udiag(σ,σ,0)VT，σ为本质矩阵的奇异值，u3为矩阵U的最后一列，S4.2：对经去除误匹配点之后最终获取的匹配点集中的特征点进行三维重建，通过三维点深度值为正来确定旋转矩阵R和平移向量T的唯一解；
S4.3：利用旋转矩阵R求解出无人机的三个姿态角：偏航角ψ、俯仰角θ、横滚角

说明书全文

辅助信息与随机抽样一致算法融合的无人机位姿估计方法

技术领域

[0001] 本发明涉及定位导航与控制技术领域，特别是涉及无人机位姿估计方法。

背景技术

[0002] 传统的位姿估计方法是利用全球定位系统(GPS)、惯性测量系统(IMU)提供的数据来得到无人机的位姿信息。然而受器件本身条件的限制，单纯的利用惯导系统等无人机系统测量设备获取无人机的位姿信息，已不能有效的应对日益复杂的作业环境。近几年国内外众多专业人士提出利用丰富的视觉信息实现无人机的位姿估计，大致可分为基于非立体视觉的方法和基于立体视觉的方法两类，但单纯利用视觉信息不仅易受工作环境因素的影响，而且对丰富的视觉信息处理上，计算过程复杂难以实现对无人机的位姿进行实时监控，且位姿估计的精度上尚有待提高。因此有部分学者提出利用无人机自身的惯导系统和视觉信息做数据融合的方法，然而其实施起来比较复杂，技术研究上尚不够成熟，因此迫切需要更为有效、实用的无人机位姿估计方法。

发明内容

[0003] 发明目的：本发明提出了一种解决现有无人机位姿估计技术中存在的缺陷的，辅助信息与随机抽样一致算法融合的无人机位姿估计方法。

[0004] 技术方案：本发明所述的辅助信息与随机抽样一致算法融合的无人机位姿估计方法，包括以下步骤：

[0005] S1：利用GPS和IMU系统提供的无人机的辅助信息计算出本质矩阵；辅助信息包括位置和姿态信息；

[0006] S2：无人机在不同角度对同一目标区域拍摄，利用SURF算法对相邻时刻拍摄的两幅航拍图像进行特征点提取与匹配，同时获得对应的匹配点集D，并对匹配点集D中的各个坐标点进行归一化处理；

[0007] S3：将步骤S1获得的本质矩阵作为辅助信息与随机抽样一致算法相融合，初步剔除匹配点集D中存在的误匹配点，再利用随机抽样一致算法结合五点算法对本质矩阵进行迭代求解，最终得到满足精度要求的本质矩阵；

[0008] S4：对获得的本质矩阵进行SVD分解得到各相邻时刻航拍图像间摄像机的平移向量和旋转矩阵，利用旋转矩阵求解无人机的三个姿态角，最终实现无人机的位姿估计。

[0009] 进一步，所述本质矩阵通过平移向量T和旋转矩阵R求得，平移向量T和旋转矩阵R分别由式(1)、(2)得到：

[0010] T＝[x1-x0 y1-y0 z1-z0]T＝[Tx Ty Tz]T (1)

[0011] 式(1)中，(x0y0z0)为GPS系统输出的无人机首次拍摄获取图片I0时对应的无人机位置点坐标，(x1y1z1)为GPS系统输出的无人机首次拍摄获取图片I1时对应的无人机位置点坐标；Tx表示无人机在两相邻时刻位置点之间沿X轴方向的平移向量，Ty表示无人机在两相邻时刻位置点之间沿Y轴方向的平移向量，Tz表示无人机在两相邻时刻位置点之间沿Z轴方向的平移向量；

[0012]

[0013] 式(2)中，Rψ表示无人机在相邻时刻两位置点间绕X轴旋转的旋转矩阵，Rθ表示无人机在相邻时刻两位置点间绕Y轴旋转的旋转矩阵，表示无人机在相邻时刻两位置点间绕Z轴旋转的旋转矩阵，ψ为偏航角，θ为俯仰角，为横滚角。

[0014] 进一步，所述步骤S3具体包括以下步骤：

[0015] S3.1：对步骤S2获得的匹配点集D中的所有匹配点进行归一化处理；

[0016] S3.2：根据式(3)计算出第n次迭代第i对匹配点ri与经本质矩阵En变化后的对应的匹配点ri'之间的距离

[0017]

[0018] 将与预先设置好的阈值L比较：时，记此点为内点；否则，记此点为外点；从而获得内点集合Dn；若内点数大于前次循环获得的内点数，则更新本质矩阵E；

[0019] S3.3：根据式(4)利用随机抽样一致算法计算满足置信率P的本质矩阵的最小迭代次数N：

[0020]

[0021] 式(4)中，ε为外点概率，S为每个样本中的匹配点的对数；

[0022] S3.4：利用五点算法计算本质矩阵，也即随机抽取S＝5对匹配点计算出本质矩阵E；

[0023] S3.5：重复步骤S3.2-S3.4，迭代N次后获得最优本质矩阵。

[0024] 进一步，所述步骤S3.2中，本质矩阵E更新过程中的初值由GPS和IMU系统提供的无人机位姿信息来提供。

[0025] 进一步，所述步骤S4具体包括以下步骤：

[0026] S4.1：对获得的本质矩阵进行SVD分解，得到旋转矩阵R和平移向量T的初步解：R＝UWVT或者R＝UWTVT；T＝u3或者T＝-u3；U和V为3×3的正交矩阵；U里面的向量是正交的，称为T左奇异向量，U的列为EE的正交特征向量；V里面的向量是正交的，称为右奇异向量，V的列为ETE的正交特征向量；E＝Udiag(σ,σ,0)VT，σ为本质矩阵的奇异值，u3为矩阵U的最后一列，[0027]

[0028] S4.2：对经去除误匹配点之后最终获取的匹配点集中的特征点进行三维重建，通过三维点深度值为正来确定旋转矩阵R和平移向量T的唯一解；

[0029] S4.3：利用旋转矩阵R求解出无人机的三个姿态角：偏航角ψ、俯仰角θ、横滚角[0030] 有益效果：本发明公开了一种辅助信息与随机抽样一致算法融合的无人机位姿估计方法，与现有技术相比，具有如下的有益效果：

[0031] 1)对比单纯利用GPS和IMU系统实现无人机位姿估计的方法，本发明弱化了在位姿估计中对GPS系统过度依赖，以及IMU器件存在的器件漂移、误差累积等问题对位姿估计的影响；

[0032] 2)对比单纯利用视觉信息实现无人机位姿估计的方法，本发明具有较好的环境适应能力和抗干扰能力；

[0033] 3)本发明在GPS、IMU与视觉信息的数据融合中，利用随机抽样一致算法结合五点算法计算无人机的位姿，本发明对此算法的处理使得位姿估计具有较高的精度和鲁棒性。附图说明

[0034] 图1为本发明具体实施方式中无人机在飞行状态下对同一目标区域不同角度的两次观测示意图；

[0035] 图2为本发明具体实施方式中方法的流程图；

[0036] 图3(a)为本发明具体实施方式中现有技术中的原始方法与本发明方法对无人机位姿估计的偏航角的误差对比图；

[0037] 图3(b)为本发明具体实施方式中现有技术中的原始方法与本发明方法对无人机位姿估计的俯仰角的误差对比图；

[0038] 图3(c)为本发明具体实施方式中现有技术中的原始方法与本发明方法对无人机位姿估计的横滚角的误差对比图。

具体实施方式

[0039] 本具体实施方式公开了一种辅助信息与随机抽样一致算法融合的无人机位姿估计方法，如图2所示，包括以下步骤：

[0040] S1：利用GPS和IMU系统提供的无人机的辅助信息计算出本质矩阵；辅助信息包括位置和姿态信息；

[0041] S2：无人机在不同角度对同一目标区域拍摄，利用SURF算法对相邻时刻拍摄的两幅航拍图像进行特征点提取与匹配，同时获得对应的匹配点集D，并对匹配点集D中的各个坐标点进行归一化处理；

[0042] S3：将步骤S1获得的本质矩阵作为辅助信息与随机抽样一致算法相融合，初步剔除匹配点集D中存在的误匹配点，再利用随机抽样一致算法结合五点算法对本质矩阵进行迭代求解，最终得到满足精度要求的本质矩阵；

[0043] S4：对获得的本质矩阵进行SVD分解得到各相邻时刻航拍图像间摄像机的平移向量和旋转矩阵，利用旋转矩阵求解无人机的三个姿态角，最终实现无人机的位姿估计。

[0044] 本质矩阵通过平移向量T和旋转矩阵R求得，平移向量T和旋转矩阵R分别由式(1)、(2)得到：

[0045] T＝[x1-x0 y1-y0 z1-z0]T＝[Tx Ty Tz]T (1)

[0046] 式(1)中，(x0y0z0)为GPS系统输出的无人机首次拍摄获取图片I0时对应的无人机位置点坐标，(x1y1z1)为GPS系统输出的无人机首次拍摄获取图片I1时对应的无人机位置点坐标；Tx表示无人机在两相邻时刻位置点之间沿X轴方向的平移向量，Ty表示无人机在两相邻时刻位置点之间沿Y轴方向的平移向量，Tz表示无人机在两相邻时刻位置点之间沿Z轴方向的平移向量；如图1为无人机对同一目标区域从不同的角度拍摄获取图像I0、I1；

[0047]

[0048] 式(2)中，Rψ表示无人机在相邻时刻两位置点间绕X轴旋转的旋转矩阵，Rθ表示无人机在相邻时刻两位置点间绕Y轴旋转的旋转矩阵，表示无人机在相邻时刻两位置点间绕Z轴旋转的旋转矩阵，ψ为偏航角，θ为俯仰角，为横滚角。

[0049] 步骤S3具体包括以下步骤：

[0050] S3.1：对步骤S2获得的匹配点集D中的所有匹配点进行归一化处理；

[0051] S3.2：根据式(3)计算出第n次迭代第i对匹配点ri与经本质矩阵En变化后的对应的匹配点ri'之间的距离

[0052]

[0053] 将与预先设置好的阈值L比较：时，记此点为内点；否则，记此点为外点；从而获得内点集合Dn；若内点数大于前次循环获得的内点数，则更新本质矩阵E；本质矩阵E更新过程中的初值由GPS和IMU系统提供的无人机位姿信息来提供；

[0054] S3.3：根据式(4)利用随机抽样一致算法计算满足置信率P的本质矩阵的最小迭代次数N：

[0055]

[0056] 式(4)中，ε为外点概率，S为每个样本中的匹配点的对数；

[0057] S3.4：利用五点算法计算本质矩阵，也即随机抽取S＝5对匹配点计算出本质矩阵E；

[0058] S3.5：重复步骤S3.2-S3.4，迭代N次后获得最优本质矩阵。

[0059] 步骤S4具体包括以下步骤：

[0060] S4.1：对获得的本质矩阵进行SVD分解，得到旋转矩阵R和平移向量T的初步解：R＝T T TUWV或者R＝UWV ；T＝u3或者T＝-u3；U和V为3×3的正交矩阵；U里面的向量是正交的，称为左奇异向量，U的列为EET的正交特征向量；V里面的向量是正交的，称为右奇异向量，V的列为ETE的正交特征向量；E＝Udiag(σ,σ,0)VT，σ为本质矩阵的奇异值，u3为矩阵U的最后一列，[0061]

[0062] S4.2：对经去除误匹配点之后最终获取的匹配点集中的特征点进行三维重建，通过三维点深度值为正来确定旋转矩阵R和平移向量T的唯一解；

[0063] S4.3：利用旋转矩阵R求解出无人机的三个姿态角：偏航角ψ、俯仰角θ、横滚角[0064] 下面利用MATLAB仿真对本发明的性能进行分析。

[0065] 步骤S1的具体内容为：由GPS和IMU系统提供的无人机在两视点的位姿信息；利用SURF算法获取两幅航拍图像的匹配点集D，并匹配点集中的各个坐标点进行归一化处理；由GPS和IMU系统提供的无人机位姿信息计算本质矩阵的初值；利用公式(3)计算出此时的内点数和外点数及对应的外点概率ε，确定置信率P、S的初值；由公式(4)计算利用随机抽样一致算法在求解本质矩阵过程中的最小迭代次数N；以及摄像机的内参矩阵其表示形式为：

[0066]

[0067] 假设摄像机内参固定，其中fu＝f/du，fv＝f/dv，分别表示焦距在像素坐标系的u轴和v轴方向上的尺度因子，其中f为像机焦距，u0,v0表示光学中心坐标点。

[0068] 步骤S2的具体内容为：

[0069] ①首先对图像进行预处理后用SURF算法检测每幅图像上的特征点并将其画出，[0070] ②利用SURF算法对两幅图像进行处理，筛选出较好的匹配点并画出匹配结果。

[0071] ③采用原始方法即：经SURF算法获得的匹配点并进行归一化处理的基础上，再经随机抽样一致算法结合五点算法对匹配点进行进一步处理最终获得最优本质矩阵及其对应内点集，并对获取的内点进行匹配。

[0072] ④采用本发明方法即：首先利用GPS、IMU系统提供的位姿信息计算对应的本质矩阵，采用SURF算法获取匹配点集D并进行归一化处理，然后，利用本质矩阵对匹配点的约束关系对匹配点集D中包含的误匹配点进行初次剔除获得内点集D0，在此基础上进一步利用随机抽样一致算法结合五点算法对本质矩阵进行优化最终获得最优本质矩阵，及其对应的内点集，并对内定进行匹配。

[0073] 利用本发明方法和原始方法分别对25幅图进行处理求解出的无人机在各个拍摄位置点的位姿，并与无人机的实际位姿信息对比，分析两种算法位姿估计误差，其结果如图3所示，由实验结果可知本发明，能够在一定程度上提高位姿估计的精度，且在GPS和IMU系统提供的位姿信息的辅助下缩短了获取位姿信息的时间，提高了位姿估计的鲁棒性。

标题	发布/更新时间	阅读量
一种基于学习的稀疏表示的超分辨率图像重建方法	2020-05-11	851
基于非凸非光滑二阶正则项和稀疏保真项的优化方法	2020-05-12	514
作业型飞行机器人的四自由度机械臂遥操作系统及方法	2020-05-13	644
成像信号提取装置及其使用方法	2020-05-12	185
保持锐利特征的三维重建方法及系统	2020-05-11	988
3D面部模型重建装置和方法	2020-05-11	75
一种基于人脸三维重建的改进PCA算法	2020-05-12	738
一种基于稀疏变换的低剂量CT图像重建方法	2020-05-08	649
一种图像超分辨率的方法、装置、电子设备及存储介质	2020-05-08	584
基于自样例增强的超分辨率图像重建方法	2020-05-08	276

辅助信息与随机抽样一致算法融合的无人机位姿估计方法

辅助信息与随机抽样一致算法融合的无人机位姿估计方法

技术领域

背景技术

发明内容

具体实施方式

该功能需要专业版企业版VIP权限，您可以：