-
1)正向运动学问题
正向运动学问题可简述为已知整车姿态和各关节转动角度,求解相机位姿。采用齐次变换描述机构运动关系,每个连杆由固连其上的坐标系描述其位置和姿态,固连坐标系的原点表述了关节的空间位置,三轴表示了连杆的空间姿态。以定点坐标系作为基准坐标系(Op-XpYpZp系),依次为每个连杆建立坐标系,给出正向运动学模型。
整车坐标系到定点坐标系的转换矩阵为
$$ {}_x^{\rm p}{\boldsymbol{T}} = \left[ {\begin{array}{*{20}{c}} {{}_{{x}}^{{\rm p}}{\boldsymbol{A}}}&{{}_{{x}}^{{\rm p}}{{\boldsymbol t}}} \\ {\boldsymbol{0}}&{\boldsymbol 1} \end{array}} \right] $$ (1) 其中:
${}_{{x}}^{\rm{p}}{\boldsymbol{A}}$ 为整车相对于定点系的方向余弦矩阵,由车体姿态得到;${}_{{x}}^{\rm{p}}{\boldsymbol{t}}$ 为整车原点在定点系中的位置。桅杆安装座坐标系到机械坐标系的转换矩阵为
$$ {}_{\rm b}^x{\boldsymbol{T}} = \left[ {\begin{array}{*{20}{c}} {{}_{\rm b}^x{\boldsymbol{A}}}&{{}_{\rm b}^x{\boldsymbol{t}}} \\ {\boldsymbol{0}}&{\boldsymbol 1} \end{array}} \right] $$ (2) 其中:
${}_{\rm b}^x{\boldsymbol{A}}$ 为桅杆安装座相对于机械系的方向余弦矩阵,通过精测得到;${}_{\rm b}^x{\boldsymbol{t}}$ 为桅杆安装座原点在机械系中的位置。同理,根据精测值,可以获得
${}_z^{\rm b}{\boldsymbol{T}}$ 、$ {}_y^z{\boldsymbol{T}} $ ,${}_{f}^{y'}{\boldsymbol{T}}$ 、${}_{\rm m}^{f'}{\boldsymbol{T}}$ 、${}_{\rm l}^{\rm m}{\boldsymbol{T}}$ ,以上矩阵分别为展开坐标系到桅杆安装座坐标系、偏航坐标系到展开坐标系、俯仰坐标系到偏航坐标系、多光谱相机基准镜坐标系到俯仰坐标系以及多光谱相机镜头坐标系到多光谱相机基准镜坐标系安装矩阵。由于桅杆偏航机构与俯仰机构转动,机构运动的齐次矩阵表示为
$ {}_{y'}^y{\boldsymbol{T}} $ 与${}_{f'}^f{\boldsymbol{T}}$ $$ {}_{y'}^y{\boldsymbol{T}} = \left[ {\begin{array}{*{20}{c}} 1&0&0&0 \\ 0&{\cos y}&{\sin y}&0 \\ 0&{ - \sin y}&{\cos y}&0 \\ 0&0&0&1 \end{array}} \right] $$ (3) $$ {}_{f'}^f{\boldsymbol{T}} = \left[ {\begin{array}{*{20}{c}} {\cos f}&0&{ - \sin f}&0 \\ 0&1&0&0 \\ {\sin f}&0&{\cos f}&0 \\ 0&0&0&1 \end{array}} \right] $$ (4) 其中:y与f分别为桅杆运动的偏航角与俯仰角。
由以上位姿矩阵可得到多光谱相机镜头坐标系相对于定点坐标系的位姿矩阵,即正向运动学模型
$$ {}_{\rm l}^{\rm p}{\boldsymbol{T}} = {}_x^p{\boldsymbol{T}}{}_z^x{\boldsymbol{T}}{}_y^z{\boldsymbol{T}}{}_{y'}^y{\boldsymbol{T}}{}_f^{y'}{\boldsymbol{T}}{}_{f'}^f{\boldsymbol{T}}{}_{\rm m}^{f'}{\boldsymbol{T}}{}_{\rm l}^{\rm m}{\boldsymbol{T}} $$ (5) 同理,亦可得到多光谱相机镜头坐标系相对于机械坐标系的位姿矩阵
$$ {}_{\rm l}^x{\boldsymbol{T}} = {}_z^x{\boldsymbol{T}}{}_y^z{\boldsymbol{T}}{}_{y'}^y{\boldsymbol{T}}{}_f^{y'}{\boldsymbol{T}}{}_{f'}^f{\boldsymbol{T}}{}_{\rm m}^{f'}{\boldsymbol{T}}{}_{\rm l}^{\rm m}{\boldsymbol{T}} $$ (6) 针对定点坐标系下的目标点,可以得出多光谱相机镜头系下点的位置为
$$ {}^{\rm l}{\boldsymbol{P} }={}_{\rm p}^{\rm l}{\boldsymbol{T}}{}^{\rm p}{\boldsymbol{P}} $$ (7) 若给出的是机械系下的目标点,可以得出多光谱相机镜头系下点的位置为
$$ {}^{\rm l}{\boldsymbol{P }}={}_x^{\rm l}{\boldsymbol{T}}{}^x{\boldsymbol{P}} $$ (8) 2)桅杆控制参数迭代求解方法
根据式(5)和式(6),由于安装误差的存在,偏航轴和俯仰轴之间存在一定的失调偏差,采用解析解法求桅杆运动较为繁琐。本文采用迭代法求解桅杆运动角度,在一定容差下求解的桅杆运动角度在任务中获得了验证。
输入:
$ {}^x{\boldsymbol{P}} $ (${}^{\rm p}{\boldsymbol{P}}$ )步骤1:在桅杆零位状态下计算桅杆俯仰系下目标点在计算初值偏航角y0及俯仰角f0。
步骤2:迭代求解偏航角y及俯仰角f。
当 |x|>ε|| |y| > ε时
计算当前偏航角y及俯仰角f条件下目标点在多光谱相机镜头坐标系下的俯仰角df,并更新f。
计算当前偏航角y及俯仰角f条件下目标点在多光谱相机镜头坐标系下的偏航角dy,并更新y。
步骤3: 得到最终偏航角y及俯仰角f。
-
误差项主要分为探测点定位误差和多光谱相机位姿误差。探测点位置是由导航相机双目视觉重建后获得的。在此过程中,桅杆控制误差和精测误差、相机参数误差均会传递至探测定位置精度上,由于双目视觉重建不是本文研究的内容,误差传递过程,在本文中不赘述。桅杆控制误差、桅杆及相机精测误差均会影响多光谱相机位置与姿态的精度,下面将重点分析桅杆控制误差及探测点定位误差引起的瞄准误差。
考虑桅杆旋转过程,将式(6)、式(8)展开并化简为
$$ {}^{\rm l}{\boldsymbol{P}} = {}_{f'}^{\rm l}{\boldsymbol{T}}{}_f^{f'}{\boldsymbol{T}}{}_{y'}^y{\boldsymbol{T}}{}_y^{y'}{\boldsymbol{T}}{}_x^y{\boldsymbol{T}}{}^x{\boldsymbol{P}} $$ (9) 其中:考虑桅杆偏航轴、俯仰轴存在运动误差
$ {}_f^{f'}{\boldsymbol{E}} $ 与$ {}_y^{y'}{\boldsymbol{E}} $ ,目标点存在定位误差$ \Delta {}^xP $ ,则方程变化为$$ {}^{\rm l}{\boldsymbol{P'}} = {}_{f'}^{\rm l}{\boldsymbol{T}}{}_f^{f'}{\boldsymbol{E}}{}_f^{f'}{\boldsymbol{T}}{}_{y'}^y{\boldsymbol{T}}{}_y^{y'}{\boldsymbol{E}}{}_y^{y'}{\boldsymbol{T}}{}_x^y{\boldsymbol{T}}\left( {{}^x{\boldsymbol{P}} + \Delta {}^x{\boldsymbol{P}}} \right) $$ (10) 则误差方程
$\Delta {}^{\rm l}{\boldsymbol{P'}}$ 为$$ \Delta {}^{\rm l}{\boldsymbol{P'}} = {}^{\rm l}{\boldsymbol{P'}} - {}^{\rm l}{\boldsymbol{P}} $$ (11) 下面分别分析机构运动误差和目标点定位误差的影响。
设置俯仰轴与偏航轴运动误差为0.2°(1σ),分析平面位于车体坐标系−0.3 m,暨通常情况下的火面高度,相机坐标系下误差的均值如图7所示。
图 7桅杆控制误差引起的瞄准误差(机械坐标系)
Figure 7.Aiming error caused by mast control error (mechanical coordinate system)
设置定位误差为0.1 m(1σ),分析平面位于车体坐标系−0.3 m,暨通常情况下的火面高度,相机坐标系下误差的均值如图8所示。
图 8目标点定位误差引起的瞄准误差(机械坐标系)
Figure 8.Aiming error caused by Target point positioning error (mechanical coordinate system)
由图7与图8可知,误差与目标在车体系下的位置呈相关性。桅杆控制误差引起的瞄准误差基本与双轴的控制误差相近,呈现靠近桅杆处误差小、远离桅杆处误差大的特征。目标点定位误差引起的目标点定位误差引起的瞄准误差在大部分区域内小于机构误差引起的瞄准误差,呈现靠近桅杆处误差大、远离桅杆处误差小的特征。根据第3节中对于开窗视场分析的结论6º× 9º为控制要求,在火星车机械坐标系6 m × 4 m范围内,桅杆控制误差引起的瞄准误差不超过0.85º(3σ),目标点定位误差引起的瞄准误差不超过0.4º(3σ),综合误差限1.25 º(3σ)。综上,误差条件下可以确保目标点出现在6º× 9º开窗视场中,满足探测要求。
Accuracy Mast Pointing Control Method for Multi-Spectral Camera of Mars Rover
-
摘要:为解决火星车桅杆搭载小视场多光谱相机对特定目标精确指向的问题,设计了火星车桅杆精确指向方法,对火星车桅杆机构运动学过程进行了建模,设计了桅杆控制参数迭代求解方法以及火星车对目标探测工作流程,对提出的控制方法进行了误差分析。经过中国首次火星探测任务实践证明,设计的方法可满足多光谱相机精确指向的要求。Abstract:To achieve the goal of accuraetly pointing to particular targets for the narrow-FOV multi-spectral camera on the mast, an accuracy mast pointing method was proposed. The mast kinetic model, the iterative solution method of mast control parameters, and the working flow of rover target detection were designed. An error analysis of the method was conducted. Through the practice of China’s first Mars exploration mission, the method is proved to satisfy the demand for accuracy pointing of the multi-spectral camera.Highlights
● An accuracy mast pointing control method for narrow-FOV strategy of multi-spectral camera exploration of Mars rover was proposed. ● The work flow of multi-spectral camera exploration for different scenarios was designed. ● The proposed method in the article was verified through China’s first Mars exploration mission. -
-
[1] 张辉,卢皓,于天一,等. “祝融号”火星车遥操作技术[J]. 深空探测学报(中英文),2021,8(6):582-591.ZHANG H,LU H,YU T Y,et al. Teleoperation technology of Zhurong Mars rover[J]. Journal of Deep Space Exploration,2021,8(6):582-591. [2] TIAN H,ZHANG T,JIA Y,et al. Zhurong:features and mission of China’s first Mars rover[J]. The Innovation,2021(3):24-25. [3] 谢涓,闫凯,康志忠,等. “祝融号”火星车多光谱相机岩矿类型识别的地面验证研究[J]. 遥感学报,2021,25(7):1385-1399.XIE J,YAN K,KANG Z Z,et al. Verification study for the mineral and rock identification using multispectral camera of the “Zhurong” Mars rover on the Earth[J]. National Remote Sensing Bulletin,2021,25(7):1385-1399. [4] 张宽, 卢皓, 邹雪梅, 等. 玉兔二号复杂月表环境下的定点探测控制方法[C]//2020年中国航天大会论文集. 北京: 中国宇航出版社, 2020. [5] 李亨, 詹亚峰, 裴玉奎. 测控数传通信系统初步设计[C]//中国宇航学会深空探测技术专业委员会第十届学术年会. 太原: 中国宇航学会, 2013. [6] 关昭,乔卫东,杨建峰,等. 火星多光谱相机的地面几何标定研究[J]. 深空探测学报(中英文),2018,5(5):465-471.GUAN Z,QIAO W D,YANG J F,et al. Ground geometric calibration of Mars multispectral camera[J]. Journal of Deep Space Exploration,2018,5(5):465-471. [7] EDWARDS C D. Relay communications for Mars exploration[J]. International Journal of Satellite Communications & Networking,2010,25(2):111-145. [8] 李惠军,高海波,邓宗全. 月球车桅杆机构设计与分析[J]. 机器人,2008,30(1):13-16.doi:10.3321/j.issn:1002-0446.2008.01.003LI H J,GAO H B,DENG Z Q. Design and analysis of the lunar rover mast mechanism[J]. Robot,2008,30(1):13-16.doi:10.3321/j.issn:1002-0446.2008.01.003 [9] 周东,徐晓伟,贾阳,等. 火星车机构集成控制系统设计与实现[J]. 深空探测学报(中英文),2020,7(5):450-457.ZHOU D,XU X W,JIA Y,et al. Design and implementation of integrated control system for Mars rover mechanism[J]. Journal of Deep Space Exploration,2020,7(5):450-457. [10] 谢圆,周建亮,王永. 面向月球车任务规划的环境建模技术[J]. 东南大学学报:自然科学版,2010(S1):292-296.XIE Y,ZHOU J L,WANG Y. Environment modeling for mission planning for lunar rovers[J]. Journal of Southeast University (Natural Science Edition),2010(S1):292-296.