第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看作 Ｄ 维搜索空间中一个没有体积和质量的微 粒［１１－１２］ ，且在搜索空间中以一定的速度飞行，通过 群体中粒子之间的合作和竞争来寻找最优解。 假设 一个包含 ｍ 个微粒的微粒群在其 Ｄ 维搜索空间中 飞行，第 ｉ 个粒子在 Ｄ 维空间的位置为 ｘｉ ＝ （ｘｉ１ ，ｘｉ２ ， …，ｘｉＤ） ，飞行速度为 ｖｉ ＝ （ｖｉ１ ，ｖｉ２ ，…，ｖｉＤ） ，每个粒 子都具有一个由被优化的目标函数决定的适应值， 微粒 ｉ 所经历的最好位置 ｐｉ ＝ （ｐｉ１ ，ｐｉ２ ，…，ｐｉＤ） ，也 就是微粒 ｉ 所经历过的具有最好适应值的位置，群 体所有微粒经历的最好位置为 ｐｇ 。 对每一次迭代， 第 ｉ 个粒子的第 ｄ 维分量（ １ ≤ｄ ≤Ｄ ）在第 ｋ ＋ １ 次 迭代时的速度和位置按式（５）和（６）变化： ｖ ｋ＋１ ｉｄ ＝ ｗ × ｖ ｋ ｉｄ ＋ ｃ１ × ｒ１ × （ｐｉｄ － ｘ ｋ ｉｄ ） ＋ ｃ２ × ｒ２ × （ｐｇｄ － ｘ ｋ ｉｄ ） （５） ｘ ｋ＋１ ｉｄ ＝ ｘ ｋ ｉｄ ＋ ｖ ｋ＋１ ｉｄ （６） 式中： ｗ 为惯性权重， ｃ１ 为自身加速常数， ｃ２ 为全局 加速常数， ｒ１ 和 ｒ２ 为 ２ 个相互独立的在［０，１］变化 的 随 机 函 数， ｖ ｋ ｉｄ ∈ － ｖｍａｘ，ｖｍａｘ [ ] ， ｘ ｋ ｉｄ ∈ － ｘｍａｘ，ｘｍａｘ [ ] ，如果 ｖ ｋ ｉｄ 、ｘ ｋ ｉｄ 超出边界值，就用边界值 取代， ｖｍａｘ、ｘｍａｘ 是依据不同的目标函数和搜索空间 而不同的常数［１３］ 。 式（５）中第 １ 部分为微粒先前的 速度，第 ２ 部分为微粒自身的思考调整，第 ３ 部分为 种群间的协同合作。 如果选择多项式的系数 ａｉｊ 作为待寻优量，则根 据式（２） ～ （４）可以得到时间变量 ｔ ｉ１ 、ｔ ｉ２ 、ｔ ｉ３ ，这时候 粒子群的维数为 １４ 维。 如果直接选择在时间变量 ｔ ｉ１ 、ｔ ｉ２ 、ｔ ｉ３ 的搜索空间进行优化，可以将维数降低，大 大减少了粒子群寻优的复杂性和困难性。 优化目标 是使各个关节在约束的速度范围内以最短的时间运 行。 其适应度函数为 ｆ（ｔ） ＝ ｍｉｎ（ｔ ｉ１ ＋ ｔ ｉ２ ＋ ｔ ｉ３ ） （７） ｍａｘ Ｖｉ { } ≤ Ｖｉｍａｘ （８） 式中： Ｖｉ 和 Ｖｉｍａｘ 分别是第 ｉ 个关节的实时速度和最 大限制速度，利用粒子群算法对复杂的约束优化问 题求解。 粒子群算法对机器人第 ｉ 个关节进行最优时间 轨迹规划，具体步骤如下： １）选定种群的规模 ｍ （一般为 ２０），在插值时 间的 ３ 维搜索空间中随机产生 ｍ 个粒子构成初始 种群，并初始化粒子的位置和速度。 ２）由 ｍ 组时间变量 ｔ ｉ１ 、ｔ ｉ２ 、ｔ ｉ３ 代入式（２） ～ （４） 中可得出 ３⁃５⁃３ 多项式的未知系数 ａｉｊ 。 ３）将 ３⁃５⁃３ 多项式的系数 ａｉｊ 代入式（１）并对时 间求导，得到关节角度的速度函数，判断实时速度是 在否满足式（８）。 ４）计算每个微粒的适应度值。 对步骤 ３）的计 算结果进行筛选，如果 ３ 段中的任何一段速度不符 合式（８），则将该粒子的适应度值设置为极大的常 数，在寻找最优粒子时，通过适应度值的比较，将会 排除这个适应度值较大的粒子，不被筛选为最优粒 子。 而这个粒子本身也会慢慢向最佳值进行靠拢， 直到满足速度的约束。 如果 ３ 段的最大速度都符合 式（８），则采用式（７）作为适应度函数，粒子群算法 迭代以获得最小插值时间为目标。 ５）对每个微粒，将其适应度值与其经历过的最 好位置 ｐｉ 的适应度值作比较，如果较好，则将其替 换为当前的最好位置 ｐｉ 。 ６）比较每个微粒当前最好位置的适应度值，得 到当前整体最优粒子，再与群体所经历的全局最好 位置 ｐｇ 作比较，如果较好，则替换 ｐｇ 。 ７）根据式（５） ～ （６）变化微粒的速度和位置，重 新整合成新的由 ｍ 个粒子构成的种群。 ８）如满足终止条件（通常为足够好的适应值或 达到一个预设最大迭代次数（Ｎｍａｘ）则算法结束，否 则返回步骤 ２）。 本文设定粒子群的个数 ｍ 为 ２０，初始的粒子位 置为［０．１，４．０］的任意随机数，粒子的最大飞行速度在 ［－２，２］之间。 粒子飞行速度的参数设置为惯性权重 ｗ ＝ （Ｗｍａｘ － ｉ × （Ｗｍａｘ － Ｗｍｉｎ ） ／ Ｎｍａｘ） ， Ｗｍａｘ ＝ ０．９， Ｗｍｉｎ ＝ ０．４， ｉ 为迭代次数，循环迭代步数 Ｎｍａｘ为 ５０。 权重因子 ｃ１ ＝ ２，ｃ２ ＝ ２， ｒ１ 和 ｒ２ 为［０，１］的随机数。 ３ 机器人建模与 ＰＳＯ 仿真 本文主要研究工业机器人六自由度机械臂，采 用标准 Ｄ⁃Ｈ 坐标系法进行运动学建模［１４］ ，如图 １ 所 示。 ６ 个关节的 Ｄ⁃Ｈ 参数见表 １。 图 １ 机械臂的 Ｄ⁃Ｈ 坐标系 Ｆｉｇ． １ Ｔｈｅ Ｄ⁃Ｈ ｃｏｏｒｄｉｎａｔｅｓ ｏｆ ｒｏｂｏｔ ｍａｎｉｐｕｌａｔｏｒｓ 第 ３ 期 李小为，等：速度约束下 ＰＳＯ 的六自由度机械臂时间最优轨迹规划 ·３９５·