一种多指标综合最优的抗冲击轨迹规划方法

荣誉 陈刚 豆天赐

摘要 ：为提高机械臂作业效率以及抗冲击能力，提出了一种综合最优轨迹规划方法。首先通過建立3-5-3多项式曲线数学模型构造出端点运动参数可控的关节运动轨迹；然后考虑关节位置、速度、加速度等约束条件，通过加权系数法定义目标函数，使机械臂的动作时间、冲击和灵巧度达到综合最优，在目标函数的设计中采用动态加权方法来处理关节速度与冲击之间的矛盾；最后，针对标准粒子群算法，利用拉丁超立方抽样函数均匀化种群， 并提出随机惯性权重更新策略，得到改进粒子群算法，利用该算法对目标函数进行优化，得到综合最优运动轨迹。进行了仿真和样机实验，实验结果证明所提方法具有可行性。

关键词 ：工业机器人；轨迹规划；多目标优化；粒子群算法

中图分类号 ：TP241.2

DOI：10.3969/j.issn.1004-132X.2024.02.015

开放科学（资源服务）标识码（OSID）：

A Multi Index Comprehensive Optimal Anti Impact Trajectory

Planning Method

RONG Yu  1，2  CHEN Gang 2 DOU Tianci  1，2

1.School of Vehicle and Energy，Yanshan University，Qinhuandao，Hebei，066004

2.Hebei Key Laboratory of Special Transport Equipment，Qinhuangdao，Hebei，066004

3.College of Mechanical and Electrical Engineering，Hebei Normal University of Science &

Technology，Qinhuangdao，Hebei，066004

Abstract ： A comprehensive optimal trajectory planning method was proposed to improve the efficiency and impact resistance of robotic arms. Firstly， by establishing a 3-5-3 polynomial curve mathematical model， a joint motion trajectory with controllable endpoint motion parameters was constructed. Secondly， considering constraints such as joint position， velocity， and acceleration， the objective function was defined using the weighted coefficient method to achieve a comprehensive optimization of the action time， impact， and dexterity of the robotic arm. The dynamic weighting method was used in the design of the objective function to address the contradiction between joint velocity and impact. Finally， for the standard particle swarm optimization algorithm， the Latin hypercube sampling function was used to homogenize the population， and a random inertia weight update strategy was proposed to obtain an improved particle swarm algorithm. This algorithm was used to optimize the objective function and obtain the comprehensive optimal motion trajectory. Simulation and prototype experiments were conducted， and the experimental results demonstrate the feasibility of the proposed method.

Key words ： industrial robot; trajectory planning; multi objective optimization; particle swarm optimization（PSO）

0 引言

轨迹规划是工业机器人完成作业任务的关键技术，其主要步骤如下：首先采用多项式插值或样条曲线插值构造关节空间运动轨迹，然后通过智能算法对构造的轨迹曲线进行优化。根据不同的工作任务，轨迹优化有不同的选择方案，主要分为单目标优化和多目标优化。

单目标优化主要有以下几种：①时间最优轨迹优化，可以最小化作业时间进而提高生产效率；②能量最优轨迹优化，可以在降低机器能耗的同时减少机械臂的应力作用；③冲击最优轨迹优化，可以减小冲击，延长机器人的使用寿命。ZHANG等  ［1］ 用五次多项式构造轨迹曲线，应用改进的自适应遗传算法对目标函数进行优化，从而实现时间最优轨迹规划。HE等  ［2］ 将机械手的时间最优路径规划（time-optimal trajectory planning， TOTP） 问题转化为多约束条件下的非线性最优值搜索问题，提出了一种基于模糊控制的时间搜索算法来求解时间最优轨迹。HUANG等  ［3］ 提出了一种粒子群优化（particle swarm optimization， PSO）算法来搜索具有动力学约束的空间机械臂全局时间最优轨迹。李小为等  ［4］ 提出了粒子群优化速度约束下的时间最优 3-5-3 多项式插值轨迹规划方法。王成  ［5］  在考虑机器人各关节速度、加速度、加加速度和力矩约束的情况下，应用遗传算法求解工业机器人能量最优轨迹。ZHANG等  ［6］ 提出了一种实用的时间最优光滑轨迹规划算法，该算法利用基于动力学模型的时间最优理论规划机器人的运动轨迹， 在几何路径和关节力矩约束下构建轨迹优化模型，在求解过程中动态选择最优轨迹参数，并利用输入整形算法实现时间最优光滑轨迹。胡智宇  ［7］ 提出在关节空间采用五次多项式插补算法对各关节设计点到点的运动规划，并用区间极大熵函数算法进行冲击最优轨迹规划。

多目标优化是指机器人两个及以上的运动指标达到综合最优。HE等  ［8］ 提出了一种约束免疫多目标优化算法（constraint immune multi-objective optimization algorithm， CIMOA），对工业机器人的总运行时间和总冲击进行寻优计算。朱世强等  ［9］  采用序列二次规划方法求解最优运动时间节点，进而规划出满足非线性运动学约束的时间最优脉动连续轨迹。徐海黎等  ［10］ 采用基因环境双演化免疫克隆算法对时间和能量的代价函数进行优化。HUANG等  ［11］ 通过5阶B样条构造轨迹曲线，然后采用非支配排序遗传算法（non-dominated sorting genetic algorithm Ⅱ， NSGA-Ⅱ）对整个轨迹行程时间和平均加加速度两个目标进行优化。施祥玲等  ［12］ 采用多目标粒子群优化（multi-objective particle swarm optimization， MOPSO）算法，以工业机器人运行时间、能量消耗和轨迹脉动为目标对其运动轨迹进行优化。吕鲲等  ［13］ 将运行时间和关节角加加速度的积分和作为目标函数，通过遗传算法得到满足运动学约束和动力学约束的时间、冲击综合最优轨迹曲线。汤小红等  ［14］ 提出一种具有混沌寻优的多目标自适应粒子群优化算法，在综合考虑时间、能耗、冲击的情况下求解最优轨迹。

由前文可知，国内外研究人员采用的各种优化方法非常丰富，优化目标也都相对完善，但以灵巧度最优为目标对工业机器人的运动轨迹进行优化的报道相对较少。对于任何拓扑结构的机器人，灵巧度始终是一项衡量运动性能的重要指标。机械臂若具有良好的灵巧度，则具有更好的运动控制能力，能够更精准地执行任务。针对机械臂灵巧度量化问题，国内外学者也开展了许多研究。于凌涛等  ［15］ 通过分析机械臂雅可比矩阵奇异值、加权全域空间条件数均值与全域空间条件数波动值，建立了综合灵巧度指标。菅奕颖  ［16］ 利用服务球理论，通过计算机械臂全部可到点构成的点的集合与总点数的商来衡量机械臂灵巧度。杨玉维等  ［17］ 采用雅可比逆矩阵的条件数来衡量并联机器人的灵巧度。杨磊等  ［18］ 采用速度雅可比矩阵条件数的倒数来衡量空间4SPS-PS并联机构的灵巧性。TCHON等  ［19］ 采用内生配置空间方法，提出了局部灵巧度和全局灵巧度两个运动学灵巧度测量指标，描述了内生灵巧度测量相对于移动操作机器人传统性能测量的优势，两种灵巧度都用于确认移动操作机器人的最优配置和最优几何形态。

本文从提高工业机器人整体运动性能出发，以3-5-3多项式曲线模型进行关节空间的轨迹插值，通过加权系数法将总时间、总冲击和灵巧度三个指标整合为综合指标，以综合指标作为目标函数。同时，为获得全局综合最优解，提出了一种改进的粒子群算法， 以该算法对轨迹曲线进行寻优。最后通过仿真和样机实验来验证所提方法的有效性。

1 机器人运动学建模

本文以FANUC M710iC50六自由度机械臂为研究对象，采用D-H表达法建立运动学模型。

1.1 机器人D-H坐标的建立

通过D-H表示法对FANUC M710iC50六自由度机械臂建立运动学模型，明确各连杆之间的运动关系。图1所示为机械臂各个关节坐标系定义。FANUC M710iC50六自由度机械臂的D-H 参数见表1，其中，α i为连杆扭角，a i为连杆长度，θ i为关节转角，d i为关节偏置，i=1，2，…，6。

1.2 机器人正运动学建模

已知机械臂几何参数和各连杆间的运动关系，采用齐次变换法求得从坐标系{0}到坐标系{i}的变换过程。按照机械臂坐标系定义，连杆变换矩阵公式为

i-1    i  T =

Rot （z  i-1 ，θ i） Trans （z  i-1 ，d i） Trans （x i，a i） Rot （x i，α i） （1）

其中， Rot （z  i-1 ，θ i）、 Rot （x i，α i）为旋转矩阵，分别表示绕坐标系{i-1}的z轴旋转θ i角度和绕坐标 系{i}的x轴旋转α i角度； Trans （z  i-1 ，d i）、 Trans （x i，a i）为平移矩阵， 分别表示沿坐标系{i-1}的z轴方向移动d i距离和沿坐标系{i}的x轴方向移动a i距离。

由式（1）可以得到连杆齐次变换矩阵  i-1    i  T 的通式：

i-1    i  T =

cos  θ i - sin  θ i cos  α i  sin  θ i sin  α i a i cos  θ i

sin  θ i  cos  θ i cos  α i - cos  θ i sin  α i a i sin  θ i 0  sin  α i  cos  α i d i 0 0 0 1   （2）

由式（2）可得相鄰两坐标系间变换矩阵 0 1 T ， 1 2 T ，…， 5 6 T ，故机器人的正运动学计算公 式为

0 6 T = 0 1 T 1 2 T 2 3 T 3 4 T 4 5 T 5 6 T =  r  11  r  12  r  13  p x r  21  r  22  r  23  p y r  31  r  32  r  33  p z 0 0 0 1   （3）

其中，  i-1    i  T 表示杆件i坐标系向i-1坐标系的齐次变换矩阵；r  ij 为坐标系{6}相对于坐标系{0}的旋转矩阵元素，描述坐标系{6}相对于坐标系{0}的姿态信息；p x、p y、p z为坐标系{6}原点相对于坐标系{0}原点的位置向量分量，描述坐标系{6}原点相对于坐标系{0}原点的位置信息。

1.3 机器人逆运动学建模

对于图1所示的空间六自由度串联机械臂，末端位姿 0 6 T 已知。机械臂逆运动学建模就是根据式（3）求各关节转角θ i。

首先将式（3）两边同时左乘 0 1 T   -1 （θ 1）、 1 2 T   -1 （θ 2）， 右乘 5 6 T   -1 （θ 6）可得

1 2 T   -1 （θ 2） 0 1 T   -1 （θ 1）  0 6 T   5 6 T   -1 （θ 6）=

2 3 T （θ 3）  3 4 T （θ 4）  4 5 T （θ 5）  （4）

根據式（4）两侧矩阵第三行第四列对应元素相等可得

-p y cos  θ 1+p x sin  θ 1+d 6（r  23  cos  θ 1-r  13  sin  θ 1）=

d 2+d 3  （5）

由式（5）可求解θ 1得

θ 1= arctan （m，n）- arctan （d 2+d 3，

± m 2+n 2-（d 2+d 3） 2 ）  （6）

m=d 6r  23 -p y  n=d 6r  13 -p x

根据式（4）两侧矩阵对应元素（1，4）（表示第一行、第四行元素，下同）和（2，4）分别相等，可得

k 2 sin  θ 2-k 1 cos  θ 2-a 2=a 3 cos  θ 3+d 4 sin  θ 3  （7）

k 1 sin  θ 2+k 2 cos  θ 2=a 3 sin  θ 3-d 4 cos  θ 3  （8）

其中，k 1、k 2分别为

k 1=（d 6r  13 -p x） cos  θ 1+（d 6r  23 -p y） sin  θ 1+a 1  （9）

k 2=p z-d 1-d 6r  33   （10）

联立式（7）和式（8），求解θ 2得

θ 2= arctan （k 3，k 4）- arctan （k 5，± k 2 3+k 2 4-k 2 5 ）  （11）

其中，k 3、k 4、k 5分别为

k 3=2a 2k 1  （12）

k 4=2a 2k 2  （13）

k 5=a 2 3+d 2 4-a 2 2-k 2 1-k 2 2  （14）

将θ 2代入式（7）整理可得

θ 3= arctan （g，± a 2 3+d 2 4-g 2 ）- arctan （a 3，d 4）  （15）

g=k 2 sin  θ 2-k 1 cos  θ 2-a 2

将式（3）等号两边同时左乘 0 1 T   -1 （θ 1）、 1 2 T   -1 （θ 2）、

2 3 T   -1 （θ 3）可得

2 3 T   -1 （θ 3）  1 2 T   -1 （θ 2）  0 1 T   -1 （θ 1）  0 6 T =

3 4 T （θ 4）  4 5 T （θ 5）  5 6 T （θ 6）  （16）

根据式（16）两侧矩阵对应元素（3，3）相等，可得

cos  θ 5=（r  13 c 1+r  23  sin  θ 1） sin （θ 2+θ 3）-

r  33  cos  （θ 2+θ 3）  （17）

其中，（3，3）表示式（16）两侧矩阵第三行第三列所对应的元素值。

由式（17）可求解得到θ 5为

θ 5= arccos （（r  13  cos  θ 1+r  23  sin  θ 1） sin （θ 2+θ 3）-

r  33  cos  （θ 2+θ 3））  （18）

根据式（16）两侧矩阵对应元素（2，3）相等，可得

- sin  θ 4 sin  θ 5=r  13  sin  θ 1-r  23  cos  θ 1  （19）

当 sin  θ 5≠0时，由式（19）可求解得到θ 4为

θ 4= arcsin （（r  23  cos   θ 1-r  13  sin  θ 1）/ sin  θ 5）  （20）

当 sin  θ 5=0时，机械手处于奇异形位。此时，关节4与关节6轴线重合，只能求解出θ 4与θ 6的和或差。在奇异形位时，可任意选取θ 4的值，再计算相应的θ 6值。

将式（3）等号两边同时左乘 0 1 T   -1 （θ 1），右乘 5 6 T   -1 （θ 6）可得

0 1 T   -1 （θ 1）  0 6 T   5 6 T   -1 （θ 6）= 1 2 T （θ 2）  2 3 T （θ 3）  3 4 T （θ 4）  4 5 T （θ 5）  （21）

根据式（21）两侧矩阵对应元素（2，2）相等，可得

r  32  cos  θ 6+r  31  sin  θ 6=- sin  θ 4 sin （θ 2+θ 3）  （22）

由式（22）可求解得到θ 6為

θ 6= arctan （- sin  θ 4 sin （θ 2+θ 3），

± r 2  31 +r 2  32 - sin  2θ 4 sin  2（θ 2+θ 3） ）-

arctan （r  32 ，r  31 ）

（23）

综上所述，给定机器人末端位姿矩阵，即可求得满足机器人工作要求的关节角度。

2 构造3-5-3多项式插值函数

在机器人轨迹规划过程中，多项式插值轨迹是一种常见的轨迹构造方法。多项式插值轨迹的 核心就是计算多项式的系数，其计算量随多项式次数的增加而逐级递增。本文在综合考虑轨迹在起点、终点和中间点的位置、速度、加速度和 Jerk （位置三阶导数）连续以及轨迹平滑性的情况下，选择计算量相对较小的3-5-3多项式插值构造轨迹曲线。

对于3-5-3多项式插值构造轨迹曲线，首先根据机器人在笛卡儿坐标系下起始点以及中间点的空间坐标，通过逆运动学求解各个关节在4个插值点处的关节角度，用θ  ij 表示关节i第j个路径点的插值角度，其中i=1，2，…，n， n表示关节数目；j=0，1，2，3表示路径点的序号。在路径点间采用3-5-3多项式构造轨迹曲线，其约束条件如下：①初始点、末端点和中间点的位置；②路径点间位置、速度和加速度连续；③始末位置速度和加速度为0。

第i关节3-5-3多项式插值函数的通式为

θ  i1 （t 1）=a  13 t 3 1+a  12 t 2 1+a  11 t 1+a  10

θ  i2 （t 2）=a  25 t 5 2+a  24 t 4 2+a  23 t 3 2+a  22 t 2 2+a  21 t 2+a  20

θ  i3 （t 3）=a  33 t 3 3+a  32 t 2 3+a  31 t 3+a  30    （24）

式中，θ  i1 （t 1）、θ  i2 （t 2）、θ  i3 （t 3）分别为多项式的角位移函数；a  ij 为第i（i=1，2，3）段多项式的第j个系数，j取决于多项式次数；t为插值的时间变量。

式（24）中的系数a  ij 可根据约束条件求出，根据约束条件和约束边界可推导出关节插值点与系数的关系矩阵 A ：

A =

t 3 1 t 2 1 t 1 0 0 0 0 0 -1 0 0 0 0 0 3t 2 1 2t 1 1 0 0 0 0 -1 0 0 0 0 0 0 6t 1 2 0 0 0 0 -2 0 0 0 0 0 0 0 0 0 0 t 5 2 t 4 2 t 3 2 t 2 2 t 2 1 0 0 0 0 -1 0 0 0 5t 4 2 4t 3 2 3t 2 2 2t 2 1 0 0 0 0 -1 0 0 0 0 20t 3 2 12t 2 2 6t 2 2 0 0 0 0 -2 0 0 0 0 0 0 0 0 0 0 0 0 t 3 3 t 2 3 t 3 1 0 0 0 0 0 0 0 0 0 0 3t 2 3 2t 3 1 0 0 0 0 0 0 0 0 0 0 0 6t 3 2 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0    （25）

式中，t i为第i段轨迹机械臂所运行时间。

约束条件和约束边界只与时间t有关。关系矩阵 A 与系数矩阵 a 的关系表达式如下：

Aa = θ   （26）

其中， θ 为关节角度矩阵。系数矩阵 a 的表达式为

a =［ a  1  a  2  a  3］  T   （27）

a  1=［a  13  a  12  a  11  a  10 ］  （28）

a  2=［a  25  a  24  a  23  a  22  a  21  a  20 ］  （29）

a  3=［a  33  a  32  a  31  a  30 ］  （30）

关节角度矩阵 θ 的表达式如下：

θ =［0 0 0 0 0 0 x  i3  0 0 x  i0  0 0

x  i2  x  i1 ］  T   （31）

式中，x  ij 为第i个关节第j个插值点的位置。

综上所述，3-5-3多项式轨迹曲线仅与时间t有关，因此可以找到一个时间，使得机器人在这段轨迹下运行时综合性能最优。

3 目标函数的构造

轨迹优化的核心就是目标函数的构造。目前国内外学者对目标函数的构造采用的各种方法非常丰富，但这些方法存在两类问题：一是采用单一的运动性能指标构造目标函数，其结构过于简单，所得解无法全面考虑机械臂运动性能和应用场合；二是选取的指标过多，导致优化问题变得更为复杂，增加了目标函数构造的难度和计算量。因此，在机械臂运动性能和目标函数构造难度的权衡下，本文选择以下三个主要指标进行目标函数的构造：①总时间，指机器人完成任务所需的实际时间，在工业自动化中，机器人的作业时间是影响生产效率的关键性因素;②总冲击，指机器人执行任务时产生的撞击和振动，冲击直接影响机械臂运动的稳定性和可靠性;③灵巧度，指机器人完成任务时的灵活性和适应性，灵巧度的优化可以使机器人适应更加复杂的任务以及具有更快的响应 速度。

3.1 时间优化问题描述

假设机器人执行一个动作，其末端执行器经过n个点（包括起点和终点）。通过机器人逆运动学转换成关节空间的n个对应的位置序列，故产生n-1段轨迹，机器人完成n-1段轨迹所需时间之和即机器人运动的总时间，故时间优化问题的目标函数为

S 1=∑ n－1 i=1 t i  （32）

式中，n-1为机械臂运动轨迹的段数，即插值点数目减1。

3.2 冲击优化问题描述

机械臂沿轨迹运动时，若存在加速度不连续的情况，则整个机械臂系统将产生较大的冲击。冲击影响机械臂运动稳定性的同时还会加速机械臂疲劳破坏、缩短机械臂使用寿命。又因为机械臂的冲击与机械臂的脉动有直接关系，脉动越小则冲击越小。而机械臂的脉动由加速度的变化（即加加速度）来体现。本文在進行冲击优化问题描述时采用绝对平均冲击s来衡量关节冲击大小，其表达式如下：

第一段轨迹冲击

s  i1 =  1 t 1 ∫  t   1  0（q …   i1 ） 2 d t   （33）

第二段轨迹冲击

s  i2 =  1 t 2 ∫  t   2  0（q …   i2 ）  2  d t   （34）

第三段轨迹冲击

s  i3 =  1 t 3 ∫  t   3  0（q …   i3 ）  2  d t   （35）

式中，t 1、t 2、t 3分别为三段轨迹运行的时间；q …   i1 、q …   i2 、q …   i3 分别为第i关节第一、二、三段冲击量，即关节加速度关于时间的变化率。

在3-5-3多项式轨迹规划中，具体可以将轨迹分为三个阶段，即初始阶段、中间阶段和末尾阶段。对于每个阶段，根据实际情况分别赋予其不同的冲击权重，进而实现全局的动态权重，使轨迹设计更加合理。具体可以将三段多项式轨迹冲击所占权重表示为

S 2=w 1s  i1 +w 2s  i2 +w 3s  i3   （36）

式中，w 1、w 2、w 3为动态权重系数，分别表示三段轨迹的冲击权重。

对于动态权重的设定，本文根据各关节冲击与各段轨迹运行时间的变化关系来构造动态权重函数，其变换关系如图2所示。其中，t 1、t 2、t 3为各段轨迹运行时间；图2 a、图2b、图2c 分别以t 1、t 2、t 3为自变量，其余两段轨迹时间不变；图2 d 以t 1、t 2、t 3为自变量。

由图2可知，关节冲击量随时间的减少呈递增的趋势。由于图2中关节冲击与各段轨迹运行时间的变化情况相似，故本文选取图2 d 中关节1绝对平均冲击量与轨迹运行时间的离散点分别进行反比例函数、指数函数、对数函数、多项式函数曲线拟合，拟合结果如图3所示。

根据图3拟合曲线分别计算各条曲线与离散点的均方根误差（ root mean squared error，RMSE ），数据见表2。

由表2可知多项式函数均方根误差最小，但多项式函数拟合的曲线不符合关节绝对平均冲击量与各段轨迹运行时间的变化关系。反比例函数、指数函数、对数函数三种函数中，以反比例函数拟合的曲线均方根误差最小，即拟合程度最高。故本文在构造动态权重函数时采用反比例函数，其表达式为

T =［t 1 t 2 t 3］  （37）

（1）当T i= max （ T ）时，有

w i=  min （ T ） t 1+t 2+t 3   （38）

（2）当T i= median （ T ） 时，有

w i=  median （ T ） t 1+t 2+t 3   （39）

（3）当T i= min （ T ），有

w i=  max （ T ） t 1+t 2+t 3   （40）

式中， T 为时间向量，其元素t i为第i段轨迹所运行的时间；i取1、2、3；w i为第i段轨迹冲击的权重系数。

3.3 灵巧度优化问题描述

机器人在靠近奇异位形区域时，其输入运动与输出运动之间的传递关系逐渐失真，而灵巧度就是衡量这种失真程度的指标。雅可比矩阵的条件数作为机器人灵巧度的度量指标，其值越接近1，机器人灵巧性越好，机器人在该位姿下的运动性能越优。雅可比矩阵条件数定义如下：

K J=‖ J ‖·‖ J   -1 ‖  （41）

式中， J 为雅可比矩阵，‖·‖为2- 范数。

机器人末端同时具有移动和转动时其雅可比矩阵存在量纲不统一问题，而条件数在计算时要求雅可比矩阵所有元素的量纲统一，因此需要对雅可比矩阵进行预处理，使其元素量纲统一。本文参照文献［20］提出的可变加权矩阵对雅可比矩阵进行规范化处理。可变加权矩阵表达式 如下：

W  V=   1 L v  I  t 0 0  1 L ω  I  r    （42）

式中， I  t、 I  r为单位矩阵，下标t、r为单位矩阵的阶数；L v为对应末端平移速度各行矢量模的均方根；L ω为对应转动自由度各行矢量模的均方根。

综上，雅可比矩阵规范量纲后的表达式为

= W  V J   （43）

式中，   为雅可比量纲规范化矩阵。

机械臂运动过程中可能存在某些位姿使得机械臂关节速度趋于无穷大，导致机械臂失去控制，无法继续运动，即奇异点。在奇异点处，机械臂的雅可比矩阵逆矩阵不存在。所以引入雅可比矩阵的伪逆矩阵来代替逆矩阵的求解。伪逆矩阵 J  +的具体形式为

J  += VΣ  + U   T   （44）

其中， V 、 Σ 、 U 為雅可比矩阵的奇异值分解，其表达式为

= UΣV   T   （45）

其中， U 为左奇异向量矩阵； V 为右奇异向量矩阵； Σ 为奇异值矩阵，也是对角矩阵，对角线上的元素是从大到小排列的奇异值； Σ  +为 Σ 的伪逆矩阵，也是对角矩阵，对角线上的元素是 Σ 中奇异值的倒数，若奇异值为0，则对应的元素为0，其表达式如下：

Σ  += diag （ 1 σ 1 ， 1 σ 2 ，…， 1 σ n ）  （46）

式中，σ i为雅可比矩阵奇异值。

在实际计算中，通常会对奇异值进行修正，以避免除以一个接近于0的数。通常设置一个阈值，若小于阈值则将其设置为0。根据总时间、总冲击和条件数的数量级关系，本文将阈值设置为0.01。

虽然条件数可以反映机械臂在指定位姿下的运动能力，但不能反映在整个轨迹内的运动能力，故构造整个轨迹灵巧度衡量指标，即对整个轨迹条件数取和求平均值，这一指标能够全面反映机械臂在整个运动过程的运动学灵巧度，其表达 式为

S 3= 1 N ∑ N i=1 K i  （47）

式中，S 3为整段轨迹条件数均值；N为轨迹插值点数目；K i为时间变量为t 1、t 2、t 3时所生成轨迹的第i个插值点处条件数的值。

3.4 机器人优化数学模型

本文以六自由度机械臂为研究对象，采用3-5-3多项式构造轨迹曲线。若直接以多项式轨迹的系数a  ij 作为待寻优量，则通过式（25）可以得到时间变量t 1、t 2、t 3，这时变量维度为14。由上文多项式轨迹的约束条件可知系数矩阵 A 仅与结束时间t有关，若直接以时间变量t 1、t 2、t 3作为待寻优量，这时变量的维度为3，搜索维度大幅度降低，可以有效降低寻优的复杂性和困难性。故本文对目标函数求解时以时间变量作为待寻 优量。

综上对优化问题的描述，可以定义以下三个优化目标：

f 1=∑ 3 i=1 t i  （48）

f 2=∑ 6 i=1 w 1s  i1 +w 2s  i2 +w 3s  i3   （49）

f 3= 1 N ∑ N i=1 K i  （50）

式中，t i为第i段轨迹运行时长，i=1，2，3；N为插值点数目；f 1为机械臂运行总时间；f 2为机械臂总冲击；f 3为机械臂全局灵巧度均值。

以上３个优化目标中，f 1可以作为衡量机械臂运动效率的指标；f 2可以作为衡量关节磨损和机械臂稳定性的指标；f 3可以作为衡量机械臂整个轨迹灵活性的指标。

设定运动学和时间约束条件，将机器人多目标综合最优问题表达如下：

f= min （ξ 1f 1+ξ 2f 2+ξ 3f 3）  （51）

其中，ξ 1、ξ 2、ξ 3为加权参数，其作用是平衡总时间、总冲击和全局灵巧度均值在数量级上存在的差别，并根据机器人不同的实际需求，对机器人总时间、总冲击、全局灵巧度均值进行加权，同时强调某项指标在整体中的重要程度。

3.5 最优问题约束条件

约束条件表示为作用于机器人上的运动学性能的物理极限。由机器人结构尺寸和运动学模型可得约束条件如下：

（1）关节位置

q   min  ≤q≤q   max    （52）

（2）关节速度

q ·    min  ≤q · ≤q ·    max    （53）

（3）关节加速度

q  ¨    min  ≤q  ¨ ≤q  ¨    max    （54）

（4）轨迹运行时间

1≤t i≤5  （55）

4 轨迹优化算法——改进粒子群算法

针对标准粒子群算法前期局部搜索能力弱、后期全局搜索能力弱的问题，本文在粒子群算法中引入人工蜂群 （artificial bee colony， ABC）算法 的采蜜蜂蜜源更新策略，同时提出基于随机惯性权重的粒子速度更新公式。最后以综合指标作为目标函数，采用改进粒子群算法对目标函数进行优化。

4.1 改进粒子群算法

粒子速度更新改进策略如下。为改善粒子群算法在迭代前期局部搜索能力的不足和迭代后期全局搜索能力的不足，并提高收斂速度和全局收敛性，本文采用随机惯性权重策略实现惯性权重系数w的动态改变。粒子速度更新公式如下：

v  id =wv  id +c 1r 1（p  id -x  id ）+c 2r 2（p  gd -x  id ）

（56）

式中，w为惯性权重系数，其值越大，全局寻优能力强，局部寻优能力弱， 其值越小，全局寻优能力弱，局部寻优能力强;c 1、c 2分别为个体学习因子和社会学习因子，表示粒子向个体历史最优位置和种群历史最优位置移动的权重；v  id 为粒子当前速度；r 1、r 2为［0，1］区间内的随机数；p  id 为当前粒子已知最优解；p  gd 为种群已知最优解；x  id 为粒子当前位置。

随机惯性权重系数表达式为

w=w   min  +（w   max  -w   min  ）rand（1）+

σ×randn（1）  （57）

式中，w   max  为随机惯性权重最大值；w   min  为随机惯性权重初始值；rand（1）为［0，1］区间内均匀分布的随机数; randn（1）为服从标准正态分布（均值为0，标准差为1）的随机数的函数;σ为权重误差系数，控制取值中的权重误差，使权重w有利于向期望权重方向进化，一般取值范围为0.2～0.5。

通过设定不同惯性权重系数和权重误差系数，对Ackley、Alpine、Rastrigin、Rosenbrock四种测试函数进行仿真实验，优化结果保留2位小数，具体优化结果见表3。

通过对优化难易程度不同的四种测试函数在不同惯性权重和权重误差下进行仿真实验，在表3中发现［0.5 0.4 0.2］这一组惯性权重和权重误差的取值更加合适，四种函数在这一取值下的平均迭代次数较小，即平均收敛速度较快，故选取该这一组策略作为惯性权重和权重误差的 取值。

4.2 改进粒子群优化算法的计算步骤

（1）使用基本的拉丁超立方抽样函数 （Latin hypercube sampling， LHS） 初始化粒子的位置、随机初始化速度。计算每个粒子对应的适应度，确定个体最优值p_best和种群最优值g_best。

（2）采用 ABC 算法采蜜蜂蜜源更新策略对粒子的位置、个体最优适应值p_best和种群最优值g_best进行更新。

（3）用改进粒子群算法速度更新公式对所有粒子的位置及速度进行更新，并计算各粒子的适应度，更新p_best、g_best。

（4）判断收敛条件是否满足，若满足， 则停止迭代，输出最优结果g_best；否则，转到步骤（２），直至收敛。

4.3 改进粒子群优化算法与其他算法的比较

对4种不同类型的测试函数分别用粒子群算法、人工蜂群算法和改进粒子群算法进行测试，测试结果如图4所示。可以看出，人工蜂群算法搜索能力最弱，标准粒子群算法次之，改进粒子群算法搜索能力最强。还可以明显发现，三种算法针对不同函数时，其稳定性也不尽相同，但改进粒子群优化算法的迭代次数始终保持在较低水平。上述分析可证明，改进粒子群算法更加有效，迭代次数更少，稳定性更好。故本文采用改进粒子群优化方法对函数进行优化。

4.4 改进粒子群优化算法的应用

本文对总时间、总冲击、灵巧度三个指标进行综合优化，将每个指标与权重系数的乘积作为目标函数，以轨迹运行时间为变量，通过改进粒子群算法寻优。

本文以机器人工作效率和稳定性最优为前提，通过设定不同的权重系数值进行仿真实验，优化结果保留４位小数，具体优化结果见表4。

由表4可知，随着ξ 1、ξ 2、ξ 3三个参数的调整，总时间、总冲击、灵巧度三个指标有着不同程度的改变。比较表4中的结果后发现，［8 10 15］这一组惯性权重的取值更加合理，冲击指标f 2的降幅较时间指标f 1的增幅大，且灵巧度指标f 3也小幅度下降。综合考虑后选取［8 10 15］这一组数据作为目标函数的权重系数。

5 仿真与实验

为验证所提轨迹优化方法的有效性，以FANUC M710iC50六自由度机械臂为仿真对象，对指定关节位置序列的时间、冲击、灵巧性综合最优连续轨迹进行仿真验证。仿真环境说明如下：①计算机软硬件配置。操作系统Windows 10；处理器Intel Core i5-4200H;显卡NVIDIA GeForce GTX 950M。②仿真软件。MATLAB 2022b。机械臂经过的关节位置序列见表5。

按照本文所提改进粒子群算法，在考虑关节位置、速度、加速度等约束条件下，求解机械臂时间、冲击、灵巧性综合最优轨迹。通过记录种群最优位置在每次迭代过程中的位置变化，得到种群最优位置进化图，如图5所示。

设置的粒子种群数目为40；最大迭代次数为100的前提条件下，仿真时长约 3 h 40 min 。由图5可见，改进粒子群算法的全局搜索能力和局部搜索能力都较为突出，在经历不超过10次迭代后就快速收敛。机械臂综合最优轨迹仿真位姿变化如图6所示。

为进一步验证提出轨迹规划方法的有效性，搭建了实验样机，如图7所示。控制系统总体结构简图见图8。

上位机通过RS-232串口/以太网与M710iC50六自由度机械臂进行通信。上位机与机械臂建立连接后，在上位机程序中通过串口向机械臂发送控制指令和任务，控制柜将接受的指令进行解析，通过内部的处理器进行运算，确定每个关节电机的控制信号并输出给相应的关节电机。这些信号控制电机的转速和转向，使机械臂关节按照预定的轨迹和运动规划进行运动。

通过离线优化得到时间、冲击、灵巧性综合最优解，在机器人控制平台上编程实现 3-5-3 多项式综合最优轨迹。机械臂工作位姿变化图见 图9。

各关节优化后各角度、速度、加速度与时间关系变化如图10所示。由图10可见，规划出的综合最优轨迹经过指定的关节位置序列，关节位置、速度均连续，且满足运动学约束，其中仅第5关节的关节角速度和角加速度波动略大。基于抗冲击的目的，优化时尽可能减小前三个关节转动幅度，以减小机械臂整体晃动幅度，因此增大了关节5的转动幅度，导致关节5的关节角速度和角加速度出现较大幅度的波动。

6 结论

（1）本文以FANUC M710iC50六自由度串联机械臂为研究对象，将关节空间中的路径点两两之间通过多项式曲线连接产生机器人的运动轨迹，该轨迹规划方法保证了运动的连续性，且关节启停速度、加速度可以根据实际需要自由设定。

（2）对运动轨迹进行优化时，综合考虑整个运动过程中的动作时间、冲击和灵巧度，并在冲击指标量化时引入动态权重系数以及在灵巧度指标中引入伪逆矩阵概念。

（3）通过加权系数法定义目标函数，以运动学边界为约束条件，提出基于随机惯性权重的粒子群算法，有利于改善算法的局部搜索和全局搜索能力，提高搜索效率，寻找全局综合最 优解。

（4）对FANUC M710iC50六自由度机械臂的实验和仿真结果表明，所提的轨迹构造方法和优化策略是真实可行的。

参考文献 ：

［1］  ZHANG  J， MENG Q， FENG X， et al. A 6-DOF Robot-time Optimal Trajectory Planning Based on an Improved Genetic Algorithm［J］. Robotics and Biomimetics， 2018， 5（1）： 1-7.

［2］  HE  F， HUANG Q. Time-optimal Trajectory Planning of 6-DOF Manipulator Based on Fuzzy Control［C］∥Actuators. MDPI， 2022， 11（11）： 332.

［3］  HUANG  Panfeng， XU Yangsheng. PSO-based Time-optimal Trajectory Planning for Space Robot with Dynamic Constraints［C］∥IEEE International Conference on Robotics and Biomimetics. Kunming， 2006： 1402-1407.

［4］  李小為，胡立坤，王琥.速度约束下PSO的六自由度机械臂时间最优轨迹规划［J］.智能系统学报，2015，10（3）：393-398.

LI Xiaowei， HU Likun， WANG Hu. Time Optimal Trajectory Planning of PSO Six Degrees of Freedom Manipulator under Speed Constraint［J］. Journal of Intelligent Systems， 2015，10（3）： 393-398.

［5］  王成. 基于能量最优六自由度串联机器人轨迹规划研究［D］. 长春：长春工业大学，2014.

WANG Cheng. Research on Trajectory Planning of Six Degrees of Freedom Serial Robot Based on Energy Optimization［D］. Changchun：Changchun University of Technology， 2014.

［6］  ZHANG  T， ZHANG M， ZOU Y. Time-optimal and Smooth Trajectory Planning for Robot Manipulators［J］. International Journal of Control， Automation and Systems， 2021， 19（1）： 521-531.

［7］  胡智宇. 基于冲击最优的弧焊机器人轨迹规划研究［D］.哈尔滨：哈尔滨工业大学，2019.

HU Zhiyu. Research on Trajectory Planning of Arc Welding Robot Based on Impact Optimization［D］. Harbin：Harbin Institute of Technology， 2019.

［8］  HE  T， ZHANG Y， SUN F， et al. Immune Optimization Based Multi-objective Six-DOF Trajectory Planning for Industrial Robot Manipulators［C］∥12th World Congress on Intelligent Control and Automation（WCICA）. Guilin， 2016： 2945-2950.

［9］  朱世强，刘松国，王宣银，等.机械手时间最优脉动连续轨迹规划算法［J］.机械工程学报，2010，46（3）：47-52.

ZHU Shiqiang， LIU Songguo， WANG Xuanyin， et al. Time Optimal Pulsation Continuous Trajectory Planning Algorithm for Robotic Arms［J］. Journal of Mechanical Engineering， 2010，46（3）： 47-52.

［10］  徐海黎，解祥荣，庄健，等.工业机器人的最优时间与最优能量轨迹规划［J］.机械工程学报，2010，46（9）：19-25.

XU Haili， XIE Xiangrong， ZHUANG Jian， et al. Optimal Time and Energy Trajectory Planning for Industrial Robots［J］. Journal of Mechanical Engineering， 2010，46（9）： 19-25.

［11］  HUANG  J， HU P， WU K， et al. Optimal Time-jerk Trajectory Planning for Industrial Robots［J］. Mechanism and Machine Theory， 2018， 121： 530-544.

［12］  施祥玲，方红根.工业机器人时间-能量-脉动最优轨迹规划［J］.机械设计与制造，2018（6）：254-257.

SHI Xiangling， FANG Honggen. Optimal Trajectory Planning for Time Energy Pulsation of Industrial Robots［J］. Mechanical Design and Manufacturing， 2018（6）：254-257.

［13］  吕鲲，陈宗元，张业明.装砖码垛机器人时间-冲击最优轨迹规划［J］.机械工程师，2019（9）：1-4.

LYU Kun， CHEN Zongyuan， ZHANG Yeming. Time Impact Optimal Trajectory Planning for Brick Stacking Robot［J］. Mechanical Engineer， 2019（9）： 1-4.

［14］  湯小红，任垒垒，张宏，等.基于CPSO优化算法的六自由度机械臂轨迹规划［J］.组合机床与自动化加工技术，2023（3）：59-62.

TANG Xiaohong， REN Leilei， ZHANG Hong， et al. Path Planning of Six Degrees of Freedom Manipulator Based on CPSO Optimization Algorithm［J］. Modular Machine Tool and Automatic Machining Technology， 2023（3）：59-62.

［15］  于凌涛，杨景，王岚，等.基于灵巧度的手术机械臂尺寸与结构优化［J］.哈尔滨工程大学学报，2017，38（12）：1943-1950.

YU Lingtao， YANG Jing， WANG Lan， et al. Optimization of Size and Structure of Surgical Robotic Arms Based on Dexterity［J］. Journal of Harbin Engineering University， 2017，38（12）： 1943-1950.

［16］  菅奕颖. 6-DOF工业机器人的工作空间与灵巧性分析及其应用［D］.武汉：华中科技大学，2015.

KAN Yiying. Analysis of Workspace and Dexterity of 6-DOF Industrial Robots and Their Applications［D］. Wuhan： Huazhong University of Science and Technology， 2015.

［17］  楊玉维，赵新华.3-RRRT并联机器人工作空间与灵巧度的分析［J］.机械设计，2005（2）：11-13.

YANG Yuwei， ZHAO Xinhua. Analysis of Workspace and Dexterity of 3-RRRT Parallel Robots［J］. Mechanical Design， 2005（2）： 11-13.

［18］  杨磊，王南，颜亮，等.空间4SPS-PS并联机构的刚度及灵巧性分析［J］.组合机床与自动化加工技术，2013（11）：23-25.

YANG Lei， WANG Nan， YAN Liang， et al.  Analysis  of Stiffness and Dexterity of Spatial 4SPS-PS Parallel Mechanism［J］. Modular Machine Tool and Automation Processing Technology， 2013（11）： 23-25.

［19］  TCHON  K， ZADARNOWSKA K．Kinematic Dexterity of Mobile Manipulators： an Endogenous Configuration Space Approach［C］∥9th IEEE International Conference on Methods and Models in Automation and Robotics. Miedzyzdroje，2003：721-732.

［20］  刘志忠，柳洪义，罗忠，等.基于可变加权矩阵的机器人雅可比矩阵规范化［J］.机械工程学报，2014， 50（23）：29-35.

LIU Zhizhong， LIU Hongyi， LUO Zhong， et al. Standardization of Robot Jacobian Matrix and Determinant Based on Variable Weighting Matrix［J］. Journal of Mechanical Engineering， 2014，50（23）： 29-35.

（ 编辑 陈 勇 ）

作者简介 ：

陈 刚 ，男，1998年生，硕士研究生。研究方向为机器人。

荣 誉 （通信作者），男，1981年生，副教授、博士。研究方向为变胞、变尺寸工业机器人等。E-mail：lixiangcg@126.com。