CN105913417B - 基于透视投影直线的几何约束位姿方法 - Google Patents

Abstract

本发明涉及单目视觉位姿解算方法，为解决非共面P4P问题，解决现有P4P算法测量范围小、测量精度低的问题，实现目标位姿的大范围、高精度测量，实现目标位姿的大范围、高精度测量，为此，本发明采用的技术方案是，根据通用摄像机成像系统，提出透视投影直线，结合4个非共面特征点之间的几何约束，建立求解4个非共面特征点在摄像机坐标系下坐标的最优目标函数；根据求解得到的特征点坐标，求解了目标的旋转矩阵和平移向量，从而得到目标的位置和姿态参数。本发明主要应用于单目视觉位姿解算场合。

Description

基于透视投影直线的几何约束位姿方法

技术领域

本发明涉及单目视觉位姿解算方法，具体涉及一种基于透视投影直线的几何约束位姿方法。

背景技术

单目视觉位姿解算算法，是通过单个视觉传感器获取目标图像特征，由二维图像特征推算目标三维空间坐标。根据图像特征种类，其又可定义为PnP问题：即通过点特征数据获取目标的位姿信息。点特征数据一般由一系列点对组成，点对包括物方空间标识点和其对应的投影点。对PnP问题的各种解决方案，例如EPnP算法、DLS算法、RPnP算法、ASPnP算法和LHM算法等等均可以快速、高精度解决PnP问题。但是，上述算法仅适用于几十个甚至上百个点对的情景，且其测量精度随着点对数量的减少而迅速下降。考虑到视觉位姿测量技术多应用于恶劣的测量环境中，测量目标很难提供足够数量的稳定的可识别的标识点。其中DLS算法可应用在n<7的情况，但是目标的运动范围和运动方式受到极大约束。因此，PnP算法，尤其是P4P算法，仍然存在很大的提高空间。现有的P4P算法可以归纳为两类：一类是基于成像模型的算法，此类算法依赖于其几何成像模型相对于实际成像模型的近似程度；另一类为几何构型算法，通过计算几何特征在图像空间和物方空间之间的旋转平移变换得到目标的位姿信息。此算法所采用的几何特征包括距离、夹角、平行和垂直等等。POSIT算法是第一类算法的代表性解决方案且是解决P4P问题的常用算法。其使用正交缩放投影获取目标的初始旋转矩阵和平移向量，然后通过迭代更新标识点在正交缩放投影中对应的图像坐标，进而获取更加准确的旋转矩阵和平移向量，重复此迭代过程直至得到足够精度的旋转矩阵和平移向量。由于POSIT算法的稳定性和高精度，其不断被引入到复杂干扰环境的视觉位姿测量应用。第二类算法利用了特殊标识点的几何构型优势，其在近年的P4P算法研究中占据了重要地位。Liu.M.L.等人充分利用4个非共面标识点的几何构型约束，包括夹角、混合积和距离约束等等。后续的算法研究均没有超出Liu.M.L.等人提出的几何构型约束。Z.Y.Hu等人使用数学方法分析了非共面4点的几何构型约束问题。他们量化了位姿求解方程可能解的个数和几何构型的数量之间的关系。Wu PC等人提出了一个解析运动模型用于排除可能解。Yang Guo研究了共面P4P问题，通过透视投影和仿射投影的转换及仿射变换不变形，提出并验证了共面P4P问题的解的上限2个。同时提出了应用于共面P4P问题的奇异值分解算法，但没有进行实验验证。Long Li等人引入了Frobenius范数替代奇异值分解，但是该方法仅提高了算法的运行速度，未能提高算法的精度和抗噪声能力。Bujnak M.等人和Kuang Y.等人研究了从P4P算法反算有效焦距的问题，忽略了P4P算法的精度。总结上述研究可以发现，P4P算法的发展过程缓慢，尤其在算法精度研究方面。

发明内容

为克服现有技术的不足，解决非共面P4P问题，解决现有P4P算法测量范围小、测量精度低的问题，实现目标位姿的大范围、高精度测量，为此，本发明采用的技术方案是，基于透视投影直线的几何约束位姿方法,设：Π_c为感光平面，1为透视投影直线，点Pⁱ为位于透视投影直线1上的待测目标点，任意一条透视投影直线1均穿过平面Π_m和平面Π_n，交感光平面Π_c于点I，则透视投影直线l由其与平面Π_m的交点P^m、与平面Π_n的交点Pⁿ确定，忽略平面的深度信息，平面∏_ε(x,y)到平面∏_c(u,v)的映射中ε为n或m，平面∏_ε(x,y)和平面∏_c(u,v)之间存在一一映射关系，前述字符组合右下角标c、m、n、ε表示平面序号，右上角标m、n、i表示点位于该平面上，g_x和g_y表示平面对应的有理函数，u、v表示特征点的图像坐标，将此映射定义为：

则点Pⁱ(xⁱ,yⁱ,zⁱ)由式(2)表示。

式(2)中，唯一的未知量为点Pⁱ的深度信息zⁱ，若可以通过多个点特征之间的几何约束得到其深度信息zⁱ，则通过式(2)可以得到特征点的三维坐标，从而解算测量目标位置和姿态参数，右下角标x和y分别表示x坐标和y坐标，f_x和f_y分别为特征点的x坐标和y坐标的函数；

深度信息zⁱ获得步骤：点和点所在坐标系为坐标系o-ijk为摄像机坐标系，则点和点之间的关系为：

式中R为旋转平移矩阵，右下角标k表示点序号，其列向量为单位向量且相互正交，选用非共面的4个特征点组成高级几何特征求解式(3)，4个特征点在世界坐标系和图像坐标系的坐标值已知，因此，4个特征点在摄像机坐标系下的坐标由式(2)表示，即对进行求解来得到特征点在摄像机坐标系下的坐标值；

特征点的连线在空间形成一个几何图形，根据该几何图像的空间几何形状，物体的空间几何形状包括线段、角度、平面，来实现对的求解，4个点围成的图形的空间几何模型已知，该几何图形包括个三角形，其中每个三角形具有3条边，任意三角形的边长可表示为：

其中，右下角标σ、τ、υ、ω表示点序号，f_xτ表示点τ的x坐标函数，任意一个三角形具有3个角，同时任意4个点可以形成3对向量，每对向量形成1个夹角，角度约束根据下式来表示：

采用式(6)中所示的约束，其中P′为点P₃到平面P₀P₁P₂的投影；

综合上述条件得到如下的方程组(角标w表示点位于世界坐标系内)：

由于方向目标函数r比距离目标函数e(σ,τ)、角度目标函数h(σ,τ,υ)和l(σ,τ,υ,ω)的收敛速度明显要快，同时距离约束的重要性要高于角度约束，所以将目标约束函数r乘以惩罚因子M₁,e(σ,τ)乘以惩罚因子M₂构建关于的无约束非线性最优化目标函数：

采用迭代方法Levenberg-Marquardt优化即可求解出最终的

此外，还包括为提供初值得步骤：使用缩放正交投影模型来近似实际透视成像模型，为提供初值,假设靶标上的4个特征点具有相同的深度zⁱ′，其可由下式获得：

在位姿测量计算过程中，目标初始位置的初值由式(9)得到，下一位置的初值使用前一位置的终值。

本发明的特点及有益效果是：

本发明提出了一种单目视觉位姿测量算法。根据通用摄像机成像系统，提出了透视投影直线，结合4个非共面特征点之间的几何约束，建立了求解4个非共面特征点在摄像机坐标系下坐标的最优目标函数。根据求解得到的特征点坐标，求解了目标的旋转矩阵和平移向量，从而得到了目标的位置和姿态参数。

该算法适用于大部分摄像机，尤其是大视场角、大畸变摄像机，而且提高了非共面P4P算法的位姿解算精度。因而对于需要目标位姿准确测量的场合，本发明有较好的技术优势。

附图说明

图1成像光线穿过光学系统被感光元件吸收。

图2成像光线追踪模型几何化。

图3参考平面和图像平面之间的点点映射。

图4位姿测量过程中的投影线。

图5投影线成像模型。

图6特征点方向约束。

图7实验设备示意图。

图8实验数据图。

具体实施方式

如图1所示，视觉传感器成像过程中，感光元件上的点有且仅有一条成像光线与之对应。基于此现象，本发明提出了基于透视投影直线的几何约束位姿算法。

由于成像光线的路径复杂，其在描述成像系统几何特性过程中数学等价为透视投影直线，如图2所示。图2中，Π_c为感光平面，1为透视投影直线，点Pⁱ为位于透视投影直线1上的待测目标点。观察图2可知，任意一条透视投影直线1均穿过平面Π_m和平面Π_n，交感光平面Π_c于点I。则透视投影直线1可由其与平面Π_m的交点P^m、与平面Π_n的交点Pⁿ确定。忽略平面的深度信息，平面∏_σ(x,y)到平面∏_c(u,v)的映射如图3所示，其中σ可为n或m。平面∏_σ(x,y)和平面∏_c(u,v)之间存在一一映射关系，g_x和g_y表示平面对应的有理函数，u、v表示特征点的图像坐标，将此映射定义为：

则点Pⁱ(xⁱ,yⁱ,zⁱ)可由式(2)表示。

式(2)中，右下角标x和y分别表示x坐标和y坐标，f_x和f_y分别为特征点的x坐标和y坐标的函数；唯一的未知量为点Pⁱ的深度信息zⁱ，若可以通过多个点特征之间的几何约束得到其深度信息zⁱ，则可得到特征点的三维坐标。

单目视觉位姿测量模型如图4所示，点和点所在坐标系为坐标系o-ijk为摄像机坐标系，则点和点之间的关系为：

式中R为旋转平移矩阵，其列向量为单位向量且相互正交，因此其自由度为3；T为平移向量，自由度为3。式(3)中共有6个未知向量，因此至少需要6个点，才可线性求解式(3)。点特征匹配难度随其数量增多而增大，因此本发明考虑用尽可能少的点特征获取足够精度的位姿信息。2个特征点仅能确定一条直线，3个特征点仅能确定一个平面，因此选用了非共面的4个特征点组成高级几何特征。4点透视投影直线模型如图5所示，4个特征点在世界坐标系和图像坐标系的坐标值已知。因此，4个特征点在摄像机坐标系下的坐标可由式(2)表示，即对进行求解就可以得到特征点在摄像机坐标系下的坐标值。

图5中特征点的连线在空间形成一个几何图形，根据该几何图像的空间几何形状(物体的空间几何形状包括线段、角度、平面)来实现对的求解，4个点围成的图形的空间几何模型已知，该几何图形包括个三角形，其中每个三角形具有3条边，任意三角形的边长可表示为：

其中，右下角标σ、τ、υ、ω表示点序号，f_xτ表示点τ的x坐标函数，任意一个三角形具有3个角，同时任意4个点可以形成3对向量，每对向量形成1个夹角，角度约束可以根据下式来表示：

如果只考虑距离与角度约束，会出现图6所示的两组解，为了避免这种情况发生还需要考虑式(6)中所示的约束，其中P′为点P₃到平面P₀P₁P₂的投影。

式(6)中w表示点在世界坐标系下坐标。

综合上述条件得到如下的方程组：

由于方向目标函数r比距离目标函数e(σ,τ)、角度目标函数h(σ,τ,υ)和l(σ,τ,υ,ω)的收敛速度明显要快，同时距离约束的重要性要高于角度约束，所以将目标约束函数r乘以惩罚因子M₁,e(σ,τ)乘以惩罚因子M₂构建关于的无约束非线性最优化目标函数：

采用Levenberg-Marquardt优化方法即可求解出最终的由于整个求解过程是一个非线性迭代求解过程，因此还需要为提供初值，保证非线性算法的精度与收敛速度。

本发明使用缩放正交投影模型来近似实际透视成像模型，为提供初值。假设靶标上的4个特征点具有相同的深度zⁱ′，其可由下式获得：

在位姿测量计算过程中，目标初始位置的初值由式(9)得到，下一位置的初值使用前一位置的终值。

使用4个非共面特征点P_k(k＝0,1,2,3)在摄像机坐标系下的坐标计算坐标系

目标位于不同位置时对应的坐标系分别为和则两个坐标系之间的旋转矩阵可根据下式获得(k和k+1表示了不同位置)：

角度Ψ、和θ分别为目标的方位角、俯仰角和横滚角，目标的平移向量T可由下式获得：

如图7所示，将视觉传感器及其支架、旋转台如图安装在光学平台上，靶标固定在旋转台的载物台上。由计算机控制视觉传感器和旋转台，在旋转台带动靶标转动的同时使用视觉传感器拍摄靶标图像；经过图像处理提取特征点图像坐标，将特征点图像坐标和特征点世界坐标输入本发明所提出的基于透视投影直线的几何约束位姿算法，获取4个特征点在摄像机坐标系下的坐标；由4个特征点在摄像机坐标系下的坐标计算目标的位置和姿态信息。

按照上述方式，对本发明所提出算法进行实验验证，实验结果如图8所示。

Claims (2)

1.一种基于透视投影直线的几何约束位姿方法，其特征是，设:Π_c为感光平面，l为透视投影直线，点Pⁱ为位于透视投影直线l上的待测目标点，任意一条透视投影直线l均穿过平面Π_m和平面Π_n，交感光平面Π_c于点I，则透视投影直线l由其与平面Π_m的交点P^m、与平面Π_n的交点Pⁿ确定，忽略平面的深度信息，平面∏_ε(x,y)和平面∏_c(u,v)之间存在一一映射关系，ε为n或m，前述字符组合右下角标c、m、n、ε表示平面序号，右上角标m、n、i表示点位于该平面上，g_x和g_y表示平面对应的有理函数，u、v表示特征点的图像坐标，将此映射定义为：

则点Pⁱ(xⁱ,yⁱ,zⁱ)由式(2)表示：

式(2)中，唯一的未知量为点Pⁱ的深度信息zⁱ，若可以通过多个点特征之间的几何约束得到其深度信息zⁱ，则通过式(2)可以得到特征点的三维坐标，从而解算测量目标位置和姿态参数，右下角标x和y分别表示x坐标和y坐标，f_x和f_y分别为特征点的x坐标和y坐标的函数；

深度信息zⁱ获得步骤：点和点所在坐标系为坐标系o-ijk为摄像机坐标系，则点和点之间的关系为：

式中式中R为旋转矩阵，T为平移矩阵，右下角标k表示点序号，其列向量为单位向量且相互正交，选用非共面的4个特征点组成高级几何特征求解式(3)，4个特征点在世界坐标系和图像坐标系的坐标值已知，因此，4个特征点在摄像机坐标系下的坐标由式(2)表示，即对进行求解来得到特征点在摄像机坐标系下的坐标值；

特征点的连线在空间形成一个几何图形，根据该几何图像的空间几何形状，物体的空间几何形状包括线段、角度、平面，来实现对的求解，4个点围成的图形的空间几何模型已知，该几何图形包括个三角形，其中每个三角形具有3条边，任意三角形的边长可表示为：

其中，右下角标σ、τ、υ、ω表示点序号，f_xτ表示点τ的x坐标函数，任意一个三角形具有3个角，同时任意4个点可以形成3对向量，每对向量形成1个夹角，角度约束根据下式来表示：

采用式(6)中所示的约束，其中P′为点P₃到平面P₀P₁P₂的投影；

综合上述条件得到如下的方程组，角标w表示点位于世界坐标系内：

由于方向目标函数r比距离目标函数e(σ,τ)、角度目标函数h(σ,τ,υ)和l(σ,τ,υ,ω)的收敛速度明显要快，同时距离约束的重要性要高于角度约束，所以将目标约束函数r乘以惩罚因子M₁,e(σ,τ)乘以惩罚因子M₂构建关于的无约束非线性最优化目标函数,k＝0,1,2,3：

采用迭代方法Levenberg-Marquardt优化即可求解出最终的

2.如权利要求1所述的基于透视投影直线的几何约束位姿方法，其特征是，还包括为提供初值得步骤：使用缩放正交投影模型来近似实际透视成像模型，为提供初值,假设靶标上的4个特征点具有相同的深度zⁱ′，其可由下式获得：

在位姿测量计算过程中，目标初始位置的初值由式(9)得到，下一位置的初值使用前一位置的终值。