[发明专利]基于几何约束的物体三维姿态测量方法

说明书

技术领域

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

背景技术

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

目前对物体三维姿态进行测量的方法大多是直接对表示物体位姿的旋转平移矩阵进行求解，这种方法一般面临两个问题：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)的无约束非线性最优化目标函数：