嫦娥五号探测器月面采样封装任务的定位精度

张 烁，陈丽平，李铁映，鄢咏折，邓湘金，顾 征，郑燕红，马友青，亓 晨，刘少创

1. 中国科学院空天信息创新研究院，北京 100101； 2. 北京空间飞行器总体设计部，北京 100094； 3. 武汉大学测绘学院，武汉 430079

嫦娥五号任务是我国探月三期计划的首个月球探测任务，所研制的嫦娥五号探测器是我国探月工程的第5个月球探测器。嫦娥五号任务的目标是前往月球风暴洋开展我国首次的无人月面自动采样返回任务，采集约2 kg的月壤、岩石样本。2020年12月1日23时11分，嫦娥五号探测器成功着陆在月球正面(51.8°W,43.1°N)的吕姆克山脉以北预选着陆点，并于着陆后的48 h以内，完成月面自动采样封装任务。2020年12月2日22时，经过19 h的月面工作，嫦娥五号探测器按照计划顺利完成月面自动钻取采样任务和表取采样任务，并按照预定形式将样品封装保存在上升器携带的密封封装装置中，这标志着我国首次突破地外天体自动采样返回技术，也为未来我国计划实施的载人登月、火星采样返回任务奠定了基础。

为了完成月面自动采样封装任务，嫦娥五号探测器主要配备了：监视相机、表取采样机械臂、钻取采样机构和密封封装装置。钻取采样任务通过钻进取样方式获取月面以下2 m深度约500 g的月壤样品。表取采样任务通过立体视觉引导采样机械臂，以铲挖加吸纳相结合的方式获取月面浅层约1.5 kg的月壤和岩石样品。

嫦娥五号探测器月面自动采样封装任务的整个工作流程如下：①开展钻取采样封装任务，钻取机构深入采样区月面以下2 m深度采集月壤，结合钻取采样封装机构的设计，样品的封装伴随着钻取过程同时进行。在钻取采样的同时，样品存储在位于钻芯内部的密封袋中，待钻取采样过程结束，密封袋收紧末端口，通过整形装置盘绕在密封罐外缘，最后放入密封舱内。②开展表取采样封装任务。利用监视相机A/B对表取采样区成像，通过立体视觉技术重建出采样区的三维地形。以采样区三维地形为依据，规划出采样点，生成采样机械臂运动策略，控制采样机械臂到达采样点以铲挖加吸纳相结合的方式采集月壤样品。在每一次表取采样完成后，采样机械臂会先将月壤样品存储在位于着陆器顶部的初级封装罐内，待1.5 kg月壤样品全部采集完毕后，再利用采样机械臂提起初级封装罐，将其精准地放入位于上升器顶部的密封舱内，完成表取采样封装任务。

为了完成月面自动采样封装任务，采样机械臂和监视相机起到了关键作用，它们在嫦娥五号探测器上的布局如图1所示。

由图1中可以看到，嫦娥五号探测器共配备了8台监视相机，其中，监视相机A/B构成立体相机，用于重建采样区的三维地形。监视C用于监视表取初级封装过程的状态；监视D用于监视密封封装过程的状态；钻取监视用于监视钻取过程的状态；近摄甲/乙互为备份，分别用于监视采样器甲/乙的工作状态；远摄用于监视采样区的局部细节地形。采样机械臂由4个转动关节构成，分别是：肩部偏航、肩部俯仰、肘部俯仰、腕部俯仰；末端器搭载了近摄甲/乙、采样器甲/乙，整臂质量约22 kg，末端最大负载30 kg。

图1 机械臂和监视相机在探测器上的布局Fig.1 Layout of the robot arm and the monitor camera on the probe

采样封装任务高精度整体几何检校是月面自动采样封装任务顺利实施的基础。采样封装任务高精度整体几何检校关系到采样区地形三维重建、采样点规划、采样机械臂运动规划、采样精调、放样精调、抓罐精调、放罐精调等关键动作。因此，在嫦娥五号发射前，开展采样封装任务高精度的整体几何检校具有重要的意义。

1 相关研究

月面采样封装任务整体几何检校涉及以下3方面内容：立体相机检校、采样机械臂运动建模、关联相机坐标系到采样机械臂基座坐标系。立体相机检校是摄影测量领域的经典研究方向。已有的方法包括：①棋盘格检校法，这种方法在检校测试时，操作过程比较简单，占用空间较少。测试过程中，只需要使用绘制有棋盘格图案的二维控制场放置在相机前方，通过变换二维控制场的位姿，同时相机对二维控制场成像完成检校测试。但是，这种方法也有缺点，即检校结果不够稳定[1-2]。②直接线性变换法，这种方法通过直接建立三维空间与二维图像之间的线性变换关系估计出检校参数。这种方法具有不需要初始值的优点，但是也具有检校结果精度不高的缺点。因此，这种方法适用于为自检校光束法平差提供初始值[3-4]。③自检校光束法平差，这种方法在检校测试时，需要在相机视场前方布设稳定的三维控制场，并需要借助经纬仪测量控制点。这种方法的优点是精度高、稳定性好；缺点是检校测试耗时长，需要预留较大的空间[5-7]。

在航天领域常用的相机模型包括JPL在各类型号探测器上使用的CAHV模型，该模型使用4个矢量C、A、H、V来描述三维世界和二维图像之间的几何变换关系[8]。通过增加O和R矢量，用于描述镜头的径向畸变，CAHV模型改进为CAHVOR模型。该模型也是应用于美国MER计划的“机遇号”“勇气号”火星车的导航相机几何建模[9]。也有学者研究了航天领域经典的CAHVOR模型和摄影测量领域经典的直接线性变换模型之间的相互转换[10]，然后将CAHVOR模型增加一个E矢量，用于描述光线束在图像上的入射点位置和偏轴角之间的几何关系，使得CAHVOR模型改进为CAHVORE模型，这一改进使得该模型可以用于对鱼眼镜头进行建模描述。该模型也用于MER计划的“机遇号”“勇气号”和MSL计划的“好奇号”火星车的避障相机几何建模[11]。参考MER计划的“机遇号”“勇气号”火星车，在MSL计划的“好奇号”火星车立体相机的设计上，为了提高视觉测量的精度，将导航相机的基线增长到42.6 cm，基线增长后使得导航相机的测量精度在纵深方向上提高了两倍。在相机模型的设计上，“好奇号”和“毅力号”火星车均沿用了“机遇号”“勇气号”的相机模型[12-13]。

常用的机械臂建模方法是DH参数法(Denavit-Hartenberg)，这种方法用4个变量α、a、θ、d来描述一个转动关节的平移和旋转，其优点是机构建模时使用的变量少，建模过程简单；缺点是当相邻关节轴线运动到接近平行时，机构会出现奇异性问题[14]。很多学者在DH参数模型的基础上进行改进，得到了Hayati模型[15]、二阶模型[16]、S模型[17]和CPC模型[18]，这些模型通过增加变量的方法来解决奇异性问题，但是又带来了模型建立过程不直接、形式复杂、缺少通用性的新问题。为了克服DH参数模型的缺点，文献[19]首次提出了基于POE模型(product of exponentials)对串联机构的运动学参数进行几何检校[20]。POE模型具有以下优点：实现了转动关节和平移关节的统一描述，具有通用性；当相邻关节轴线接近平行时，关节运动是连续变化的，不会出现奇异性问题。文献[21]证明了POE模型具有连续性、完整性和最小性。

2 整体几何检校方法

检校的未知量如下：①监视相机A/B的内方位元素，包括：主点(x0,y0)、主距(fx,fy)、径向畸变(k1,k2)、切向畸变(p1,p2)。②监视相机A坐标系到监视相机B坐标系的外方位元素，包括：线元素(XCA-CB,YCA-CB,ZCA-CB)、角元素(φCA-CB,ωCA-CB,κCA-CB)。③监视相机B坐标系到采样机械臂基座坐标系的外方位元素，包括：线元素(XCB-B,YCB-B,ZCB-B)、角元素(φCB-B,ωCB-B,κCB-B)。④机械臂的4个转动关节的旋量：单位方向矢量(w1i,w2i,w3i)、位移矢量(v1i,v2i,v3i)。

检校的已知量如下：①控制点的三维坐标(X,Y,Z)。②控制点对应的图像坐标(x,y)。③机械臂末端坐标系到基座坐标系的旋转(ρ,ζ,θ)和平移(DX,DY,DZ)。④监视相机A/B内方位元素的初始值。⑤监视相机A坐标系到监视相机B坐标系外方位元素的初始值。⑥监视相机B坐标系到机械臂基座坐标系的外方位元素初始值。⑦机械臂4个转动关节的旋量的理论值。

2.1 坐标系定义

各个坐标系之间的解析关系如图2所示。

图2 各坐标系之间的解析关系Fig.2 Relationship between coordinate systems

像平面坐标系o-xy：原点o位于图像中心，x轴指向右，y轴指向上。

相机坐标系OC-XCYCZC：原点OC位于相机投影中心，XC轴指向右，YC轴指向上，ZC轴沿主光轴指向后。

机械臂基座坐标系OB-XBYBZB：原点OB位于采样机械臂基座几何中心，XB轴指向监视C，YB轴指向钻取采样机构，ZB轴沿铅垂线指向上。

机械臂末端坐标系OE-XEYEZE：原点OE位于采样机械臂末端几何中心，XE轴指向近摄乙，YE轴指向腕部关节转轴，ZE轴与XE轴、YE轴构成右手系。

将像平面坐标系o-xy转换到相机坐标系OC-XCYCZC的方法为

(1)

将相机坐标系OC-XCYCZC转换到采样机械臂基座坐标系OB-XBYBZB的方法为

(2)

式中，RC-B和TC-B为已知旋转矩阵和平移矢量，表示将OC-XCYCZC转换到OB-XBYBZB的旋转矩阵和平移矢量。

将采样机械臂基座坐标系OB-XBYBZB转换到采样机械臂末端坐标系OE-XEYEZE的方法是基于采样机械臂正向运动学模型，其中涉及4个关节旋转。

2.2 监视相机A/B检校

采用共线方程来描述物方空间的一个三维点P，相机投影中心s和图像上的像点p之间的几何关系为

(3)

式中，(x,y)表示像点p的图像坐标；(Δx,Δy)表示像点坐标的畸变差；(X,Y,Z)表示物方点P的坐标；(Xs,Ys,Zs)表示相机的外方位线元素，(a1,a2,a3,b1,b2,b3,c1,c2,c3)是由外方位角元素(φ,ω,κ)构建的旋转矩阵R的9个元素。畸变差的具体表达式为

(4)

式中，r表示像点的向径；k1、k2表示径向畸变系数；p1、p2表示切向畸变系数。

对式(3)进行一阶泰勒级数展开，可以得到的线性化的误差方程

v=Ax-l

(5)

式中，l表示像点坐标的残差矢量；v表示像点坐标系的改正数矢量；x表示监视相机B和监视相机A的内方位元素、外方位元素、畸变系数的增量；A表示对应的偏导数项系数矩阵。对于监视相机A/B，可以在联合平差模型中加入立体约束条件[22]

(6)

式中，RB是监视B像片的旋转矩阵；RA是监视A像片的旋转矩阵；TB是监视B像片的位移矢量；TA是监视A像片的位移矢量；下标i和j表示第i对立体像对和第j对立体像对，下标数字表示计算后所得矩阵的行、列号，例如：12表示所得矩阵的第1行、第2列。式(6)包含了两对立体像对相关的4张像片的外方位元素，按照一阶泰勒级数进行线性展开，可得立体约束的线性化表达式

Ut-G=0

(7)

式中，t表示监视相机B和监视相机A外方位元素的增量；U表示G对外方位元素求偏导数得到的系数矩阵；G表示将外方位元素的初始值代入式(6)后计算得到的残差矢量。

立体约束条件的物理意义如图3所示。

图3 立体约束条件的物理意义Fig.3 Physical meaning of stereo constraint

2.3 采样机械臂运动约束

采用POE公式建立了采样机械臂的正向运动模型[19-20]。当采样机械臂位于零位时，坐标系及各个关节转动极性的定义，如图4所示。

基于POE公式建立的采样机械臂正向运动模型为

g=exp(ξ1q1)·exp(ξ2q2)·exp(ξ3q3)·

exp(ξ4q4)·gEB(0)

(8)

式中，g为4×4的齐次矩阵，表示末端坐标系到基座坐标系的旋转和平移；exp(ξiqi)是一个4×4的矩阵，表示第i个转动关节的指数积表达式；ξi是第i个关节的旋量；qi是第i个关节的转动角；gEB(0)表示当采样机械臂位于零位状态时，末端坐标系到基座坐标系的位姿矩阵。

图4 采样机械臂处于零位状态Fig.4 Sampling robot arm in the zero position

式(8)是非线性的，采用一阶泰勒级数对其进行线性化，只考虑关节旋量作为未知数。然后，在等式左右两边同时乘以g-1，得到采样机械臂运动误差方程

(9)

(10)

假设对采样机械臂末端进行了m次测量，可得到如下的线性化表达式

Ja-y=0

(11)

2.4 联合平差估计

2.2节和2.3节推导了监视A/B的立体约束条件(式(6))和采样机械臂的运动约束条件(式(9))，可以将它们加入平差模型中进行联合估计，得到附有约束条件的误差方程

(12)

式中，X=[x,t,a]T代表待求解的几何参数，x表示监视相机B和监视相机A的内方位元素、畸变系数、外方位元素的增量，t表示监视相机B和监视相机A的外方位元素增量，a表示采样机械臂的4个转动关节，分别为肩偏航、肩俯仰、肘俯仰、腕俯仰的旋量增量；P代表权矩阵，采用选权迭代法或验后权估计法进行定权[23-24]；B和C表示对应的系数矩阵；L表示像点坐标的残差矢量；Wx表示分别由监视相机A/B的外方位元素构成的残差矢量与采样机械臂末端器位姿构成的残差矢量；V=[v,0,0]T表示像点坐标观测值的改正量。B和C，L和Wx的表达式为

(13)

式(12)描述的是一个附有约束条件的间接平差问题，可以采用加权最小二乘法进行最优估计求解。式(12)的解如下

(14)

式中，一些未定义的矩阵和矢量参见式(15)

(15)

单位权中误差的计算方法为

(16)

式中，σ0表示单位权中误差；n表示观测值个数；u表示未知数个数；s表示约束条件个数。未知数的中误差计算方法为

(17)

式中，σXX表示未知数的中误差；QXX表示协因数矩阵，计算方法为

(18)

3 试验验证

3.1 试验条件

月面采样封装任务整体几何检校试验在中国空间技术研究院建立的月球模拟试验场(以下简称“内场”)进行。内场占地面积600 m2，利用火山灰模拟月壤，在北侧墙壁上安装了大型灯阵，用于调节模拟的月球光照，在内场顶部安装了低重力模拟装置用于模拟月球1/6重力。此外，内场还配备了高精度测量设备，包括室内GPS、激光跟踪仪、经纬仪、三维激光扫描仪等。为充分演练月面自动采样封装过程，研制了采样封装验证器，验证器除了不具备飞行功能外，其他设计均与正样器相同。此外，针对表取采样区域和钻取采样区域，还研制了可升降的模拟月壤箱，尺寸为3 m×3 m，可以模拟不同工况下的采样试验。

3.2 试验结果

在立体相机几何检校试验中，设计了两种检校场，分别是二维检校场和三维检校场。二维检校场采样棋盘格图案的标定板，格网角点的加工精度优于0.008 mm。三维检校场内布设了约100个控制点，采用经纬仪测量控制点的三维坐标，点位实测精度优于0.2 mm。在采样机械臂运动试验中，机械臂以不同构型进行24次运动，并利用激光跟踪仪测量了末端器的位姿。在提取像点坐标观测值时，采用自动提取像点坐标的方法。对于圆形靶标，采用椭圆拟合法提取；对于直线格网角点，采用最小二乘直线拟合法[25-27]。检校场如图5所示。

图5 检校场Fig.5 The calibration field

采用本文方法估计出监视相机A/B的内方位元素、外方位元素和采样机械臂的旋量参数。采用以下4组试验对几何检校结果的精度进行验证。

试验1：布设了一种检核场景，只验证立体相机的单机精度，即不将外方位元素外引到采样机械臂基座坐标系下。此时，只检核在相机坐标系下的点位误差。所采用的外测方法首先利用千分尺测量检核点之间的距离，然后与双像前方交会法估计出的距离进行对比。所布设的检核场景如图6所示。

图6 监视相机A/B检核场景Fig.6 Checking scene of the monitor camera A/B

检核场景中共布设了9个盒子，每个盒子布设了6个检核点，每个盒子计算3个距离值。因此，共可以计算出27个距离值。对比分析了本文方法与其他几种方法的计算精度，见表1。

表1 长度的计算误差

表1中，本文方法是指在光束法平差模型的基础上加入了监视相机A/B立体约束条件。而其他3种方法，均未加入监视相机A/B立体约束条件。平差方法1是指光束法平差模型，模型中不包括监视相机A/B的立体约束条件。

试验2：在三维控制场中保留7个控制点作为检核点，这7个检核点不参与检校估计运算。利用经纬仪对检核点的实测精度优于0.2 mm。在机械臂的基座上安装了基准镜用于经纬仪瞄准。首先，采用3台经纬仪，利用1号经纬仪准直-YB轴，利用2号经纬仪准直-XB轴。然后，拟合出+ZB轴，从而建立采样机械臂基座坐标系。最后，利用3号经纬仪与1、2号经纬仪组合，采用双测回前方交会法，测量出检核点在采样机械臂基座坐标系下的三维坐标。

将立体相机的外方位元素外引到采样机械臂基座坐标系下，这是通过安装在监视相机A/B上的基准镜实现的。首先，利用经纬仪将控制点测量在监视相机B基准镜坐标系下，基准镜坐标系到采样机械臂基座坐标系的安装矩阵采用精测手段获取。然后，采用后方交会法估计出监视相机B坐标系到监视相机B基准镜坐标系的安装矩阵。最后，采用最小二乘位姿转换法[28-29]将监视相机B像片的外方位元素变换到采样机械臂基座坐标系下。利用双像前方交会法估计出检核点在采样机械臂基座坐标系下的三维坐标，并与检核点的经纬仪实测坐标进行对比。对比分析了本文提出的方法与其他几种方法的计算精度，见表2。

表2 检核点的均方根误差

表2中，本文方法是指在光束法平差模型的基础上加入了监视相机A/B立体约束条件和机械臂运动约束条件。而其他3种方法均未加入机械臂运动约束条件。在这里，平差方法2是指在光束法平差模型的基础上加入了监视相机A/B的立体约束条件，但是却不包括机械臂运动约束条件。

试验3：对本文方法估计出的几何参数进行系统误差补偿，开展模拟采样测试。首先，将检校得到的监视相机A/B的内方位元素和外方位元素代入双像前方交会算法，估计出10个采样点在机械臂基座坐标系下的坐标。然后，对机械臂关节旋量误差进行补偿。依据机械臂末端器期望到达的点位(即采样点)及各关节转动约束，采用Paden-Kahan子问题法[30]计算出机械臂各个转动关节的转动角度。最后，注入指令，控制机械臂到达采样点。对比到达点与规划点之间的误差，如图7所示。机械臂末端的定位误差间接反映了本文方法的绝对精度。

图7中，红色的标记表示规划的采样点，蓝色区域表示机械臂末端触地后留下的痕迹，黄色数字表示采用千分尺测量得到的点位误差，单位mm，其中10号点为机械臂无法到达点。在模拟采样中，得到机械臂末端定位误差的平均值为5.118 mm，最大值为10.8 mm，最小值为1.25 mm。

图7 模拟采样任务的机械臂定位误差Fig.7 Simulated sampling experiment

试验4：采用月面采样任务数据，对本文方法的精度进行验证。月面采样流程与地面模拟采样流程相同。精度验证时，采用双像前方交会法测量出规划采样点与机械臂到达点之间的偏差。对比月面到达点与规划点之间的误差，如图8所示。

图8 月面采样任务的机械臂定位误差Fig.8 Lunar surface sampling mission

图8中，红色的标记表示规划的采样点，蓝色区域表示机械臂采样后留下的痕迹，紫色数字表示双目视觉测量得到的点位误差，单位mm。在月面真实采样任务中，机械臂末端定位误差的平均值为4.745 mm，最大值为8.28 mm，最小值为1.86 mm。

4 讨 论

表1统计了加入监视相机A/B的立体约束条件之后的定位精度。平差方法1是指光束法平差，平差模型中不包括监视相机A/B的立体约束条件。通过与平差方法1进行比较，发现在光束法平差中加入立体约束条件之后，监视相机A/B的视觉定位精度得到了提高。由表1可以看出，本文方法的精度要优于其他3种方法，这证明：①本文方法具有较高的精度；②加入监视相机A/B立体约束条件能够有效提高视觉定位的精度。

表2统计了加入机械臂运动约束条件后的定位精度。平差方法2是指在平差模型中加入了监视相机A/B的立体约束条件，但是却不包括机械臂运动约束条件。通过与平差方法2的比较，发现在平差模型中加入机械臂运动约束条件之后。监视相机A/B的视觉定位精度得到了提高。由表2可以看出，本文方法的精度优于其他3种方法，这证明：①本文方法具有较高的精度；②加入机械臂运动约束条件能够有效提高视觉定位的精度。

通过对比表2的统计结果与表1的统计结果可以发现：在平差模型中加入机械臂运动约束条件之后的定位误差(表2)要大于未加入机械臂运动约束的定位误差(表1)。这是因为加入机械臂运动约束的前提条件是需要将监视相机A/B的外方位元素转换到机械臂基座坐标系下，这同时加大了精测误差。精测误差是指利用经纬仪测量监视相机B基准镜坐标系到机械臂基座坐标系的安装矩阵的误差。表1反映的是监视相机A/B的视觉定位误差，不代表采样封装任务的视觉定位误差。

图7绘制了模拟采样任务的机械臂定位误差。由图7可以看出，位于采样区域边缘的定位误差要大于位于采样区中心的定位误差。原因是机械臂末端定位误差会受监视相机A/B视觉定位误差的影响。从图像畸变分布的规律来分析，图像畸变呈现出外边缘大，靠近图像中心小的规律。因此，这使得采样区边缘的机械臂定位误差要大于采样区中心的定位误差。

图8绘制了月面采样任务的机械臂定位误差。由图8可以看出，位于采样区域边缘的定位误差要大于位于采样区中心的定位误差。对比图8和图7的结果可以发现，月面采样任务的定位误差要小于地面模拟采样试验的定位误差，其原因是：经过地面模拟采样试验发现，机械臂末端定位误差受监视相机A/B视觉定位误差影响较大，位于采样区边缘的定位误差要大于位于采样区中心区域的定位误差。因此，在正式执行月面采样任务时，将采样点尽可能地选择在采样区的中心区域，以保证高精度、高效地完成月面采样任务。

5 结 论

本文提出的嫦娥五号月面采样封装任务高精度整体几何检校方法，在联合平差模型中，加入了监视相机A/B的立体约束和采样机械臂运动约束。

在立体相机的单机检校试验中，获得的长度误差平均值为2.321 mm。将外方位元素外引到采样机械臂基座坐标系的试验中，检核点的均方根误差平均值为3.106 mm。在模拟采样试验中，获得采样点位误差平均值为5.118 mm。在月面采样任务中，获得采样点位误差平均值为4.745 mm。地面试验和月面采样任务验证了本文提出的方法能够对采样封装任务进行高精度的整体几何检校，有效地保障了嫦娥五号探测器月面自动采样封装任务的顺利实施。

致谢：感谢嫦娥五号探测器系统的采样封装分系统团队及工程参数测量分系统团队全体科研人员的合作与支持。