全断面岩石隧道掘进机（TBM）具有掘进安全、掘进效率高、对环境影响小等优点，已广泛应用于各类基础设施的隧道施工中^[1]. 在TBM掘进过程中，岩石破碎将产生强烈的振动与冲击^[2]，刀具消耗大、更换频繁，严重影响了隧道施工效率. 在高压、高湿的施工环境下，换刀作业存在较大的安全隐患，近70%的盾构安全事故都与人工换刀作业直接相关^[3]. 随着我国产业自动化与智能化水平的不断提高，引入机器人完成自动换刀作业已是势在必行.

在换刀机器人运动过程中，为了避免机器人振动、延长关节使用寿命和提高轨迹跟踪精度，对换刀机器人进行轨迹规划. 机器人的运动学分析是进行轨迹规划的基础，王明斗等^[4]提出新型的管片拼装机器人逆运动学解析解的求解方法，极大地提高了逆运动学求解的效率. 赵京等^[5]利用肘部的自运动理论解耦了7自由度拟人臂的冗余度，验证了该解的仿人性和连续性. 轨迹在进行优化前，通常需要经过规划插值得到基本轨迹曲线，曲线需要满足速度、加速度约束，保证轨迹的连续与平滑. 传统的插值函数主要是多项式插值，包括经典的3次多项式与5次多项式插值法^[6]以及Boscariol等^[7]提出的‘5455’混合多项式插值法. 秦超等^[8]采用5次多项式插值法，得到7自由度番茄采摘机械手从复位点到采摘点的连续平滑轨迹. 多项式插值法计算量小，但是容易出现失真现象. 参数化表达的样条函数可以更加灵活地控制曲线的阶次，Xu等^[9]应用贝塞尔曲线拟合关节轨迹，得到连续光滑的曲线，然而节点的改变会导致贝塞尔曲线的整体重构. 经过轨迹插值后得到的运动轨迹，往往不符合预期，需要进一步进行轨迹优化. 轨迹优化的优化目标主要包括时间、能量和冲击等^[10]. Xidias^[11]提出超冗余度机械臂时间最优轨迹规划的方法，采用多种群遗传算法求解优化问题，寻找执行所需任务的最小时间轨迹. Lu等^[12]提出基于序列二次规划算法冲击最优的轨迹规划方法，将轨迹规划问题转化为约束非线性多变量优化问题，显著提高了轨迹的连续性和平滑性. 随着研究的深入，单一的优化目标已难以满足实际作业任务的要求，越来越多的研究着手考虑多目标综合优化^[13]. Liu等^[14]采用多目标粒子群优化算法来优化柔性机械手的残余振动与运行时间，通过假设模态法建立机械手运动方程，利用分段多项式进行插值得到平滑轨迹，但是该方法的轨迹高阶连续性可以进一步改善. 为了满足轨迹的时间冲击综合最优，Huang等^[15]采用精英非支配排序遗传算法，得到满足运动学约束的帕累托最优解集，决策者在选择最优解时需要进一步权衡. Lan等^[16]提出改进的多目标粒子群算法，针对时间、能量、冲击3个指标同时进行优化，虽然得到了满足目标的优化轨迹，但是算法的收敛需要一定的时间.

针对TBM换刀机器人的轨迹规划问题，本文建立新型冗余换刀机器人的Denavit-Hartenberg(DH)模型，采用位姿分离算法与关节变量最小策略得到换刀机器人逆运动学的解析解. 在运动学分析的基础上，利用NURBS曲线保证关节空间轨迹的冲击连续，采用改进型PSO算法进行时间-冲击最优轨迹规划. 通过轨迹优化的仿真，验证了该算法的适用性与有效性.

新型的8自由度换刀机器人^[17]如图1所示，机器人本体包括移动大臂和伸缩小臂，前端设有末端执行机构,本体滑动设置在滑移轨道上. 整个机构共有3个移动关节与5个转动关节，包括主移动关节1、主臂伸缩关节2、主回转驱动关节3、主摆动关节4、副臂伸缩关节5、副回转关节6、副摆动关节7及末端回转关节8. 该换刀机器人满足对刀盘不同位置滚刀更换的要求，可以实现对滚刀的高精度快速拆装.

换刀机器人的机构简图如图2所示. 图中， $L $为换刀机器人本体连杆几何结构的长度.

采用修正的DH建模方法，得到如图3所示的换刀机器人运动学模型.

换刀机器人的DH参数如表1所示. 表中， $i$为连杆编号，机器人的每个连杆都可以用5个运动学参数来表示，分别为关节转角 $\theta $、连杆长度 $a$、连杆扭角 $\alpha $、关节距离 $d$与关节偏置 ${\rm{offset}}$. 通常情况下， $\theta $为转动关节的关节变量， $d$为移动关节的关节变量，其他连杆参数是常量，保持不变.

在建立DH模型后，根据

(1) $ {}_i^{i - 1}{\boldsymbol{T}} = \left[ {\begin{array}{*{20}{c}} {\begin{array}{*{20}{c}} {{\rm{c}}\,{\theta _i}}&{ - {\rm{s}}\,{\theta _i}} \\ {{\rm{s}}\,{\theta _i}{\rm{c}}\,{\alpha _{i - 1}}}&{{\rm{c}}\,{\theta _i}{\rm{c}}\,{\alpha _{i - 1}}} \end{array}}&{\begin{array}{*{20}{c}} 0&{{\alpha _{i - 1}}} \\ { - {\rm{s}}\,{\alpha _{i - 1}}}&{ -{d_i} {\rm{s}}\,{\theta _i}} \end{array}} \\ {\begin{array}{*{20}{c}} {{\rm{s}}\,{\theta _i}{\rm{s}}\,{\alpha _{i - 1}}}&{{\rm{c}}\,{\theta _i}{\rm{s}}\,{\alpha _{i - 1}}} \\ 0&0 \end{array}}&{\begin{array}{*{20}{c}} {{\rm{c}}\,{\alpha _{i - 1}}}&{{\theta _i}{\rm{c}}\,{\alpha _{i - 1}}} \\ 0&1 \end{array}} \end{array}} \right] , $

可以得到换刀机器人正运动学的位姿矩阵：

(2) $ {}_8^0{{{\boldsymbol{T}}}} = \left[ {\begin{array}{*{20}{c}} {\begin{array}{*{20}{c}} {{n_x}}&{{o_x}} \\ {{n_y}}&{{o_y}} \end{array}} & {\begin{array}{*{20}{c}} {{a_x}}&{{p_x}} \\ {{a_y}}&{{p_y}} \end{array}} \\ {\begin{array}{*{20}{c}} {{n_z}}&{{o_z}} \\ 0&0 \end{array}} & {\begin{array}{*{20}{c}} {{a_z}}&{{p_z}} \\ 0&1 \end{array}} \end{array}} \right]. $

式中： ${\rm{c}}\,{\theta _i}$、 ${\rm{s}}\,{\theta _i}$为 $\cos \;{\theta _i}$与 $\sin \;{\theta _i}$的缩写，

$ \begin{split} {n_x} =& \;\; - \;{\rm{c}}\,{\theta _8} [{\rm{c}}\,{\theta _7} ({\rm{s}}\,{\theta _3} \cdot {\rm{s}}\,{\theta _6}\; - \;{\rm{c}}\,{\theta _3} \cdot {\rm{c}}\,{\theta _4} \cdot {\rm{c}}\,{\theta _6})\; + {\rm{c}}\,{\theta _3} \times \\ & {\rm{s}}\,{\theta _4} \cdot {\rm{s}}\,{\theta _7}]\; - \;{\rm{s}}\,{\theta _8} ({\rm{c}}\,{\theta _6} \cdot {\rm{s}}\,{\theta _3}\; + \;{\rm{c}}\,{\theta _3} \cdot {\rm{c}}\,{\theta _4} \cdot {\rm{s}}\,{\theta _6}),\end{split} $

$ \begin{split} {n_y} = &\;\;\;{\rm{c}}\,{\theta _8} [{\rm{c}}\,{\theta _7} ({\rm{c}}\,{\theta _3} \cdot {\rm{s}}\,{\theta _6}\; + \;{\rm{c}}\,{\theta _4} \cdot {\rm{c}}\,{\theta _6} \cdot {\rm{s}}\,{\theta _3})\; - \;{\rm{s}}\,{\theta _3} \times \\ & {\rm{s}}\,{\theta _4} \cdot {\rm{s}}\,{\theta _7}]\; + \;{\rm{s}}\,{\theta _8} ({\rm{c}}\,{\theta _3} \cdot {\rm{c}}\,{\theta _6}\; - \;{\rm{c}}\,{\theta _4} \cdot {\rm{s}}\,{\theta _3} \cdot {\rm{s}}\,{\theta _6}),\\ {n_z} = & \;\;{\rm{s}}\,{\theta _4} \cdot {\rm{s}}\,{\theta _6} \cdot {\rm{s}}\,{\theta _8}\; - \;{\rm{c}}\,{\theta _8} ({\rm{c}}\,{\theta _4} \cdot {\rm{s}}\,{\theta _7}\; + \;{\rm{c}}\,{\theta _6} \cdot {\rm{c}}\,{\theta _7} \cdot {\rm{s}}\,{\theta _4}),\\ {o_x} = & \;\;\;{\rm{s}}\,{\theta _8} [{\rm{c}}\,{\theta _7} ({\rm{s}}\,{\theta _3} \cdot {\rm{s}}\,\,{\theta _6}\; - \;{\rm{c}}\,{\theta _3} \cdot {\rm{c}}\,{\theta _4} \cdot {\rm{c}}\,{\theta _6})\; + \;{\rm{c}}\,{\theta _3} \times \\ & {\rm{s}}\,{\theta _4} \cdot {\rm{s}}\,{\theta _7}]\; - \;{\rm{c}}\,{\theta _8} ({\rm{c}}\,{\theta _6} \cdot {\rm{s}}\,{\theta _3}\; + \;{\rm{c}}\,{\theta _3} \cdot {\rm{c}}\,{\theta _4} \cdot {\rm{s}}\,{\theta _6}),\\ {o_y} = & \;\;\;{\rm{c}}\,{\theta _8} ({\rm{c}}\,{\theta _3} \cdot {\rm{c}}\,{\theta _6}\; - \;{\rm{c}}\,{\theta _4} \cdot {\rm{s}}\,{\theta _3} \cdot {\rm{s}}\,{\theta _6})\; - \;{\rm{s}}\,{\theta _8} [{\rm{c}}\,{\theta _7} \times \\ & ({\rm{c}}\,{\theta _3} \cdot {\rm{ }}{\rm{s}}\,{\theta _6}\; + \;{\rm{c}}\,{\theta _4} \cdot {\rm{c}}\,{\theta _6} \cdot {\rm{s}}\,{\theta _3})\; - \;{\rm{s}}\,{\theta _3} \cdot {\rm{s}}\,{\theta _4} \cdot {\rm{s}}\,{\theta _7}],\\ {o_z} = & \;\;\;{\rm{s}}\,{\theta _8} ({\rm{c}}\,{\theta _4} \cdot {\rm{s}}\,{\theta _7}\; + \;{\rm{c}}\,{\theta _6} \cdot {\rm{c}}\,{\theta _7} \cdot {\rm{s}}\,{\theta _7})\; + \;{\rm{c}}\,{\theta _8} \cdot {\rm{s}}\,{\theta _4} \cdot {\rm{s}}\,{\theta _6},\\ {a_x} = & \;\;\;{\rm{c}}\,{\theta _3} \cdot {\rm{c}}\,{\theta _7} \cdot {\rm{s}}\,{\theta _4} - \;{\rm{s}}\,{\theta _7} ({\rm{s}}\,{\theta _3} \cdot {\rm{s}}\,{\theta _6} - \;{\rm{c}}\,{\theta _3} \cdot {\rm{c}}\,{\theta _4} \cdot {\rm{c}}\,{\theta _6}),\\ {a_y} = & \;\;\;{\rm{s}}\,{\theta _7} ({\rm{c}}\,{\theta _3} \cdot {\rm{s}}\,{\theta _6}\; + \;{\rm{c}}\,{\theta _4} \cdot {\rm{c}}\,{\theta _6} \cdot {\rm{s}}\,{\theta _3})\; + \;{\rm{c}}\,{\theta _7} \cdot {\rm{s}}\,{\theta _3} \cdot {\rm{s}}\,{\theta _4},\\ {a_z} = & \;\;\;{\rm{c}}\,{\theta _4} \cdot {\rm{c}}\,{\theta _7}\; - \;{\rm{c}}\,{\theta _6} \cdot {\rm{s}}\,{\theta _4} \cdot {\rm{s}}\,{\theta _7},\\ {p_x} = & \;\;\;{\rm{c}}\,{\theta _3} \cdot {\rm{s}}\,{\theta _4} ({d_5}\; + \;{L_5})\; - \;{L_8} {\rm{s}}\,{\theta _7} ({\rm{s}}\,{\theta _3} \cdot {\rm{s}}\,{\theta _6}\; - \\ & \;{\rm{c}}\,{\theta _3} \cdot {\rm{c}}\,{\theta _4} \cdot {\rm{c}}\,{\theta _6})\; + \;{L_8} {\rm{c}}\,{\theta _3} \cdot {\rm{c}}\,{\theta _7} \cdot {\rm{s}}\,{\theta _4},\\ {p_y} = & \;\;\;{L_8} {\theta _7} ({\rm{c}}\,{\theta _3} \cdot {\rm{s}}\,{\theta _6}\; + \;{\rm{c}}\,{\theta _4} \cdot {\rm{c}}\,{\theta _6} \cdot {\rm{s}}\,{\theta _3})\; + \; \\ &{L_8} {\rm{c}}\,{\theta _7} \cdot {\rm{s}}\,{\theta _3} \cdot {\rm{s}}\,{\theta _4}\; + \;{\rm{s}}\,{\theta _3} \cdot {\rm{s}}\,{\theta _4} ({L_5}\; + \;{d_5}),\\ {p_z} = & \;\;{d_1}\; + \;{d_2}\; + \;{\rm{c}}\,{\theta _4} ({L_8}\; + \;{d_5})\; + {L_8} {\rm{c}}\,{\theta _4} \cdot {\rm{c}}\,{\theta _7}\; - \;\\ & {L_8} {\rm{c}}\,{\theta _6} \cdot {\rm{s}}\,{\theta _4} \cdot {\rm{s}}\,{\theta _7}\; + {L_1}. \end{split}$

换刀机器人通过增加冗余自由度，增强了自身的工作能力，但增加了机器人逆运动学的求解难度. 采用位姿分离法，初步得到换刀机器人逆解的解析解. 换刀机器人最后3个关节轴相交于腕点，根据坐标变换，对于每一个给定末端位姿矩阵，可以得到腕点的位置坐标：

(3) $ {}_{\rm{w}}^0{\boldsymbol{P}} = {\boldsymbol{T}} \times \left[ \begin{gathered} 0 \\ 0 \\ - {L_8} \\ 1 \\ \end{gathered} \right] = \left[ \begin{gathered} {}_{\rm{w}}^0{{{P}}_x} \\ {}_{\rm{w}}^0{{{P}}_y} \\ {}_{\rm{w}}^0{{{P}}_z} \\ 1 \\ \end{gathered} \right]. $

根据换刀机器人的正解连杆矩阵，得到腕点在基坐标系中的位置坐标为 ${}_5^0{{{\boldsymbol{T}}}}$的最后一列.

(4) $ \left[ \begin{gathered} {}_{\rm{w}}^0{{{P}}_x} \\ {}_{\rm{w}}^0{{{P}}_y} \\ {}_{\rm{w}}^0{{{P}}_z} \\ 1 \\ \end{gathered} \right] = \left[ \begin{gathered} p_x^5 \\ p_x^5 \\ p_x^5 \\ 1 \\ \end{gathered} \right] = \left[ \begin{gathered} {\rm{c}}\,{\theta _3} \cdot {\rm{s}}\,{\theta _4} ({L_5}\;+\;{d_5}) \\ {\rm{s}}\,{\theta _3} \cdot {\rm{s}}\,{\theta _4} (\;{L_5}+\;{d_5}) \\ {d_1}\;+\;{d_2}\;+\;{\rm{c}}\,{\theta _4} ({L_5}\;+\;{d_5})\;+\;{L_5} \\ 1 \\ \end{gathered} \right]. $

联立式（3）、（4），将 $ {d_5} $作为常量，分别求得 ${\theta _3}$、 ${\theta _4}$以及对应的 ${d_1}+{d_2}$.

(5) $ {\theta }_{3}={\mathrm{arctan}}\;{({}_{{\rm{w}}}^{0}{P}{}_{y}/{}_{{\rm{w}}}^{0}{P}{}_{x})}_{·} $

(6) $ {\theta }_{4}={\mathrm{arcsin}}\;{[{}_{{\rm{w}}}^{0}{{{P}}}{}_{x}/(({L}_{5}+{d}_{5})\mathrm{cos}\;{\theta }_{3})]}_{·} $

(7) $ {d}_{1}+{d}_{2}={}_{{\rm{w}}}^{0}{{{P}}}{}_{z}-{\rm{c}}\,{\theta }_{4} \left({L}_{5}+{d}_{5}\right)-{L}_{1}{}_{{}_{·}} $

利用式（5）得到2个 ${\theta _3}$，每个 ${\theta _3}$由式（6）分别得到2个 ${\theta _4}$. 根据已知的末端位姿矩阵 ${\boldsymbol{T}}$与正运动学位姿矩阵，可得

(8) $ \begin{split} & {}_5^4{{\boldsymbol{T}}^{ - 1}} \times {}_4^3{{\boldsymbol{T}}^{ - 1}} \times {}_3^2{{\boldsymbol{T}}^{ - 1}} \times {}_2^1{{\boldsymbol{T}}^{ - 1}} \times {}_1^0{{\boldsymbol{T}}^{ - 1}} {\boldsymbol{ }} \times {\boldsymbol{T}} = \\ &\qquad\;\;{}_6^5{{\boldsymbol{T}}^{ - 1}} \times {}_7^6{{\boldsymbol{T}}^{ - 1}} \times {}_8^7{{\boldsymbol{T}}^{ - 1}}. \end{split} $

令

(9) $ {{\boldsymbol{T}}_{\rm{t}}} = {}_5^4{{\boldsymbol{T}}^{ - 1}} \times {}_4^3{{\boldsymbol{T}}^{ - 1}} \times {}_3^2{{\boldsymbol{T}}^{ - 1}} \times {}_2^1{{\boldsymbol{T}}^{ - 1}} \times {}_1^0{{\boldsymbol{T}}^{ - 1}} \times {\boldsymbol{T}}, $

求解得到 ${\theta _6}$、 ${\theta _7}$与 ${\theta _8}$：

(10) $ {\theta }_{6}={\mathrm{arctan}}\;{({{T}}_{{\rm{t}}}(2,3)/{{T}}_{{\rm{t}}}(1,3))}_{,} $

(11) ${\theta }_{7}={\mathrm{arccos}}\;{{T}}_{{\rm{t}}}{(3,3)}_{,} $

(12) $ {\theta }_{8}={\mathrm{arctan}}\;{({{T}}_{{\rm{t}}}(3,2)/{{T}}_{{\rm{t}}}(3,1))}_{·} $

对于每一组 $ {\theta }_{3}、{\theta }_{4} $与 ${d_1}+{d_2}$，根据式（10）~（12）计算后，分别得到2组 ${\theta _6}$、 ${\theta _7}$与 ${\theta _8}$的解. 当利用位姿分离法计算逆运动学的解析式时，将关节变量 ${d_5}$作为常量，关节1与关节2的合并为1个关节变量后，共得到8组逆解. 为了确定一组最优逆解，对换刀机器人的多解问题设置约束条件与优化策略，保证每组逆解的关节空间变量在表1所示的实际关节的可运动区间中. 在得到满足实际运动范围的各组逆解后，采取关节变量最小策略. 根据上一时刻各关节状态变量 $ x_{i}^{t-1} $与各关节运动代价系数 $ \lambda_{i} $，由下式选取1组最优逆解：

(13) $ \begin{gathered} {\rm{min}}\;\sum\limits_{i = 1}^8 {\left( {{\lambda _i}\left| {x_i^t - x_i^{t - 1}} \right|} \right)} ； {\text{ }}{x_{i,{\rm{min}}}} \leqslant {x_i} \leqslant {x_{i,{\rm{max}}}}. \\ \end{gathered} $

式中： $ x_{i}^{t} $为 $t$​时刻 $i$关节的关节变量； $ \lambda_{i} $为 $i$关节的运动代价系数，运动代价系数的确定与换刀机器人各关节的平均驱动负载有关. 从换刀机器人的能耗角度进行考虑，负载较大的关节运动代价系数取较大值.

非均匀有理B样条(non-uniform rational B-splines, NURBS)是参数化的曲线表达，便于工程数据库的存取与应用. NURBS曲线具有良好的局部支撑特性，改变一个路径点只会引起相邻的曲线发生变化，不会影响整个轨迹曲线的形状. 利用节点向量非均匀分布的NURBS基函数，可以处理轨迹点的突变现象. 5次NURBS曲线可以保证C4连续，满足关节轨迹的脉动约束与连续. NURBS是由分段有理B样条多项式基函数定义的，形式是：

(14) $ {\boldsymbol{C}}(u) = \frac{{\displaystyle\sum\limits_{i = 0}^n {{w_i}} {{{{\boldsymbol{P}}}}_i}N_i^k(u)}}{{\displaystyle\sum\limits_{i = 0}^n {{w_i}} N_i^k(u)}}. $

式中： ${\boldsymbol{C}}\left( u \right)$为 ${u}$时刻对应的曲线位置坐标向量； ${{{{{{\boldsymbol{P}}}}}}_i}$为顶点位置矢量； $ n+1 $为控制点的个数； $ w_{{i}} $为相应控制点 ${{{{\boldsymbol{P}}}}_i}$的权因子； $ N_{i}^{k}(u) $​为k次B样条基函数，按照递归公式可以定义为

(15) $ \left.\begin{array}{l}{N}_{i}^{k}(u)=\dfrac{(u-{t}_{i}){N}_{i}^{k-1}(u)}{{t}_{i+k}-{t}_{i}}+\dfrac{({t}_{i+k+1}-u){N}_{i+1}^{k-1}(u)}{{t}_{i+k+1}-{t}_{i+1}}.\\ {N}_{i}^{0}\left(u\right)=\left\{ \begin{array}{l}1，{t}_{i}\leqslant u < {t}_{i+1}；\\ 0，其他.\end{array}\right.\end{array} \right\}$

式中： ${t_i}$​为节点值； ${{{\boldsymbol{T}}}} = \left[ {{t_0},{t_1},{t_2}, \cdots ,{t_{n+2k - 1}}} \right]$构成了k次NURBS的节点向量，对于NURBS曲线而言，其中的节点是非均匀序列，使用积累弦长参数化法生成节点向量，可以避免由控制点各弦长差异过大导致的曲线形状畸变问题. 该参数化方法满足：

(16) $ {t_i} = \left\{ \begin{gathered} 1，\qquad 0 \leqslant i < k ； \\ {t_{i - 1}}+\frac{{\left| {{{{{\boldsymbol{P}}}}_{i - k}} - {{{{\boldsymbol{P}}}}_{i - k - 1}}} \right|}}{{\displaystyle\mathop \sum \nolimits_{j = 1}^n \left| {{{{{\boldsymbol{P}}}}_j} - {{{{\boldsymbol{P}}}}_{j - 1}}} \right|}}，\quad k+1 \leqslant i < n+k - 2 ； \\ 1，\qquad{\text{ }}n - k - 1 \leqslant i < n+2k - 1. \\ \end{gathered} \right. $

NURBS曲线通常不通过除首末点之外的其他控制点，保证曲线可以通过规划要求的轨迹型值点，在k次NURBS曲线中，给定 $n+1$个轨迹型值点. 根据曲线的几何关系可以生成 $n+k$个控制点，5次NURBS曲线轨迹型值点与控制点满足：

(17) $ {{{\boldsymbol{Q}}}}_{i}=\left\{ {\begin{array}{l}{{{\boldsymbol{P}}}}_{0},\qquad i=0;\\ {\displaystyle \sum _{j=0}^{4}{k}_{j}{{{\boldsymbol{P}}}}_{i+j},\qquad 1\leqslant i < n-1};\\ {{{\boldsymbol{P}}}}_{n+4} ,\qquad i=n.\end{array}} \right. $

式中： ${{{{\boldsymbol{Q}}}}_i}$​为已知轨迹型值点，共有 $n+1$个； ${{{{\boldsymbol{P}}}}_i}$ ​为反求控制点，共有 $n+5$个，故须添加边界约束条件，给定始、终点的切矢量与2阶导数矢量：

(18) $ {\boldsymbol{Q}}_0^1 = {{\boldsymbol{P}}_1} - {\boldsymbol{P}}_0, $

(19) $  {\boldsymbol{Q}}_{0}^{2}={\boldsymbol{P}}_{2}-2{\boldsymbol{P}}_{1}+{\boldsymbol{P}}_{0}, $

(20) $ {\boldsymbol{Q}}_n^1 = {{\boldsymbol{P}}_{n+4}} - {{\boldsymbol{P}}_{n+3}}, $

(21) $ {\boldsymbol{Q}}_n^2 = {{\boldsymbol{P}}_{n+4}} - 2{{\boldsymbol{P}}_{n+3}}+{{\boldsymbol{P}}_{n+2}}. $

联立式（17）~（21），得到由轨迹型值点反求控制点的矩阵方程：

$ \left[ {\begin{array}{*{20}{c}} 1&{}&{}&{}&{}&{}&{}&{}&{} \\ { - 1}&1&{}&{}&{}&{}&{{\boldsymbol{0}}}&{}&{} \\ 1&{ - 2}&1&{}&{}&{}&{}&{}&{} \\ 0&{{a_3}}&{{b_3}}&{{c_3}}&{{d_3}}&{{e_3}}&0& \cdots &0 \\ 0&0&{{a_4}}&{{b_4}}&{{c_4}}&{{d_4}}&{{e_4}}& \cdots &0 \\ \vdots & {\vdots} & \vdots & \vdots & \vdots & \vdots & \vdots & {} & \vdots \\ 0& \cdots &{{a_n}}&{{b_n}}&{{c_n}}&{{d_n}}&{{e_n}}&{}&{} \\ 0& \cdots &0&{{a_{n+1}}}&{{b_{n+1}}}&{{c_{n+1}}}&{{d_{n+1}}}&{{e_{n+1}}}&{} \\ {}&{}&{}&{}&{}&{}&1&{ - 2}&1 \\ {}&{}&{}&{{\boldsymbol{0}}}&{}&{}&{}&{ - 1}&1 \\ {}&{}&{}&{}&{}&{}&{}&{}&1 \end{array}} \right] \times \left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{P}}_0}} \\ {{{\boldsymbol{P}}_1}} \\ {{{\boldsymbol{P}}_2}} \\ {{{\boldsymbol{P}}_3}} \\ {{{\boldsymbol{P}}_4}} \\ \vdots \\ {{{\boldsymbol{P}}_n}} \\ {{{\boldsymbol{P}}_{n+1}}} \\ {{{\boldsymbol{P}}_{n+2}}} \\ {{{\boldsymbol{P}}_{n+3}}} \\ {{{\boldsymbol{P}}_{n+4}}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{Q}}_0}} \\ {{{\boldsymbol{Q}}_1}} \\ {{{\boldsymbol{Q}}_2}} \\ {{{\boldsymbol{Q}}_3}} \\ {{{\boldsymbol{Q}}_4}} \\ \vdots \\ {{{\boldsymbol{Q}}_n}} \\ {{{\boldsymbol{Q}}_{n+1}}} \\ {{{\boldsymbol{Q}}_{n+2}}} \\ {{{\boldsymbol{Q}}_{n+3}}} \\ {{{\boldsymbol{Q}}_{n+4}}} \end{array}} \right]. $

式中：

$ {a_i} = N_{i - 2}^5\left( {{t_{i+3}}} \right),\;{b_i} = N_{i - 1}^5\left( {{t_{i+3}}} \right), $

$ {c_i} = N_i^5\left( {{t_{i+3}}} \right),\;{d_i} = N_{i+1}^5\left( {{t_{i+3}}} \right), $

$ {e_i} = N_{i+2}^5\left( {{t_{i+3}}} \right),\;i = 3,4,5, \cdots ,n+1. $

在通过NURBS曲线构造出光滑连续的曲线后，应在实际应用过程中考虑机器人运动学和动力学约束、工作效率和平稳性等因素，找到每种工况下的最优轨迹来指导机器人的作业^[14].

在人工换刀作业效率低下的情况下，采用换刀机器人代替传统的人工换刀，要保证换刀作业时间最优，得到优化目标 ${S_1}$：

(22) $ {S_1} = \sum\limits_{i = 0}^{D - 1} {\left( {{t_{i+1}} - {t_i}} \right)} . $

式中： $D$为轨迹点数， ${t_i}$为 $i$轨迹点对应的时刻.

考虑到换刀机器人的工作空间狭小、末端承载大、具有微动特性等问题，对换刀机器人运行轨迹的冲击变化与平稳性提出进一步的要求，提出冲击最优目标，包括冲击最大绝对值 ${S_2}$与绝对平均冲击 ${S_3}$：

(23) ${S_2} = \sum\limits_{j = 0}^N {\left( {{\rm{max}}\left| {p_j^{'''}\left( t \right)} \right|} \right)} , $

(24) $ {S_3} = \sum\limits_{j = 1}^N {\sqrt {\int_0^T {} {{\left( {p_j^{'''}\left( t \right)} \right)}^2}{\rm{d}}t/T} } . $

式中： $ N $为机器人关节数目； $p_{j}^{'''}({{t}})$为 $ j$关节的冲击量，即关节变量加速度对时间的导数.

对于多目标轨迹优化的问题，利用加权系数法，将多目标函数转化为单目标函数. 采用单目标优化的方法，达到对多目标函数的求解. 最终的轨迹优化目标函数如下:

(25) $ \left.{\begin{gathered} {\rm{min}}\left( {\sum\limits_{i = 1}^3 {({k_i} {S_i})} } \right); \\ {{\rm{s}}.{\rm{t}}.\;v_{{\rm{min}}}} \leqslant p_j^{'}\left( t \right) \leqslant {v_{{\rm{max}}}}{\text{ }}, \\ {a_{{\rm{min}}}} \leqslant p_j^{''}\left( t \right) \leqslant {a_{{\rm{max}}}}. \\ \end{gathered}}\right\} $

式中： ${k_i}$为第 $i$个优化目标函数 ${S_i}$的加权系数， $p_{j}^{\prime}(t)$与 $p_{ j}^{\prime \prime}(t)$分别为 $j $关节的速度与加速度.

粒子群优化算法(particle swarm optimization, PSO)由于参数较少、寻优效率高，目前被广泛应用于工业领域. 经典PSO中，所有粒子每次都基于个体最优与群体最优进行学习，这可能导致早熟收敛. 为了避免陷入局部最优解，提出改进PSO算法. 在经典PSO算法中引入遗传算法(genetic algorithm, GA)的思想，利用遗传进化技术为粒子群优化算法培育出多样性样本，称为genetic learning PSO (GL-PSO). 不同于传统的PSO-GA并行式混合算法，GL-PSO将遗传算法和粒子群优化算法以级联方式进行融合，利用遗传技术为粒子群优化算法提供优质基因. 根据粒子历史信息为基因进化提供指导，提高了粒子群优化算法的全局搜索能力和搜索效率^[18].

综合轨迹优化目标函数与优化算法，可以得到如图4所示的轨迹优化流程.

1）初始化一群随机粒子，评估粒子群轨迹优化目标函数，寻找个体最优与群体最优，通过迭代找到最优解. 在每一次的迭代中，粒子通过跟踪个体最优与群体最优，更新自己的速度和位置.

假设粒子维度为 $D$, ${{{\boldsymbol{V}}}}_{i}＝ \left[{v}_{i,1},{v}_{i,2},\cdots ,{v}_{i,D}\right]$和 ${{{\boldsymbol{X}}}}_{i}＝ \left[ {x}_{i,1},{x}_{i,2},\cdots ,{x}_{i,D} \right]$ 分别为i粒子的速度和位置( $ i ＝ 1, 2, \cdots , M $，其中 $M$为种群粒子的总数）. 设 ${{{{\boldsymbol{P}}}}_i} = [ {p_{i,1}},{p_{i,2}}, \cdots , {p_{i,D}} ]$表示粒子 $i$的个体最优， ${{{\boldsymbol{G}}}} = \left[ {{g_1},{g_2}, \cdots ,{g_D}} \right]$表示整个粒子群的群体最优. $i$粒子的 ${v_{i,d}}$与 ${x_{i,d}}$的更新方程定义为

(26) $ \left.\begin{gathered} {\text{ }}{v_{i,d}} \leftarrow \omega {v_{i,d}}+{c_1} {r_{1,d}} \left( {{p_{i,d}} - {x_{i,d}}} \right)+ {c_2} {r_{2,d}} \left( {{g_d} - {x_{i,d}}} \right); \\ {x_{i,d}} \leftarrow {x_{i,d}}+{v_{i,d}}. \end{gathered} \right\}$

式中： $ d＝1,2,\cdots ,D $，其中 $D$为粒子的总维度； $\omega $为惯性权重，影响搜索的广度与深度； ${c_1}$和 ${c_2}$为决定 ${{{{\boldsymbol{P}}}}_i}$与 ${{\boldsymbol{G}}}$相对重要性的参数； ${r_{1,d}}$和 ${r_{2,d}}$为在[0,1.0]内均匀选择的随机数.

为了防止 ${{{{\boldsymbol{P}}}}_i}$与 ${ {{{\boldsymbol{G}}}}}$在 ${{{{{\boldsymbol{X}}}}}_i}$异侧引起粒子更新振荡而产生过早收敛的问题，使用 ${{\boldsymbol{E}}_i} = \left[ {{e_{i,1}},{e_{i,2}}, \cdots ,{e_{i,D}}} \right]$代替 ${{{{\boldsymbol{P}}}}_i}$与 ${{{\boldsymbol{G}}}}$：

(27) $ \left.\begin{gathered} {v_{i,d}} \leftarrow {\omega v_{i,d}}+c {r_d} \left( {{e_{i,d}} - {x_{i,d}}} \right) ， \\ {\text{ }}{e_{i,d}} = \frac{{{c_1} {r_{1,d}} {p_{i,d}}+{c_2} {r_{2,d}} {g_d}}}{{{c_1} {r_{1,d}}+{c_2} {r_{2,d}}}} . \\ \end{gathered}\right\} $

2）利用遗传算子构建粒子的样本. 其中，交叉、变异和选择构成了遗传算法的遗传操作.

对于每个粒子 $i$的 ${{{{\boldsymbol{P}}}}_i}$与 ${{{\boldsymbol{G}}}}$执行交叉操作，生成子代 ${{{{\boldsymbol{O}}}}_i} = \left[ {{o_{i,1}},{o_{i,2}}, \cdots ,{o_{i,D}}} \right]$：

(28) $ {o}_{i,d}=\left\{ {\begin{array}{l}{r}_{d} {p}_{i,d}+\left(1-{r}_{d}\right) {g}_{d} ,\;\;f\left({{{\boldsymbol{P}}}}_{i}\right) < f\left({{{\boldsymbol{P}}}}_{{k}_{d}}\right);\\ {{{{P}}}}_{{k}_{d},d},\;\;其他. \end{array}} \right. $

式中： ${r_d}$为[0,1.0]内的随机数； $f\left( {\boldsymbol{x}} \right)$为适应度函数； ${k_d}$为随机粒子的指数， $ {k}_{d}＝1,2,\cdots ,M\text{} $.

对后代 ${{{{\boldsymbol{O}}}}_i}$进行变异操作. 对于每个维度 $d$，生成一个随机数，根据随机数与变异概率重新初始化 ${{{{\boldsymbol{O}}}}_i}$​的这个维度：

(29) $ {o}_{i,d}={\rm{rand}}\left({b}_{d,{\rm{l}}},{b}_{d,{\rm{u}}}\right),{r}_{d} < {p}_{{\rm{m}}}. $

式中： ${p_{\rm{m}}}$为变异概率， ${b_{d,{\rm{l}}}}$和 ${b_{d,{\rm{u}}}}$分别为 $i$粒子 $d$维度的 下、上限.

对交叉与变异创造的后代进行选择：

(30) $ {{{\boldsymbol{E}}}}_{i}\leftarrow \left\{ {\begin{array}{l}{{{\boldsymbol{O}}}}_{i},\;\;f\left({{{\boldsymbol{O}}}}_{i}\right) < f\left({{{\boldsymbol{E}}}}_{i}\right);\\ {{{\boldsymbol{E}}}}_{i}, \;\;其他. \end{array} } \right.$

随机选择多个样本加入竞争，选取具有最佳适应度的样本替换粒子的当前样本，使得粒子产生突变，改变搜索方向，跳出局部最优解.

3）对更新后的粒子群评估个体最优与群体最优，进行收敛性判断. 若满足条件，则跳出循环，程序结束运行.

为了验证提出的轨迹规划方法的有效性，以8自由度换刀机器人为研究对象，根据实际换刀作业的需求，规划如图5所示的期望路径. 对路径进行采样，得到一系列三维空间路径点.

利用提出的逆运动学求解方法，将笛卡尔空间路径点映射到关节空间，得到如表2所示的各关节经过的位置点.

以时间冲击最优为目标函数，利用GL-PSO算法优化各个关节的NURBS轨迹，保证各关节的运动参数满足如表3所示的约束. 各关节的速度 $v$与加速度 $a$约束由换刀机器人的液驱功率、关节惯量、传动机构效率与运动稳定性等因素确定. 为了延长关节的使用寿命，避免换刀机器人在重载换刀过程中的关节力矩突变，冲击约束 $j$综合考虑了各关节的最大驱动力矩、最大运动惯量及自身体抵抗冲击的能力. 确定如表3所示的各关节运动约束值. 表中，v_p、a_p、j_p分别为移动关节的速度、加速度和冲击，v_r、a_r、j_r分别为转动关节的速度、加速度和冲击.

将起点和终点的速度和加速度设置为零，优化后得到如图6、7所示的各关节运动曲线. 图中，各关节经过的关节位置用星号标出，关节位置均在表2各关节的运动范围中，规划出的时间冲击最优轨迹均通过指定关节位置.

在换刀机器人的运动过程中，各关节速度、加速度最大值均满足如表3所示的运动学约束. 8个关节中的移动关节冲击最大绝对值分别为359.84、73.43、338.92 ${\text{mm/}}{{\text{s}}^{\text{3}}}$，转动关节冲击最大绝对值分别为34.78、33.09、29.10、23.64、40.56 $^\circ {\text{/}}{{\text{s}}^{\text{3}}}$，均未超过关节冲击最大值约束，且随时间变化最终为0. 其中冲击峰值较大是因为在确定优化目标的加权系数时给予了时间最优更多的关注，确保所得轨迹在满足运动约束的条件下能够以最短的时间完成作业.

采用GL-PSO轨迹优化算法得到最优时间序列后，利用NURBS曲线规划出给定采样点轨迹的平均规划时间为50.37 ${\text{ms}}$. 利用传统的PSO算法作为对比进行轨迹优化，结果如图8所示. 图中，N为迭代次数，V为轨迹目标最优值. 可以看出，GL-PSO算法的最优寻值能力与收敛速度分别提高了30.34%与52.08%，具有明显的优势.

利用换刀机器人正运动学，将关节空间的轨迹映射到笛卡尔空间，得到如图9所示的三维优化轨迹. 可以看出，利用本文方法对采样点进行轨迹优化后，可以与换刀机器人笛卡尔空间的期望路径具有良好的重合度. 受益于NURBS曲线良好的局部支撑性，采样点数的增加仅对它附近的一段曲线产生影响，保证提出的轨迹规划方法对高精度轨迹跟踪的换刀作业需求具有很好的延展性与适应性.

（1）以8自由度换刀机器人为研究对象，提出基于位姿分离法与关节变量最小策略的逆运动学解析解求解方法. 通过仿真验证了该逆解求解方法的有效性与正确性，运算速度与求解精度满足机器人轨迹规划的要求.

（2）基于5次NURBS曲线基函数构造的轨迹满足位置、速度、加速度与冲击的光滑连续，可以有效地提高换刀机器人运行的平稳性. 仿真结果表明，与多项式插值算法相比，采用NURBS曲线对复杂轨迹进行规划，可以更好地控制轨迹曲线平滑度和曲率连续性.

（3）考虑到换刀机器人运动的时间最优与冲击最优，提出多目标轨迹优化轨模型. 将多目标进行归一化处理，基于GL-PSO算法完成轨迹的时间冲击最优规划. 结果表明，GL-PSO算法在全局搜索能力、求解精度、搜索速度上较传统PSO算法具有更好的表现.