基于多传感融合的吊装过程监测系统联合标定方法研究

马玉栋，杜福洲，樊洪良，吴 典，陈 婷

（1.北京航空航天大学 机械工程及自动化学院，北京 100191；2.沪东中华造船（集团）有限公司，上海 200129）

0 引言

随着航空、航天、船舶等传统工业领域在工艺水平上不断提升，大尺度产品部件吊装对接任务的对接精度要求逐渐提高，数字化智能化程度要求也越来越高[1]。现阶段单一传感器在大尺度产品部件吊装对接过程难以取得更好的效果，往往需要多传感器联合测量，通过对多组测量数据的调整、分析和融合，完成对接任务。多传感器集成与数据融合技术已经成为测量辅助装配领域的重要研究方向，它涉及信号处理、硬件设计、智能算法、机器视觉等多个领域，是新一代智能制造技术的核心基础之一。

为实现大尺度产品部件吊装对接实时位姿安全监控，利用多传感器数据融合技术，组建了多传感器联合测量系统-集成测量单元。在多传感器系统中，传感器主要包括单目视觉测量系统、九轴MEMS（陀螺仪）以及激光测距传感器。工业相机是单目视觉测量系统中的关键组件，其本质的功能就是将光信号转变成有序的电信号，利用传回的图像来解算位姿；九轴MEMS集成高精度的角速度计、加速度计、磁力计，求解姿态；激光测距传感器则是利用激光对目标的距离进行准确测定（又称激光测距）的传感器。

多传感器数据融合的关键在于各传感器之间的所测数据坐标系标定。起初独立传感器之间并未关联，各个传感器所测数据在未标定情况下，只在自身定义的坐标系下有意义。标定的作用就是求得各个独立传感器所测数据坐标系之间的转换关系，只有将多传感器标定于同一个坐标系下，所测数据才能进行数据融合。早期的Tsai[2～4]等建立手眼约束方程AX=BX来求解手眼关系X，确定相机与机器人的位姿标定关系。温卓漫[5]等在基于合作靶标的在轨手眼标定中提出了一种利用合作靶标位姿求解手眼关系的方法。李醒飞[6]等在多传感器测量系统联合标定中完成了坐标测量机与单目视觉系统的坐标系标定。

本文针对组建的集成测量单元，设计了利用工业机器辅助单目视觉测量系统、九轴MEMS以及激光测距传感器坐标系联合标定的方法和试验。本方法操作过程简单易实现，机器人安装集成测量单元运动任意的几个位置（转动角度不得超过90°），即可完成标定。

1 监测系统方案设计

由单目视觉、九轴MEMS陀螺以及测距传感器组成的集成测量单元封装于密闭设备中，如图1所示。集成测量单元还包括电源模块、数据传输模块（RF传输）、计算终端。在大尺度产品部件吊装对接过程中，在工业相机视场内能捕捉到靶标板时，根据标志点进行位姿解算，陀螺与激光测量数据提供校正功能；在靶标板不可见时，仅由陀螺和激光提供4个自由度的位姿引导，同时还能监测对接过程中部件的速度、加速度等运动信息。通过联合标定参数与传感器所测数据，计算终端进行多传感器数据融合后，利用RF无线模块将最终融合后的数据传回上位机进行下一步处理。

图1 多传感器测量系统-集成测量单元

1.1 基于单目视觉的位姿测量

单目视觉利用工业相机采集目标图像后，对观测靶标进行特征识别处理为特征点，利用多点对PNP算法计算相机外参数，最终处理为相对位姿。靶标板上标志点构型方案设计多种多样，只要标志点个数大于3个即可构建PnP模型，相机即可解算出标志点所在坐标系与相机坐标系的相对位姿，测量步骤如图2所示。

单目视觉位姿测量主要步骤包括：

1）进行相机内参数的标定，采用张正友标定法；

2）通过在世界坐标系Ow-xyz指定位置安放多个（通常放置6个）靶标点，或是任意安放靶标点后依靠第三方测量仪器确定其位置，可以获得靶标点在Ow-xyz下的三维坐标；

3）根据图像处理方法获得靶标点对应的二维图像坐标，构建PnP（Perspective-n-Point）问题模型；

4）根据EPnP（Efficient Perspective-n-Point）算法求解相机在世界坐标系中位姿，即Ai。

1.2 基于九轴MEMS的姿态测量

图2 单目视觉位姿测量步骤

九轴MEMS测量姿态由电子罗盘校正，其参考坐标系OG-xyz与东北天坐标系方向一致；测量位置通过加速度对时间二次积分获得[7]，OG-xyz位置由积分起始时刻传感器位置决定，即陀螺仪显示姿态为陀螺仪本身在东北天坐标系姿态。

1.3 基于激光测距传感器的距离测量

激光测距传感器是利用激光对目标的距离进行准确测定（又称激光测距）的仪器[8]。激光测距仪在工作时向目标射出一束很细的激光，由光电元件接收目标反射的激光束，计时器测定激光束从发射到接收的时间，计算出从观测者到目标的距离。

2 系统联合标定方法

本文借助工业机器人与单目视觉的手眼标定，将多传感器系统中各传感器数据标定于集成测量单元坐标系下。其中单目视觉构建PnP（Perspective-n-Point）问题模型求解相机与靶标点的位置与姿态，九轴MEMS测量显示姿态为陀螺仪本身在东北天坐标系姿态，激光测距传感器则是利用激光对目标的距离进行准确测定（又称激光测距）的仪器。最终集成测量单元所需标定：1）单目视觉中相机自身测量坐标系与集成测量单元坐标系的转换矩阵关系；2）九轴MEMS自身测量坐标系与集成测量单元坐标系转换矩阵关系；3）激光测距传感器在集成测量单元坐标系下激光发射点坐标以及激光射线方向，即激光射线在集成测量单元坐标系下的空间直线方程。

2.1 单目视觉与九轴MEMS联合标定

单目视觉与九轴MEMS融合测量模型涉及的各坐标系之间关系如图3所示。相机与九轴MEMS固定于集成测量单元中，测量单元通过基准孔与被测物固连（转换矩阵TCD已知），则被测物实时位姿可以由集成测量单元获得。设世界坐标系为Ow-xyz，相机测量靶标点在Ow-xyz下坐标已知，则由相机获得的被测物在世界坐标系下位姿为：

图3 融合测量模型示意图1

考虑集成测量单元具有多种工作模式，最终测量结果如下，其中f(Pa，Pb)表示利用数据融合算法。

上述测量结果Pa，Pb包含未知参数X,Y,TGW，需要利用机器人末端执行器作为被测物进行标定，如图3所示。根据机器人末端执行器与测量单元转换矩阵TCD已知，将被测物位姿变动D1→D2转换为测量单元在机器人基准坐标系OB-xyz下的位姿变动C1→C2，再由刚体位姿变换关系易得：

其中：

由手眼标定算法可知，当机器人运动到k（k＞3）个不同位置，即式(4)取k-1组不同值时，对于X，可以通过先求解最小二乘拟合问题：

得到旋转矩阵RX，其中α，β分别表示A，B对应的旋转向量[9,10]，再通过求解：

得到平移矩阵TX，从而求解出：

同理可求解Y,TGW。即求得单目视觉系统和九轴MEMS各自坐标系与集成测量单元坐标系的关系。

2.2 单目视觉与激光传感器联合标定

单目视觉与激光传感器融合测量模型涉及的各坐标系之间关系如图4所示。相机与激光测距传感器固定于集成测量单元中，测量单元通过基准孔与被测物固连（转换矩阵TCD已知），则被测物实时位姿可以由集成测量单元获得。设世界坐标系（靶标点所在地面）为O3-xyz，相机测量靶标点在O3-xyz下坐标已知，则由相机获得的被测物在世界坐标系下位姿为：

图4 融合测量模型示意图2

由单目视觉与九轴MEMS融合测量已经得到X矩阵，合作靶标点坐标系O3在集成测量单元坐标系下位姿转换矩阵已知，即Pa为已知条件；合作靶标点坐标系下取三个点m,n,p，其坐标分别为(1,0,0)(0,1,0)(0,0,1)，由平面方程关系可知，合作靶标点坐标系下三点可以确定一个平面方程；同理集成测量单元坐标系下对应三个点也可以确定一个平面方程，集成测量单元坐标系下对应合作靶标点坐标系三个点m1,n1,p1坐标值如下：

则集成测量单元坐标系下三个点m1,n1,p1即可确定一个平面方程：

在集成测量单元坐标系下激光所测距离，存在激光起点（x,y,z）以及激光终点（x1,y1,z1）则存在公式（其中D为激光测距示数）：

激光直线在集成测量单元坐标系下存在空间直线方程：

由式(8)～式(12)五个式子，通过至少6组参数即可消掉多余参数[11]，解得激光线的空间向量，然后即可求得激光射线在集成测量单元坐标系下的空间向量，即求得我们所需求的激光射线起点在集成测量单元坐标系下坐标，以及射线方向。

2.3 总结

由单目视觉与九轴MEMS的联合标定以及单目视觉与激光测距传感器的联合标定，可以将单目视觉测量、九轴MEMS陀螺仪、激光测距所测到的姿态、距离、位置标定于集成测量单元统一坐标系中。

3 试验与数据分析

3.1 试验环境搭建

多传感器测量系统联合标定试验由库卡机器人、集成测量单元以及合作靶标点构成，如图6所示。

试验使用库卡机器人的重复定位精度达到±0.03mm，集成测量单元包括单目视觉测量系统、九轴MEMS陀螺仪以及激光测距传感器等传感器，合作靶标点粘贴与长200mm，宽130mm的铝板上，如图5所示，利用六个靶标点构建PnP（Perspective-n-Point）问题模型，根据EPnP（Efficient Perspective-n-Point）算法求解相机位姿，靶标点坐标分别为（0，-33，0）（-66，-29，0）（-66，33，0）（0，33，0）（66，33，0）（66，-29，0），单位：mm。

单目视觉测量通过工业相机获得合作靶标图像，进行图像处理获得所需位姿，九轴MEMS和激光测距传感器也通过相应的程序获得所需数据，机器人位姿则由机器人PAD获得所需的机械臂末端位姿。

图5 合作靶标点

3.2 试验流程

图6 试验场景

将多传感器测量系统的设备安装于库卡机器人机械臂末端，设备本身已经完成多传感器系统的集成，已具备工业相机抓取图像求解位姿、通过串口获取九轴MEMS和激光测距传感器数据的功能。

试验主要步骤包括：

1）通过机器人PAD控制机器人机械臂运动到某位置，记录机器人机械臂末端位姿，待相机位姿测量、陀螺仪位姿、激光数据稳定后，记录各自数据；

2）控制机械臂继续运动，但运动转动角度不能超过90°，同步骤1）记录机器人机械臂末端位姿，待相机位姿测量、陀螺仪位姿、激光数据稳定后，记录各自数据；

3）重复步骤2），记录9组数据，完成试验数据记录。

3.3 试验结果与数据分析

在大尺度产品部件吊装对接过程中，利用集成测量单元实时监控位姿，其中单目视觉系统与九轴MEMS负责对接过程中的最终位姿调整，精度要求较高，激光测距传感器负责吊装过程中未达到相机测量范围时的引导过程，精度要求较低。

单目视觉系统与九轴MEMS联合标定过程主要运用手眼标定原理，手眼标定问题其实就是求解AX=XB方程问题。其中A为机器人机械臂末端坐标系在机器人-单目视觉系统移动前后的转换关系，B为单目视觉系统坐标系在移动前后的相对关系。Tsai[2]指出要唯一确定手眼矩阵的各分量，至少需要旋转轴不平行的两组运动。由于在观测中一般存在噪声，因此在实际测量中一般需要多组运动来求解该方程。通过多次手眼关系矩阵求解并利用最小二乘法拟合得到最后手眼标定结果来降低误差和不确定性。

表1 基于单目视觉的位姿测量标定精度

表2 基于九轴MEMS的姿态测量标定精度

本文中采用将获得的手眼关系矩阵X代入传统问题方程AX=XB中，选取一组矩阵关系为代表，通过比较AX和XB的值来验证混合标定的精度[12]。其中相机手眼关系矩阵包含旋转矩阵和平移矩阵，九轴MEMS则只存在旋转矩阵。

由表1可知相机标定到测量单元坐标系下旋转矩阵部分误差最大为0.006，平移矩阵部分误差最大为-1.4377；由表2可知九轴MEMS标定到集成测量单元坐标系下旋转矩阵部分误差最大为0.1558。通过讨论验证，单目视觉与九轴MEMS联合标定具有足够的精度与特性，满足项目任务要求。

激光射线在集成测量单元坐标系下的标定是在相机标定辅助下完成的，通过数学公式计算得出激光射线在测量单元坐标系下的空间射线方程，再通过多组直线方程消去多余参数求解空间射线方程。激光测距传感器只在吊装中间过程起引导作用，无具体精度要求，精度要求较低，在相机标定误差和激光传感器误差满足精度要求的情况下，计算而得激光射线起点在集成测量单元坐标系下坐标，以及射线方向也满足项目精度要求。

4 结束语

为实现大尺度产品部件吊装对接实时位姿安全监控，利用多传感器数据融合技术，组建了多传感器联合测量系统-集成测量单元。本文针对多传感器联合测量系统设计了单目视觉系统、九轴MEMS陀螺仪、激光测距传感器联合标定方法。通过该方法将单目视觉系统和九轴MEMS陀螺仪标定于集成测量单元坐标系中，同时将激光测距传感器的激光射线空间方程标定于集成测量单元坐标系中，最终将多传感器测量系统归于统一坐标系中。同时对所设计的标定方法与流程做了实际试验，通过对试验数据的分析与论证，证明多传感器测量系统的联合标定方法满足项目任务的精度与特性要求，达到了预期的效果。