基于D-H坐标变换的复合轮式海底行走机构的运动学
姜勇，冯雅丽，张文明，徐鸣冲
(北京科技大学 机械工程学院，北京，100083)
摘要：为使海底行走机构具有良好的自主导航能力，完成深海复杂地形路径规划需建立一个合理的、基于多传感器实时监测信息的运动学模型。以复合轮式海底行走机构为研究对象，基于D-H坐标建模方法和深海复杂地形环境特征，推导出海底行走机构的运动学方程，得到车体的位姿信息，并利用MATLAB工具获得车体的运动学参数理论计算曲线。在运动学分析的基础上，利用ADAMS软件建立该行走机构的运动学仿真模型，得到车体的运动学参数仿真曲线。通过理论计算和仿真试验对比，验证了所建立运动学模型的正确性，并为该行走机构路径规划和运动控制奠定了基础。
关键词：复合轮式海底行走机构；D-H坐标变换；运动学方程；运动学仿真
中图分类号：TP242.6 文献标志码：A 文章编号：1672-7207(2011)S2-0324-06
Kinematics of composite wheeled submarine vehicle based on D-H coordinate transformation
JIANG Yong, FENG Ya-li, ZHANG Wen-ming, XU Ming-chong
(School of Mechanical Engineering, University of Science and Technology Beijing, Beijing 100083, China)
Abstract: To enable submarine vehicle having a good autonomous navigation to complete the simulation test in deep-sea complex terrain and rough terrain path, it is required to establish a reasonable kinematic model based on multi-sensor of a real-time monitoring information. Taking composite wheeled submarine vehicle as the research object, based on D-H transform modeling method and the feature of complex topographical environment in deep sea, the kinematical equation of the vehicle of deep seabed was deduced and the gesture information of vehicle was obtained, and relevant kinematic parameters curves were gained by using MATLAB tools. Based on the kinematic analysis, using AMAMS software to establish the kinematic simulation model of the vehicle, the kinematic parameters curves were obtained. By comparing theoretical calculations and simulation experimental results the correctness of the established kinematic model was verified, laying the foundation for path planning and motion control of the vehicle.
Key words: composite wheeled submarine vehicle; D-H coordinate transformation; kinematics equations; kinematic simulation
复合轮式行走机构在深海复杂地形和多变地质环境中以自主方式行进，需要具备良好的自主导航能力，以实现路径规划和避障。此外，还需要建立基于多传感器实时监测信息的运动学模型，根据即时位置、方向、速度和加速度，规划下一步的控制策略[1-3]。
在海底车运动学研究领域，大多数研究者所建立的模型局限于平坦、光滑的深海地形环境[4-6]。这些模型只能用于海底行走机构在二维平面中的运动控制，即X和Y方向的平移和绕Z轴的转动，而不适于描述崎岖不平的三维深海复杂地形上的运动。本文作者提出一种基于D-H坐标变换的复合轮式海底行走机构运动学建模方法，推导出复合轮式海底行走机构的运动学方程。通过理论计算和仿真对比可知，该方程可较好地描述海底行走机构的运动，解决在有壕沟、台阶和斜坡的深海复杂三维地形上行驶的海底行走机构的运动学问题，为海底行走机构的路径规划和运动控制提供理论基础。
1 复合轮式海底行走机构的运动学
建模
复合轮式海底行走机构(图1)包括4套轮组机构，相当于传统的4×4车辆。每个轮组中的3个车轮在辅助油缸的拉推作用下可实现一定程度的空间结构变化，从而可以在深海复杂三维地形上行驶。采用铰接式车架，运动灵活，能提高通过的稳定性。
图1 海底复合轮式行走机构的基本结构
Fig.1 Basic structure of composite wheeled submarine vehicle
海底行走机构的运动学模型将建立车轮运动与车体之间的关系，从而确定车体在不同运动状态的位姿和运动学参数，为控制系统提供参数[7-9]。建模中做以下假设：(1)车轮与深海底面无变形；(2)车轮与深海底面之间为点接触；(3)接触点的几何中心位于过轮心的深海底面法线与接地侧车轮圆周的交点。
1.1 坐标系的建立及坐标变换
为建立运动学模型，首先参照D-H坐标变换规 则[10]，在海底行走机构各关节处建立一系列坐标系，用坐标系间的齐次变换表示(见图2)。
根据D-H坐标变换规则，θi表示绕Zi-1轴旋转由Xi-1转向Xi的角度；di表示从第i-l坐标系的原点到Zi-1轴和Xi轴的交点间沿Zi-1的距离；ai表示Zi-1轴和海底行走机构运动模型，得到各相邻坐标系对应的D-H参数(见表1)。
根据以上定义和D-H参数，建立坐标系i相对于坐标系i-l的位姿变换矩阵：
(1)
式中：C表示cos；S表示sin。
1.2 考虑滑移的轮地接触模型
车轮与海底表面的接触情况如图3所示，建立固定在海底表面的坐标系，定义Xci轴为沿接触点的切点方向，Zci轴为沿接触点的法线方向。在车轮的轮心处定义固定在车轮轴上的坐标系，R为车轮半径，Xwi轴沿水平方向，Zwi轴沿竖直方向。Zwi轴和Zci轴之间的夹角δi定义为接触角。
图2 海底行走机构左侧坐标系
Fig.2 Left coordinate of composite wheeled submarine vehicle
表1 D-H坐标参数
Table 1 D-H coordinate parameters
图3 轮地接触坐标系转换模型
Fig .3 Transformation model of wheel-ground coordinate
Wi系与Ci系之间的坐标变换矩阵可以由二者的旋转变换与沿
轴的平移变换得到，即
(2)
当车轮Wi在地面Ci上运动时，可能存在转向滑移εi、侧向滑移ηi以及车轮滚动滑移ζi，得到相应的坐标变换矩阵，即
(3)
记ζ=
为滑移向量。
1.3 基于坐标变换的运动学建模
由坐标系的定义，海底行走机构运动模型采用的坐标变换链如图4所示。
图4 坐标变换关系图
Fig. 4 Coordinate transformation diagram
图4中：Ci为第i轮地接触点坐标系；Wi为第i车轮轴心坐标系；D为中节车体坐标系；R为参考坐标系Bi前后轮组铰接点坐标系；Pi为各关节点坐标系；L为海底面坐标系。
根据图2建立的车体坐标系以及图4所确定的坐标变换关系图，海底行走机构各车轮轴心处Wi系相对于参考坐标系R的位姿为：
(4)
因此，将表1中D-H参数代入式(4)就能确定各轮中心与参考坐标系之间的位姿关系。
海底行走机构的各车轮轴心Wi相对于海底表面的坐标变换矩阵为：
(5)
根据轮地接触变换矩阵及轮地滑移变换矩阵，建立车体参考坐标系与车轮接地点Ci′ 坐标系之间的瞬时位姿关系[11-14]。若已知各车轮在海底面的运动状态，则可分析得到车体的运动状态。即
(6)
为量化海底行走机构的运动，使海底行走机构配置速度矢量
(
为车体前进方向速度；
为车体侧向速度；
为车体上下方向速度；
为车体偏航角速度；
为车体俯仰角速度；
为车体横滚角速度)。车体参考坐标系在滑移的状态下，位姿
描述可用转换矩阵
。由于
和时间无关，故
。
表示海底车的车体参考坐标系和第i个车轮接地点坐标系
之间的变换矩阵。即
(7)
再根据矩阵变换微分的定义可以得到变换矩阵
，即
(8)
综合上式可得到海底行走机构各车轮运动速度和车体运动速度之间的线性变换的关系——雅克比矩 阵[15-17]：
(9)
式中：
为前、后连接板旋转角速度；
为前、后轮组铰接点旋转角速度；
为轮地接触角速度；
为车轮滚动角速度；
为车轮转向滑移速率；
为车轮侧向滑移速率；
为车轮滚动滑移速率。
2 复合轮式海底车运动学理论分析与仿真验证
基于D-H坐标变换的运动学方程，在MATLAB中，采用n阶递归牛顿-欧拉法，得出复合轮式海底行走机构主车体重心在X方向的速度理论曲线(图5)、X方向的位移理论曲线(图6)和Y方向的位移理论曲线(图7)。
在ADAMS/View中建立复合轮式行走机构在深海复杂地形环境中行驶的运动学仿真模型，得到车体重心在X方向的仿真速度曲线(图5)和仿真位移曲线(图6)及Y方向的仿真位移曲线(图7)。
图5 主车体重心在X方向的速度曲线
Fig.5 Curves of main body centre’s velocity along X direction
图6 主车体重心在X方向的位移曲线
Fig.6 Curves of main body centre’s displacement along X direction
图7 主车体重心在Y方向的位移曲线
Fig.7 Curves of main body centre’s displacement along Y direction
(1) 在越障过程中的5~15 s、25~35 s、55~65 s以及75~85 s的区间内，主车体重心位移曲线发生了一定的倒退波动，说明主车体在越障过程中发生了往后倒退的现象。
(2) 在越障的5~15 s、25~35 s、55~65 s以及75~ 85 s的区间内，速度曲线存在上下波动和小于零的波段，说明主车体在越障过程中有速度不稳和往后倒退的现象。
3 结论
(1) 提出一种基于D-H坐标变换的复合轮式海底车运动学建模方法，建立适应深海三维复杂地形环境的运动学模型，得到即时位置、方向、速度和加速度参数，为控制系统提供参数。
(2) 在ADAMS/View中建立复合轮式行走机构在深海复杂地形环境中行驶的运动仿真模型，仿真结果与运动学模型曲线的变化趋势基本一致，所建运动学模型可描述海底行走机构的运动学特性，为海底行走机构的路径规划和运动控制提供理论基础。
参考文献：
[1] 邓宗全, 李所军, 高海波. 行星探测车被动摇臂悬架的研究与发展[J]. 宇航学报, 2008, 29(6): 1655-1700.
DENG Zong-quan, LI Suo-jun, GAO Hai-bo. Research and development of passive rocker suspension of planetary exploration rover[J]. Journal of Astronautics, 2008, 29(6): 1655-1700.
[2] 陶建国, 邓宗全, 高海波, 等. 六圆柱-圆锥轮式月球车的设计[J].哈尔滨工业大学学报, 2006, 38(1): 4-7.
TAO Jian-guo, DENG Zong-quan, GAO Hai-bo, et al. Design of a lunar rover with six cylinder-conical wheels[J]. Journal of Harbin Institute of Technology, 2006, 38(1): 4-7.
[3] 邓宗全, 高海波, 王少纯, 等. 行星轮式月球车的越障能力分析[J]. 北京航空航天大学学报, 2004, 30(3): 197-201.
DENG Zong-quan, GAO Hai-bo, WANG Shao-chun, et al. Analysis of climbing obstacle capability of lunar rover with planetary wheel[J]. Journal of Beijing University of Aeronautics and Astronautic, 2004, 30(3): 197-201.
[4] Alexander J C, Maddocks J H. On the kinematic of wheeled mobile robots[J]. International Journal Robotics Research, 1989, 8(5): 15-26.
[5] Kanayama Y. Two-dimensional wheeled vehicle kinematics[C]// Proceedings of the 1994 IEEE International Conference on Robotics and Automation. San Diego: IEEE, 1994: 3079-3084.
[6] Borenstein J. Control and kinematics design of multi-degree of freedom mobile robots with compliant linkage[J]. IEEE Transactions Robotics and Automation, 1995, 11(1): 21-35.
[7] 宋小康, 谈大龙, 吴镇炜, 等. 全地形轮式移动机器人的运动学建模与分析[J]. 机械工程学报, 2008, 44(6): 148-154.
SONG Xiao-kang, TAN Da-long, WU Zhen-wei, et al. Kinematics modeling and analyses of all-terrain wheeled mobile robots[J]. Chinese Journal of Mechanical Engineering, 2008, 44(6): 148-154.
[8] 王佐伟, 梁斌, 吴宏鑫. 六轮月球探测车运动学建模与分析 [J]. 宇航学报, 2003, 24(5): 456-462.
WANG Zuo-wei, LIANG Bin, WU Hong-xin. Kinematical modeling and analysis of six-wheel lunar rover[J]. Journal of Astronautics, 2003, 24(5): 456-462.
[9] 蔡则苏, 洪炳熔, 吕德生. HIT-1型月球车的运动学分析[J]. 哈尔滨工业大学学报, 2003, 35(9): 1098-1101.
CAI Ze-su, HONG Bing-rong, L? De-sheng. Kinematic analysis of HIT-1 lunar rover[J]. Journal of Harbin Institute of Technology, 2003, 35(9): 1098-1101.
[10] 张铁. 机器人学[M]. 广州: 华南理工大学出版社, 2001: 85- 87.
ZHANG Tie. Robotics[M ]. Guangzhou: South China University of Technology Press, 2001: 85-87.
[11] Richard V. Rover technology development and mission infusion [C]// IEEE ICRA 2005 Planetary Rover Workshop. Barcelona, 2005.
[12] Nohmi M, Miyaharat A. Modeling for lunar lander by mechanical dynamics software[C]// AIAA Modeling and Simulation Technologies Conference and Exhibit 2005. California: AIAA, 2005: 61-68.
[13] LI Yong-quan, LIU Jian-zhong, YUE Zong-yu. NAO-1: Lunar highland soil simulant developed in China[J]. Aerosp Engrg, 2009, 22(1): 53-57.
[14] Lizuka K. Experimental study of wheeled forms for lunar rover on slope terrain[C]// Proceedings of the 9th IEEE International Workshop. 2006: 266-271.
[15] Baller R, Leung W, Barfoot T. Experiment and simulation results of wheel soil interaction for planetary rovers[C]// Proceedings of IROS. Edmonton, 2005: 586-591.
[16] Volpe R, Balaram J, Ohm T, et al. Rocky 7 Mars rover prototype[C]// Proceedings of IEEE International Conference on Intelligent Robots and Systems. Piscataway, NJ: IEEE, 1996: 1558-1564.
[17] 尚伟燕, 李舜酩, 鲍庆勇. 新型探测车复合式移动系统结构设计及运动仿真[J]. 机械科学与技术, 2009, 28(7): 881-885.
SHANG Wei-yan, LI Shun-ming, BAO Qing-yong. Structure design and kinematics modeling of a new compound moving system of an exploration robot[J]. Mechanical Science and Technology for Aerospace Engineering, 2009, 28(7): 881-885.
(编辑 陈卫萍)
收稿日期：2011-06-15；修回日期：2011-07-15
基金项目：国家自然科学基金资助项目(50874006)；国际海底区域性研究开发“十一五”项目(DYXM-115-04-02-03)
通信作者：冯雅丽(1967-)，女，北京人，教授，从事整车技术研究；电话：010-62332467；E-mail: ylfeng126@126.com
