基于激光雷达的火箭推进剂加注机器人姿态估计研究

陆 煜 蒋 赞 洪 刚 顿向明

1. 上海交通大学机械与动力工程学院，上海，200240 2. 上海航天技术研究院第805研究所，上海，201108

0 引言

在运载火箭发射的环节中，对火箭箭体进行推进剂燃料加注的操作是重要的一环，目前我国主要还是依靠人工来进行燃料加注的操作[1]。由于推进剂具有易挥发性、易燃易爆性、剧毒性和腐蚀性等化学性质，因此对火箭进行燃料加注的操作具有极大危险性，且会持续损害工作人员的健康[2]。近年来，对火箭箭体进行推进剂燃料加注正逐渐用机器人代替人工加注，而其中加注机器人对火箭箭体活门的精准定位和姿态估计是其成功对接的关键，是实现自动化的重要一步，因此进行火箭加注机器人对接的姿态估计研究具有积极意义[3]。

目前，美国、俄罗斯及中国等航天大国都在积极进行火箭推进剂加注机器人的研究。美国基于火箭箭体的加注对接技术[4-6]，将加注对接装置安装于火箭箭体，在火箭发射时加注对接装置会依靠惯性自主掉落。这种方法操作简单，成本低，但并非完全自动化。俄罗斯火箭脐带自动对接是采用机械导向的方法来进行定位[7-8]，这种方法同样操作简单，但是仅靠机械定位时，对火箭箭体的初始姿态精度要求极高。赵帅锋等[9]开发的自动对接机器人可利用摄像头获取环境信息，并通过视觉算法计算得到箭体活门的位置姿态信息，但采用视觉算法受光照和火箭泄漏的四氧化二氮影响较大。游嘉伟等[10]采用单线激光雷达识别人工信标的方式定位，这种方法的鲁棒性和精度高，然而由于单线激光雷达只能获取二维平面点云，因此要求目标板初始位置与雷达在同一平面，这同样对火箭箭体的初始姿态有较高要求。

为解决上述火箭推进剂加注机器人在对接过程中的姿态估计问题，本文提出了一种基于单线激光雷达的姿态估计方法，在文献[10]定位方法的基础上，通过电机旋转给激光雷达一个旋转自由度，基于单线激光雷达获取三维点云信息，并设计了一种对人工信标进行识别和姿态估计的算法。最后，通过实验验证了该算法的稳定性、精确性和可靠性，该算法可在火箭停放位置不平整时检测出偏差的姿态。

1 箭体活门的姿态估计策略

对于箭体活门的定位，主要通过设计一个形状特殊又易于识别的人工信标，将其固定安装于火箭箭体上，然后通过传感器识别信标并定位。本研究设计的人工信标为一块目标板，其模型如图1所示。本研究采用单线激光雷达[11]作为识别信标的传感器，在能够准确识别目标的基础上，相比于视觉，单线激光雷达受光照和环境的影响较小，且精度更高，而相比于多线激光雷达，它体积小、价格便宜、算法简易、实时性高。由于单线激光雷达只能获取一个平面的点云信息，故通过加装电机给雷达一个旋转自由度，使其能够获取三维点云信息，并计算出目标板相对于雷达的相对姿态。

图1 目标板Fig.1 Target board

定位的整体策略如下：

(1)通过雷达扫描获取单帧二维点云信息，从二维点云中提取直线拐点信息；

(2)电机带动雷达摆动，重复步骤(1)获取多帧的直线拐点信息；

(3)当电机摆动完一个周期后，将二维的拐点通过坐标转换到三维坐标中，从而得到三维点云；

(4)从三维点云中识别出目标板；

(5)计算得到目标板相对于雷达的姿态；

(6)根据计算得到的姿态控制加注机器人逐步调整姿态，直到与信标在同一平面；

(7)通过雷达二维点云对目标板进行定位和识别，并控制加注机器人靠近箭体活门对接。

2 目标姿态估计算法

2.1 二维点云直线拐点提取

对扫描得到的二维点云数据进行中值滤波，消除噪声干扰，得到原始二维点云图，见图2。

图2 原始二维点云图Fig.2 Original 2D point cloud picture

激光雷达扫描同一物体得到的两相邻反射点之间的距离变化较小。根据这一特点，若相邻两点之间的间距小于某个阈值TV，则可将其归为同一个簇类(即聚类)，后续将对每个簇类进行特征提取。设激光雷达扫描一圈得到的反射点集为{ai}，每个点ai在雷达极坐标系下的坐标为(Di，θi)，其中Di为位置偏移量，θi为旋转角度。由于激光雷达点云有近密远疏的特性，故本算法中使用的阈值公式需要考虑反射点的位置偏移量，可表示为

TV=C0+C1min(Di,Di+1)

(1)

式中，C0为根据雷达角度分辨力而人为调整的参数，实验中取C0=8 cm；α为雷达的角度分辨力。

目标板的水平投影如图3所示，目标板由三条线段组成，这个形状特殊，可区别于环境其他物体，又易于通过二维激光雷达识别。根据这个形状特点，目标板的目标识别主要是提取点云中的直线和直线拐点特征。假设目标板所在的簇类包含的点数为k，则将簇类第一个点记为a1，最后一个点记为ak，如图4所示。

图3 目标板水平投影Fig.3 Horizontal projection of target board

图4 拐点提取Fig.4 Finding turning point

拐点提取算法的流程如下：

(1)选取中间点ak/2(k/2向下取整)；

(2)计算点a1与点ak/2之间所有点到直线a1ak/2的距离；

(3) 将其中距离超过阈值且距离最大的点作为拐点；

(4) 将点a1改为点ak，重复步骤(2)、步骤(3)；

(5) 若在上述步骤中找到了拐点，则将点a1和点ak也作为拐点。

如图5所示，提取的拐点为图中白色的点。通过拐点提取算法识别了符合目标板截面形状的簇类，并提取了线段的拐点，对每一帧二维数据仅保留拐点信息来进行后续姿态识别，节省了内存空间和计算时间。

图5 二维拐点Fig.5 2D turning point

2.2 二维拐点变换为三维拐点簇

将每帧二维雷达点云利用拐点提取算法得到的拐点通过坐标变换转为三维点云数据，并进行数据关联，从而获得三维的拐点簇，后续对每个拐点簇的点进行线段拟合，并进行目标识别。三维雷达坐标的原点取二维雷达坐标的原点，x轴、y轴方向分别为电机停在零点时雷达0°和90°的方向，z轴方向根据右手法则，指向雷达正上方。

将二维雷达坐标转为三维雷达坐标，首先需要将雷达数据与电机编码器数据根据数据采集 的时间戳进行时间同步，每帧雷达数据将匹配一个电机转角β，并根据下式将每个拐点从雷达二维极坐标(Di,θi)转换到三维雷达坐标(xi,yi,zi)：

(2)

之后，对每帧拐点采用最近邻域法[12]进行数据关联，将多帧拐点关联成多个拐点簇。最近邻域法是一种简单的数据关联算法，它认为前一帧数据中的某点距离后一帧数据中最近的点即为其相关联的点。这种关联算法的优点是实现简单、计算快速，但这不是一种最优关联算法，易发生错误关联。由于是对提取的拐点进行数据关联，各个拐点间的距离相比关联阈值有较大的差异，错误关联发生的可能性低，且对后续拟合直线影响不大，故采用该算法进行分析研究。数据关联算法的流程如下：

(1)计算第k帧所有拐点与第k-1帧某一拐点之间的距离，选取距离最小且未关联的拐点；

(2)若距离小于设定的关联阈值TD，则将这两点关联；

(3)选择第k-1帧另一拐点，重复步骤(1)、步骤(2)，直到遍历完第k-1帧的所有拐点；

(4)对已关联的点进行卡尔曼滤波预测[13]，对未关联的点记丢失次数加1，当丢失次数过多则认为目标丢失；

(5)在电机摆动的一个周期内，对所有相邻两帧雷达数据重复步骤(1)～步骤(4)，以获取多个关联的拐点簇。

由于电机转动缓慢，z轴方向的点相比x轴、y轴的点排列较密，因此两点之间的距离d2采用加权距离计算公式，根据点云密度关系，将z轴权重值设置为x轴与y轴权重值的1/2，如下式所示：

d2=0.4(xk-xk-1)2+0.4(yk-yk-1)2+
0.2(zk-zk-1)2

(3)

关联阈值TD同样需要考虑雷达点的位置偏移量，采用的公式为

TD=C2+C3min(Di,Di+1)

(4)

式中，C2、C3为根据电机转速而人为调整的参数，实验中分别取C2=1 cm,C3=0.01。

2.3 目标板的识别与姿态估计

利用上述算法可得到雷达摆动一个扫描周期内的三维拐点簇，如图6中的白点所示。假设第j条直线Lj可以表示为

Lj=(Pj,vj)

(5)

Pj=(px,py,pz)vj=(vx,vy,vz)

式中，Pj为直线j上的某点;vj为直线j的单位方向向量。

图6 目标板识别Fig.6 Recognition of target board

则目标板识别算法的流程如下：

(1)对每个拐点簇的点采用最小二乘法进行直线拟合，可以得到每条直线的Pj和vj；

(2)计算所有拟合直线的vj与三维坐标系z轴方向上单位向量的点乘积，若大于设定的常数阈值则剔除该拐点簇；

(3) 选取第k个拐点簇，计算点Pk到直线Lk+1的距离；

(4) 若两个拐点簇拟合直线的距离与目标板某一段长度M、L、N相差在设定的阈值内，则认为该段距离长度是目标板的一段长度；

(5) 重复步骤(3)、步骤(4)，如果找到4个拐点簇，且相邻两个拐点簇拟合直线的距离与目标板的三段长度相近，则认为这4个拐点簇为目标板。

3 实验与分析

为测试姿态估计算法的可靠性与准确性，设计了基于二维激光雷达和闭环伺服电机的扫描装置(图7)，并根据算法编写了程序。二维激光雷达为SLAMTIC公司的RPLIDAR A2，扫描范围为0°～360°，采样频率达每秒8000次。闭环伺服电机为NiMotion公司的STM5776A，编码器精度达0.1°。实验场景仅将扫描装置放置于目标板前的一定距离进行扫描，由于实际火箭对接过程所处环境较为简单，故不考虑目标板遮挡的情况。

图7 雷达扫描装置Fig.7 Lidar scanning device

目标板的识别是整个算法的第一步，从环境中准确地识别出目标板是后续姿态估计算法的基础。目标识别算法的检测方法为：将目标板放置于距离雷达0.5 m处，人为改变目标板的初始姿态，运行程序执行目标识别算法，对目标进行100次目标识别，并计算识别准确率。将文献[10]中基于二维点云的目标板识别原始算法与本文提出的基于三维点云的目标识别算法的目标识别结果进行对比分析，并对实验结果进行讨论。

目标板识别算法结果如表1所示，由实验结果可知，基于二维点云的目标识别算法对目标板初始姿态的精度要求较高，当目标板的俯仰角或翻滚角较大时，线段夹角和线段长度的判断阈值失效，从而导致目标识别率降低；而本文提出的算法将扫描得到的三维拐点簇进行了卡尔曼滤波处理，误差较小，且将拟合直线间的距离作为识别判断条件，可以在目标板初始姿态偏移的情况下准确识别结果。

表1 目标板识别准确率

为检验本文算法对目标板姿态估计的精度和可靠性，设计了姿态估计实验。实验过程中，将目标板放置于距离雷达0.5 m处，人为改变目标板的初始姿态，运行程序执行目标识别和姿态估计算法，对目标进行100次姿态估计，计算出估计的姿态与真(TV)的均值误差(ME)和标准误差(SE)。

目标板的姿态估计结果如表2所示。文献[10]中设计的火箭推进剂加注机器人的最大允许误差为4°，由表2实验结果可见，本文算法的姿态估计误差值在4°以内，具有较高的准确性，满足对接的精度要求。

表2 目标板姿态估计

4 结论

(1)为解决运载火箭燃料推进剂加注过程中加注机器人与箭体活门对接环节中产生的姿态估计问题，提出了一种基于二维激光雷达识别人工信标的姿态估计方法。通过电机带动二维激光雷达旋转，获取三维点云信息，并从中识别出人工设置的目标板，对其进行姿态估计来获得箭体活门与加注机器人的相对姿态，根据姿态调整加注机器人与箭体活门间的姿态关系，使得雷达扫描平面与目标板横截面平行。

(2)根据需求搭建了实验装置，测试并验证了激光雷达目标识别和姿态估计算法的识别准确率及姿态估计精度。实验结果表明，该算法能够在目标板处于不同姿态时准确地识别目标板，且对目标板的姿态估计精度较高，最大允许误差在4°以内，证明了该算法的可靠性与准确性。

(3)相比原始的二维雷达目标板识别算法，本文算法有如下优点：①能够在目标板初始姿态不平整的状况下完成目标板的准确识别和姿态估计；②只保留二维拐点信息进行三维目标识别，节省了内存和计算时间。

综上，本文算法能够确保火箭加注机器人与箭体活门的准确对接，具有一定的实际应用价值。