基于几何约束的物体三维姿态测量方法

技术领域

本发明涉及视觉检测技术、物体三维姿态测量，具体讲涉及基于几何约束的物体三维姿 态测量方法。

背景技术

物体的位置与姿态测量在三维姿态测量技术在航空航天、汽车四轮定位、光笔测量技术、 头盔瞄准定位系统、水下机器人悬停定位等工业领域有重要的应用价值。相比基于磁场的位 姿测量方法，机器视觉法由于不受电磁场的干扰而被广泛的研究。

目前对物体三维姿态进行测量的方法大多是直接对表示物体位姿的旋转平移矩阵进行求 解，这种方法一般面临两个问题：1进行位姿求解特征点的个数偏多，导致计算过程复杂；2 求解过程中特征点的几何形状有特殊要求；这里拟将摄像机坐标系作为中介，利用4个非共 面特征点，首先求解出定位特征点在摄像机坐标系下的坐标，然后再根据特征点在摄像机坐 标系下的坐标得出物体旋转平移矩阵进而得出物体的位姿。

发明内容

为克服现有技术的不足，提供一种基于几何约束的物体三维姿态测量方法，满足现实中 物体三维姿态智能、快速、高精度、低成本的检测需要，本发明采取的技术方案是，基于几 何约束的物体三维姿态测量方法，借助于sigma二维转台、两个摄像机、摄像机支撑架、测 量用靶标和计算机实现，将两个摄像机分别固定在各自的支撑架上并根据转台的高度调整摄 像机的位置，测量靶标为一个立方体除底面外，每个平面上有4个非共面的红外LED作为定 位特征点，使二维转台带动靶标做偏转、俯仰两个自由度的运动，使用两部摄像机分别从不 同角度拍摄定位标志，通过计算机采用串口发送信号至转台的控制箱，控制转台的运动，所 述方法进一步包括如下步骤：

设特征点在摄像机坐标系下的坐标可以表示为，上标c表示摄像机坐标系，在靶标局 部坐标系即世界坐标系下的坐标值可以表示为，上标w表示世界坐标系，相应的图像在世 界坐标系下的坐标I_i＝(x_ui，y_ui)^T(i取值为0、1、2、3中的一个)，T表示转置，因此的 关系可以描述为：

(1)

O_c为摄像机坐标系原点；对h_i进行求解就可以得到特征点在摄像机坐标系下的坐标值， 特征点的连线在空间形成一个几何图形，根据该几何图形的空间形状实现对hi的求解，4个 特征点围成的图形的空间几何模型已知，该几何图形包括个三角形，其中每个三角形具有 三条边，任意三角形的边长可表示为：

任意一个三角形具有三个角，同时任意四个点可以形成三对向量，每对向量形成一个夹 角，角度可以根据向量(j，k，m＝0...3)来表示：

还需要考虑公式(4)中所表示的约束，其中p′为p₂点到平面p₀p₁p₃的投影：

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

将目标约束函数h(i)乘以罚因子M₁，e(i，j)乘以罚因子M₂构建关于h_i(i＝0，1，2，3)的无约 束非线性最优化目标函数：

$F=M_2·\underset{i=0}{\overset{3}{\Sigma }}\underset{j=i+1}{\overset{3}{\Sigma }}e(i,j)+M_1·\underset{i=0}{\overset{3}{\Sigma }}h\left(i\right)+\underset{i=0}{\overset{3}{\Sigma }}\underset{\underset{k\ne i,j}{j=i+1}}{\overset{3}{\Sigma }}f(i,j,k)$

$+\underset{i=0}{\overset{3}{\Sigma }}\underset{\underset{\underset{m\ne i,j,k}{k\ne i,j}}{j=i+1}}{\overset{3}{\Sigma }}g(i,j,k,m)---\left(6\right)$

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

采用缩放正交投影模型用来近似实际透视成像模型，从而获得h_i的初值。

所述采用缩放正交投影模型用来近似实际透视成像模型，从而获得h_i的初值，具体为：

根据弱透视模型，假设靶标上的4个特征点具有相同的深度：将除0号特征点外的特征 点垂直投影到经过0号点且与CCD成像面平行的平面上，设为R_i，其中s＝f/Z₀为缩放因子， 可以通过公式(12)计算出s的值，1/s用来作为h_i的初值：

$\left(\begin{array}{c}a=(P_0{P}_1+P_0{P}_2+P_1{P}_2)(-P_0{P}_1+P_0{P}_2+P_1{P}_2)\\ (P_0{P}_1-P_0{P}_2+P_1{P}_2)(P_0{P}_1+P_0{P}_2-P_1{P}_2)\\ b=I_0{I_1}^2(-P_0{P_1}^2+P_0{P_2}^2+P_1{P_2}^2)+I_0{I_2}^2(P_0{P_1}^2-P_0{P_2}^2+P_1{P_2}^2)\\ +I_1{I_2}^2(P_0{P_1}^2+P_0{P_2}^2-P_1{P_2}^2)\\ c=(I_0{I}_2+I_1{I}_2)(-I_0{I}_1+I_0{I}_2+I_1{I}_2)(I_0{I}_1-I_0{I}_2+I_1{I}_2)(I_0{I}_1+I_0{I}_2-I_1{I}_2)\\ w=\sqrt{\frac{b+\sqrt{b^2-\mathrm{ac}}}{a}}\end{array}\right)---\left(7\right)$

根据系统转轴标定后的结果可以建测量系统坐标系，根据选取偏转旋转轴的O_Ay(x_Ay，y_Ay， z_Ay)点为坐标原点，转轴朝上方向为O_AyY_Ay建立右手坐标系Ay，其确定的旋转变换矩阵为 matrix_Ay；根据俯仰旋转轴的标定结果，可以确定两点s1、s2，物体绕俯仰旋转轴旋转的变换 矩阵为matrix_Ax；设靶标从位置1到位置2绕偏转旋转轴转动的角度为a，绕俯仰旋转轴转动 的角度为b，转动前后特征点在摄像机坐标系下的坐标可以表示为式(8)。

$\left(\begin{array}{cccc}\cos a& 0& \sin a& 0\\ 0& 1& 0& 0\\ -\sin a& 0& \cos a& 0\\ 0& 0& =& 1\end{array}\right){\mathrm{matrix}}_{\mathrm{Ax}}\left(b\right){\mathrm{matrix}}_{\mathrm{Ay}}{\left(\begin{array}{c}{x}_{\mathrm{ci}}^1\\ y_{\mathrm{ci}}^1\\ z_{\mathrm{ci}}^1\\ 1\end{array}\right)}^T={\mathrm{matrix}}_{\mathrm{Ay}}\left(\begin{array}{c}{x}_{\mathrm{ci}}^2\\ y_{\mathrm{ci}}^2\\ z_{\mathrm{ci}}^2\\ 1\end{array}\right)---\left(8\right)$

公式(8)可以改写为其中

跟据旋转平移矩阵的定义可以得到式(9)，跟据公式(9)可以确定2个3×3的单位正交矩 阵，并通过这两个单位正交矩阵最终确定R，根据式(10)来确定单位正交矩阵并最终得到R 与T：

本发明其特点在于：本发明在4个非共面特征点的基础上采用基于几何约束的测量技术 实现物体三维姿态的快速测量。在满足测量精度要求的前提下，可以实现智能、快速的在线 测量。尤其对于航空航天领域的应用，本技术优势更为明显。

附图说明

图1系统结构图。图中，1为转台，2为摄像机，3为标准球，4为红外LED。

图24点透视投影成像模型。

图34点透视投影的两种情况。

图44点缩放正交投影模型。

图5转轴标定过程示意图。

具体实施方式

测量系统如图1所示，系统由sigma二维转台、两个摄像机、测量用靶标组成。摄像机 固定在支撑架上并根据转台的高度调整摄像机的位置，测量靶标为一个立方体除底面外，每 个平面上有4个非共面的红外LED作为定位特征点，二维转台带动靶标做偏转、俯仰两个自 由度的运动，当使用单个摄像机拍摄定位特征点进行位姿计算时，为了避免拍摄的盲区，同 时也为了实现对二维转台转轴的标定使用两部摄像机从而可以分别从不同角度拍摄定位标 志。计算机通过串口发送信号至转台的控制箱，控制转台的运动。

特征点在摄像机坐标系下的坐标可以表示为在靶标局部坐标系下的坐标值可以表示 为相应的理想图像坐标Ii＝(xui，yui)T(i＝0…3)，因此的关系可以描述为：

对h_i进行求解就可以得到特征点在摄像机坐标系下的坐标值，特征点的连线在空间形成 一个几何图形，可以根据该几何图形的空间形状(物体的空间几何形状包括线段、角度、平面) 来实现对hi的求解，如图2所示，4个点围成的图形的空间几何模型已知，该几何图形包括 个三角形，其中每个三角形具有三条边，任意三角形的边长可表示为：

任意一个三角形具有三个角，同时任意四个点可以形成三对向量，每对向量形成一个夹 角，角度可以根据向量来表示：

如果只考虑距离与角度约束，会出现图3中所示的两种解，为了避免这种情况发生还需 要考虑公式(4)中所表示的约束，其中p’为p2点到平面p0p1p3的投影。

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

由于目标函数h(i)比目标函数e(i，j)、f(i，j，k)、g(i，j，k，m)的收敛速度明显要快，同时 距离约束的重要性要高于角度约束，将目标约束函数h(i)乘以罚因子M1，e(i，j)乘以罚因子 M2构建关于h_i(i＝0，1，2，3)的无约束非线性最优化目标函数：

$F=M_2·\underset{i=0}{\overset{3}{\Sigma }}\underset{j=i+1}{\overset{3}{\Sigma }}e(i,j)+M_1·\underset{i=0}{\overset{3}{\Sigma }}h\left(i\right)+\underset{i=0}{\overset{3}{\Sigma }}\underset{\underset{k\ne i,j}{j=i+1}}{\overset{3}{\Sigma }}f(i,j,k)$

$+\underset{i=0}{\overset{3}{\Sigma }}\underset{\underset{\underset{m\ne i,j,k}{k\ne i,j}}{j=i+1}}{\overset{3}{\Sigma }}g(i,j,k,m)---\left(6\right)$

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

缩放正交投影模型可以用来近似实际透视成像模型，从而获得hi的初值。根据弱透视模 型，假设靶标上的4个特征点具有相同的深度(将除0号特征点外的特征点垂直投影到经过 0号点且与CCD成像面平行的平面上，设为Ri)，如图4所示，其中s＝f/Z₀为缩放因子，可 以通过公式(12)计算出s的值，1/s用来作为h_i的初值。

$\left(\begin{array}{c}a=(P_0{P}_1+P_0{P}_2+P_1{P}_2)(-P_0{P}_1+P_0{P}_2+P_1{P}_2)\\ (P_0{P}_1-P_0{P}_2+P_1{P}_2)(P_0{P}_1+P_0{P}_2-P_1{P}_2)\\ b=I_0{I_1}^2(-P_0{P_1}^2+P_0{P_2}^2+P_1{P_2}^2)+I_0{I_2}^2(P_0{P_1}^2-P_0{P_2}^2+P_1{P_2}^2)\\ +I_1{I_2}^2(P_0{P_1}^2+P_0{P_2}^2-P_1{P_2}^2)\\ c=(I_0{I}_2+I_1{I}_2)(-I_0{I}_1+I_0{I}_2+I_1{I}_2)(I_0{I}_1-I_0{I}_2+I_1{I}_2)(I_0{I}_1+I_0{I}_2-I_1{I}_2)\\ w=\sqrt{\frac{b+\sqrt{b^2-\mathrm{ac}}}{a}}\end{array}\right)---\left(7\right)$

根据系统转轴标定后的结果可以建测量系统坐标系，如图5所示：根据选取偏转旋转轴 的OAy(xAy，yAy，zAy)点为坐标原点，转轴朝上方向为OAyYAy建立右手坐标系Ay，其确定 的旋转变换矩阵为matrix_Ay；根据俯仰旋转轴的标定结果，可以确定两点s1、s2，物体绕俯 仰旋转轴旋转的变换矩阵为matrix_Ax；设靶标从位置1到位置2绕偏转旋转轴转动的角度为a， 绕俯仰旋转轴转动的角度为b，转动前后特征点在摄像机坐标系下的坐标可以表示为式(8)。

$\left(\begin{array}{cccc}\cos a& 0& \sin a& 0\\ 0& 1& 0& 0\\ -\sin a& 0& \cos a& 0\\ 0& 0& =& 1\end{array}\right){\mathrm{matrix}}_{\mathrm{Ax}}\left(b\right){\mathrm{matrix}}_{\mathrm{Ay}}{\left(\begin{array}{c}{x}_{\mathrm{ci}}^1\\ y_{\mathrm{ci}}^1\\ z_{\mathrm{ci}}^1\\ 1\end{array}\right)}^T={\mathrm{matrix}}_{\mathrm{Ay}}\left(\begin{array}{c}{x}_{\mathrm{ci}}^2\\ y_{\mathrm{ci}}^2\\ z_{\mathrm{ci}}^2\\ 1\end{array}\right)---\left(8\right)$

公式(8)可以改写为其中 跟据旋转平移矩阵的定义可以得到式(9)，跟据公式 (9)可以确定2个3×3的单位正交矩阵，并通过这两个单位正交矩阵最终确定R(两个单位正 交矩阵的乘积也是一个单位正交矩阵，从而保证R的单位正交性)，根据式(10)来确定单位正 交矩阵并最终得到R与T：

测量前首先对相机进行标定，然后在4个非共面特征点的基础上根据特征点连线的空间 几何形状形成的几何约束以及特征点在CCD像面上坐标值求解出特征点在摄像机坐标系下的 坐标值；在求解过程中通过缩放正交投影近似透视投影模型，得到4个特征点在摄像机坐标 系下的坐标值，为上述迭代求解过程提供了初值，保证非线性算法的精度与收敛速度；由于 在对物体三维姿态进行求解的过程中需要用到转台的转轴，利用一个标准球实现转台旋转轴 标定；最后对物体三维姿态进行测量。