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

权利要求书

1.一种基于改进李群非线性优化的行车吊臂空间位姿测量方法，其特征在于，该方法包括以下步骤：

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

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

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

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

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

2.根据权利要求1所述的基于改进李群非线性优化的行车吊臂空间位姿测量方法，其特征在于，所述步骤S1中，对采集到的图像中红外光标提取重心坐标包含以下步骤：

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

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

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

3.根据权利要求1所述的基于改进李群非线性优化的行车吊臂空间位姿测量方法，其特征在于，所述步骤S3具体包括以下步骤：

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

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

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

S32：对步骤S31的表达式用李群的方法表示，即用李代数表示相机坐标与标定物坐标之间的旋转矩阵和平移量：

ξ为李代数，∧广泛的表达为“从向量到矩阵”。

4.根据权利要求3所述的基于改进李群非线性优化的行车吊臂空间位姿测量方法，其特征在于，所述步骤S4具体包括以下步骤：

S41：利用四个虚拟空间坐标点的加权和来表示一个合作靶标的三维控制点；

S42：计算出四个虚拟空间点在世界坐标系下的坐标，利用相机投影模型转换到相机坐标系中，计算出在相机坐标系下的坐标；

S43：根据3D-3D的匹配，采用ICP方法计算出对应的旋转矩阵R和平移量t，将其作为初值带入李群算法进行最小化寻优。

5.根据权利要求3所述的基于改进李群非线性优化的行车吊臂空间位姿测量方法，其特征在于，所述步骤S5具体包括以下步骤：

S51：求出作为雅克比矩阵计算提前量，并作为后续雅克比矩阵计算的初始量，其中A为为克罗内克积，R为旋转矩阵，t为平移量，I为单位矩阵，用李代数表示相机坐标与标定物坐标之间的旋转矩阵和平移量；

S52：求解最小化重投影误差项关于优化变量的导数，即对第K次迭代下的目标函数进行线性化，作一阶泰勒展开后消除二次项信息，对李代数左乘扰动量exp(δξ^)来迭代收敛；

S53：对步骤S52线性化表示后的式子与非线性迭代误差表达式合并，并对其进行求导，求解雅克比矩阵时使用克罗内克积对雅克比矩阵进行向量化计算，每次计算时都利用S51中的提前量；

S54：对步骤S53得到的求导后式子设置为0，求得下一次迭代所需的变化量李代数，设第k次迭代时初始条件为ξ_k、E(ξ_k)，带入下式后求得第K+1次迭代下的李代数；

exp(ξ_k∧)exp(Δξ_k∧)＝exp(ξ_k+1∧)

上式中，exp(*∧)表示矩阵指数，ξ_k表示第K次迭代后李代数，ξ_k+1表示第K+1次迭代后李代数，Δξ_k表示李代数的变化量；

S55：更新f(exp(ξ_k+1^∧)P_i^w)得到新的E(ξ_k+1)，判断是否达到收敛终止条件，若达到则终止迭代，若未达到则重复S52到S54直至收敛到真实值。