[发明专利]基于改进李群非线性优化的行车吊臂空间位姿测量方法

说明书

本发明公开了一种基于改进李群非线性优化的行车吊臂空间位姿测量方法，包括：S1：利用相机采集合作四个呈矩形分布的靶标目标点，并对采集到的图像中红外光标提取四个重心坐标；S2：对采集相机进行内外参数标定，确定成像中心、畸变系数，对步骤S1采集到的图像做成像畸变参数矫正；S3：将相机与合作靶标的相对位姿问题转换为最小化重投影问题；S4：利用EPNP法计算相机相对位姿初始旋转矩阵和平移量；S5：利用高斯牛顿法进行非线性迭代后最终收敛到真实解，得到旋转矩阵和平移量。本发明不仅提高了测量结果的精度，还提高了检测和计算速度，可以进行快速的目标检测和空间参数获取，提升了无人行车吊臂空间角度测量的实时性和准确性。

技术领域

本发明涉及工业系统无人行车控制，具体涉及一种测量行车吊臂空间位姿的方法。

背景技术

无人行车系统是一个包含了多个子系统的复杂系统，防摇控制系统是其中一个子系统，负责对运动作业过程中行车吊臂的精确控制并保证行车能够平稳运行。防摇控制的前提是需要获取实时精准的行车吊臂空间位姿参数。目前，行车吊臂空间角度参数检测难点在于需要实时高速的采集计算并发送给行车控制器，因此高效快速的目标检测和空间参数计算显得尤为重要。在测量角度方面，应用在无人行车的测角技术主要有倾角仪直接测量和视觉目标间接测量。相比于倾角仪方法，采用视觉测量行车吊臂摆动角度的方法得到的数据更为全面和直观，更有利于防摇控制算法的优化和计算。但是在车辆实时高速运行过程中，难以准确测量工业行车吊臂的空间位姿参数。

发明内容

发明目的：为了克服现有技术的缺陷和不足，本发明提出一种基于改进李群非线性优化的行车吊臂空间位姿测量方法，该方法可以快速计算行车吊臂的空间位姿参数，有效解决难以实时准确测量工业行车吊臂的空间位姿参数的问题。

技术方案：一种基于改进李群非线性优化的行车吊臂空间位姿测量方法，包括以下步骤：

S1：采用相机采集合作靶标目标点，合作靶标为四个呈矩形分布的激光点组成。对采集到的图像中红外光标提取四个重心坐标，；

S2：对采集相机进行内外参数标定，确定成像中心、畸变系数，对步骤S1采集到的图像做成像畸变参数矫正；

S3：将相机与合作靶标的相对位姿问题转换为最小化重投影问题；

S4：利用EPNP法计算相机相对位置初始旋转矩阵和平移量；

S5：利用高斯牛顿方法进行非线性迭代后最终收敛到真实解，得到旋转矩阵和平移量。

进一步地，所述步骤S1中，对采集到的图像中红外光标提取四个重心坐标的过程包含以下步骤：

S11：对原图像先进行混合滤波操作，即根据设定的阈值判断采用高斯滤波和中值滤波结合的方法，对图像作平滑处理消除噪声；

S12：采用最佳阈值分割法，对图像进行形态学二值化开运算，找到连通域并消除图像背景中的孤立噪声点和毛刺；

S13：采用灰度重心方法，对图像中的光标连通域进行寻找并逐一求取重心坐标。

进一步地，所述步骤S3具体包括以下步骤：

S31：将相机与合作靶标的相对位姿问题转化为非线性优化迭代，即求解图像坐标真实解与步骤S1求得的图像坐标的误差最小化，如下式所示：

ε²＝∑||q_i′-f(M₂P_i^w)||²

式中，ε²为误差项，q′_i为步骤S1得到的测量标定点的坐标，为红外光标的实际物理坐标，M₂为旋转矩阵和平移量组成的坐标变换矩阵；