月球低重力模拟设备位姿测控系统优化设计

王 鹤，樊世超，王婉秋，赵振庆

（北京卫星环境工程研究所，北京 100094）

0 引言

地面行走试验是月面巡视器研制过程的重要环节，必须在地面对巡视器移动性能、路径规划及自主导航等功能进行综合测试验证，以考察巡视器行走能力和工作的可靠性[1]。为了巡视器地面行走验证试验的需要，专门研制了月球低重力环境模拟设备。该设备由恒拉力子系统、位姿测控子系统、支撑子系统等组成[2]，其中：恒拉力子系统以悬吊方式与支撑子系统共同作用，给巡视器提供低重力环境；位姿测控子系统包括位姿测量和二维跟踪两个部分，首先通过位姿测量获取巡视器在水平面的二维位置信息，运算后可得到吊点在水平面投影的二维位置信息即跟踪偏差，然后将这一偏差量作为控制器的输入，控制器根据偏差输出控制指令，驱动二维跟踪平台运动到目标位置，再次测量跟踪偏 差，从而形成控制闭环，实现对运动物体（巡视器）的目标识别和位姿测量。

国内外对运动物体的目标识别和位姿测量研究很多，主要是通过计算机视觉检测技术完成对动态或静态物体的目标识别。根据视觉传感器（一般为CCD 相机）的数量可分为单目视觉和多目视觉的测量方法，其基本工作流程都是将视觉传感器获得的测量图像经图像采集卡传送到计算机，再使用软件进行图像处理后获得感兴趣的测量参数[3]。

单目视觉测量方法较为广泛地应用于随动机器人、航空航天以及军工等领域的目标跟踪，该方法仅需1 台相机，结构简单，相机内参数标定简单，同时避免了多个视觉传感器带来的视场小、立体匹配难的问题。哈尔滨工业大学对基于单目视觉的目标位姿测量算法进行了理论和实际应用研究：利用投影仪、CCD 摄像机、图像采集卡及PC 机组成了一个半物理仿真实验系统，着重研究了对目标的位置及姿态测量；针对人为生成的目标及特征光标，利用坐标变换和几何关系推导了一种用于测量目标三维位置和姿态的简便解析算法；在均匀光照和一般室内背景条件下，完成了模拟空间交会对接时的目标飞行器位置姿态测量试验[4-6]。

在综合考虑系统需求和可靠性的基础上，本文提出了一种基于双相机的位姿测控系统，利用LED红外光源作为特征点即靶标，二维跟踪平台上安装的2 个CCD 相机作为视觉图像捕捉设备，通过光学成像原理解算巡视器位姿信息，并完成跟踪控制。

1 非接触式视觉测量原理

立体靶标上分布着5 个空间位置坐标已知的红外发光二极管，即5 个特征点。测量过程中，系统通过相机获取场景的灰度图像；在从图像中提取靶标位姿信息的过程中，对灰度图像进行二值化处理。所谓二值化处理是将图像分割成背景和目标两部分，其灰度值分别用二进制数0 与1 来表示。对图像进行二值化处理可以有效减少数据量、部分消除背景噪声，提取目标几何特征。根据这5 个特征点的成像分析以及各特征点之间的几何约束，利用姿态估计算法确定这些特征点相对于相机坐标系的坐标。

假设世界坐标系是绝对坐标系，定义为相机坐标系，且空间特征点Pi投影在u-v像平面上的像点ai用相机坐标系下的坐标表示。光学靶标测量原理如图1所示，可以看出，qi为从相机坐标系原点出发指向像点ai的单位向量，空间特征点Pi处于相同的方向。因此，假如已知比例因子xi，则可以通过像点ai求得Pi的坐标位置，即：

用像点ai代替Pi且计算空间特征点之间的距离，并利用点积和qi·qi=1 得到二次方程形式

式(2)可改写为

对应着空间的n个特征点，可以得到关于n个 未知数的n(n-1)/2 维二次方程组，其中为由靶标上特征点分布所决定的已知量，且qi·qj可以由像点坐标ai和aj以及相机焦距确定。因此可以通过非线性优化算法对该方程组进行求解得到未知的比例系数xi，并进而通过式(1)确定点Pi的空间坐标[7]。

图1 光学靶标测量原理Fig.1 Measurement principle of the optical drone

得到特征点在空间坐标系的位置和在相机像平面中的像点坐标ai后，可以根据式(3)得到一个超定方程。如何根据这个超定方程解出所需的位置参数，这是典型的投影n点姿态估计问题（PnP问题）。对于空间3 个特征点的情况，理论上式(3)对于[x1,x2,x3]存在8 种可能的解；考虑到特征点实际上不可能处于相机的另一侧，因此对于实际的姿态估计至多存在4 个解。

假设靶标上有5个特征点，对系统进行求解，利用式(3)，可以得到10个方程：

通过抵消可以得到的6个四次多项式，将其排列在矩阵方程中有

对该超定方程进行奇异值分解可得到向量t5，并求得x值。由于xi不可能为负，根据x=x12可以得到x1=，并可将此结果代入方程组(5)求得对应的x2～x5。

最后可以利用式(1)求得光学靶标上特征点在相机坐标系的坐标，当设置巡视器的质心为靶标空间坐标系的原点时，就可以得到巡视器质心在相机坐标系的坐标，再根据相机和吊点的位置就可得到巡视器质心相对于吊点的偏移。

2 基于双相机的位姿测控系统

2.1 硬件结构优化设计

位姿测控系统（见图2）由两个经过精确标定的高分辨率CCD 相机、立体靶标、交换机、控制一体机、驱动电机和跟踪平台等组成。由于月球车停止位置和行进路径不同，可能造成相机的测量光路被恒拉力子系统的吊索遮挡，因此，将2 个相机分别安装在恒张力索两侧，这样既可以覆盖整个视场且不会同时被恒张力索遮挡[8]，又解决了月球车单向运动时的信息丢失问题。系统启动后，2 个相机同时工作，测量结果通过网络交换机传输至控制一体机；用户根据工况选择相机，对所选择相机的测量结果进行处理并将处理后的偏差量输入至电机控制卡，由电机驱动器驱动电机旋转，使平台运动。每次切换，保证2 个相机测量的目标位置信息在允许的偏差范围内。控制一体机定时将获取的位置信息和跟踪偏差传回地面综合控制系统显示并存储。

图2 位姿测控系统示意图Fig.2 The schematic diagram of the position-pose measurement and control system

单个相机测量如图3所示。为提高系统的可靠性，采用封装的CCD 模块和成熟的信号处理电路，通过视频采集卡及DSP 进行数据采集和处理；采用带通数字滤波的方式对采集到的信号的特定频段进行提取，进一步降低外界杂光及多个LED之间的干扰。采用大功率的红外发光二极管作为特征点，同时在镜头前面加上一个红外滤光片，这样可以有效过滤非红外波段，提高视觉测量信噪比。

图3 单个相机的测量示意图Fig.3 Measurement of single camera

2.2 软件设计

系统软件包括网络通信模块、控制模块、磁尺信息采集模块、相机测量模块、电机控制模块、双相机切换模块、相机图像监测模块和LED 光源监测模块，主要实现以下任务：

1）根据切换指令选择相机；

2）根据控制指令按照控制策略将指令分配给二维平台；

3）接收位姿测定子系统的偏差信息；

4）采集码盘、磁尺等敏感器的信息；

5）将二维跟踪系统的状态信息及敏感器信息以固定周期发送给综合控制系统。

软件的工作过程可划分为初始化、工作和关闭三部分（见图4）。

图4 位姿测控系统软件工作流程Fig.4 Flowchart of the software of the position-pose measurement and control system

3 试验验证

基于双相机的位姿测控系统优化完成后，利用经纬仪测试双相机安装后的数据一致性，如表1所示，可以看出，巡视器行进至不同位置，2 个相机切换时x向、y向（见图5）的跟踪偏差在8 mm 以内，满足巡视器二维随动跟踪的误差要求。

图5 巡视器移动示意图Fig.5 Rover movement

表1 双相机数据一致性记录Table 1 Data consistency of the two cameras

在巡视器发射前的行走试验中，应用该系统对 巡视器在模拟地形行走中进行了位姿测定和跟踪，应用效果良好。x、y向跟踪误差如图6所示。

图6 吊点x 向和y 向跟踪误差曲线Fig.6 Tracking error curves of the hoisting point in x and y directions

4 结束语

本文对月球低重力模拟试验的位姿测量方法进行了研究，提出了基于双相机的非接触测量方法，对硬件结构和软件进行了优化设计和分析。利用该位姿测控系统进行了巡视器行走的验证试验测试，测试结果表明该系统很好地解决了由于遮挡引发的位姿跟踪失效问题，满足巡视器行走试验要求，可以在后续型号试验中投入使用。

（References）

[1]樊世超, 贾阳, 向树红, 等.月面地形地貌环境模拟初步研究[J].航天器环境工程, 2007, 24(1)∶15-20 Fan Shichao, Jia Yang, Xiang Shuhong, et al.A preliminary study on simulation of lunar surface terrain[J].Spacecraft Environment Engineering, 2007, 24(1)∶15-20

[2]齐乃明, 张文辉, 高九州, 等.三维空间微重力地面模拟试验系统设计[J].机械工程学报, 2011, 47(9)∶16-20 Qi Naiming, Zhang Wenhui, Gao Jiuzhou, et al.Design of ground simulation test system for three-dimensional spatial microgravity environment[J].Journal of Mechanical Engineering, 2011, 47(9)∶16-20

[3]陈杉.物体位姿单目视觉测量系统的研究[D].天津∶天津大学, 2007∶6-12

[4]马国松.基于单目视觉的位姿测量方法及仿真系统研究[D].哈尔滨 ∶哈尔滨工业大学, 2005∶8-37

[5]侯建, 齐乃明.月球车立体视觉系统的一种实现方 案[C]∥中国宇航学会深空探测技术专业委员会第一届学术会议.哈尔滨, 2005∶29-32

[6]侯建, 齐乃明.月球车视觉系统立体匹配算法研究[J].南京理工大学学报 ∶自然科学版, 2008, 32(2)∶176-180 Hou Jian, Qi Naiming.Stereo matching algorithm for lunar rover[J].Journal of Nanjing University of Science and Technology, 2008, 32(2)∶176-180

[7]徐杰.机器视觉中摄像机标定Tsai两步法的分析与改进[J].计算机工程与科学, 2010, 32(4)∶46-47 Xu Jie.Analyzing and improving the Tsai camera calibration method in machine vision[J].Computer Engineering＆Science, 2010, 32(4)∶46-47

[8]李春艳, 王立, 卢欣, 等.一种双目立体视觉相机的标定方法[J].空间控制技术与应用, 2010, 36(3)∶51-53 Li Chunyan, Wang Li, Lu Xin, et al.A binocular stereo camera calibration method[J].Aerospace Control and Application, 2010, 36(3)∶51-53