机器人操作臂奇异路径约束最优轨迹规划_连广宇 2002.pdf

《机器人操作臂奇异路径约束最优轨迹规划_连广宇 2002.pdf》由会员分享，可在线阅读，更多相关《机器人操作臂奇异路径约束最优轨迹规划_连广宇 2002.pdf（4页珍藏版）》请在得力文库 - 分享文档赚钱的网站上搜索。

1、文章编号:1002-0446(2002)06-0550-04机器人操作臂奇异路径约束最优轨迹规划X连广宇赵清杰孙增圻(清华大学 计算机科学与技术系 智能技术与系统国家重点实验室北京100084)摘要:路径约束最优轨迹规划的关键是引入标量路径参数来降低优化问题的维数.当路径穿越奇异点时,由于关节位移难以表示为任务空间路径参数的解析函数,给常规的路径参数化方法带来困难.本文引入一种新的参数化方法,采用路径跟踪方程解曲线的弧长为参数,解决了奇异点邻域的路径跟踪问题,把奇异路径轨迹规划转化为常规规划问题,并采用动态规划方法求解轨迹规划问题.仿真表明,本文提出的参数化方法与动态规划结合起来,是解决奇异路

2、径最优轨迹规划的有效途径.关键词:奇异点;奇异路径;最优轨迹规划;动态规划;中图分类号:TP24文献标识码:BSINGULAR PATH-CONSTRAINED OPTIMAL TRAJECTORYPLANNING FOR ROBOTIC MANIPULATORSLIAN Guang-yuZHAO Qing-JieSUN Zeng-qi(Dep artment of Comp uter Science and T echnology,T singhua University,Beijing100084)Abstract:Key to the path-constrained trajector

3、y planning is to introduce a path parameter to reduce the probleminto a low-dimension one.While the path passing through singularities,joint variable can hardly be presented asanalytical functions of task space-defined parameters,which causes difficulties given to conventional trajectoryplanning.In

4、this paper,a new parameter,arc-length of the solution curve to the path tracking equation,isintroduced.Based on this,the path-tracking problem near singularities is addressed,and singular path-constrainedtrajectory planningis transformed into a standard optimization problem,which can be solved by dy

5、namicprogramming.Simulation shows the parameterization combined with dynamic programming performs effectively insingular path trajectory planning.Keywords:kinematic singularity,singular path,optimal trajectory planning,dynamic programming1引言(Introduction)作为机器人学的基本问题之一,机械臂最优运动规划得到了广泛的研究 15,分别在路径约束(Pa

6、th-constrained)最优轨迹规划以及点到点最优运动规划两个方向取得丰富的研究结果.较早受到重视的是满足路径约束的最小时间轨迹规划问题(MTT P).早期研究的一个重要结果是提出约束路径的标量参数化方法 1,2,把轨迹优化问题降为二维,采用相平面打靶法得出最优轨迹;随后文 3 提出最大速度曲线特征点概念,通过构建约束曲线提高了相平面方法的效率;文 4 讨论了动力学奇异情形时的相平面方法;此外,动态规划技术也成功应用于 MTT P 问题 5.上述方法的核心在于引入标量路径参数,并假设任务空间路径参数到关节变量的解析映射总是存在的,从而利用路径参数来降低优化问题的维数;然而,对于包含运动学

7、奇异点的路径,奇异点邻域内的路径参数化会遇到所谓奇异问题,即在奇异点上:(1)关节变量并不总能表示为这些路径参数的解析函数,(2)即使存在这种函数关系也很难用常规方法求得.对于此类奇异路径,需借助新的路径参数化方法才能完成最优轨迹规划,为此,本文引入一种新的路径参数化方法,采用路径跟踪方程在扩展关节空间中的解曲线弧长作为参数,该路径参数的引入,使得奇异点邻域内存在路径参数到关节变量的光滑映第 24 卷第 6 期2002 年 11 月机器人ROBOTVol.24,No.6Nov.,2002X基金项目:国家 863 资助项目(编号:863-704-7-17).收稿日期:2002-04-23射,且该

8、参数的时间轨迹可自由规划,从而奇异路径轨迹规划变为常规的轨迹规划;基于此参数化方法,本文采用动态规划方法求得最优关节轨迹.仿真研究表明本文方法对于解决奇异路径最优轨迹规划是有效的.此外,此种参数化方法和轨迹规划算法也适用于非奇异路径约束的轨迹优化问题.2 最 优 轨 迹 规 划 问 题(Formulation ofoptimal trajectory planning)设机械臂期望路径可用标量 s 来参数化,即H=f(s),f(s)C2,s s0,sf(1)其中 H Rn为关节变量,f(s)为路径参数到关节变量的映射.优化问题的约束条件为H H H-,Ha Ha Ha,Hb Hb Hb-(2)

9、上式为运动学约束,不等式两边分别表示相应变量的上下限值.优化问题的目标为:确定路径参数的时间轨迹 s(t):t 0,tf,满足边界条件:H(0)=0,H(tf)=0Ha(0)=0,Ha(tf)=0(3)且使如下性能指标函数达到最优:mintf0dt=tf(4)式(1)(4)构成基于运动学约束的最小时间轨迹规划问题.(1)式中的参数 s,在前述文献中为任务空间路径长度或关节空间路径长度,以下介绍一种新的路径参数s,用于参数化奇异路径.3奇异路径的参数化(Parameterization ofsingular path)使机械臂 Jacobian 矩阵降秩的关节位形称为运动学奇异点,穿越奇异点的路

10、径称为奇异路径.在奇异点邻域内,求取常规路径参数到关节变量的映射因 Jacobian 矩阵降秩而变得困难,为此引入路径跟踪方程在扩展关节空间中解曲线的概念,用于求解奇异路径跟踪问题,并以此定义广义路径参数.3.1扩展关节空间路径参数刚性机械臂正运动学和微分运动学可表示为k:Rn Rm:x=k(H)(5)xa=5k5HHa=J(H)Ha(6)其中 x,xaRm为机械臂末端位姿和速度,H,HaRn为关节角位移和速度.本文讨论非冗余机械臂即 m=n.J(H)为机械臂的 Jacobian 矩阵.设任务空间路径可表达为参数 K的函数x=g(K)(7)其中 K K0,Kf.代入式(6)可得路径跟踪方程F(

11、H,K)=K(H)-g(K)=0(8)F(H,K)对变量求导数可得H(H,K)=5k5H-5g5K(9)称式(9)为扩展 Jacobian 矩阵.方程(8)为含标量参数 K的非线性方程组,由隐函数定理 8,在正则点方程(8)的解 H可表示为 K的函数,即存在H=f(K)(10)因此可用 K来参数化正则路径;但在奇异点,由于前述原因,必须寻求新的参数化方法.考察(8),若把参数 K视为普通变量,则(8)为含 n+1个变量的 n 维非线性方程,其解为 n+1 维空间的曲线,若存在参数来表示该解曲线,则此参数即为所求的奇异路径参数 s.一个自然的选择是解曲线弧长,因解曲线所在空间包含关节变量和 K,

12、称此弧长参数为扩展关节空间路径参数 s.由拟弧长算法可求取方程(8)的解曲线,选定 s 后,即得到 H(s),K(s).3.2基于拟弧长法的奇异路径跟踪设(H0K0)为方程(8)的初始解,从该解出发采用拟弧长法可求得直路径终点(Hf,Kf)的解曲线数值解.记 yk=(Hk,Kk)为方程(8)的第 k 个已知解,yak(Hak,Kak)为该点处解曲线的单位切向量.在正则点 H(H,K)的零空间是一维的,此时切向量 yak=(Hak.Kak)可由下式求得H(H,K)yak=0,yak=1(11)构造辅助方程N(yk,$sk)=yTk(y-yk)-$sk=0(12)上式的几何意义为垂直于 yak且与

13、 yk点距离为$sk的超平面,$sk为局部参数.联立(8)(12)得G=F(y)N(y,$s)=0(13)以 yk为初值,用 Newton 法求解此方程,即可得到解曲线的下一个解点 yk+1.辅助方程的引入使方程(12)在正则点上Newton 迭代二阶收敛.注意到上述算法中引入解曲线的局部参数$sk.式(1)中所需的全局参数 s 可通过积分$sk来获得s(k)=ki=0$sik=0,1,L(14)k=0 对应于解曲线初始点,有 s(0)=0,当 k=L 时,551第 24 卷第 6期连广宇等:机器人操作臂奇异路径约束最优轨迹规划对应于解曲线终点,有 s(L)=sf,L 为计算解曲线所需的最大计

14、算步数目.4路径参数与轨迹规划(Path parameter andtrajectory planning)记 H*为机械臂的奇异点,若下式成立则路径在该点上称为退化的UTs(H*)dxdK 0(15)其中 Us(H*)为 H*处末端速度奇异子空间,由非线性分支理论知,此类奇异点对应于解曲线上的极限点,极限点上 dK/ds=0.穿越奇异点时,任务空间期望速度必需满足以下约束条件:UTs(H*)xa=0(16)其中xa为末端的空间速度.由链式规则xa=dxdKdKdssa(17)由于极限点处 dK/ds=0,(16)恒成立,不受 sa的影响,因而使 s 成为可自由规划的路径参数.可见 s 的引入

15、不仅解决了奇异路径参数化问题,而且由于解曲线的几何性质吸收了奇异点对运动规划带来的约束,使 s 成为可自由规划的参数,从而奇异路径轨迹规划化为常规路径轨迹规划.5 基于 动态 规 划的 轨迹 优化(Dynamicprogramming-based optimization)在奇异路径参数化的基础上,本文采用动态规划算法来求解式(1)-(4)所定义的优化问题.将式(2)的约束条件化为(s,sa)函数形式,把 H=f(s)写成标量函数形式Hi=fi(s),i=1,n(18)易得Hai=dfidssa,i=1,n(19)Hbi=df2ids2(sa)2+dfidssbi=1,n(20)其中 i 为关

16、节序号,fi,dfi/ds.df2i/ds2均为方程(8)解曲线的局部几何性质,可通过插值方法求得,代入(2)即得(s,sa)的函数形式的约束条件.为提高算法效率,在(s,sa)平面上构造最大速度曲线可减少所需计算的网格点数目.由式(2)的速度约束及式(19),并记 dfi/ds=ai可得Hai aisa Hai,i=1,n(21)sai=Haia-1iai 0Haia-1iai 0无约束ai=0(22)其中,sai是与第 i 关节速度极值相对应的最大路径速度,则最大速度曲线可表示为0 sa(k)mini(sai(k)(23)其中 k 为解曲线上的离散步序数,同(14)中的 ko.等分参数 s

17、 为 N 份,离散步距为$1=sf/N,得网格点si,i=0,1,N.对于sa,只在最大速度曲线范围内进行离散化,设samax=maxk(sa(k)(24)等分sa为 M 份,离散步距为$2=samax/M,得网格点saj,j=0,1,M.考虑式(4)所给出的最小时间指标,定义如下的最优值函数:V(si,saj)=minl=0,M($1saj+sal+V(si+1,sal)i=0,1,N,j=0,1,M(25)其中$1saj+sal为相邻节点间的运动时间近似值.以下为动态规划方法求解最优轨迹的算法步骤.算法:奇异路径最优轨迹规划(1)由式(11-16)计算参数化解曲线;(2)由式(23)计算最

18、大速度曲线;(3)生成(s,sa)网格:节点为一结构型数据,内含三成员:有效标志,值函数,路径指针;(4)网格预处理:最大速度曲线以下区域标为有效;边界点仅标记(s0,sa0),(sN,sa0)有效;(5)在有效区内按式(25)逆序计算值函数;(6)按指针顺序找到最优网格点轨迹;(7)计算关节运动的时间轨迹.(8)结束.6仿真研究(Simulation study)机械臂结构及期望路径如图 1 所示.图 1二连杆机械臂奇异路径跟踪Fig.1Singular path tracking of 2-linked arm连杆长度长度为 l1=l2=1m;期望路径为圆心位于(1.5,0)半径为 r=0

19、.5 的圆,任务空间路径参数 H0552 机器人2002 年 11 月定义为机械臂末端对应的圆心角.机械臂初始位置在圆弧左端点,相应初始值分别为 H1=P/3,H2=-2P/3,H0=-P,期望路径沿圆弧按 H0增加(逆时针)方向运动,H0的终点值为 H0=P.要求规划最小时间关节运动轨迹,平滑地穿越奇异点.图 2 为奇异路径的参数化解,横坐标为解曲线弧长s,纵坐标为关节位移曲线.图 2关节位移解曲线Fig.2 Curve solution of joint displacement当 H1=0,H2=0 时末端穿越奇异点,图中可见在此点关节变量为参数 s 的光滑映射.下表列出轨迹优化问题的运

20、动约束:表 1运动学约束条件Table 1Kinematic constraints关节关节角 rad速度 rad/s加速度rad/s2H1-P/P-2/2-10/10H2-P/P-4/4-15/15图 3 给出相平面中的规划结果,虚线为最大速度曲线(vMax),实线为参数 s 的最优速度 曲线(vOpti).图 3相平面中最大速度曲线及最优速度曲线Fig.3Maximal and optimal velocity in phase plane图 4 给出关节空间的最优加速度规划结果.仿真结果表明,关节位移、速度及加速度的规划结果均满足设定的约束条件.轨迹规划计算程序由 C+语言编制,运行在

21、PentiumII/350M 的 PC 上,N=50,M=300,计算时间 3s,最优运动时间值为 800ms.图 4最优关节角加速度Fig.4Optimal joint accelerations7结论(Conclusion)本文研究奇异路径的最优轨迹规划问题,提出基于扩展关节空间解曲线弧长的路径参数化方法,采用拟弧长算法来求解奇异点邻域内的逆运动学解;在此基础上,以最小时间轨迹为例,给出采用动态规划方法求解奇异路径最优轨迹规划问题的求解方法.本文所述的路径参数化方法,以及基于此参数的轨迹优化方法不仅适用于奇异路径,也适用于常规路径轨迹规划问题.参考文献(References)1Bobrow

22、 J E,Dubosky S,Gibson J S.T ime-optimal Control ofrobotic manipulators along specified paths.Int J Robotics Res,1985,4(3):3-17 2 Shin KG,McKayND.Minimum-timecontrol roboticmanipulators with geometric path constraints.IEEETransAutomat Contr,1985,30(6):531-541 3 Slotine J J E,Yang H S.Improving the ef

23、ficiency of pathfollowing algorithms.IEEE J Robot Automat,1989,5(1):18-124 4Shiller Z,Lu H H.Computation of path constrained time optimalmotions with dynamic singularities.T rans ASME J DynamicSyst.Measurement Contr,1992,114:34-405 Shin K G,McKay N D.A dynamic programming approach totrajectoryplanni

24、ngofroboticmanipulators.IEEERobotAutomat,1986,AC-31(6):491-500 6 Kieffer Jon,Cahill Aidan J,James Matthew R.Robust andAccurateTime-OptimalPath-T rajectoryControlforRobotM anipulator.IEEE T rans Robotics and Automation,1997,13(6):880-890 7 蔡大用,白峰杉.高等数值分析.北京:清华大学出版社,1997作者简介:连广宇(1969-),男,博士研究生.研究领域:机器人规划和控制.赵清杰(1966-),女,博士研究生.研究领域:机器人控制、视觉伺服.孙增圻(1943-),男,教授,博士生导师.研究领域:智能控制、机器人等.553第 24 卷第 6期连广宇等:机器人操作臂奇异路径约束最优轨迹规划