一种基于多相机系统的动平台相对位姿估计方法技术方案

本发明专利技术涉及视觉导航技术领域，提供一种基于多相机系统的动平台相对位姿估计方法，包括：利用动平台中多相机系统连续三个时刻同步采集图像，连续三个时刻的图像组成图像对；结合FAST算子、金字塔LK光流法追踪匹配点对；基于前后向误差策略剔除误匹配点对；随机选取一匹配点对，基于阿克曼运动模型，求解动平台连续三个时刻的相对位姿，利用每个解对每个匹配点对对应的空间点重投影并计算重投影误差来判断内点，将内点总数最大者作为较优解，将本次迭代的较优解与上次迭代的最优解中内点总数较大者作为本次迭代的最优解，直到迭代结束，将最优解作为相对位姿。本发明专利技术能够提高动平台相对位姿估计的精度与效率，降低成本，适用于纹理缺乏场景。用于纹理缺乏场景。用于纹理缺乏场景。

【技术实现步骤摘要】
一种基于多相机系统的动平台相对位姿估计方法


[0001]本专利技术涉及视觉导航
，尤其是涉及一种基于多相机系统的动平台相对位姿估计方法。

技术介绍

[0002]目前，动平台相对位姿估计很大程度上依赖于雷达、激光、GPS等传感器，这些传感器体积大、成本高，而体积小、成本低的相机一般只作为辅助传感器出现。因为体积、成本等因素，舰船、飞机、汽车尤其是无人机、无人车、轮式机器人、手机等动平台上普遍安装了相机，使得相机具有很大的潜能成为动平台位姿估计任务的主要传感器。
[0003]为充分利用相机在动平台相对位姿估计中的优势，现有技术中出现了很多纯视觉相对位姿估计方法。虽然这些纯视觉相对位姿估计方法具有很多优点，但是其在实际工程应用中面临着各种各样的问题：(1)部分方法因需离线处理、计算代价较高或运算速度较慢等原因而无法满足实际工程应用领域对高实时性、低运算成本的需求，尤其是无人驾驶等领域对较高实时性的需求；(2)部分方法对处理器的要求较高，造成动平台位姿估计系统的成本增加；(3)部分方法只适用于一些特殊的图像视频采集设备，适用性及移植性不高；(4)通常位姿估计方法需要较多的特征匹配点对，在纹理缺乏的场景下很难独立完成位姿估计任务。上述问题导致现有纯视觉相对位姿估计方法仍然很少应用于动平台的相对位姿估计中，尤其难以应用于无人驾驶领域中。因此，迫切需要更加高效的纯视觉相对位姿估计方法。

技术实现思路

[0004]针对现有技术存在的问题，本专利技术提供一种基于多相机系统的动平台相对位姿估计方法，能够提高动平台相对位姿估计的精度与效率，降低成本，且适用于纹理缺乏场景。
[0005]本专利技术的技术方案为：
[0006]一种基于多相机系统的动平台相对位姿估计方法，包括下述步骤：
[0007]步骤1：获取图像对
[0008]利用动平台中多相机系统的所有相机连续三个时刻同步采集周围环境的图像，从每一时刻所有相机采集的图像中随机选取一个图像，连续三个时刻选出的图像组成图像对，多次选取得到图像对集合；
[0009]步骤2：追踪匹配点对
[0010]对于图像对集合中的每个图像对，利用FAST算子检测第一帧图像的特征点，利用金字塔LK光流法在第二帧图像、第三帧图像中分别正向跟踪FAST算子提取到的特征点，得到的三个特征点构成匹配点对，所有图像对的匹配点对构成匹配点对初始集合；
[0011]步骤3：剔除误匹配点对
[0012]基于前后向误差策略，剔除匹配点对初始集合中的误匹配点对，得到匹配点对集合；
[0013]步骤4：基于阿克曼运动约束及RANSAC框架估计相对位姿
[0014]步骤4.1：从匹配点对集合中随机选取一个匹配点对；
[0015]步骤4.2：基于动平台的阿克曼运动模型，利用选取的匹配点对求解动平台连续三个时刻之间的相对位姿，得到多个解；
[0016]步骤4.3：对于每个解，利用该解对应的相对位姿，将匹配点对集合中每个匹配点对对应的空间3D点重投影到图像平面上，计算每个匹配点对的重投影误差，当重投影误差小于或等于预设的误差阈值时判断该匹配点对为内点，当重投影误差大于预设的误差阈值时判断该匹配点对为外点，记录该解下的内点总数，将选取的匹配点对的所有解中内点总数最大的解作为较优解，将本次迭代的较优解与上次迭代的最优解中内点总数较大者作为本次迭代的最优解；
[0017]步骤4.4：重复步骤4.1至步骤4.3，直到迭代结束，将最后一次迭代的最优解对应的位姿作为动平台连续三个时刻的相对位姿。
[0018]进一步的，所述步骤3具体包括下述步骤：
[0019]步骤3.1：对匹配点对初始集合中的每个匹配点对，以在第三帧图像中跟踪到的特征点为基准，利用金字塔LK光流法反向跟踪第二帧图像、第一帧图像中对应的特征点；
[0020]步骤3.2：计算第一帧图像中FAST算子正向提取的特征点与金字塔LK光流法反向跟踪的特征点之间的坐标差；
[0021]步骤3.3：剔除坐标差超出设定的坐标差阈值的匹配点对，剩下的匹配点对构成匹配点对集合。
[0022]进一步的，所述步骤4.2中，动平台的阿克曼运动模型为：动平台中多相机系统连续时刻做匀速阿克曼运动，在转弯时绕瞬时旋转中心ICR做圆周运动，在直线运动时为半径无穷大的圆周运动，多相机系统的极坐标为(ρ,δ)，多相机系统的x轴向前垂直于圆周运动的半径、y轴指向瞬时旋转中心ICR，从而θ为前后时刻多相机系统的偏航角，ρ为前后时刻多相机系统的平移向量尺度。
[0023]进一步的，所述步骤4.2中，利用选取的匹配点对求解动平台连续三个时刻之间的相对位姿，具体包括：
[0024]选取的匹配点对(p1,p2,p3)中p1、p2、p3分别为空间点P在连续三个时刻k、k+1、k+2的像点，其Pl
ü
cker直线坐标分别为cker直线坐标分别为假设动平台在短时间间隔内的速度相同，则连续时刻k、k+1、k+2的相对位姿参数(ρ,θ)相同，相邻两帧的相对旋转矩阵R和相对平移向量t分别为
[0025][0026]将式(1)分别代入广义相机对极约束中，得到时刻k与k+1之间、k+1与k+2之间的新的广义相机对极约束分别为
[0027][0028][0029]其中，
[0030]利用三角函数的半角公式
[0031][0032][0033][0034]令结合式(4)至式(6)，利用式(2)、式(3)分别得到平移向量尺度ρ的两个表达式为
[0035][0036][0037]因此
[0038][0039]由式(9)，得到
[0040]f(n)＝En2+Fn+G＝0 (10)
[0041]其中，E、F和G均为m的函数，
[0042][0043]令H＝m2，则可写为g(n)＝n2+H
‑
1＝0；
[0044]f(n)和g(n)有公根的充分必要条件是结式
[0045][0046]从而得到关于m的6阶方程
[0047]Am6+Bm4+Cm2+D＝0 (13)
[0048]其中，A,B,C和D由系数(a1,b1,c1,d1,e1)和(a2,b2,c2,d2,e2)组成；
[0049]当匹配点对来自同一个相机的三帧图像时，a1＝
‑
e1，a2＝
‑
e2，则
[0050][0051]令q＝m2，则式(13)简化为
[0052]q(Aq2+Bq1+C)＝0 (15)
[0053]从而得到q的三个解为
[0054][0055]由于q＝m2，从而得到m的5个解，其中一个为零解；
[0056]根据公式及式(7)，得到偏航角θ＝2arcsin(m)、平移向量尺度代入式(1)，得到动平台连续三个时刻之间的相对位姿(R,t)的多个值。
[0057]进一步的，所述步骤4.3中，对于每个解，利用该解对应的相对位姿，将本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于多相机系统的动平台相对位姿估计方法，其特征在于，包括下述步骤：步骤1：获取图像对利用动平台中多相机系统的所有相机连续三个时刻同步采集周围环境的图像，从每一时刻所有相机采集的图像中随机选取一个图像，连续三个时刻选出的图像组成图像对，多次选取得到图像对集合；步骤2：追踪匹配点对对于图像对集合中的每个图像对，利用FAST算子检测第一帧图像的特征点，利用金字塔LK光流法在第二帧图像、第三帧图像中分别正向跟踪FAST算子提取到的特征点，得到的三个特征点构成匹配点对，所有图像对的匹配点对构成匹配点对初始集合；步骤3：剔除误匹配点对基于前后向误差策略，剔除匹配点对初始集合中的误匹配点对，得到匹配点对集合；步骤4：基于阿克曼运动约束及RANSAC框架估计相对位姿步骤4.1：从匹配点对集合中随机选取一个匹配点对；步骤4.2：基于动平台的阿克曼运动模型，利用选取的匹配点对求解动平台连续三个时刻之间的相对位姿，得到多个解；步骤4.3：对于每个解，利用该解对应的相对位姿，将匹配点对集合中每个匹配点对对应的空间3D点重投影到图像平面上，计算每个匹配点对的重投影误差，当重投影误差小于或等于预设的误差阈值时判断该匹配点对为内点，当重投影误差大于预设的误差阈值时判断该匹配点对为外点，记录该解下的内点总数，将选取的匹配点对的所有解中内点总数最大的解作为较优解，将本次迭代的较优解与上次迭代的最优解中内点总数较大者作为本次迭代的最优解；步骤4.4：重复步骤4.1至步骤4.3，直到迭代结束，将最后一次迭代的最优解对应的位姿作为动平台连续三个时刻的相对位姿。2.根据权利要求1所述的基于多相机系统的动平台相对位姿估计方法，其特征在于，所述步骤3具体包括下述步骤：步骤3.1：对匹配点对初始集合中的每个匹配点对，以在第三帧图像中跟踪到的特征点为基准，利用金字塔LK光流法反向跟踪第二帧图像、第一帧图像中对应的特征点；步骤3.2：计算第一帧图像中FAST算子正向提取的特征点与金字塔LK光流法反向跟踪的特征点之间的坐标差；步骤3.3：剔除坐标差超出设定的坐标差阈值的匹配点对，剩下的匹配点对构成匹配点对集合。3.根据权利要求1所述的基于多相机系统的动平台相对位姿估计方法，其特征在于，所述步骤4.2中，动平台的阿克曼运动模型为：动平台中多相机系统连续时刻做匀速阿克曼运动，在转弯时绕瞬时旋转中心ICR做圆周运动，在直线运动时为半径无穷大的圆周运动，多相机系统的极坐标为(ρ,δ)，多相机系统的x轴向前垂直于圆周运动的半径、y轴指向瞬时旋转中心ICR，从而θ为前后时刻多相机系统的偏航角，ρ为前后时刻多相机系统的平移向量尺度。4.根据权利要求3所述的基于多相机系统的动平台相对位姿估计方法，其特征在于，所
述步骤4.2中，利用选取的匹配点对求解动平台连续三个时刻之间的相对位姿，具体包括：选取的匹配点对(p1,...

【专利技术属性】
技术研发人员：田苗，周峰，韩国良，徐文玲，雷成敏，马瑶瑶，陈河，
申请(专利权)人：中国人民解放军九三二三六部队，
类型：发明
国别省市：