改进的基于测地线的机器人轨迹规划方法
张延恒，孙汉旭，贾庆轩
(北京邮电大学 自动化学院，北京，100876)
摘要：针对机器人运动规划过程中测地线微分方程初始条件的确定问题，提出一种基于微分运动的初始条件求解方法。该方法通过对比分析黎曼空间与三维空间中机器人末端运动轨迹，确定两者之间的空间对应关系，在此基础上建立测地线微分方程初始条件的求解模型。最后，以2自由度机器人为例，对比验证该方法的有效性。研究结果表明：该模型解决了以往采用线性搜索方法求解微分方程初始条件所造成的求解不确定性问题。
关键词：机器人；轨迹规划；测地线
中图分类号：TP242 文献标志码：A 文章编号：1672-7207(2011)05-1344-04
An improved robot trajectory planning method based on geodesics
ZHANG Yan-heng, SUN Han-xu, JIA Qing-xuan
(Automation School, Beijing University of Posts and Telecommunications, Beijing 100876, China)
Abstract: To solve the problem of the initial conditions of geodesics differential equations, a novel method based on differential motion was proposed. By way of analyzing the motion trajectory of robot between Riemannian space and three-dimensional space, their spatial mapping relationship was confirmed and a model used to compute the initial conditions was developed. Finally, a two-link manipulator was used to verify the application of this method. The results show that this model avoids the uncertainty problem existing in the linear search method used to solve initial conditions of the geodesics differential equations.
Key words: robot; trajectory planning; geodesic
机器人运动规划是机器人研究首先要面对的问题。目前，关于机器人运动规划的研究很多，但大多是以运动学逆解和多项式插值为基础进行的规划研 究[1-5]。随着研究手段的发展，李群李代数及微分几何等数学工具被越来越多地运用到机器人运动规划的研究中[6-12]。特别地，张连东等[6-8]采用微分几何中活动标架方法，对机器人的操作能力、运动规划等问题进行了研究，所采用的基于黎曼曲面上的测地线进行机器人最优轨迹规划的方法，具有非时间参考轨迹规划的优点。但他们在求解测地线微分方程时，采用的是线性搜索的方法，该方法通过设定关节初始广义速度范围，在小步长增量下，设定搜索误差范围；当搜索到的目标曲线上任意点与理想终点之间的误差小于设定的搜索误差时，认为搜索点满足条件，即由该搜索点(广义速度)作为测地线微分方程组的初始广义速度，从而完成对微分方程组的求解。这种方法存在2个缺点：一是在设定搜索范围时，较难确定广义初始速度的取值范围；二是这种搜索方法并不是一个准确的求解方法，在关节变量较多时搜索计算量大，而且搜索收敛性下降。针对此问题，本文作者提出了一种采用微分运动求解测地线微分方程初始条件的方法。该方法通过建立测地线初始方向与关节速度间的空间映射关系，从而实现精确的求解微分方程初始条件，简化了测地线微分方程的求解过程。
1 黎曼度量与测地线
黎曼度量是黎曼曲面上一个对称正定的张量 场[13]，对于机器人末端运动曲面，其运动的轨迹弧长的平方是一个正定对称的二次形式，因而就构成了一个黎曼度量。此黎曼度量是黎曼曲面上2点之间距离的度量方式。
在黎曼曲面上的测地线为曲线上每一个点的测地曲率为0的曲线，即沿测地线方向曲面上2个点之间的距离最短。因此，对机器人末端进行轨迹规划时，首先要将机器人的运动学特性进行几何化，并将其映射成黎曼曲面的形式，建立黎曼曲面后，就可以对曲面进行分析，从而脱离机器人实体，而单纯通过几何方法来研究机器人的运动，即通过研究机器人运动曲面上的测地线来对机器人进行最优轨迹规划。在利用测地线对机器人进行运动规划时，机器人各关节之间的运动不再是独立的，它们之间的耦合关系由测地线微分方程决定。
2 改进的机器人轨迹规划方法
2.1 两自由度机器人黎曼度量建模
平面两自由度机器人如图1所示，其操作臂末端位置可表示为：
式中：l1和l2分别为连杆1和连杆2的杆长；q1和q2分别为关节1和关节2的转角。
图1 两自由度机器人模型
Fig.1 Model of two-degree robot
据此建立以弧长平方为度量的机器人黎曼度量：
其中：s为轨迹弧长；a1, a2和a3为与系统结构及状态有关的参数。将其写成黎曼度量系数矩阵的形式为：
(1)
式中：
，为该机器人的黎曼度量系数 矩阵。
根据式(1)确定了黎曼度量系数矩阵后，机器人的运动就从真实的物理空间转换到了几何空间，即完成了对机器人运动性质的几何化。对机器人运动的研究就转换为对该几何空间中对应的黎曼曲面的研究。机器人末端执行器2点之间的最短运动路径对应于该黎曼曲面上的测地线[14]，所得到的测地线微分方程 如下：
式中：
为克里斯托弗系数。
为求解此二元二次微分方程组，将此微分方程组进行降阶处理，转换成标准的状态方程的形式，可得：
(2)
2.2 确定微分方程初始条件
由式(2)可看到，要对该微分方程进行求解，必须已知微分方程求解的初始条件，即初始角度和广义速度。对于机器人运动规划，通常情况下初始时各关节角度
为已知量，因而还必须确定初始时的广义速度，该广义速度是关于末端轨迹弧长的微分，其表示式为：
对于机器人，其末端运动的所有可达空间构成了抽象的空间曲面——黎曼曲面，在进行机器人空间2点之间的运动规划时，采用微分几何中的测地线方法。测地线为黎曼曲面上2点之间长度最短的曲线，因此，可以将黎曼曲面上的测地线等距对应于三维欧氏空间中的曲线。在三维欧氏空间中，2点之间的最短曲线即为直线。该最短直线与黎曼曲面上的测地线对应。由此可知，对应于机器人末端执行器最短运动距离，末端执行器必须沿空间运动始末2点之间的连线运动，因而可以通过如下方法确定运动初始广义速度。
对于空间2点
和
(其中为运动起始点，为运动终止点)，其运动矢量为：
(3)
对式(3)所表示的矢量单位化，可得到：
(4)
在此设定末端运动初始速度v，则机器人末端运动速度矢量
可表示为：
根据机器人运动学关系[15]可以得到机器人运动雅可比矩阵J (该雅可比矩阵必须为满秩矩阵，对于冗余度机器人，可通过设定约束条件使其成为满秩)，则由末端运动速度与关节运动速度之间的雅可比矩阵可以得到此速度所对应的关节速度：
(5)
根据测地线微分方程可知该微分方程所解得的关节角度是关于末端运动距离s的函数，即：
式中：
；
。
所以，可得：
从而可以推得：
(6)
由于机器人末端执行器运动距离从0逐渐增大，因此，距离增量ds为正值，有：
(7)
将式(5)和(7)代入式(6)可得到最终形式的机器人关节速度：
(8)
由式(8)可以看出：对于给定机器人末端执行器运动始末2点，其关节角度关于末端运动弧长的微分在给定点处的大小只与该点处的运动雅可比矩阵及2点之间的方向矢量有关，而与末端执行器在此位置时的实际运动速度无关。对应于测地线微分方程，可以看出：对于黎曼曲面上的给定点，存在无数条测地线，而对于给定的末端执行器运动终止点，对应于黎曼曲面上的测地线只存在1条。
3 方法验证
对于图1所示两自由度机器人，机械臂各连杆杆长为1 m。处于图示位置时，各关节转角q1=π/3，q2= -2π/3。机械臂由当前位置P1=(1, 0)T运动至空间点P2=(0, 1)T，即沿方向
运动
个单位，所得到的仿真结果如图2和图3所示。
与文献[6-7]所得到的轨迹规划结果进行对比可知：本文所得结果与文献[6-7]中所得结果一致，证明了本文所提出的确定测地线微分方程初始条件的方法的正确性。本文所采用的基于微分运动的测地线微分
图2 关节角度规划结果
Fig.2 Planning results of joint angle
图3 角度耦合关系
Fig.3 Relationship of joint angles
方程初始条件求解方法，可以精确求解测地线微分方程的初始方向(广义速度)。
4 结论
(1) 本文所提出的计算方法解决了以往采用线性搜索方法求解微分方程初始条件所造成的求解不确定性问题。
(2) 在给定规划起始点和终止点时，测地线微分方程的初始方向仅与该点处的运动雅可比矩阵及2点之间的方向矢量有关，而与末端执行器在此位置时的实际运动速度无关。
参考文献：
[1] 李东洁, 邱江艳, 尤波. 一种机器人轨迹规划的优化算法[J]. 电机与控制学报, 2009, 13(1): 123-127.
LI Dong-jie, QIU Jiang-yan, YOU Bo. Optimal algorithm for trajectory planning of the robot[J]. Electric Machines and Control, 2009, 13(1): 123-127.
[2] Park J J, Kim J H, Song J B. Path planning for a robot manipulator based on probabilistic roadmap and reinforcement learning[J]. International Journal of Control, Automation and Systems, 2007, 5(6): 674-680.
[3] Blackmore L, Williams B. Optimal manipulator path planning with obstacles using disjunctive programming[C]//Proceedings of the 2006 American Control Conference. Minneapolis, 2006: 3200-3202.
[4] 谭冠政, 徐雄, 肖宏峰. 工业机器人实时高精度路径跟踪与轨迹规划[J]. 中南大学学报: 自然科学版, 2005, 36(1): 102-107.
TAN Guan-zheng, XU Xiong, XIAO Hong-feng. Real-time and accurate hand path tracking and joint trajectory planning for industrial robots[J]. Journal of Central South University: Science and Technology, 2005, 36(1): 102-107.
[5] 黄志雄, 何清华, 吴万荣, 等. 大型水轮机叶形现场检测机械臂运动学分析[J]. 中南工业大学学报: 自然科学版, 2003, 34(2): 170-172.
HUANG Zhi-xiong, HE Qing-hua, WU Wan-rong, et al. Kinematics analysis of field measuring manipulator of large turbine blades[J]. Journal of Central South University: Science and Technology, 2003, 34(2): 170-172.
[6] 张连东, 王德伦. 一种基于测地线的机器人轨迹规划方法[J]. 机器人, 2004, 26(1): 83-86.
ZHANG Lian-dong, WANG De-lun. Robot trajectory planning based on geodesics[J]. Robot, 2004, 26(1): 83-86.
[7] 张连东. 基于微分几何学的机器人操作性能的研究[D]. 大连: 大连理工大学机械工程学院, 2004: 53-54.
ZHANG Lian-dong. Robot manipulability research based on differential geometry[D]. Dalian: Dalian University of Technology. School of Mechanical Engineering, 2004: 53-54.
[8] ZHANG Lian-dong, ZHOU Chang-jiu. Robot optimal trajectory planning based on geodesics[C]//2007 IEEE International Conference on Control and Automation. Guangzhou, 2007: 2433-2436.
[9] Altafini C. Redundant robotic chains on riemannian manifolds[C]//Proceedings of the 41st IEEE Conference on Decision and Control. Lasvegas, 2002: 1534-1539.
[10] Altafini C. Geometric motion control for a kinematically redundant robotic chain: Application to a holonomic mobile manipulator[J]. Journal of Robotic Systems, 2003, 20(5): 211-227.
[11] Rodnay G, Rimon E. Isometric visualization of configuration spaces of two degrees of freedom mechanisms[C]//Proceeding of the 1998 IEEE International Conference on Robotics and Automation, Belgium, 1998: 2928-2934.
[12] Zefran M, Kumar V, Croke C B. On the generation of smooth three dimensional rigid body motions[J]. IEEE Transactions on Robotics and Automation, 1998, 14(4): 576-589.
[13] 陈省身, 陈维桓. 微分几何讲义[M]. 北京: 北京大学出版社, 2001: 129-139.
CEHN Xing-shen, CHEN Wei-huan. Lectures on differential geometry[M]. Beijing: Beijing University Press, 2001: 129-139.
[14] 梅向明, 黄敬之. 微分几何[M]. 北京: 高等教育出版社, 2003: 213-227.
MEI Xiang-ming, HUANG Jing-zhi. Differential geometry[M]. Beijing: Higher Education Press, 2003: 213-227.
[15] 熊有伦, 丁汉, 刘恩沧. 机器人学[M]. 北京: 机械工业出版社, 1993: 87-100.
XIONG You-lun, DING Han, LIU En-cang. Robotics[M]. Beijing: China Machine Press, 1993: 87-100.
(编辑 赵俊)
收稿日期：2010-01-28；修回日期：2010-05-10
基金项目：国家自然科学基金青年科学基金资助项目(50905019)；中央高校基本科研业务费专项资金资助项目(2010PTB-07-01)
通信作者：张延恒(1978-)，男，山东烟台人，博士，讲师，从事机器人运动学、动力学及控制研究；电话：010-62281368；E-mail: zyh620@163.com
