基于多目视觉的工程车辆运动学参数校准系统

朱本龙 李祥坤 黄 欢

（1．徐州市检验检测中心，江苏 徐州 221111；2．江苏师范大学电气工程及自动化学院，江苏 徐州 221116）

0 引言

工程车辆应用广泛，随着技术发展与应用的需要，无人工程车辆技术已成为未来发展的趋势。和传统工程车辆相比，无人操控对车辆机械臂控制精度有更高的要求[1]。提高机械臂末端定位精度的方法有两种[2]，即使用更好的生产工艺和对机械臂进行运动学参数标定。第一种方法会极大地增加生产成本，所以参数标定是最好的选择。

进行工程车辆运动学参数标定对工程车辆末端执行机构的位置测量精度有很高要求。常用测量仪器有拉线传感器和激光跟踪仪。激光跟踪仪设备昂贵，拉线传感器操作步骤多，相比之下，基于相机的视觉测量方案具有成本低、检测精度高且检测速度快等优点。

该文设计了一个基于多目视觉的工程车辆机械臂运动学参数校准系统。首先根据相机视场的范围对多目视觉系统中相机的布局进行调整，使该系统能够适用于不同大小的工程车辆。其次对多目视觉系统的精度进行校准，使用校准后的系统测量工程车辆机械臂的末端位置。最后使用最小二乘法计算机械臂运动学参数误差，并对原参数进行补偿，以提高无人工程车辆的控制精度。

1 参数校准系统设计

1.1 多目视觉系统相机阵列的规划

相机视场是指相机的有效观测范围，因为相机使用的是透视原理，所以相机的视场是一个锥体。多目视觉系统相机阵列规划的目标是使被测量的物体至少同时处于2个或2个以上的相机视场中，这样才能得到深度信息。对运动物体来说，相机规划尤其重要，因为一旦物体脱离了多目相机的视场，就容易出现误跟踪。

该文将无人工程车辆机械臂铲斗末端的运动轨迹限制在一个平面上。进行位置测量时，为了确保有足够的测量精度，必须要令2个或者2个以上的相机同时观测到铲斗末端，这样就一定会有2个或者2个以上相机视场在运动平面上的投影相互重叠。虽然每个相机视场在运动平面上的投影区域的顶点可以为相机位姿的函数，但是却很难得到多个相机视场在运动平面上投影的重叠面积的解析形式，也就无法直接对该目标进行优化[3]。因此，该文使用软件辅助手动调节的方式在投影平面上对相机位姿进行布局，具体步骤如下：1）将无人工程车辆机械臂铲斗末端的运动约束在一个平面上，并确定其运动轨迹。2）根据投影方程，获得每个相机视场在运动平面上的投影。3）手动调节相机位姿，保证待检测轨迹上的每点至少位于2个相机视场的重叠范围之内。4）在保证以上条件的前提下缩短相机和投影面的距离，并增加相机视野的重叠程度，进而提高测量精度，最终得到各相机的位姿信息。

1.2 多目视觉系统标定

在进行多目相机阵列规划和无人工程车辆机械臂铲斗末端位置测量时，要用到相机的内、外参数，根据相机的像素坐标可以对相机的内、外参数进行估计。该文使用一维标定杆对多目视觉系统进行标定[4]。

使用一维标定杆法对多目视觉系统标定的步骤主要如下：首先，对相机拍到的一维标定杆图像进行二值化处理[5]，以便对标定杆上的标记点进行边缘提取。因为反光靶点可以与周围环境产生很大的亮度差，灰度对比很明显，所以对标定杆的灰度图进行二值化操作可以突出反光靶点的亮度。在图像中，如果第r行c列的像素灰度值小于60，则将灰度值变为0；如果大于等于60，灰度值变为255，二值化设置如公式（1）所示。

在二值化灰度图像之后，对标记点边缘进行提取。

其次，由于噪声和畸变的影响，二值化图像后得到的标记点是一个不均匀的近圆图像，因此该文通过将近圆拟合成圆并求解圆心的方法来获取标记点的二维位置。利用提取到的标记点边缘，使用最小二乘法求取标记点中心的像素坐标[6]。

最后，相机参数估计。一维标定杆在多目相机下的投影关系如图1所示。3个共线标记点为A、B和C，记Aj、Bj和Cj为标记点在空间中的不同位置，j=1，2，3，…，n。Oi为第i个相机的光心，i=0，1，2，3，…，m。记L为不同点之间的距离，那么L1=||A-B||，L2=||B-C||，L3=||A-C||。标记点在相机成像平面中的成像点分别为aij、bij和cij。

图1 一维标定杆多相机成像示意图

假设经过估计的重建矩阵H后得到的图像点称为重投影点，将该点和观测点之间的距离最小化，如公式（2）所示。

函数d（x，y）为x和y的几何距离函数，该函数称为重投影误差。根据Aj、Bj和Cj的共线性质，以Aj为中心建立球坐标系，用Aj来描述Bj、Cj的位置，那么空间点Bj、Cj与Aj的关系如公式（3）所示。

将公式（3）带入投影坐标公式（4）。

式中：矩阵K的参数由相机内部结构参数决定，称为内部参数矩阵；矩阵M为相机相对世界坐标的位姿，称为相机外部参数矩阵；H为一个投影变换矩阵，称为单应性矩阵；XW、YW和ZW是世界坐标系3个方向上的位置；u和v为像素坐标系2个方向上的位置；ZC为相机坐标系中物体在光轴方向上的位置。

对Aj、Bj和Cj这3个点的重投影进行整体误差最小化求解，最小化重投影误差表达式如公式（5）所示。

该过程同时对相机内、外参数和标定杆上标记点的位置进行优化。需要优化的参数过多，如果直接使用LM算法对其进行求解的话，由于其雅可比矩阵具有稀疏性，会消耗大量计算时间，因此使用稀疏LM算法进行求解。此算法可将参数按一定分块结构进行求解，能够有效减少计算时间。

1.3 机械臂末端位置定位

联立所有相机的投影坐标公式，可以获得观测点的三维信息。该文使用一台无人挖掘机模型对该方法进行验证。测量前需要先完成无人挖掘机模型机械臂的运动学参数建模。不考虑模型车转台的水平转动，其机械臂可以近似看成一个3自由度连杆机构[7]。该文使用D-H参数模型对机械臂进行运动学建模，这是一种对连杆和关节进行建模的通用方法。在机械臂的底座上建立基坐标系，每到一个关节进行一次坐标系的变换，每次变换都会得到一个变换关系矩阵，最终得到执行机构坐标相对基座标的变换关系。

从坐标系i到坐标系 1i+ 的D-H参数坐标变换如公式（6）所示。

式中：i为每个关节的标号；αi为关节扭角；θi为关节转角；αi为连杆长度；di为2个相邻坐标系i和i+1关于i坐标系Z轴上的距离。

将每个工作装置的变换矩阵按顺序相乘就能得到机械臂铲斗末端相对基坐标系的坐标变换矩阵，表达式如公式（7）所示。

测量过程如下：在机械臂铲斗末端的运动平面上选定若干空间坐标点作为给定轨迹；使用铲斗末端相对无人挖掘机模型基坐标系的坐标变换求这些点的运动学逆解，求得运动平面上这些空间点对应的机械臂关节角度；使用这些关节角度对机械臂进行控制，最后利用多目视觉得到无人挖掘机模型铲斗末端标记点的实际位置数据。

1.4 误差参数辨识

根据机械臂的运动学模型确定机械臂铲斗末端的位置，如公式（8）所示。

由于制造工艺和装配的影响，机械臂的4个运动学参数必然会和理论上的值有一些差距，又因为机械臂各个关节的转角都有编码盘进行读取，所以不认为关节转角存在误差，对应3个运动学参数a、d和α存在误差Δa、Δd和Δα，机械臂的实测位置如公式（9）所示。

则机械臂铲斗末端的位置误差为公式（10）所示。

在运动学参数误差比较小的情况下，公式（10）可以近似为一个线性方程，如公式（11）所示。

测得机械臂工作空间内n个点的坐标后，可以将公式（11）转换成公式（12）。

式中：A为关于无人挖掘机模型3个连杆D-H参数的雅可比矩阵；ΔX为D-H参数的误差向量；b为n组数据的末端位置偏差向量。

得到D-H参数的误差模型后，可以使用最小二乘法求D-H参数的最佳值，以使机械臂末端位置误差最小[8]。具体表达式如公式（13）所示。

2 试验与结果分析

试验使用的多目视觉系统包括4个相机，相机型号为Prime13，相机参数为焦距5.5 mm，图像分辨率1280×1024 pixel，水平视场角56°，垂直视场角46°。试验对象为一台小型无人挖掘机模型，其初始参数如下：大臂等效为一个长58 cm的连杆，小臂等效为长25 cm的连杆，铲斗等效为长10 cm的连杆。为了提高控制精度，初始参数需要进行校准。

2.1 相机阵列规划

使用4个相机对小型无人挖掘机模型机械臂铲斗末端位置进行测量。4个相机之间相距1.5 m，各自距地面1.5 m。同时4个相机在一个和机械臂铲斗末端运动平面平行的直线上，4个相机和运动平面之间的距离为3.5 m。此时4个相机视场在运动平面上的投影两两覆盖，使多目相机系统的探测范围可以完全覆盖住机械臂铲斗末端的运动范围。多目相机测量系统4个相机视场在运动平面上的投影如图2所示，C1区域表示第1个相机视场在运动平面上的投影；C1&C2表示第1个相机视场在运动平面上的投影和第2个相机视场在运动平面上的投影相重叠的区域；C2&C3表示第2个相机视场在运动平面上的投影和第3个相机视场在运动平面上的投影相重叠的区域；C3&C4表示第3个相机视场在运动平面上的投影和第4个相机视场在运动平面上的投影相重叠的区域；C4区域表示第4个相机视场在运动平面上的投影；虚线为铲斗末端在运动平面上的运动轨迹。图2中两两相交的视场覆盖了长为4.5 m、宽为3.75 m的矩形。

图2 相机在铲斗末端运动平面上的投影图

经过标定杆校准，在当前多目相机布局下，多目测量系统的测量平均误差为0.155 mm。

2.2 铲斗位置测量

设置完30个在机械臂铲斗末端运动平面上的目标空间点后，按照上文测量位置的方法可以测得30个标记点的实际位置数据。标记点的位置与铲斗末端的位置相差一个坐标变换，对标记点的位置使用坐标变换得到铲斗末端的实际位置。

2.3 运动学参数校准

将采集到的数据使用D-H参数误差模型进行处理，得到符合预期的D-H参数误差向量，对原D-H参数进行补偿，得到校准后的D-H参数。在D-H参数校正后，将上文中运动平面上前5个点的机械臂关节角度数据输入小型无人挖掘机模型控制器中，再使用多目视觉系统测量末端执行机构的位置，得到5个空间点坐标。将其与这5个点的给定位置做比较可以得到位置上的误差，取这5个点误差的平均值作为机械臂校准后的绝对定位误差，见表1。未标定时末端绝对定位误差为1 cm～4 cm，标定后机械臂末端绝对定位误差为0.2 cm～0.4 cm，控制精度大为提升。

表1 无人挖掘机模型机械臂标定前、后控制效果对比

3 结论

该文所设计的系统可以精确地测得机械臂末端的位置，使用的基于运动学参数误差模型的方法也可以很好地对无人挖掘机模型的机械臂进行运动学参数的辨识标定。未来希望能够利用算法自动规划相机位姿，快速又准确地对多目相机阵列进行位置部署。