张博闻，宋雪萍，张连东，平原，陈勇

(大连交通大学 机械工程学院，辽宁 大连 116028)*



基于测地线的3D-LIPM机器人轨迹规划方法

张博闻，宋雪萍，张连东，平原，陈勇

(大连交通大学 机械工程学院，辽宁 大连 116028)*

提出一种基于测地线的3D-LIPM(3D Linear Inverted Pendulum model)机器人轨迹规划方法.以3D-LIPM为研究对象，构建使系统能量(动能)为最优的黎曼度量，建立3D-LIPM的测地线方程.采用数值分析方法对测地线微分方程组进行求解，得出3D-LIPM机器人的轨迹变化规律：当倒立摆与水平面的夹角θ位于0～π/2和π/2～π之间时，其投影与X轴的夹角ρ会随之增大且曲率半径也随之增大；当θ为π/2时，在一定范围内，曲率半径无穷大，曲线随之出现转折点且呈下降趋势；当ρ改变时，角位移θ、线位移l与ρ的曲线变化关系不变，只是随着ρ值的改变图像呈平移关系；当l改变时，角位移θ、角位移ρ与线位移l曲线变化关系近似不变，只是随着l值的改变图像呈现近似平移关系.

机器人轨迹规划；测地线；能量最优

0 引言

机器人轨迹规划对研究机器人来说非常重要，机器人的轨迹规划合理，可以准确和快速地对机器人进行一系列的控制.轨迹规划包括关节空间的轨迹规划和笛卡尔空间的轨迹规划，前者是通过关节之间的变化来描述机器人轨迹的，后者是通过一种时间函数来描述机器人手部的位姿、速度和加速度[1].国内外许多学者针对测地线方法进行了相关研究，Milos Zefran等[2]应用测地线研究了单自由度刚体的平滑轨迹；Park[3]将黎曼度量的概念用于机器人机构的设计；范国良等[4]基于几何方法对多智能体群体刚性运动的路径规划进行了研究；Rodnay与Rimon[5]用测地线定性分析了二自由度机器人的动力学性质.但并未给出一般条件下的测地线的确定方法.张连东等[6-8]采用微分几何中活动标架方法，对平面二自由度机器人的相关问题运用测地线方法方法进行了研究以系统动能作为黎曼度量， 使两节点之间动能最优，将轨迹进行优化.

目前将测地线应用于3D-LIPM机器人的最优轨迹规划方面的研究比较少.本文从系统模型3D-LIPM机器人的动能出发，通过构建测地线黎曼度量的方法，得出动能最优的轨迹，此分析方法在一定程度上减小了计算量，如果用双腿12个自由度把系统的动能表达出来，用测地线求其计算量庞大.该方法同样适用于三维以上的机器人使用.

1 建立3D-LIMP机器人测地线方程

1.1 测地线

测地线是n维曲面上两点之间的最短距离.黎曼度量是黎曼曲面上两点之间距离的度量方式，不同的黎曼曲面对应着不同的黎曼度量，不同的黎曼度量对应着不同的测地线方程，在实际方程将黎曼度量中的系数带入到测地线微分方程即可对其求解算出系统最优轨迹.

1.2 构建系统能量最优的黎曼度量

把仿人机器人看成三维倒立摆模型，机器人在X-Y-Z坐标中运动学正解(见图1)：

(1)

图1 关节空间坐标中的机器人倒立摆

已知机器人端执行器的角速度ωi，弧长s平方的黎曼度量表达式为：

(2)

式中,i的取值范围为1～3.

弧长平方的黎曼度量表达式可写成黎曼度量系数矩阵的形式为：

(3)

式中，矩阵G为黎曼度量的度量系数矩阵，gij为黎曼度量系数.

根据上述已知可以得到机器人轨迹弧长的黎曼度量为：

ds2=dX2+dY2+dZ2

(4)

其中， s为机器人轨迹的弧长；ds2为弧长的黎曼度量.

将机器人的速度式中的X、Y、Z变量转换成为极坐标形式，从而得到弧长的黎曼度量为：

ds2=l2dθ2+l2cos2θdθ2+dl2

(5)

将式(5)写成黎曼度量系数矩阵的形式为:

(6)

1.3 求Christoffel符号

黎曼度量系数已求出即式(6)，已知Christoffel符号公式：

(7)

为第一类克里斯托弗符号(Christoffel)，所有指标i，j，k，m的取值范围为1～n，gij为度量系数矩阵的逆矩阵所对应的各元素.

1.4 求3D-LIMP机器人测地线微分方程

已知测地线的微分方程为：

(8)

已知测地线方程组为：

(9)

(10)

式(10)为最终的测地线微分方程组.

2 求解测地线微分方程

2.1 测地线微分方程初始条件

给定机器人关节空间坐标内的初值，随意选一组解：

l=1.0; l=0.6

2.2 解测地线的微分方程

根据解出的测地线微分方程，在给定的初始条件下，求解微分方程组，得到该机器人的轨迹规划结果，如图2所示.

(a) 弧长-角位移θ

(b) 弧长-角速度

(c) 弧长-角位移ρ

(e) 弧长-线位移

(f) 弧长-线速度

3 对测地线方程进行轨迹规划

3.1 给定若干θ的值时的轨迹规划图

当θ为某一定值时，得出角位移ρ和线位移l与角位移θ之间变化关系曲线如图3～ 5所示.

由图3可得：

(1)当θ从0-π/2，角位移ρ随着θ的增加而增加，而在θ=π/4，ρ=2.5时，曲线呈水平线，角位移θ最大值为1.25，这几种情况表示机器人质心轨迹呈弧线摆动；θ=π/3时，曲线先增加后下降(转折点在ρ=3处)，此时机器人质心轨迹摆到一定角度后会往回运动，对应的3D-LIPM不断调整自己的姿势以保持平衡.而θ=π/2时，当角位移ρ增加，角位移θ呈水平线，当ρ=3.8时图像会垂直下降；机器人质心会绕着z轴进行旋转，θ角始终不变，在转折点处质心会上升但杆在平面上的投影角度不会改变，只是投影长度变化；

(2)当θ=2π/3和θ=3π/4时，曲线为凹曲线且线长较短.θ=2π/3时，曲线增长速率较快；θ=3π/4时，曲线增长速率较慢，表示倒立摆活动范围变小.

图3 角位移ρ与角位移θ的关系图

图4 角位移ρ与线位移l的关系图

由图4可得：

(1)当θ=π/8、π/6、π/4、π/3、2π/3、3π/4时，曲线为凹曲线增长，随着θ增加，角位移ρ和线位移l都增加，但曲线增长率会逐渐降低.对应的3D-LIPM旋转的弧度半径增大;

(2)当θ=π/2，图像先呈水平线增长，然后会垂直上升.此时的3D-LIPM与地面倾角最大，质心点离地面高度不变，处于平稳的过程，当达到最平稳的时候，3D-LIPM质心与地面距离达到最大.

由图5可得：

(1)当θ=π/2时，线位移随着角位移增大而减小.对应的3D-LIPM与地面倾角会逐渐减小，但3D-LIPM质心高度会增加.当θ为0.8(角度值为45°)时，质心高度不变；此时如果角度再减小的话，3D-LIPM机器人就会有不稳定，产生摔倒趋势;

(2)而对于其他情况，线位移随着角位移增大而增大.对应的3D-LIPM与地面倾角增大，3D-LIPM质心高度会增加.

图5 角位移θ与线位移l的关系图

当θ位于0～π/2之间以及π/2～π之间时，随着θ的增加，ρ会随之增大且曲率半径增大，3D-LIPM机器人不断进行调整以保持平稳；当为π/2时，在一定范围内，曲率半径无穷大，随之出现转折点且曲线呈下降趋势，3D-LIPM机器人此时最平稳.

3.2 给定若干ρ的值时的轨迹规划图

当ρ为某一定值时，得出角位移和线位移l与角位移ρ之间变化关系曲线如图6～ 7所示.

图6 角位移ρ与角位移θ的关系图

由图6可得：角位移随着角位移ρ的增加而先增加后下降.表示此时3D-LIPM与地面倾角先增加后减小，3D-LIPM远离x轴，将向下一个位置迈出，机器人质心轨迹相同.

由图7可得：图像呈曲线增长，随着横坐标增加曲线斜率会增加，表示3D-LIPM机器人的质心点离地面距离增加，质心轨迹相同.

图7 角位移ρ与线位移l的关系图

由图6和图7可以判断出角位移与线位移l的关系图，图中会出现多条线重叠于一条线，说明3D-LIPM与地面倾角先增加后减小，3D-LIPM机器人质心高度增加.当ρ增大时，曲线形状趋于相同，图像呈平移关系.3D-LIPM与地面倾角先增加后减小，3D-LIPM机器人质心点离地面距离增加，以便向下一步迈出做好准备.

3.3 给定若干l的值时的轨迹规划图

当l为某一定值时，得出角位移、角位移ρ与线位移l之间变化关系曲线如图8所示.

图8 角位移ρ与角位移θ、线位移l的关系图

由图8角位移ρ与角位移的关系可得：当杆长改变的时候，角位移θ与角位移ρ的关系变化不大，基本重叠于同一条曲线.表明杆长的改变对角位移和角位移ρ的影响不大.由图8角位移ρ与线位移l的关系、角位移与线位移l的关系表明：杆长随着角位移ρ的增加而增大，当达到一定值时，变化加剧；当l增大时，曲线形状近似趋于相同，呈平移关系.

4 结论

(1)本文应用测地线方法对仿人机器人的倒立摆模型进行了分析，通过求解测地线方程得到了3D-LIPM倒立摆的测地线模型；

(2)分析了各参数之间的变化规律，可知当位于0～π/2之间以及π/2～π之间时，随着的增加，ρ会随之增大且曲率半径增大；当为π/2时，在一定范围内，曲率半径无穷大，随之出现转折点且曲线呈下降趋势；当分别给定角位移ρ和线位移l时，曲线关系变化不大，近似呈平移关系；

(3)文中的分析对象是三维模型，如果探讨多维情况下的，测地线也是一种可行的研究方法.

[1]蔡自兴,谢斌.机器人学[M]. 3版，北京:清华大学出版社，2015.

[2]ZEFRAN M, KUMAR V, CROKE B C.On the generation of smooth three-dimensional rigid body motions[J]. IEEE Transactions on Robotics and Automation, 1998, 14(4): 576-589.

[3]PARK FC. Distance Metrics on the Rigid Body Motions with Applications to Mechanism Design[J].ASME Journal of Mechanical Design, 1995,117(3):48-54.

[4]范国梁,王云宽.基于几何方法的多智能体群体刚性运动的路径规划[J].机器人，2005,27(4):362-366.

[5]RODNAY G, RIMON E. Isometric visualization of configuration spaces of two degrees of freedom mechanisms[J]. Mechanism and Machine Theory, 2001, 36(4): 523-545.

[6]张连东,王德伦.一种基于测地线的机器人轨迹规划方法[J].机器人, 2004, 26(1): 83-86.

[7]张连东.基于微分几何学的机器人操作性能的研究[D].大连,大连理工大学, 2004.

[8]ZHANG LIANDONG,ZHOU CHANGJIU.Robot optimal trajectory planning based on geodesics[C]//2007 IEEE International Conference on Control and Automation .Guangzhou, 2007: 2433-2436.

3D-LIPM Robot Trajectory Planning Method based On Geodesic

ZHANG Bowen, SONG Xueping, ZHANG Liandong, PING Yuan, CHEN Yong

(School of Mechanical Engineering, Dalian Jiaotong University, Dalian 116028, China)

A geodesic based trajectory planning method for 3D-LIPM(3D Linear Inverted Pendulum model) robots is proposed. Taking 3D-LIPM as the research object, the system energy (kinetic energy) is constructed as the optimal Riemann metric, and the geodesic equation of 3D-LIPM is established. Numerical analysis method is used to solve the differential equations of the geodesic line, and the change law of the trajectory of the 3D-LIPM robot is obtained. When the inverted pendulum and the horizontal plane angle θ is located in between 0-π/2 and π/2-π, the angle ρ between the projection and the X axis will subsequently increases, and the radius of curvature increases. When θ is π/2, the radius of curvature is infinity in a certain range, then the curve has turned point and downward trend. When changing ρ, invariant curve variation of θ angle displacement and angular displacement ρ and line displacement l, the image show a translation relation with the ρ value change. When changing l, invariant curve variation of θ angle displacement and angular displacement ρ and line displacement l, the image show the approximate relation shift with value l change.

robot trajectory planning; geodesics; energy optimal

1673- 9590(2016)06- 0089- 06

2016-06-23

国家自然科学基金资助项目(51275065，51305055)

张博闻(1992-)，男，硕士研究生；张连东(1971-)，男，副教授，博士，主要从事机器人轨迹规划方面的研究E- mail:2285084880@qq.com.

A