[发明专利]基于透视投影直线的几何约束位姿方法

摘要

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

主权项

1.一种基于透视投影直线的几何约束位姿方法，其特征是，设:Πc为感光平面，l为透视投影直线，点Pi为位于透视投影直线l上的待测目标点，任意一条透视投影直线l均穿过平面Πm和平面Πn，交感光平面Πc于点I，则透视投影直线l由其与平面Πm的交点Pm、与平面Πn的交点Pn确定，忽略平面的深度信息，平面∏ε(x,y)和平面∏c(u,v)之间存在一一映射关系，ε为n或m，前述字符组合右下角标c、m、n、ε表示平面序号，右上角标m、n、i表示点位于该平面上，gx和gy表示平面对应的有理函数，u、v表示特征点的图像坐标，将此映射定义为：则点Pi(xi,yi,zi)由式(2)表示：式(2)中，唯一的未知量为点Pi的深度信息zi，若可以通过多个点特征之间的几何约束得到其深度信息zi，则通过式(2)可以得到特征点的三维坐标，从而解算测量目标位置和姿态参数，右下角标x和y分别表示x坐标和y坐标，fx和fy分别为特征点的x坐标和y坐标的函数；深度信息zⁱ获得步骤：点和点所在坐标系为坐标系o‑ijk为摄像机坐标系，则点和点之间的关系为：式中式中R为旋转矩阵，T为平移矩阵，右下角标k表示点序号，其列向量为单位向量且相互正交，选用非共面的4个特征点组成高级几何特征求解式(3)，4个特征点在世界坐标系和图像坐标系的坐标值已知，因此，4个特征点在摄像机坐标系下的坐标由式(2)表示，即对进行求解来得到特征点在摄像机坐标系下的坐标值；特征点的连线在空间形成一个几何图形，根据该几何图像的空间几何形状，物体的空间几何形状包括线段、角度、平面，来实现对的求解，4个点围成的图形的空间几何模型已知，该几何图形包括个三角形，其中每个三角形具有3条边，任意三角形的边长可表示为：其中，右下角标σ、τ、υ、ω表示点序号，fxτ表示点τ的x坐标函数，任意一个三角形具有3个角，同时任意4个点可以形成3对向量，每对向量形成1个夹角，角度约束根据下式来表示：采用式(6)中所示的约束，其中P′为点P3到平面P0P1P2的投影；综合上述条件得到如下的方程组，角标w表示点位于世界坐标系内：由于方向目标函数r比距离目标函数e(σ,τ)、角度目标函数h(σ,τ,υ)和l(σ,τ,υ,ω)的收敛速度明显要快，同时距离约束的重要性要高于角度约束，所以将目标约束函数r乘以惩罚因子M₁,e(σ,τ)乘以惩罚因子M₂构建关于的无约束非线性最优化目标函数,k＝0,1,2,3：采用迭代方法Levenberg‑Marquardt优化即可求解出最终的