基于GA的时间最优机械臂轨迹规划算法_居鹤华 2012.pdf

2012 年5 月第19卷第3期控制工程Control Engineering of ChinaMay 2 0 1 2Vol 19，No 3文章编号:1671-7848(2012)03-0472-06收稿日期:2011-02-28;收修定稿日期:2011-04-26基金项目:国家 863 项目(2008AA0085);国家自然科学基金(60975065)作者简介:居鹤华(1969-)，男，江苏高邮人，教授，博士，主要从事深空探测等方面的教学与科研工作。基于 GA 的时间最优机械臂轨迹规划算法居鹤华，付荣(北京工业大学 电子信息与控制工程学院，北京100124)摘要:由于多项式插值轨迹规划具有阶次高、没有凸包性质等特点，传统优化方法难以应用的特点，根据机械臂运动学约束，提出了关节空间基于遗传算法(GA)的 3-5-3 多项式插值轨迹规划算法。利用运动学约束，以最优时间为目标，针对关节型机器人在静态环境下的点到点的轨迹规划问题，利用 GA 算法解算多项式插值的时间。通过与基于 PSO 的 3-5-3 多项式机械臂轨迹规划运动位置、速度、加速度曲线对比，证明该方法在运行时间和运行平稳度上都有突出优点。关键词:机械臂;轨迹规划;时间最优;遗传算法;多项式插值中图分类号:TP 27文献标识码:ATime-Optimal Trajectory Planning Algorithm Based on GA for ManipulatorJU He-hua，FU Rong(College of Electronic Information and Control Engineering，Beijing University of Technology，Beijing 100124，China)Abstract:Since polynomial is high order and has not share the characteristic of convex hull，it is hard to be optimized by traditionaloptimization methods.According to the velocity limitation of manipulator，a Time-optimal 3-5-3 polynomial interpolation trajectory plan-ning algorithm based on Genetic Algorithm(GA)is proposed.To plan the point-to-point trajectories for manipulator working in a staticenvironment for time-optimal，GA is used to get the time of polynomial interpolation.The simulation results show that the algorithm hasgood performance on time-optimal and stability compared with 3-5-3 polynomial interpolation trajectory planning algorithm based onPSO.Key words:manipulator;trajectory planning;time-optimal;GA;polynomial interpolation1引言机械臂轨迹规划的算法中，对最优时间下机器人轨迹规划算法研究的较多。近年来，采用智能方法优化轨迹方法成为热点。其目的是在给定其路径点的情况下，使其性能和时间最优。孙亮1 在逆运动学基础上采用基本样条插值方法对机械臂轨迹规划作为研究，但未考虑到优化问题。于乃功2 针对二自由度机械手动力学模型提出了一种神经网络与逆模控制相结合的控的轨迹规划，但不适用于多自由度系统。朱世强、徐向荣等提出了采用 3-5-3 样条函数法对机器人轨迹进行规划3-4。文献 5 对文献 4进行了修正，但基于多项式插值的轨迹规划具有阶次高、没有凸包性质等特点，使其很难用传统方法优化，利用智能方法优化轨迹规划的插值时间，使多项式插值可以应用到更高要求的轨迹规划中。粒子群算法(PSO)结构简单、参数易调整，是控制优化里使用频率较多的一种。文献 6-7 采用 PSO 算法对空间机器人进行动力学约束的时间最优轨迹规划。遗传算法8 提供了一种求解复杂系统优化问题的通用框架，它不依赖于问题的具体领域，对问题的种类有很强的鲁棒性，广泛应用于机械臂轨迹规划方面。甘亚辉、张勇提出利用遗传算法方法优化多项式规划运行轨迹实现机械臂避障高性能功能9-10。本文提出基于 GA 的时间最优 3-5-3 样条插值机械臂轨迹规划。考虑运动学约束，实现运行时间和平稳性性能优化。针对 CH2 型月球车上驱动相机盒运动六自由度正交解耦机械臂进行仿真实验，与基于 PSO 的时间最优 3-5-3 样条插值机械手臂轨迹规划的运动位置、速度、加速度曲线对比，证明该方法在运行时间和运行平稳度上都有突出优点。23-5-3 样条插值函数的构造一般来说，样条曲线是在插值点具有(k 1)阶导数连续性的 k 次多项式。对于多项式样条函数，一阶导数代表速度的连续性，二阶导数代表加速度的连续性，三阶导数代表脉动。3-5-3 样条多项式的通式为hj1(t)=aj13t3+aj12t2+aj11t1+aj10(1)hj2(t)=aj25t5+aj24t4+aj23t3+aj22t2+aj21t1+aj20t(2)hj3(t)=aj33t3+aj32t2+aj31t1+aj30(3)j=1，2，3，4，n未知系数 aj1i，aj2i，aj3i为第 j 个关节轨迹第一段、第 二 段、第 三 段 插 值 函 数 的 第 i 个 系 数，hj1(t)，hj2(t)，hj3(t)分别代表第 j 段关节的第一段三次多项式轨迹、第二段五次多项式轨迹和第三段三次多项式轨迹，n 表示关节个数。Tj，Jj，Aj，Vj，Xj表示第 j 段关节插值的时间、脉动、加速度、速度和位置。Xj表示的位置是笛卡尔系下机械臂运动的空间坐标通过逆运动学解算得出的关节角度。已知条件为第 j 个关节各段的初始点 Xj0、路径点Xj1，Xj2、末端点 Xj3以及初始点和终点的加速度、速度 Aj0，Aj3，Vj0，Vj3(一般取为零)。路径点之间的速度与加速度连续。根据以上条件，推导出求解未知系数 aji与插值点的关系如下:A=t31t21t1100000100003t212t11000001000006t120000020000000000t52t42t32t22t21000100005t424t323t222t21000100000 20t3212t226t2220002000000000000t33t23t3100000000003t232t31000000000006t232000001000000000000100000000000010000000000000000000000000100000000010000(4)b=0 0 0 0 0 0 Xj30 0 Xj00 0 Xj2Xj1T(5)a=inv(A)*b(6)a=aj13aj12aj11aj10aj25aj24aj23aj22aj21aj20aj33aj32aj31aj30T(7)式(4)中的 t1，t2，t3表示第 j 段关节的 3 段多项式插值的时间。式(6)即是系数 aji的解。3时间最优问题求解式(4)式(7)对 3-5-3 多项式未知系数的解算是以每段多项式的插值时间已知为基础的。如何选择最优的时间使机械臂在最短的时间内完成运动并且满足速度约束，是本文研究的主要内容。优化目标是满足运动学约束的所有关节的运动时间最短。其函数如下:f(t)=minnj=0(tj1+tj2+tj3)(8)s.t.max|Vj1|Vmaxmax|Vj2|Vmax(9)max|Vj3|Vmaxj=1，2，3，4，n式中，Vj1，Vj2，Vj3为第 j 个关节每段多项式随时间变化的速度。3-5-3 多项式插值不具备传统优化方法优化的性质，其时间优化只能采用智能方法。遗传算法与传统的优化方法(枚举，启发式等)相比较，以生物进化为原型，具有很好的收敛性，在计算精度要求时，计算时间少，鲁棒性高等都是它的优点。采用遗传算法优化每段插值时间。流程图，如图 1 所示。对机械臂每个关节单独进行优化。第 j 个关节优化目标函数为f(t)=min(tj1+tj2+tj3)(10)3-5-3 多项式插值函数应用粒子群算法，如何选择自变量以确定因变量 xkid，并选择出最佳 xkid是关键问题。式(4)式(7)中时间 t1，t2，t3是待优化的未知量，系数 aji也是待求解的未知数。如果把未知系数 aij当做自变量，t1，t2，t3当做因变量。待优化的变量维数为 14，计算量大且复杂。本文直接在待优化时间 t1，t2，t3的搜索空间里优化，搜索维数降低为3 维，避免了推导复杂的映射关系。t1，t2，t3是未知量，其搜索空间需要根据经验设定在尽可能大的范围。为了使关节运动速度尽快收敛到约束内，采用两种适应度函数的开关控制。在满足运动学约束后，进行时间最优的优化迭代。具体步374第 3 期居鹤华等:基于 GA 的时间最优机械臂轨迹规划算法图 1基于遗传算法的最优轨迹规划流程Fig.1Flowchart of optimal manipulator trajectoryplanning based on genetic algorithm骤如下:Step1初始化种群对第 j 个关节的三段插值函数时间的搜索空间中分别随机产生 M 个 l 位二进制串基因码组成的种群。构成种群 POPt1，POPt2，POPt3。假设 xit是 ml 维的行向量，表示第 t 代的第i 个个体。其中，i 1，2，M。每个个体用 l 位二进制表示。个体 xit的第 k 个长度为 l 的二进制编码串转化为实数的解码函数 为(xit，k)=uk+vk uk2l 1(ln=1xi(kl+n)t 2n1(11)式中，vk，uk分别为第 k 个实数范围上限和下限。随机产生基因码的值，并计算相应的实数。Step2根据产生 M 组 3-5-3 多项式插值时间组合，代入式(4)式(7)。求解出 aj1的系数矩阵 a。Step3将系数行矩阵 a，代入式(1)式(3)求解出三段多项式。多项式对时间求导，得到三段多项式的速度函数。判断多项式的最大速度是否符合式(9)。Step4适应度函数的构建有 2 个任务:首先使机械臂运动速度收敛到运动学约束内，然后使各段插值时间尽量减小。对 Step3 的计算结果采用 2个适应度函数切换的开关控制，即，如果三段中的任一速度函数不符合式(9)，该段的适应度函数为|v(j，i)|。如果三段的速度全部满足式(9)的约束，则适应度函数切换为 min(tj1)，以减小每段关节运行时间为优化目标进行迭代。其中，j 为 M 组种群的第 j 组，i 为多项式插值的段数，取值范围是 1，2，3。如果有的个体不满足其运动学约束，在下一步中，该个体被淘汰的几率会增加。Step5遗传操作如下:选择:轮盘赌选择法将适应度函数按升序排列每次随机的从种群中挑选一定数目的个体，并将其中最好的选作父个体。交叉选用均匀交叉方式为 2 个父代染色体执行交叉操作，染色体中的每个基因按随机产生的掩码挑选父个体组成新一代的子染色体。在每一代种群中交叉的概率为 pc。变异对于完成交叉操作后的子代种群采用二进制码变异的方式形成新一代的种群，即按照设定的变异概率 pm选择一定数目的子个体，随机挑选其中某个基因并用其相反值代替。Step6构成新 M 3 的种群 POPt1，POPt2，POPt3。Step7满足终止条件则算法结束，否则转Step2。Step8完成所有关节的时间优化。每段时间取各关节该段时间最大值，以确保满足运动学约束。t1=max tj1;t2=max tj2，t3=max tj3j=1，2，3，4，n遗传算法需要循环迭代。本文研究的遗传算法是在优化目标的搜索空间内搜索，在计算式(4)时，若迭代步数很大，易造成矩阵不满秩。这影响了该方法在复杂目标优化的应用，所以在每一步迭代过程中，都要检验新产生种群是否有实数值为零的个体。如果有，则要重新随机产生该个体的值。4仿真与实验结果本文研究 CE2 型月球车上驱动相机盒运动的六自由度正交解耦机械臂，整体结构图，如图 2 所示。图 2机械臂整体结构Fig.2Overall structure of manipulator运动学建模用标准 D-H 坐标系法，如图3 所示。图 3D-H 坐标系Fig.3D-H Coordinate of manipulator474控制工程第 19 卷机械臂腕心位置即相机盒位置。关节 1 至 3 旋转方向两两正交，解耦于控制相机盒定向的关节 4至 6，见图 3。为了研究其位置控制，实验仿真只针对前 3 个关节。驱动位置控制的前3 个关节 D-H 参数表，见表 1。表 1D-H 参数表Table 1 D-H parameters of manipulator关节cossinad1190 01L102290 010L23390 010L3采用欧几里德范数可以求解出机械臂位置控制和方向控制的各关节角度的逆运动学解析解。其逆运动学推导已在前期工作中完成11。在笛卡尔系下给定机械臂末端的轨迹插值点，如图 4 所示。图 4笛卡尔系下的机械臂末端轨迹插值点Fig.4Interpolation points of the end ofmanipulator in Cartesian由逆运动学将各空间位置插值点转化为关节空间的角度插值点。第一关节至第三关节的初始位置、路径点和终点所对应的角度，见表 2。表 2各关节的角度插值点Table 2Interpolation points of joints spaceX关节位置 x关节 1关节 2关节 3X01.5690.7610.731X11.5320.9780.525X21.3511.5110.731X31.1312.1400.896采用基于 PSO 的 3-5-3 多项式插值算法，确定每段插值的最优时间。每个关节三段插值都选择粒子数 M 为 20，即每段关节共有 60 个粒子，表示插值时间。初始的粒子位置为 0 3(s)之间的任意随机数，每个粒子的位置按下式进行变化:vk+1id=w vkid+c1 r1(pid xkid)+c2 r2(pgd xkid)(12)xk+1id=xkid+vk+1id(13)式中，vkid为第 i 个粒子第 k 次迭代中飞行速度的第d 维分量;xkid为第 i 个粒子第 k 次迭代中位置的第d 维分量;pgd为群体最好位置 pg的第 d 维分量;pid为粒子 i 最好位置 pi的第 d 维分量;r1和 r2为随机数;c1和 c2为权重因子;w 为惯性权重。粒子飞行速度的参数设置如下:惯性权重 w=0.5+rand()/2，权重因子 c1=0.05，c2=0.05，r1和 r2为 1 之间的随机数。关节允许的最大运动速度是 Vmax=3 m/s。初始和终点的速度和加速度都是 0。循环迭代步数为50。跟踪每段插值的 20 个粒子中，某个粒子进行50 次基于 PSO 的 3-5-3 多项式插值运算的位置变化，得到节粒子位置进化图，如图 5 图 7 所示。图 5第一关节粒子位置进化Fig.5Particle swarm position change of the first joint图 6第二关节粒子位置进化Fig.6Particle swarm position change of the second joint图 7第三关节粒子位置进化Fig.7Particle swarm position change of the third joint可以看出，通过在不同的适应度函数之间切换574第 3 期居鹤华等:基于 GA 的时间最优机械臂轨迹规划算法使粒子位置快速的收敛。经过 PSO 的 50 次迭代得到关节每段插值时间为 t1=1.04;t2=1.37;t3=0.76。采用该时间优化之后的机械臂的位置变化较有起伏，但速度临近 Vmax，加速度也比较平滑，如图8 图 10 所示。图 8基于 PSO 的时间最优的机械臂关节位置曲线Fig.8joints position with time optimization based on PSO图 9基于 PSO 的时间最优的机械臂关节速度曲线Fig.9joints velocity with time optimization based on PSO图 10基于 PSO 的时间最优的机械臂关节加速度曲线Fig.10joints acceleration with time optimizationbased on PSO按照 3 节中 Step1 至 Step8 的说明，采用基于GA 的 3-5-3 多项式插值算法，确定每段插值的最优时间。每个关节三段插值都有 100 个二进制字符串长度为 8 的个体。二进制解码为实数的上下限为 0 2.55 s。遗传算法迭代时间为 50 步。交叉率和变异率为 0.8和 0.01。关节允许的最大运动速度是 Vmax=3 m/s。初始和终点速度和加速度都是 0。遗传算法迭代中，某个个体的进化过程，如图11 图 13 所示。图 11第一关节遗传算法进化Fig.11GA position change of the first joint图 12第二关节遗传算法进化Fig.12GA position change of the second joint图 13第三关节遗传算法进化Fig.13GA position change of the third joint3 个关节每段插值都收敛到一个实数。为了确保收敛迅速，在交叉和变异过程中，保留一组适应度函数最小值的个体作为下一代父类中的一个个体。可以看出由于变异的存在，在收敛过程中，结果会有突变。根据 Step8，得到关节插值时间为t1=0.69 s，t2=1.3 s，t3=0.69 s。通过与粒子群算法的优化时间对比，可以看出遗传算法比粒子群优化算法节省了 0.49 s。2 个算法虽然在优化上都使在满足运动学约束的条件下性能达到优化，但遗传算法的优化更能使机械臂接近最大速度。粒子群算法在收敛后期易抖动，在收敛速度和时间上，遗传算法优于粒子群算法。根据该插值时间，得到关节的运动位置、速度674控制工程第 19 卷和加速度的变化曲线如图 14 图 16 所示。图 14基于 GA 的时间最优的机械臂关节位置曲线Fig.14joints position with time optimizationbased on GA图 15基于 GA 的时间最优的机械臂关节速度曲线Fig.15joints velocity with time optimizationbased on GA图 16基于 GA 的时间最优的机械臂关节加速度曲线Fig.16joints acceleration with time optimizationbased on GA5结语本文以六自由度解耦机械手臂位置控制(三关节机械臂)为例，根据机械臂运动的速度约束，提出了在关节空间中基于遗传算法的时间最优的 3-5-3 多项式插值轨迹规划方法。论文核心是利用遗传算法优化多项式插值时间，通过与粒子群算法的优化时间做对比，在运行时间和运行平稳度上都有突出优点。粒子群算法虽然具有结构简单、参数易调整的特点，但是在产生新种群时，需要利用随机数产生粒子速度，这减小了粒子收敛速度。遗传算法新种群的产生虽然也基于随机产生的掩码交叉，但是本文利用交叉率的设置可以迅速淘汰劣势个体，使种群中的个体保持高适应度。遗传算法和粒子群算法的迭代过程都有随机数构成，所以每次迭代收敛结果会有微小差异。遗传算法虽然收敛性高，但是解决大规模计算量问题，它很容易陷入“早熟”。本文对象为空间三自由度机械臂，实验中迭代步数只有 50，如果对象模型复杂或者迭代步数增加，可以用混合遗传算法，合作型协同进化算法等来进行时间优化避免“早熟”问题。参考文献(References):1孙亮，马江，阮晓钢六自由度机械臂轨迹规划与仿真研究 J 控制工程，2010，17(3):388 392(Sun Liang，Ma Jiang，Ruan Xi-aogang Trajectory planning and simulation of 6 DOF manipulator J Control Engineering of China，2010，17(3):388 392)2于乃功，李明，李建更 机械手轨迹规划的神经网络逆模控制 J 控制工程，2010，17(3):388 392(Yu Naigong，Li Ming，Li Jiangeng Neural network inverse model control for robot armtrajectory planningJ Control Engineering of China，2008，15(3):225 227)3朱世强，刘松国，王宣银 机械手时间最优脉动连续轨迹规划算法J 机械工程学报，2010，46(3):47-52(Zhu Shiqiang，LiuSongguo，WangXuanyin Time-optimal and jerk-continuous trajecto-ry planning algorithm for manipulators J Journal of MechanicalEngineering，2010，46(3):47-52)4Xu X R，Wang X G，Qin F Trajectory planning of robot manipula-tors by using spline function approachA Proceedings of the 3rdWorld Congress on Intelligent Control and AutomationC，2000:15-12190 5何平，刘宏，金明河 基于样条函数的机器人轨迹规划方法 J 机器人，2003(7):614-618(HePing，Liuhong，JinMingheTrajectory planning of robot manipulator based on spline function J Robotics，2003(7):614-618)6Huang P F，Xu Y S PSO-Based time-optimal trajectory planning forspace robot with dynamic constraintsC Proceeding of the 2006IEEE International Conference on Robotics and Biominmetics2006(9):1402-1407 7Huang G，Li D，Yang J A research on partical swarm optimizationand its application in robot manipulatorsC IEEE Pacific-AsicWorkshop on Computational Intelligence and Industrial Applica-tion2008:377-381 8王小平，曹立明 遗传算法理论应用与软件实现M 西安:西安交通大学出版社，2002(Wang Xiaoping，CAO Liming GeneticAlgorithm theoretics application and software implementMXIAN:XI AN JiaoTong University press，2002)9甘亚辉，戴先中 基于遗传算法的多机器人系统的最优轨迹规划J 控制理论与应用，2010，27(9):1245-1251*GanYahui，Dai Xianzhong Optimal Trajectory-planning based on genet-ic algorithm for multi-robot aystemJ Control Theory and Appli-cations，2010，27(9):1245-1251)10 张勇，张宪民，胡峻峰，等 高速并联机械手最优时间轨迹规划及实现 J 机电工程技术，2010，39(10):42-45(Zhang Yong，Zhang Xianmin，HU Junfeng High-Speed parallel manipulator opti-malTime trajectory planning and implementJ Mechanical E-lectrical Engineering Technology，2010，39(10):42-45)11 付荣，居鹤华 高精度解耦六自由度机械臂逆运动学解法J计算机测量与控制，2010，18(7):1637-1640(FuRong，JU He-hua Real-time and high-accurate inverse kinematic control algo-rithm for decoupled six-DOF manipulatorJ computer measure-ment and Control，2010，18(7):1637-1640)774第 3 期居鹤华等:基于 GA 的时间最优机械臂轨迹规划算法