第10卷第3期 智能系统学报 Vol.10 No.3 2015年6月 CAAI Transactions on Intelligent Systems Jun.2015 D0:10.3969/j.issn.1673-4785.201404035 网络出版地址：htp:/www.cnki.net/kcms/detail/23.1538.tp.20150526.0853.001.html 速度约束下P$0的六自由度机械臂时间最优轨迹规划 李小为，胡立坤，王琥 (广西大学电气工程学院，广西南宁530004) 摘要：以六自由度机械臂的运动学正逆解为前提条件，在关节空间中根据插值点设计机器人的运动轨迹，为使机 械臂在不同的速度约束下以最短时间运行，提出了粒子群优化速度约束下的时间最优353多项式插值轨迹规划方 法。粒子群算法结构简单、实现容易，参数易调整，直接选择以多项式插值时间为变量的搜索空间中进行PS0优化， 并且对不符合速度约束条件的插值时间进行筛选。通过离化得到六自由度机械臂限速运行下的最短时间，在机器 人控制平台上进行实时实验，得到关节运动位置、速度、加速度曲线，证明了该方法能够准确地实现任意速度约束的 时间最优轨迹规划。 关键词：机器人：六自由度机械臂：粒子群优化：轨迹规划：多项式插值：速度约束；时间最优 中图分类号：TP183文献标志码：A文章编号：1673-4785(2015)03-0393-06 中文引用格式：李小为，胡立坤，王琥.速度约束下PS0的六自由度机械臂时间最优轨迹规划[J].智能系统学报，2015,10(3)： 393-398. 英文引用格式：LI Xiaowei,HU Likun,WANG Hu..PSO-based time optimal trajectory planning for six degrees of freedom robot manipulators with speed constraints [].CAAI Transactions on Intelligent Systems,2015,10(3):393-398. PSO-based time optimal trajectory planning for six degrees of freedom robot manipulators with speed constraints LI Xiaowei,HU Likun,WANG Hu (College of Electrical Engineering,Guangxi University,Nanning 530004,China) Abstract:The trajectory planning is designed for robot manipulators in the joint space according to interpolation points on the premise of the solutions to the forward and inverse kinematics problems of 6-DOF(depth of field)ma- nipulators.This paper puts forward the particle swarm optimization(PSO)-based time optimal trajectory planning of the 3-5-3 polynomial interpolation method in order to make mechanical arms run in the shortest time at a constrain- ed speed.The PSO is proposed to optimize run time due to its simple structure and easily adjustable parameters. The polynomial interpolation time rather than coefficient is selected as searching variable in PSO optimization.If the interpolation time of three polynomials doesn't meet the constraints,the particle will be excluded by comparison. The shortest time of the 6-DOF manipulators running at different speeds is obtained by offline PSO.Real-time ex- periments are conducted on the robot control platform.It shows that this method can accurately realize time optimal trajectory planning at any speed through its position,velocity and acceleration curves. Keywords:robot;6 DOF manipulators;particle swarm optimization;trajectory planning;polynomial interpola- tion;speed constraints;time optimal 目前轨迹规划主要从2个方面进行优化，一是 对时间的优化：二是对系统能量的优化：其中对最优 时间的研究最多。机器人时间最优轨迹规划是指在 收稿日期：2014-04-18.网络出版日期：2015-05-26. 基金项目：广西自然科学基金资助项目(2012 GXNSF BA053144) 满足各种约束条件下，以机器人运动时间最短为目 通信作者：李小为.E-mail:maxwelllxw@163.com
第 １０ 卷第 ３ 期 智 能 系 统 学 报 Ｖｏｌ．１０ №．３ ２０１５ 年 ６ 月 ＣＡＡＩ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ Ｊｕｎ． ２０１５ ＤＯＩ：１０．３９６９ ／ ｊ．ｉｓｓｎ．１６７３⁃４７８５．２０１４０４０３５ 网络出版地址：ｈｔｔｐ： ／ ／ ｗｗｗ．ｃｎｋｉ．ｎｅｔ ／ ｋｃｍｓ／ ｄｅｔａｉｌ ／ ２３．１５３８．ｔｐ．２０１５０５２６．０８５３．００１．ｈｔｍｌ 速度约束下 ＰＳＯ 的六自由度机械臂时间最优轨迹规划 李小为，胡立坤，王琥 （广西大学 电气工程学院，广西 南宁 ５３０００４） 摘 要：以六自由度机械臂的运动学正逆解为前提条件，在关节空间中根据插值点设计机器人的运动轨迹，为使机 械臂在不同的速度约束下以最短时间运行，提出了粒子群优化速度约束下的时间最优 ３⁃５⁃３ 多项式插值轨迹规划方 法。 粒子群算法结构简单、实现容易，参数易调整，直接选择以多项式插值时间为变量的搜索空间中进行 ＰＳＯ 优化， 并且对不符合速度约束条件的插值时间进行筛选。 通过离化得到六自由度机械臂限速运行下的最短时间，在机器 人控制平台上进行实时实验，得到关节运动位置、速度、加速度曲线，证明了该方法能够准确地实现任意速度约束的 时间最优轨迹规划。 关键词：机器人；六自由度机械臂；粒子群优化；轨迹规划；多项式插值；速度约束；时间最优 中图分类号：ＴＰ１８３ 文献标志码：Ａ 文章编号：１６７３⁃４７８５（２０１５）０３⁃０３９３⁃０６ 中文引用格式：李小为，胡立坤，王琥． 速度约束下 ＰＳＯ 的六自由度机械臂时间最优轨迹规划［ Ｊ］． 智能系统学报， ２０１５， １０（ ３）： ３９３⁃３９８． 英文引用格式：ＬＩ Ｘｉａｏｗｅｉ， ＨＵ Ｌｉｋｕｎ，ＷＡＮＧ Ｈｕ． ＰＳＯ⁃ｂａｓｅｄ ｔｉｍｅ ｏｐｔｉｍａｌ ｔｒａｊｅｃｔｏｒｙ ｐｌａｎｎｉｎｇ ｆｏｒ ｓｉｘ ｄｅｇｒｅｅｓ ｏｆ ｆｒｅｅｄｏｍ ｒｏｂｏｔ ｍａｎｉｐｕｌａｔｏｒｓ ｗｉｔｈ ｓｐｅｅｄ ｃｏｎｓｔｒａｉｎｔｓ ［Ｊ］． ＣＡＡＩ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ， ２０１５， １０（３）： ３９３⁃３９８． ＰＳＯ⁃ｂａｓｅｄ ｔｉｍｅ ｏｐｔｉｍａｌ ｔｒａｊｅｃｔｏｒｙ ｐｌａｎｎｉｎｇ ｆｏｒ ｓｉｘ ｄｅｇｒｅｅｓ ｏｆ ｆｒｅｅｄｏｍ ｒｏｂｏｔ ｍａｎｉｐｕｌａｔｏｒｓ ｗｉｔｈ ｓｐｅｅｄ ｃｏｎｓｔｒａｉｎｔｓ ＬＩ Ｘｉａｏｗｅｉ， ＨＵ Ｌｉｋｕｎ， ＷＡＮＧ Ｈｕ （Ｃｏｌｌｅｇｅ ｏｆ Ｅｌｅｃｔｒｉｃａｌ Ｅｎｇｉｎｅｅｒｉｎｇ， Ｇｕａｎｇｘｉ Ｕｎｉｖｅｒｓｉｔｙ， Ｎａｎｎｉｎｇ ５３０００４， Ｃｈｉｎａ） Ａｂｓｔｒａｃｔ：Ｔｈｅ ｔｒａｊｅｃｔｏｒｙ ｐｌａｎｎｉｎｇ ｉｓ ｄｅｓｉｇｎｅｄ ｆｏｒ ｒｏｂｏｔ ｍａｎｉｐｕｌａｔｏｒｓ ｉｎ ｔｈｅ ｊｏｉｎｔ ｓｐａｃｅ ａｃｃｏｒｄｉｎｇ ｔｏ ｉｎｔｅｒｐｏｌａｔｉｏｎ ｐｏｉｎｔｓ ｏｎ ｔｈｅ ｐｒｅｍｉｓｅ ｏｆ ｔｈｅ ｓｏｌｕｔｉｏｎｓ ｔｏ ｔｈｅ ｆｏｒｗａｒｄ ａｎｄ ｉｎｖｅｒｓｅ ｋｉｎｅｍａｔｉｃｓ ｐｒｏｂｌｅｍｓ ｏｆ ６⁃ＤＯＦ （ｄｅｐｔｈ ｏｆ ｆｉｅｌｄ） ｍａ⁃ ｎｉｐｕｌａｔｏｒｓ． Ｔｈｉｓ ｐａｐｅｒ ｐｕｔｓ ｆｏｒｗａｒｄ ｔｈｅ ｐａｒｔｉｃｌｅ ｓｗａｒｍ ｏｐｔｉｍｉｚａｔｉｏｎ （ＰＳＯ）⁃ｂａｓｅｄ ｔｉｍｅ ｏｐｔｉｍａｌ ｔｒａｊｅｃｔｏｒｙ ｐｌａｎｎｉｎｇ ｏｆ ｔｈｅ ３⁃５⁃３ ｐｏｌｙｎｏｍｉａｌ ｉｎｔｅｒｐｏｌａｔｉｏｎ ｍｅｔｈｏｄ ｉｎ ｏｒｄｅｒ ｔｏ ｍａｋｅ ｍｅｃｈａｎｉｃａｌ ａｒｍｓ ｒｕｎ ｉｎ ｔｈｅ ｓｈｏｒｔｅｓｔ ｔｉｍｅ ａｔ ａ ｃｏｎｓｔｒａｉｎ⁃ ｅｄ ｓｐｅｅｄ． Ｔｈｅ ＰＳＯ ｉｓ ｐｒｏｐｏｓｅｄ ｔｏ ｏｐｔｉｍｉｚｅ ｒｕｎ ｔｉｍｅ ｄｕｅ ｔｏ ｉｔｓ ｓｉｍｐｌｅ ｓｔｒｕｃｔｕｒｅ ａｎｄ ｅａｓｉｌｙ ａｄｊｕｓｔａｂｌｅ ｐａｒａｍｅｔｅｒｓ． Ｔｈｅ ｐｏｌｙｎｏｍｉａｌ ｉｎｔｅｒｐｏｌａｔｉｏｎ ｔｉｍｅ ｒａｔｈｅｒ ｔｈａｎ ｃｏｅｆｆｉｃｉｅｎｔ ｉｓ ｓｅｌｅｃｔｅｄ ａｓ ｓｅａｒｃｈｉｎｇ ｖａｒｉａｂｌｅ ｉｎ ＰＳＯ ｏｐｔｉｍｉｚａｔｉｏｎ． Ｉｆ ｔｈｅ ｉｎｔｅｒｐｏｌａｔｉｏｎ ｔｉｍｅ ｏｆ ｔｈｒｅｅ ｐｏｌｙｎｏｍｉａｌｓ ｄｏｅｓｎ＇ｔ ｍｅｅｔ ｔｈｅ ｃｏｎｓｔｒａｉｎｔｓ， ｔｈｅ ｐａｒｔｉｃｌｅ ｗｉｌｌ ｂｅ ｅｘｃｌｕｄｅｄ ｂｙ ｃｏｍｐａｒｉｓｏｎ． Ｔｈｅ ｓｈｏｒｔｅｓｔ ｔｉｍｅ ｏｆ ｔｈｅ ６⁃ＤＯＦ ｍａｎｉｐｕｌａｔｏｒｓ ｒｕｎｎｉｎｇ ａｔ ｄｉｆｆｅｒｅｎｔ ｓｐｅｅｄｓ ｉｓ ｏｂｔａｉｎｅｄ ｂｙ ｏｆｆｌｉｎｅ ＰＳＯ． Ｒｅａｌ⁃ｔｉｍｅ ｅｘ⁃ ｐｅｒｉｍｅｎｔｓ ａｒｅ ｃｏｎｄｕｃｔｅｄ ｏｎ ｔｈｅ ｒｏｂｏｔ ｃｏｎｔｒｏｌ ｐｌａｔｆｏｒｍ． Ｉｔ ｓｈｏｗｓ ｔｈａｔ ｔｈｉｓ ｍｅｔｈｏｄ ｃａｎ ａｃｃｕｒａｔｅｌｙ ｒｅａｌｉｚｅ ｔｉｍｅ ｏｐｔｉｍａｌ ｔｒａｊｅｃｔｏｒｙ ｐｌａｎｎｉｎｇ ａｔ ａｎｙ ｓｐｅｅｄ ｔｈｒｏｕｇｈ ｉｔｓ ｐｏｓｉｔｉｏｎ， ｖｅｌｏｃｉｔｙ ａｎｄ ａｃｃｅｌｅｒａｔｉｏｎ ｃｕｒｖｅｓ． Ｋｅｙｗｏｒｄｓ： ｒｏｂｏｔ； ６ ＤＯＦ ｍａｎｉｐｕｌａｔｏｒｓ； ｐａｒｔｉｃｌｅ ｓｗａｒｍ ｏｐｔｉｍｉｚａｔｉｏｎ； ｔｒａｊｅｃｔｏｒｙ ｐｌａｎｎｉｎｇ； ｐｏｌｙｎｏｍｉａｌ ｉｎｔｅｒｐｏｌａ⁃ ｔｉｏｎ； ｓｐｅｅｄ ｃｏｎｓｔｒａｉｎｔｓ； ｔｉｍｅ ｏｐｔｉｍａｌ 收稿日期：２０１４⁃０４⁃１８． 网络出版日期：２０１５⁃０５⁃２６． 基金项目：广西自然科学基金资助项目（２０１２ＧＸＮＳＦ ＢＡ０５３１４４）． 通信作者：李小为． Ｅ⁃ｍａｉｌ： ｍａｘｗｅｌｌｌｘｗ＠ １６３．ｃｏｍ． 目前轨迹规划主要从 ２ 个方面进行优化，一是 对时间的优化；二是对系统能量的优化；其中对最优 时间的研究最多。 机器人时间最优轨迹规划是指在 满足各种约束条件下，以机器人运动时间最短为目
·394. 智能系统学报 第10卷 标来规划机器人的运动轨迹。对轨迹规划的时 式中：h,(t)、hz(t)、h=(t)分别代表3段多项式的轨 间最优，一般从2个方面考虑，一是以速度和加速度 迹，未知系数aaay为3-5-3样条多项式的第i 为约束条件；二是以运动学或动力学为约束，采用各 个关节轨迹各段插值函数的第j个系数。用以上的 种非线性约束的最优化算法实现机器人最优轨迹规 14个边界和过渡的约束条件，可以推导求解出3-5-3 划)。文献[3]以机器人的位置、速度、加速度和二 多项式的14个未知系数a,,推导公式为式(2)~ 阶加速度的运动学约束为前提，路径点之间用高次 (4)。式(2)中A仅与第i个关节3段多项式的插值 多项式作曲线轨迹，采用柔性多面体搜索的算法求 时间a23有关，式(3)代表第i个关节角的位置 解最优时间。由于在连接点处引人某一确定的加速 矩阵6。 度值使得抑制振动变得十分困难。为了避免机械手 B C 0 振动，延长关节使用寿命，关节轨迹都采用二次多项 E 式加余弦函数的形式[)，不仅可以保证各关节运动 A=0 0 F (2) 的位移、速度、加速度连续，而且还可以保证各关节 G 0 0 运动的二阶加速度连续。但是采用二次多项式加余 0 H I 弦函数的形式增加了计算的复杂度，需要耗费大量 0= 的计算时间。利用具有光滑特性的三次样条曲线段 来连接路径点是最简单的多项式方法，在速度和加 [000000.00002] 速度的约束下能够减小两点之间的运行时间，同时 (3) 避免振荡)。基于多项式插值的轨迹规划，具有阶 a=A-0=[A,A2 A3 ]T (4) 次高、没有凸包性质等特点，很难用传统方法优 式中： 化6)。文献[7]采用遗传算法对关节空间中各关 「a 品 1 键点间的运行时间间隔进行规划。但是遗传算 ta B= 法需要进行编码、解码过程，操作复杂。粒子群 3 2t1 10 优化方法具有算法简单、参数设置少、实现简单， 6ta 00 能够有效解决真实世界的复杂优化问题，与其他 「000 0 0 -1 求解约束优化问题的方法相比有一定的优势[劉。 C= 000 0 -1 0 文献[9-10]采用粒子群优化算法对关节角轨迹 000 -2 0 0 进行时间最优化求解。 台 后 飞0 12 17 不同于以往对速度的单一限制，本文采用粒子 D= 5t 42 3品 2t2 1 0 群算法对任意关节速度下的机械臂进行运行时间的 203122 612 2 00 优化。 「0 1多项式插值函数的构造 层 ta E= 3哈 2t3 0 已知机器人在笛卡尔坐标系下起始点，2个路 0-2 0 0 6t3 0 0 径点以及终点的空间坐标，通过逆运动学求解得到 「000 各个关节在4个插值点处的关节角度，用0.表示关 17 「00000 07 G= 0010,H= 节i插值的角度，其中i=1,2,…,n,n表示关节个 o 00010 0100 数，j=1,2,3,4代表4个插值点的序号。点与点之 「000 1 间采用3-5-3多项式插值的曲线轨迹。其约束条件 是：已知第i个关节各段的初始点0o、路径点01和 1=0000 日2、末端点03，路径点之间的位置、速度与加速度 A=[aas an2 aa dao] 连续以及初始点和终点的速度、加速度均为0。 A2=[ans dns ans am an ano 第i段关节3-5-3样条多项式的通式为 A3=[a83a2a31a30] ha(t)=dast+aat2 aat aao ha(t)=anst+anat+anst+amt+ant+ago 2PSO求解速度约束下的最优时间 ha(t)=aaat +aat2 +dait +a3o 粒子群算法是一种基于群体智能的全局进化优 (1) 化算法，它源于对鸟类捕食行为的模拟，将每个个体
标来规划机器人的运动轨迹［１］ 。 对轨迹规划的时 间最优，一般从 ２ 个方面考虑，一是以速度和加速度 为约束条件；二是以运动学或动力学为约束，采用各 种非线性约束的最优化算法实现机器人最优轨迹规 划［２］ 。 文献［３］以机器人的位置、速度、加速度和二 阶加速度的运动学约束为前提，路径点之间用高次 多项式作曲线轨迹，采用柔性多面体搜索的算法求 解最优时间。 由于在连接点处引入某一确定的加速 度值使得抑制振动变得十分困难。 为了避免机械手 振动，延长关节使用寿命，关节轨迹都采用二次多项 式加余弦函数的形式［４］ ，不仅可以保证各关节运动 的位移、速度、加速度连续，而且还可以保证各关节 运动的二阶加速度连续。 但是采用二次多项式加余 弦函数的形式增加了计算的复杂度，需要耗费大量 的计算时间。 利用具有光滑特性的三次样条曲线段 来连接路径点是最简单的多项式方法，在速度和加 速度的约束下能够减小两点之间的运行时间，同时 避免振荡［５］ 。 基于多项式插值的轨迹规划，具有阶 次高、没有凸包性质等特点， 很难用传统方法优 化［６］ 。 文献［ ７］采用遗传算法对关节空间中各关 键点间的运行时间间隔进行规划。 但是遗传算 法需要进行编码、解码过程，操作复杂。 粒子群 优化方法具有算法简单、参数设置少、实现简单， 能够有效解决真实世界的复杂优化问题，与其他 求解约束优化问题的方法相比有一定的优势［ ８］ 。 文献［ ９⁃１０］ 采用粒子群优化算法对关节角轨迹 进行时间最优化求解。 不同于以往对速度的单一限制，本文采用粒子 群算法对任意关节速度下的机械臂进行运行时间的 优化。 １ 多项式插值函数的构造 已知机器人在笛卡尔坐标系下起始点，２ 个路 径点以及终点的空间坐标，通过逆运动学求解得到 各个关节在 ４ 个插值点处的关节角度，用 θｉｊ 表示关 节 ｉ 插值的角度，其中 ｉ ＝ １，２，…，ｎ ， ｎ 表示关节个 数， ｊ ＝ １，２，３，４ 代表 ４ 个插值点的序号。 点与点之 间采用 ３⁃５⁃３ 多项式插值的曲线轨迹。 其约束条件 是：已知第 ｉ 个关节各段的初始点 θｉ０ 、路径点 θｉ１ 和 θｉ２ 、末端点 θｉ３ ，路径点之间的位置、速度与加速度 连续以及初始点和终点的速度、加速度均为 ０。 第 ｉ 段关节 ３⁃５⁃３ 样条多项式的通式为 ｈｉ１（ｔ） ＝ ａｉ１３ ｔ ３ ＋ ａｉ１２ ｔ ２ ＋ ａｉ１１ ｔ １ ＋ ａｉ１０ ｈｉ２（ｔ） ＝ ａｉ２５ ｔ ５ ＋ ａｉ２４ ｔ ４ ＋ ａｉ２３ ｔ ３ ＋ ａｉ２２ ｔ ２ ＋ ａｉ２１ ｔ １ ＋ ａｉ２０ ｈｉ３（ｔ） ＝ ａｉ３３ ｔ ３ ＋ ａｉ３２ ｔ ２ ＋ ａｉ３１ ｔ １ ＋ ａｉ３０ （１） 式中： ｈｉ１（ｔ）、ｈｉ２（ｔ）、ｈｉ３（ｔ） 分别代表 ３ 段多项式的轨 迹，未知系数 ａｉ１ｊ、ａｉ２ｊ、ａｉ３ｊ 为 ３⁃５⁃３ 样条多项式的第 ｉ 个关节轨迹各段插值函数的第 ｊ 个系数。 用以上的 １４ 个边界和过渡的约束条件，可以推导求解出 ３⁃５⁃３ 多项式的 １４ 个未知系数 ａｉｊ ，推导公式为式（２） ～ （４）。 式（２）中 Ａ 仅与第 ｉ 个关节 ３ 段多项式的插值 时间 ｔ ｉ１ 、ｔ ｉ２ 、ｔ ｉ３ 有关，式（３）代表第 ｉ 个关节角的位置 矩阵［６］ 。 Ａ ＝ Ｂ Ｃ ０ ０ Ｄ Ｅ ０ ０ Ｆ Ｇ ０ ０ ０ Ｈ Ｉ é ë ê ê ê ê ê ê ù û ú ú ú ú ú ú （２） θ ＝ ０ ０ ０ ０ ０ ０ θｉ３ ０ ０ θｉ０ ０ ０ θｉ２ θ [ ｉ２ ] Ｔ （３） ａ ＝ Ａ －１ θ ＝ [Ａ１ Ａ２ Ａ３ ] Ｔ （４） 式中： Ｂ ＝ ｔ ３ ｉ１ ｔ ２ ｉ１ ｔ ｉ１ １ ３ｔ ２ ｉ１ ２ｔ ｉ１ １ ０ ６ｔ ｉ１ ２ ０ ０ é ë ê ê ê ê ù û ú ú ú ú Ｃ ＝ ０ ０ ０ ０ ０ － １ ０ ０ ０ ０ － １ ０ ０ ０ ０ － ２ ０ ０ é ë ê ê ê ù û ú ú ú Ｄ ＝ ｔ ５ ｉ２ ｔ ４ ｉ２ ｔ ３ ｉ２ ｔ ２ ｉ２ ｔ ｉ２ １ ５ｔ ４ ｉ２ ４ｔ ３ ｉ２ ３ｔ ２ ｉ２ ２ｔ ｉ２ １ ０ ２０ｔ ３ ｉ２ １２ｔ ２ ｉ２ ６ｔ ｉ２ ２ ０ ０ é ë ê ê ê ê ù û ú ú ú ú Ｅ ＝ ０ ０ ０ － １ ０ ０ － １ ０ ０ － ２ ０ ０ é ë ê ê ê ù û ú ú ú ，Ｆ ＝ ｔ ３ ｉ３ ｔ ２ ｉ３ ｔ ｉ３ １ ３ｔ ２ ｉ３ ２ｔ ｉ３ １ ０ ６ｔ ｉ３ ２ ０ ０ é ë ê ê ê ê ù û ú ú ú ú Ｇ ＝ ０ ０ ０ １ ０ ０ １ ０ ０ １ ０ ０ é ë ê ê ê ù û ú ú ú ，Ｈ ＝ ０ ０ ０ ０ ０ ０ ０ ０ ０ ０ １ ０ é ë ê ê ù û ú ú Ｉ ＝ ０ ０ ０ １ ０ ０ ０ ０ é ë ê ê ù û ú ú Ａ１ ＝ [ａｉ１３ ａｉ１２ ａｉ１１ ａｉ１０ ] Ａ２ ＝ [ａｉ２５ ａｉ２４ ａｉ２３ ａｉ２２ ａｉ２１ ａｉ２０ ] Ａ３ ＝ [ａｉ３３ ａｉ３２ ａｉ３１ ａｉ３０ ] ２ ＰＳＯ 求解速度约束下的最优时间 粒子群算法是一种基于群体智能的全局进化优 化算法，它源于对鸟类捕食行为的模拟，将每个个体 ·３９４· 智 能 系 统 学 报 第 １０ 卷
第3期 李小为，等：速度约束下PS0的六自由度机械臂时间最优轨迹规划 ·395· 看作D维搜索空间中一个没有体积和质量的微 在否满足式(8)。 粒[1-1]，且在搜索空间中以一定的速度飞行，通过 4)计算每个微粒的适应度值。对步骤3)的计 群体中粒子之间的合作和竞争来寻找最优解。假设 算结果进行筛选，如果3段中的任何一段速度不符 一个包含m个微粒的微粒群在其D维搜索空间中 合式(8)，则将该粒子的适应度值设置为极大的常 飞行，第i个粒子在D维空间的位置为x:=(x1,x2, 数，在寻找最优粒子时，通过适应度值的比较，将会 …,xD),飞行速度为y:=(1,2,…,D),每个粒 排除这个适应度值较大的粒子，不被筛选为最优粒 子都具有一个由被优化的目标函数决定的适应值， 子。而这个粒子本身也会慢慢向最佳值进行靠拢， 微粒i所经历的最好位置p:=(P1P2,…PD),也 直到满足速度的约束。如果3段的最大速度都符合 就是微粒i所经历过的具有最好适应值的位置，群 式(8)，则采用式(7)作为适应度函数，粒子群算法 体所有微粒经历的最好位置为P。。对每一次迭代， 迭代以获得最小插值时间为目标。 第i个粒子的第d维分量(1≤d≤D)在第k+1次 5)对每个微粒，将其适应度值与其经历过的最 迭代时的速度和位置按式(5)和(6)变化： 好位置P:的适应度值作比较，如果较好，则将其替 培=0×点+c1×n1×(Pa-x)+ 换为当前的最好位置p:。 c2XT2×(Pd-xa) (5) 6)比较每个微粒当前最好位置的适应度值，得 x=x+ (6) 到当前整体最优粒子，再与群体所经历的全局最好 式中：0为惯性权重，c1为自身加速常数，c2为全局 位置P。作比较，如果较好，则替换P。。 加速常数，r,和r2为2个相互独立的在[0,1]变化 7)根据式(5)~(6)变化微粒的速度和位置，重 的随机函数，培∈【-tr,】,x点∈ 新整合成新的由m个粒子构成的种群。 【-xr,x】,如果哈、x超出边界值，就用边界值 8)如满足终止条件（通常为足够好的适应值或 取代，vsxx是依据不同的目标函数和搜索空间 达到一个预设最大迭代次数(N)则算法结束，否 而不同的常数)。式(5)中第1部分为微粒先前的 则返回步骤2)。 速度，第2部分为微粒自身的思考调整，第3部分为 本文设定粒子群的个数m为20，初始的粒子位 种群间的协同合作。 置为[0.1,4.0]的任意随机数，粒子的最大飞行速度在 如果选择多项式的系数α：作为待寻优量，则根 [-2,2]之间。粒子飞行速度的参数设置为惯性权重 据式(2)~(4)可以得到时间变量a2a,这时候 0=(Wmx-i×(Wx-Win)/Nx）,Wnx=0.9, 粒子群的维数为14维。如果直接选择在时间变量 Wm=0.4,i为迭代次数，循环迭代步数Nm为50。 tt2t妇的搜索空间进行优化，可以将维数降低，大 权重因子c1=2,c2=2,r1和r2为[0,1]的随机数。 大减少了粒子群寻优的复杂性和困难性。优化目标 是使各个关节在约束的速度范围内以最短的时间运 3 机器人建模与PS0仿真 行。其适应度函数为 本文主要研究工业机器人六自由度机械臂，采 f八t)=min(ta+t2+ta） (7) max{|V:l}≤Vms 用标准D-H坐标系法进行运动学建模，如图1所 (8) 式中：V:和Vx分别是第i个关节的实时速度和最 示。6个关节的D-H参数见表1。 X 大限制速度，利用粒子群算法对复杂的约束优化问 X Xs 题求解。 650mm 105mm 粒子群算法对机器人第i个关节进行最优时间 150n1 Z 轨迹规划，具体步骤如下： X 1)选定种群的规模m(一般为20)，在插值时 间的3维搜索空间中随机产生m个粒子构成初始 .e 种群，并初始化粒子的位置和速度。 150mn -X 2)由m组时间变量ta da ta代入式(2)~(4) 中可得出3-5-3多项式的未知系数a。 of 1. 3)将3-5-3多项式的系数a,代入式(1)并对时 图1机械臂的D-H坐标系 间求导，得到关节角度的速度函数，判断实时速度是 Fig.1 The D-H coordinates of robot manipulators
看作 Ｄ 维搜索空间中一个没有体积和质量的微 粒［１１－１２］ ，且在搜索空间中以一定的速度飞行，通过 群体中粒子之间的合作和竞争来寻找最优解。 假设 一个包含 ｍ 个微粒的微粒群在其 Ｄ 维搜索空间中 飞行，第 ｉ 个粒子在 Ｄ 维空间的位置为 ｘｉ ＝ （ｘｉ１ ，ｘｉ２ ， …，ｘｉＤ） ，飞行速度为 ｖｉ ＝ （ｖｉ１ ，ｖｉ２ ，…，ｖｉＤ） ，每个粒 子都具有一个由被优化的目标函数决定的适应值， 微粒 ｉ 所经历的最好位置 ｐｉ ＝ （ｐｉ１ ，ｐｉ２ ，…，ｐｉＤ） ，也 就是微粒 ｉ 所经历过的具有最好适应值的位置，群 体所有微粒经历的最好位置为 ｐｇ 。 对每一次迭代， 第 ｉ 个粒子的第 ｄ 维分量（ １ ≤ｄ ≤Ｄ ）在第 ｋ ＋ １ 次 迭代时的速度和位置按式（５）和（６）变化： ｖ ｋ＋１ ｉｄ ＝ ｗ × ｖ ｋ ｉｄ ＋ ｃ１ × ｒ１ × （ｐｉｄ － ｘ ｋ ｉｄ ） ＋ ｃ２ × ｒ２ × （ｐｇｄ － ｘ ｋ ｉｄ ） （５） ｘ ｋ＋１ ｉｄ ＝ ｘ ｋ ｉｄ ＋ ｖ ｋ＋１ ｉｄ （６） 式中： ｗ 为惯性权重， ｃ１ 为自身加速常数， ｃ２ 为全局 加速常数， ｒ１ 和 ｒ２ 为 ２ 个相互独立的在［０，１］变化 的 随 机 函 数， ｖ ｋ ｉｄ ∈ － ｖｍａｘ，ｖｍａｘ [ ] ， ｘ ｋ ｉｄ ∈ － ｘｍａｘ，ｘｍａｘ [ ] ，如果 ｖ ｋ ｉｄ 、ｘ ｋ ｉｄ 超出边界值，就用边界值 取代， ｖｍａｘ、ｘｍａｘ 是依据不同的目标函数和搜索空间 而不同的常数［１３］ 。 式（５）中第 １ 部分为微粒先前的 速度，第 ２ 部分为微粒自身的思考调整，第 ３ 部分为 种群间的协同合作。 如果选择多项式的系数 ａｉｊ 作为待寻优量，则根 据式（２） ～ （４）可以得到时间变量 ｔ ｉ１ 、ｔ ｉ２ 、ｔ ｉ３ ，这时候 粒子群的维数为 １４ 维。 如果直接选择在时间变量 ｔ ｉ１ 、ｔ ｉ２ 、ｔ ｉ３ 的搜索空间进行优化，可以将维数降低，大 大减少了粒子群寻优的复杂性和困难性。 优化目标 是使各个关节在约束的速度范围内以最短的时间运 行。 其适应度函数为 ｆ（ｔ） ＝ ｍｉｎ（ｔ ｉ１ ＋ ｔ ｉ２ ＋ ｔ ｉ３ ） （７） ｍａｘ Ｖｉ { } ≤ Ｖｉｍａｘ （８） 式中： Ｖｉ 和 Ｖｉｍａｘ 分别是第 ｉ 个关节的实时速度和最 大限制速度，利用粒子群算法对复杂的约束优化问 题求解。 粒子群算法对机器人第 ｉ 个关节进行最优时间 轨迹规划，具体步骤如下： １）选定种群的规模 ｍ （一般为 ２０），在插值时 间的 ３ 维搜索空间中随机产生 ｍ 个粒子构成初始 种群，并初始化粒子的位置和速度。 ２）由 ｍ 组时间变量 ｔ ｉ１ 、ｔ ｉ２ 、ｔ ｉ３ 代入式（２） ～ （４） 中可得出 ３⁃５⁃３ 多项式的未知系数 ａｉｊ 。 ３）将 ３⁃５⁃３ 多项式的系数 ａｉｊ 代入式（１）并对时 间求导，得到关节角度的速度函数，判断实时速度是 在否满足式（８）。 ４）计算每个微粒的适应度值。 对步骤 ３）的计 算结果进行筛选，如果 ３ 段中的任何一段速度不符 合式（８），则将该粒子的适应度值设置为极大的常 数，在寻找最优粒子时，通过适应度值的比较，将会 排除这个适应度值较大的粒子，不被筛选为最优粒 子。 而这个粒子本身也会慢慢向最佳值进行靠拢， 直到满足速度的约束。 如果 ３ 段的最大速度都符合 式（８），则采用式（７）作为适应度函数，粒子群算法 迭代以获得最小插值时间为目标。 ５）对每个微粒，将其适应度值与其经历过的最 好位置 ｐｉ 的适应度值作比较，如果较好，则将其替 换为当前的最好位置 ｐｉ 。 ６）比较每个微粒当前最好位置的适应度值，得 到当前整体最优粒子，再与群体所经历的全局最好 位置 ｐｇ 作比较，如果较好，则替换 ｐｇ 。 ７）根据式（５） ～ （６）变化微粒的速度和位置，重 新整合成新的由 ｍ 个粒子构成的种群。 ８）如满足终止条件（通常为足够好的适应值或 达到一个预设最大迭代次数（Ｎｍａｘ）则算法结束，否 则返回步骤 ２）。 本文设定粒子群的个数 ｍ 为 ２０，初始的粒子位 置为［０．１，４．０］的任意随机数，粒子的最大飞行速度在 ［－２，２］之间。 粒子飞行速度的参数设置为惯性权重 ｗ ＝ （Ｗｍａｘ － ｉ × （Ｗｍａｘ － Ｗｍｉｎ ） ／ Ｎｍａｘ） ， Ｗｍａｘ ＝ ０．９， Ｗｍｉｎ ＝ ０．４， ｉ 为迭代次数，循环迭代步数 Ｎｍａｘ为 ５０。 权重因子 ｃ１ ＝ ２，ｃ２ ＝ ２， ｒ１ 和 ｒ２ 为［０，１］的随机数。 ３ 机器人建模与 ＰＳＯ 仿真 本文主要研究工业机器人六自由度机械臂，采 用标准 Ｄ⁃Ｈ 坐标系法进行运动学建模［１４］ ，如图 １ 所 示。 ６ 个关节的 Ｄ⁃Ｈ 参数见表 １。 图 １ 机械臂的 Ｄ⁃Ｈ 坐标系 Ｆｉｇ． １ Ｔｈｅ Ｄ⁃Ｈ ｃｏｏｒｄｉｎａｔｅｓ ｏｆ ｒｏｂｏｔ ｍａｎｉｐｕｌａｔｏｒｓ 第 ３ 期 李小为，等：速度约束下 ＰＳＯ 的六自由度机械臂时间最优轨迹规划 ·３９５·
·396· 智能系统学报 第10卷 表1六自由度机械臂D-H参数表 0.8 n0.7升 Table 1 The D-H parameters of six degrees of robot manipulators 0.6 0.7 关节 关节变量 04 d/mm d/mm a/()0/() 8:9 i 范围/（°） 8 0.4 思0.3 05 1 0 150 90 0 [-150,150] 10 20.3040 50 010 20.304050 进化代数 进化代数 0 570 0 90 [-80,65] (a)-115<v<115 (b)-57<v<57 4.0 3 0 150 90 0 [-80,80] 3.5 3.0 650 0 -90 0 [-175,175] 4444444444444444444 2.5 0 0 90 0 [-110,110] ®2.0 19 0 105 0 [-200,200] 10 进化数 4050 01020304050 进化代数 (c-20<v<20 (d)-10<P<10 通过解析法用4×4的齐次变换矩阵可以求解 +t1,●t2,*l13 出机械臂位置控制和方向控制的各关节角度的逆运 图2关节1的最优粒子P,位置进化 动学解析解。在直角坐标系下给定机械臂末端的轨 Fig.2 The optimal particle p of joints 1 evolution 迹插值点，如表2。由逆运动学将各空间笛卡尔位 表4关节1在不同速度约束下的最优时间 置插值点转化为关节空间的角度插值点。 Table 4 The optimal time of joints 1 under the restriction 表2笛卡尔空间的路径表 different speeds Table 2 The path in the Dartesian space 速度范围(/s) t11/s tn/s t13/s 0.2229 0.2949 0.1384 起始点 路径点1 路径点2 终点 (-115,115) (-57,57) 0.4449 0.5924 0.2856 (800,0.615)(950.100.560)(750.300.560)(550.200.700) (-20.20) 1.2747 1.6361 0.8485 (-10,10) 2.5671 3.2458 1.7209 通过逆运动学解，得到1~6关节的初始位置 路径点和终点所对应的角度如表3所示。 对于其他关节也同样采用上述方法进行寻优，从 表3关节空间的角度插值点 而得到各个关节在不同速度约束下的运行最优时间。 Table 3 The interpolation points in the joint space 由于研究目标是大型的工业机器人，关节速度不宜采 关节i p 61 02 9p 取过大，否则会产生强烈的振动，不利于机器人长期、 高效、稳定的运行。所以取关节速度在[-20,20]（°/s) 关节1 -6.009 -21.803 -19.983 为例，按照与关节1同样的优化方法，得到各个关节的 关节2 a 15.053 -0.851 -18.161 最短插值时间，如表5。从表中可以看出，第4关节的 关节3 0 11.948 -10.833 -7.900 位置与机器人的姿态完全相关，因为机器人姿态在运 关节4 0 0 0 0 动过程中始终保持不变，所以关节4的位置也没有发 关节5 90 86.871 79.975 100.240 生变化，插值时间和速度都为0。关节1和关节6的运 关节6 0 -6.009 -21.803 -19.983 动状态完全重合。 表5各关节在速度[-20,20]下的最优时间 按照第2节中的方法，在不同速度约束下，关节 Table 5 The optimal time of each joint under speed [20,20] 1采用PSO求解最优时间，跟踪群体最好位置p,在 关节i ta/s to/s ta/s 每次迭代过程中的位置变化，得到关节1的最优粒 关节1 1.2747 1.6361 0.8485 子p。位置进化图，如图2。可以看出，在不同速度 关节2 3.1162 2.1888 3.2800 约束下，关节1最优粒子p最多经过30次迭代就快 关节3 3.1070 3.9977 1.6050 速收敛。其各个收敛值即为在特定的速度约束之 关节4 0 0 0 下，关节1运行三段插值多项式所需要的最短时间 关节5 0.8131 1.1702 3.0620 为t12和t13,如表4所示。 关节6 1.2747 1.6361 0.8485
表 １ 六自由度机械臂 Ｄ⁃Ｈ 参数表 Ｔａｂｌｅ １ Ｔｈｅ Ｄ⁃Ｈ ｐａｒａｍｅｔｅｒｓ ｏｆ ｓｉｘ ｄｅｇｒｅｅｓ ｏｆ ｒｏｂｏｔ ｍａｎｉｐｕｌａｔｏｒｓ 关节 ｉ ｄｉ ／ ｍｍ ａｉ ／ ｍｍ ａｉ ／ （°） θｉ ／ （°） 关节变量 范围／ （°） １ ０ １５０ ９０ ０ ［－１５０，１５０］ ２ ０ ５７０ ０ ９０ ［－８０，６５］ ３ ０ １５０ ９０ ０ ［－８０，８０］ ４ ６５０ ０ －９０ ０ ［－１７５，１７５］ ５ ０ ０ ９０ ０ ［－１１０，１１０］ ６ ０ １０５ ０ ０ ［－２００，２００］ 通过解析法用 ４×４ 的齐次变换矩阵可以求解 出机械臂位置控制和方向控制的各关节角度的逆运 动学解析解。 在直角坐标系下给定机械臂末端的轨 迹插值点，如表 ２。 由逆运动学将各空间笛卡尔位 置插值点转化为关节空间的角度插值点。 表 ２ 笛卡尔空间的路径表 Ｔａｂｌｅ ２ Ｔｈｅ ｐａｔｈ ｉｎ ｔｈｅ Ｄａｒｔｅｓｉａｎ ｓｐａｃｅ 起始点 路径点 １ 路径点 ２ 终点 （８００，０，６１５） （９５０，１００，５６０） （７５０，３００，５６０） （５５０，２００，７００） 通过逆运动学解，得到 １ ～ ６ 关节的初始位置， 路径点和终点所对应的角度如表 ３ 所示。 表 ３ 关节空间的角度插值点 Ｔａｂｌｅ ３ Ｔｈｅ ｉｎｔｅｒｐｏｌａｔｉｏｎ ｐｏｉｎｔｓ ｉｎ ｔｈｅ ｊｏｉｎｔ ｓｐａｃｅ 关节 ｉ θｊ０ θｊ１ θｊ２ θｊ３ 关节 １ ０ －６．００９ －２１．８０３ －１９．９８３ 关节 ２ ０ １５．０５３ －０．８５１ －１８．１６１ 关节 ３ ０ １１．９４８ －１０．８３３ －７．９００ 关节 ４ ０ ０ ０ ０ 关节 ５ ９０ ８６．８７１ ７９．９７５ １００．２４０ 关节 ６ ０ －６．００９ －２１．８０３ －１９．９８３ 按照第 ２ 节中的方法，在不同速度约束下，关节 １ 采用 ＰＳＯ 求解最优时间，跟踪群体最好位置 ｐｇ 在 每次迭代过程中的位置变化，得到关节 １ 的最优粒 子 ｐｇ 位置进化图，如图 ２。 可以看出，在不同速度 约束下，关节 １ 最优粒子 ｐｇ最多经过 ３０ 次迭代就快 速收敛。 其各个收敛值即为在特定的速度约束之 下，关节 １ 运行三段插值多项式所需要的最短时间 为 ｔ １１ 、ｔ １２和 ｔ １３ ，如表 ４ 所示。 ＋ ｔ １１ ， •ｔ １２ ， ∗ｔ １３ 图 ２ 关节 １ 的最优粒子 ｐｇ位置进化 Ｆｉｇ． ２ Ｔｈｅ ｏｐｔｉｍａｌ ｐａｒｔｉｃｌｅ ｐｇ ｏｆ ｊｏｉｎｔｓ １ ｅｖｏｌｕｔｉｏｎ 表 ４ 关节 １ 在不同速度约束下的最优时间 Ｔａｂｌｅ ４ Ｔｈｅ ｏｐｔｉｍａｌ ｔｉｍｅ ｏｆ ｊｏｉｎｔｓ １ ｕｎｄｅｒ ｔｈｅ ｒｅｓｔｒｉｃｔｉｏｎ ｄｉｆｆｅｒｅｎｔ ｓｐｅｅｄｓ 速度范围（°／ ｓ） ｔ １１ ／ ｓ ｔ １２ ／ ｓ ｔ １３ ／ ｓ （－１１５，１１５） ０．２２２ ９ ０．２９４ ９ ０．１３８ ４ （－５７，５７） ０．４４４ ９ ０．５９２ ４ ０．２８５ ６ （－２０，２０） １．２７４ ７ １．６３６ １ ０．８４８ ５ （－１０，１０） ２．５６７ １ ３．２４５ ８ １．７２０ ９ 对于其他关节也同样采用上述方法进行寻优，从 而得到各个关节在不同速度约束下的运行最优时间。 由于研究目标是大型的工业机器人，关节速度不宜采 取过大，否则会产生强烈的振动，不利于机器人长期、 高效、稳定的运行。 所以取关节速度在［－２０，２０］（°／ ｓ） 为例，按照与关节 １ 同样的优化方法，得到各个关节的 最短插值时间，如表 ５。 从表中可以看出，第 ４ 关节的 位置与机器人的姿态完全相关，因为机器人姿态在运 动过程中始终保持不变，所以关节 ４ 的位置也没有发 生变化，插值时间和速度都为 ０。 关节 １ 和关节 ６ 的运 动状态完全重合。 表 ５ 各关节在速度［－２０，２０］下的最优时间 Ｔａｂｌｅ ５ Ｔｈｅ ｏｐｔｉｍａｌ ｔｉｍｅ ｏｆ ｅａｃｈ ｊｏｉｎｔ ｕｎｄｅｒ ｓｐｅｅｄ ［－２０， ２０］ 关节 ｉ ｔ ｉ１ ／ ｓ ｔ ｉ２ ／ ｓ ｔ ｉ３ ／ ｓ 关节 １ １．２７４ ７ １．６３６ １ ０．８４８ ５ 关节 ２ ３．１１６ ２ ２．１８８ ８ ３．２８０ ０ 关节 ３ ３．１０７ ０ ３．９９７ ７ １．６０５ ０ 关节 ４ ０ ０ ０ 关节 ５ ０．８１３ １ １．１７０ ２ ３．０６２ ０ 关节 ６ １．２７４ ７ １．６３６ １ ０．８４８ ５ ·３９６· 智 能 系 统 学 报 第 １０ 卷
第3期 李小为，等：速度约束下PS0的六自由度机械臂时间最优轨迹规划 ·397. 实验结果与分析 图6是粒子群优化的机械臂关节脉动曲线，由 于3-5-3多项式3次导数只有在某些特殊的情况下 由于机器人各个关节是在同一时间内运动，所 才能保持连续。机械手的轨迹精度和关节脉动有密 以每段插值时间取各关节该段插值时间的最大值， 切关系，关节脉动越小则机械手的轨迹精度越 t=maxta,=maxt,t max (i=1,2, 高。此时，机械手的轨迹精度较低，误差较大，同 …,6)。则t1=3.1162,t2=3.9977,13=3.2800。 时使用寿命也会减少。如果要使脉动连续，则要以 通过离线优化得到最短的插值时间，在机器人控 提高阶次或复杂度为代价。在采集关节角度数据的 制平台上编程实现3-5-3多项式时间最优轨迹规划，并 同时采集机器人的实时空间坐标，图7是在基于粒 进行实验。图3~5分别是各关节的位置、速度、加速度 子群优化的机械臂笛卡尔坐标下三维立体实验轨迹 的实验曲线。从图中可以看出关节位置，速度，加速度 曲线。可以看出曲线经过预设的点。实验验证了速 均满足14个运动学约束条件。图4中，各个关节速度 度约束下的PS0六自由度机械臂多项式插值轨迹 的最大值和最小值都在第2段插值时出现，第2关节 规化的有效性。 和第3关节的速度在第2段时趋近于-V,第5关节 40 的速度在第2段时趋近于V。 20 100 -0 80 --0 03 60 ..·…03 20 40 --0 过 -…-0 20 -0 40 --0 0 -…0 60 -20, 6 8 10 6 10 t/s t/s 图3粒子群优化的机械臂关节位置曲线 图6粒子群优化的机械臂关节脉动曲线 Fig.3 Mechanical arms'joint position curves of PSO Fig.6 Mechanical arms'joint pulsation curves of PSO 20- 700H 15 ! …0 600 -- -…-0 500 0 N 400 -5 600 10 300 15 200 1050 100 200 300 400 -2 0 0 6 10 t/s 图7基于粒子群优化的机械臂轨迹曲线 图4粒子群优化的机械臂关节速度曲线 Fig.7 Mechanical arms'joint trajectory curve of PSO Fig.4 Mechanical arms'joint speed curves of PSO 5结束语 30 采用速度约束下的PS0算法在计算机上离线 20 --6 寻优出机器人3段插值最优时间，在机器人控制平 6。) 10 --0 台上进行编程实验，实验结果证明了速度约束下的 -…0 PS0六自由度机械臂时间最优轨迹规划的可靠性。 岗 传统3-5-3多项式插值，没有确定各段插值时 -10 间的准则，插值时间是随便选取的已知数。根据14 个已知条件，求出各个关节在3段插值时间内的多 -20 项式，对于3-5-3多项式求导得到速度函数2-4-2多 10 项式，要求各个关节的速度满足一定的约束条件，即 t/s 要使这些多项式的值在约束范围内，要求高次多项 图5粒子群优化的机械臂关节加速度曲线 式满足限定条件，这本身就是非常复杂的数学问题， Fig.5 Mechanical arms'joint acceleration curves of PSO
４ 实验结果与分析 由于机器人各个关节是在同一时间内运动，所 以每段插值时间取各关节该段插值时间的最大值， ｔ １ ＝ ｍａｘ｛ｔ ｉ１ ｝，ｔ ２ ＝ ｍａｘ｛ｔ ｉ２ ｝，ｔ ３ ＝ ｍａｘ｛ｔ ｉ３ ｝ （ｉ ＝ １，２， …，６） 。 则 ｔ １ ＝ ３．１１６ ２，ｔ ２ ＝ ３．９９７ ７，ｔ ３ ＝ ３．２８０ ０。 通过离线优化得到最短的插值时间，在机器人控 制平台上编程实现 ３⁃５⁃３ 多项式时间最优轨迹规划，并 进行实验。 图 ３～５ 分别是各关节的位置、速度、加速度 的实验曲线。 从图中可以看出关节位置，速度，加速度 均满足 １４ 个运动学约束条件。 图 ４ 中，各个关节速度 的最大值和最小值都在第 ２ 段插值时出现，第 ２ 关节 和第３ 关节的速度在第２ 段时趋近于 － Ｖｍａｘ ，第５ 关节 的速度在第 ２ 段时趋近于 Ｖｍａｘ 。 图 ３ 粒子群优化的机械臂关节位置曲线 Ｆｉｇ． ３ Ｍｅｃｈａｎｉｃａｌ ａｒｍｓ’ ｊｏｉｎｔ ｐｏｓｉｔｉｏｎ ｃｕｒｖｅｓ ｏｆ ＰＳＯ 图 ４ 粒子群优化的机械臂关节速度曲线 Ｆｉｇ． ４ Ｍｅｃｈａｎｉｃａｌ ａｒｍｓ’ ｊｏｉｎｔ ｓｐｅｅｄ ｃｕｒｖｅｓ ｏｆ ＰＳＯ 图 ５ 粒子群优化的机械臂关节加速度曲线 Ｆｉｇ． ５ Ｍｅｃｈａｎｉｃａｌ ａｒｍｓ’ ｊｏｉｎｔ ａｃｃｅｌｅｒａｔｉｏｎ ｃｕｒｖｅｓ ｏｆ ＰＳＯ 图 ６ 是粒子群优化的机械臂关节脉动曲线，由 于 ３⁃５⁃３ 多项式 ３ 次导数只有在某些特殊的情况下 才能保持连续。 机械手的轨迹精度和关节脉动有密 切关系， 关 节 脉 动 越 小 则 机 械 手 的 轨 迹 精 度 越 高［１５］ 。 此时，机械手的轨迹精度较低，误差较大，同 时使用寿命也会减少。 如果要使脉动连续，则要以 提高阶次或复杂度为代价。 在采集关节角度数据的 同时采集机器人的实时空间坐标，图 ７ 是在基于粒 子群优化的机械臂笛卡尔坐标下三维立体实验轨迹 曲线。 可以看出曲线经过预设的点。 实验验证了速 度约束下的 ＰＳＯ 六自由度机械臂多项式插值轨迹 规化的有效性。 图 ６ 粒子群优化的机械臂关节脉动曲线 Ｆｉｇ． ６ Ｍｅｃｈａｎｉｃａｌ ａｒｍｓ’ ｊｏｉｎｔ ｐｕｌｓａｔｉｏｎ ｃｕｒｖｅｓ ｏｆ ＰＳＯ 图 ７ 基于粒子群优化的机械臂轨迹曲线 Ｆｉｇ． ７ Ｍｅｃｈａｎｉｃａｌ ａｒｍｓ’ ｊｏｉｎｔ ｔｒａｊｅｃｔｏｒｙ ｃｕｒｖｅ ｏｆ ＰＳＯ ５ 结束语 采用速度约束下的 ＰＳＯ 算法在计算机上离线 寻优出机器人 ３ 段插值最优时间，在机器人控制平 台上进行编程实验，实验结果证明了速度约束下的 ＰＳＯ 六自由度机械臂时间最优轨迹规划的可靠性。 传统 ３⁃５⁃３ 多项式插值，没有确定各段插值时 间的准则，插值时间是随便选取的已知数。 根据 １４ 个已知条件，求出各个关节在 ３ 段插值时间内的多 项式，对于 ３⁃５⁃３ 多项式求导得到速度函数 ２⁃４⁃２ 多 项式，要求各个关节的速度满足一定的约束条件，即 要使这些多项式的值在约束范围内，要求高次多项 式满足限定条件，这本身就是非常复杂的数学问题， 第 ３ 期 李小为，等：速度约束下 ＰＳＯ 的六自由度机械臂时间最优轨迹规划 ·３９７·
·398· 智能系统学报 第10卷 加之多项式的系数本身就与插值时间有关，在满足 2013,12:1236-1239. 速度约束的条件下同时还要进行时间最优，就更是 [9]GUO Tongying,LI Feng,HUANG Kuan,et al.Application 一个复杂的难题，难以用传统的方法解决。而粒子 of optimal algorithm on trajectory planning of mechanical 群算法能有效地解决这种复杂的约束优化问题。 arm based on B-Spline curve[J].Applied Mechanics and Materials,2013,376:253-256. 在计算式(2)时，由于粒子群中的每个粒子都 [10]SASKA M,MACAS M,PREUCIL L,et al.Robot path 是随机产生的，可能造成矩阵不满秩，不利于矩阵求 planning using particle swarm optimization of Ferguson 逆。但是对寻优过程并不会有太大影响，如果矩阵 splines[C]//IEEE Conference on Emerging Technologies 不满秩，求解式(4)时使得a的各个系数为无穷大， and Factory Automation.Prague,Czech Republic,2006: 在后续粒子群寻优过程中，这个粒子会不满足速度 833-839. 的约束条件从而被筛选出来。 [11]谢晓锋，张文俊，杨之廉.微粒群算法综述[J].控制与 本文在研究过程中并没有考虑脉动不连续对机 决策，2003,18(2)：129-134. 器人造成的不利影响，下一步的工作就是在分析脉动 XIE Xiaofeng,ZHANG Wenjun,YANG Zhilian.Overview 对系统的作用基础之上考虑脉动不连续的解决方案。 of particle swarm optimization[J].Control and Decision, 2003,18(2):129-134. 参考文献： [12]SUPAKAR N,SENTIHIL A.PSO obstacle avoidance algo- rithm for robot in unknown environment[C]//International [1]李达.工业机器人轨迹规划控制系统的研究[D].哈尔 Conference on Communication and Computer Vision IC- 滨：哈尔滨工业大学，2011：2-3. CCV).Coimbatore,India,2013:1-7. LI Da.Study on industrial robot control system of trajectory [13]HU Menqi,WU T,WEIR J D.An adaptive particle swarm planning[D].Harbin,China:Harbin Institute of Technolo- optimization with multiple adaptive methods [J].IEEE 段，2011：2-3. Transactions on Evolutionary computation,2013,17(5): [2]张红强.工业机器人时间最优轨迹规划[D].长沙：湖南 705-720. 大学，2004：9-10. [14]尼库拉库克.机器人学导论一分析、控制及应用[M]. ZHANG Hongqiang.Time-optimal trajectory planning of in- 孙富春，朱纪洪，刘国栋等，译.第4版.北京：电子工 dustrial robot[D.Changsha,China:Hunan University, 业出版社，2004：60-72. 2004:9-10. [15]BORYGA M,GRABOS A.Planning of manipulator motion [3]LIN C S,CHANG P R,LUH J Y S.Formulation and opti- trajectory with higher-degree polynomials use[]].Mecha- mization of cubic polynomial joint trajectories for industrial nism and Machine Theory,2009,44(7):1400-1419. robots[J].IEEE Transactions on Automatic Control,1983, 作者简介： 28(12):1066-1074. 李小为，女，1989年生，硕士研究 [4]谭冠政，王越超.工业机器人时间最优轨迹规划及轨迹 生，主要研究方向为机器人、先进控制 控制的理论与实验研究[J].控制理论与应用，2003,20 理论与技术。 (2):185-192. TAN Guanzheng,WANG Yuechao.Theoretical and experi- mental research on time-optimal trajectory planning and con- trol of industrial robots[].Control Theory Applications, 2003,20(2):185-192. [5]BAZAZ S A,TONDU B.Minimum time on-line joint trajec- 胡立坤，男，1977年生，教授，主要 tory generator based on low order spline method for industri- 研究方向是非线性系统动力学与控制、 al manipulators [J].Robotics and Autonomous Systems, 光/风/蓄分布式电源转换与控制、工业 1999,29(4):257-268. 测控网络应用。作为主要参与人参与 [6]付荣，居鹤华.基于粒子群优化的时间最优机械臂轨迹 国家自然科学基金项目2项，承担广西 规划算法[J].信息与控制，2011,40(6)：802-808 科技开发计划项目2项、广西自然科学 FU Rong,JU Hehua.Time-optimal trajectory planning algo- 基金1项和企业横向项目5项。获得 rithm for manipulator based on PSO[J].Information and 软件著作权1项，实用新型专利1项，申请发明专利8项。 Control.2011,40(6):802-808. 发表学术论文50余篇，其中被SCI检索2篇，E检索14篇。 [7]李东洁，邱江艳，尤波.一种机器人轨迹规划的优化算 法[J].电机与控制学报，2009,13(1)：123-127. 王琥.男，1990年生，硕士研究生 LI Dongjie,QIU Jiangyan,YOU Bo.Optimal algorithm for 主要研究方向为机器人视觉、先进控制 trajectory planning of the robot[].Electric Machines and 理论与技术。 Control,2009,13(1):123-127. [8]LI Yanliang,SHAO Wei,YOU Long,et al.An improved PSO algorithm and its application to UWB antenna design [J].IEEE Antennas and Wireless Propagation Letters
加之多项式的系数本身就与插值时间有关，在满足 速度约束的条件下同时还要进行时间最优，就更是 一个复杂的难题，难以用传统的方法解决。 而粒子 群算法能有效地解决这种复杂的约束优化问题。 在计算式（２）时，由于粒子群中的每个粒子都 是随机产生的，可能造成矩阵不满秩，不利于矩阵求 逆。 但是对寻优过程并不会有太大影响，如果矩阵 不满秩，求解式（４）时使得 ａ 的各个系数为无穷大， 在后续粒子群寻优过程中，这个粒子会不满足速度 的约束条件从而被筛选出来。 本文在研究过程中并没有考虑脉动不连续对机 器人造成的不利影响，下一步的工作就是在分析脉动 对系统的作用基础之上考虑脉动不连续的解决方案。 参考文献： ［１］李达． 工业机器人轨迹规划控制系统的研究［Ｄ］． 哈尔 滨： 哈尔滨工业大学， ２０１１： ２⁃３． ＬＩ Ｄａ． Ｓｔｕｄｙ ｏｎ ｉｎｄｕｓｔｒｉａｌ ｒｏｂｏｔ ｃｏｎｔｒｏｌ ｓｙｓｔｅｍ ｏｆ ｔｒａｊｅｃｔｏｒｙ ｐｌａｎｎｉｎｇ［Ｄ］． Ｈａｒｂｉｎ， Ｃｈｉｎａ： Ｈａｒｂｉｎ Ｉｎｓｔｉｔｕｔｅ ｏｆ Ｔｅｃｈｎｏｌｏ⁃ ｇｙ， ２０１１： ２⁃３． ［２］张红强． 工业机器人时间最优轨迹规划［Ｄ］． 长沙： 湖南 大学， ２００４： ９⁃１０． ＺＨＡＮＧ Ｈｏｎｇｑｉａｎｇ． Ｔｉｍｅ⁃ｏｐｔｉｍａｌ ｔｒａｊｅｃｔｏｒｙ ｐｌａｎｎｉｎｇ ｏｆ ｉｎ⁃ ｄｕｓｔｒｉａｌ ｒｏｂｏｔ ［ Ｄ］． Ｃｈａｎｇｓｈａ， Ｃｈｉｎａ： Ｈｕｎａｎ Ｕｎｉｖｅｒｓｉｔｙ， ２００４： ９⁃１０． ［３］ＬＩＮ Ｃ Ｓ， ＣＨＡＮＧ Ｐ Ｒ， ＬＵＨ Ｊ Ｙ Ｓ． Ｆｏｒｍｕｌａｔｉｏｎ ａｎｄ ｏｐｔｉ⁃ ｍｉｚａｔｉｏｎ ｏｆ ｃｕｂｉｃ ｐｏｌｙｎｏｍｉａｌ ｊｏｉｎｔ ｔｒａｊｅｃｔｏｒｉｅｓ ｆｏｒ ｉｎｄｕｓｔｒｉａｌ ｒｏｂｏｔｓ［Ｊ］． ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ａｕｔｏｍａｔｉｃ Ｃｏｎｔｒｏｌ， １９８３， ２８（１２）： １０６６⁃１０７４． ［４］谭冠政， 王越超． 工业机器人时间最优轨迹规划及轨迹 控制的理论与实验研究［Ｊ］． 控制理论与应用， ２００３， ２０ （２）： １８５⁃１９２． ＴＡＮ Ｇｕａｎｚｈｅｎｇ， ＷＡＮＧ Ｙｕｅｃｈａｏ． Ｔｈｅｏｒｅｔｉｃａｌ ａｎｄ ｅｘｐｅｒｉ⁃ ｍｅｎｔａｌ ｒｅｓｅａｒｃｈ ｏｎ ｔｉｍｅ⁃ｏｐｔｉｍａｌ ｔｒａｊｅｃｔｏｒｙ ｐｌａｎｎｉｎｇ ａｎｄ ｃｏｎ⁃ ｔｒｏｌ ｏｆ ｉｎｄｕｓｔｒｉａｌ ｒｏｂｏｔｓ［Ｊ］． Ｃｏｎｔｒｏｌ Ｔｈｅｏｒｙ ＆ Ａｐｐｌｉｃａｔｉｏｎｓ， ２００３， ２０（２）： １８５⁃１９２． ［５］ＢＡＺＡＺ Ｓ Ａ， ＴＯＮＤＵ Ｂ． Ｍｉｎｉｍｕｍ ｔｉｍｅ ｏｎ⁃ｌｉｎｅ ｊｏｉｎｔ ｔｒａｊｅｃ⁃ ｔｏｒｙ ｇｅｎｅｒａｔｏｒ ｂａｓｅｄ ｏｎ ｌｏｗ ｏｒｄｅｒ ｓｐｌｉｎｅ ｍｅｔｈｏｄ ｆｏｒ ｉｎｄｕｓｔｒｉ⁃ ａｌ ｍａｎｉｐｕｌａｔｏｒｓ ［ Ｊ ］． Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｎｏｍｏｕｓ Ｓｙｓｔｅｍｓ， １９９９， ２９（４）： ２５７⁃２６８． ［６］付荣， 居鹤华． 基于粒子群优化的时间最优机械臂轨迹 规划算法［Ｊ］． 信息与控制， ２０１１， ４０（６）： ８０２⁃８０８． ＦＵ Ｒｏｎｇ， ＪＵ Ｈｅｈｕａ． Ｔｉｍｅ⁃ｏｐｔｉｍａｌ ｔｒａｊｅｃｔｏｒｙ ｐｌａｎｎｉｎｇ ａｌｇｏ⁃ ｒｉｔｈｍ ｆｏｒ ｍａｎｉｐｕｌａｔｏｒ ｂａｓｅｄ ｏｎ ＰＳＯ［ Ｊ］． Ｉｎｆｏｒｍａｔｉｏｎ ａｎｄ Ｃｏｎｔｒｏｌ， ２０１１， ４０（６）： ８０２⁃８０８． ［７］李东洁， 邱江艳， 尤波． 一种机器人轨迹规划的优化算 法［Ｊ］． 电机与控制学报， ２００９， １３（１）： １２３⁃１２７． ＬＩ Ｄｏｎｇｊｉｅ， ＱＩＵ Ｊｉａｎｇｙａｎ， ＹＯＵ Ｂｏ． Ｏｐｔｉｍａｌ ａｌｇｏｒｉｔｈｍ ｆｏｒ ｔｒａｊｅｃｔｏｒｙ ｐｌａｎｎｉｎｇ ｏｆ ｔｈｅ ｒｏｂｏｔ［ Ｊ］． Ｅｌｅｃｔｒｉｃ Ｍａｃｈｉｎｅｓ ａｎｄ Ｃｏｎｔｒｏｌ， ２００９， １３（１）： １２３⁃１２７． ［８］ ＬＩ Ｙａｎｌｉａｎｇ， ＳＨＡＯ Ｗｅｉ， ＹＯＵ Ｌｏｎｇ， ｅｔ ａｌ． Ａｎ ｉｍｐｒｏｖｅｄ ＰＳＯ ａｌｇｏｒｉｔｈｍ ａｎｄ ｉｔｓ ａｐｐｌｉｃａｔｉｏｎ ｔｏ ＵＷＢ ａｎｔｅｎｎａ ｄｅｓｉｇｎ ［ Ｊ ］． ＩＥＥＥ Ａｎｔｅｎｎａｓ ａｎｄ Ｗｉｒｅｌｅｓｓ Ｐｒｏｐａｇａｔｉｏｎ Ｌｅｔｔｅｒｓ， ２０１３， １２： １２３６⁃１２３９． ［９］ＧＵＯ Ｔｏｎｇｙｉｎｇ， ＬＩ Ｆｅｎｇ， ＨＵＡＮＧ Ｋｕａｎ， ｅｔ ａｌ． Ａｐｐｌｉｃａｔｉｏｎ ｏｆ ｏｐｔｉｍａｌ ａｌｇｏｒｉｔｈｍ ｏｎ ｔｒａｊｅｃｔｏｒｙ ｐｌａｎｎｉｎｇ ｏｆ ｍｅｃｈａｎｉｃａｌ ａｒｍ ｂａｓｅｄ ｏｎ Ｂ⁃Ｓｐｌｉｎｅ ｃｕｒｖｅ ［ Ｊ］． Ａｐｐｌｉｅｄ Ｍｅｃｈａｎｉｃｓ ａｎｄ Ｍａｔｅｒｉａｌｓ， ２０１３， ３７６： ２５３⁃２５６． ［１０］ ＳＡＳＫＡ Ｍ， ＭＡＣＡＳ Ｍ， ＰＲＥＵＣＩＬ Ｌ， ｅｔ ａｌ． Ｒｏｂｏｔ ｐａｔｈ ｐｌａｎｎｉｎｇ ｕｓｉｎｇ ｐａｒｔｉｃｌｅ ｓｗａｒｍ ｏｐｔｉｍｉｚａｔｉｏｎ ｏｆ Ｆｅｒｇｕｓｏｎ ｓｐｌｉｎｅｓ［Ｃ］ ／ ／ ＩＥＥＥ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｅｍｅｒｇｉｎｇ Ｔｅｃｈｎｏｌｏｇｉｅｓ ａｎｄ Ｆａｃｔｏｒｙ Ａｕｔｏｍａｔｉｏｎ． Ｐｒａｇｕｅ， Ｃｚｅｃｈ Ｒｅｐｕｂｌｉｃ， ２００６： ８３３⁃８３９． ［１１］谢晓锋， 张文俊， 杨之廉． 微粒群算法综述［Ｊ］． 控制与 决策， ２００３， １８（２）： １２９⁃１３４． ＸＩＥ Ｘｉａｏｆｅｎｇ， ＺＨＡＮＧ Ｗｅｎｊｕｎ， ＹＡＮＧ Ｚｈｉｌｉａｎ． Ｏｖｅｒｖｉｅｗ ｏｆ ｐａｒｔｉｃｌｅ ｓｗａｒｍ ｏｐｔｉｍｉｚａｔｉｏｎ［ Ｊ］． Ｃｏｎｔｒｏｌ ａｎｄ Ｄｅｃｉｓｉｏｎ， ２００３， １８（２）： １２９⁃１３４． ［１２］ＳＵＰＡＫＡＲ Ｎ， ＳＥＮＴＩＨＩＬ Ａ． ＰＳＯ ｏｂｓｔａｃｌｅ ａｖｏｉｄａｎｃｅ ａｌｇｏ⁃ ｒｉｔｈｍ ｆｏｒ ｒｏｂｏｔ ｉｎ ｕｎｋｎｏｗｎ ｅｎｖｉｒｏｎｍｅｎｔ［Ｃ］ ／ ／ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｍｍｕｎｉｃａｔｉｏｎ ａｎｄ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ （ ＩＣ⁃ ＣＣＶ）． Ｃｏｉｍｂａｔｏｒｅ， Ｉｎｄｉａ， ２０１３： １⁃７． ［１３］ＨＵ Ｍｅｎｑｉ， ＷＵ Ｔ， ＷＥＩＲ Ｊ Ｄ． Ａｎ ａｄａｐｔｉｖｅ ｐａｒｔｉｃｌｅ ｓｗａｒｍ ｏｐｔｉｍｉｚａｔｉｏｎ ｗｉｔｈ ｍｕｌｔｉｐｌｅ ａｄａｐｔｉｖｅ ｍｅｔｈｏｄｓ ［ Ｊ ］． ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｅｖｏｌｕｔｉｏｎａｒｙ ｃｏｍｐｕｔａｔｉｏｎ， ２０１３， １７（５）： ７０５⁃７２０． ［１４］尼库拉 库克． 机器人学导论—分析、控制及应用［Ｍ］． 孙富春， 朱纪洪， 刘国栋等， 译． 第 ４ 版． 北京： 电子工 业出版社， ２００４： ６０⁃７２． ［１５］ＢＯＲＹＧＡ Ｍ， ＧＲＡＢＯＳ Ａ． Ｐｌａｎｎｉｎｇ ｏｆ ｍａｎｉｐｕｌａｔｏｒ ｍｏｔｉｏｎ ｔｒａｊｅｃｔｏｒｙ ｗｉｔｈ ｈｉｇｈｅｒ⁃ｄｅｇｒｅｅ ｐｏｌｙｎｏｍｉａｌｓ ｕｓｅ［ Ｊ］． Ｍｅｃｈａ⁃ ｎｉｓｍ ａｎｄ Ｍａｃｈｉｎｅ Ｔｈｅｏｒｙ， ２００９， ４４（７）： １４００⁃１４１９． 作者简介： 李小为，女，１９８９ 年生，硕士研究 生，主要研究方向为机器人、先进控制 理论与技术。 胡立坤，男，１９７７ 年生，教授，主要 研究方向是非线性系统动力学与控制、 光／ 风／ 蓄分布式电源转换与控制、工业 测控网络应用。 作为主要参与人参与 国家自然科学基金项目 ２ 项，承担广西 科技开发计划项目 ２ 项、广西自然科学 基金 １ 项和企业横向项目 ５ 项。 获得 软件著作权 １ 项，实用新型专利 １ 项，申请发明专利 ８ 项。 发表学术论文 ５０ 余篇，其中被 ＳＣＩ 检索 ２ 篇，ＥＩ 检索 １４ 篇。 王琥，男，１９９０ 年生，硕士研究生， 主要研究方向为机器人视觉、先进控制 理论与技术。 ·３９８· 智 能 系 统 学 报 第 １０ 卷