CN108000501A

CN108000501A - 一种用于串联机器人的新型轨迹规划方法

Info

Publication number: CN108000501A
Application number: CN201711173902.5A
Authority: CN
Inventors: 任军; 严子成; 陈智龙; 吴正虎; 苏业环; 王君
Original assignee: Hubei University of Technology
Current assignee: Hubei University of Technology
Priority date: 2017-11-22
Filing date: 2017-11-22
Publication date: 2018-05-08
Anticipated expiration: 2037-11-22
Also published as: CN108000501B

Abstract

本发明提供一种用于串联机器人的新型轨迹规划方法，利用本发明的方法，通过将6自由度开链机器人转变为12自由度闭链机器人，利用虚拟机器人的虚拟关节变量表示机器人的末端位姿，得到关节变量与位置姿态之间的直接关系。采用B样条曲线在笛卡尔空间进行轨迹规划，并对机器人各关节运动轨迹进行间接控制使其满足关节空间的速度、加速度要求。利用遗传算法在满足关节空间和笛卡尔空间的条件约束下求解轨迹规划的时间最优解。该方法的优点是使得规划出的轨迹能够同时满足笛卡尔空间以及关节空间的约束需求，从而达到保证机器人运动精度的条件下，能够减小机器人关节磨损以及机械振动。

Description

一种用于串联机器人的新型轨迹规划方法

技术领域

本发明属于工业机器人领域，具体涉及一种用于串联机器人在笛卡尔空间和关节空间联合轨迹规划的方法。

背景技术

自20世纪60年代以来，串联机器人在越来越多的领域，尤其在汽车制造领域得到了应用，并在生产制造过程中代替人工实现机械加工、焊接、热处理、表面涂覆、打磨抛光、上下料、装配、检测及仓库堆垛等作业，极大的提高了生产效率，和保证制造的产品的一致性。

串联机器人在执行作业时，合理的空间轨迹规划，可以提高机器人的工作效率，减少机械震动以及关节磨损。轨迹规划分两种，一种是在关节空间进行轨迹规划，要求所规划的轨迹函数连续平滑，使得机械臂运动平稳；另一种是在笛卡尔空间进行轨迹规划，要求手部的位姿，速度满足约束要求。将两个坐标空间的轨迹规划联合起来，使得具备两种轨迹规划的优点，将极大的提升串联机器人的生产效率。

发明内容

针对背景技术存在的问题，本发明的目的在于提供一种用于串联机器人在笛卡尔空间和关节空间联合轨迹规划的方法。

为了达到上述目的，本发明所采用的技术方案是：一种用于串联机器人的新型轨迹规划方法，其特征在于，包括以下步骤：

步骤1、在串联机器人机械臂末端构造虚拟机器人，利用虚拟机器人的空间位姿变化来表示机器人的空间位姿变化，虚拟机器人的基坐标系与串联机器人的基坐标系重合，将开链串联机器人构造成闭链串联机器人，利用螺旋理论以及 POE公式，得到其刚体运动公式为其中虚拟关节的关节变量为θ₁，θ₂，θ₃，...θ_n，表示位于关节轴线的运动螺旋，g(0)表示刚体的初始位形，g(a)表示最终位形；

步骤2、在虚拟关节中利用3次非有理B样条曲线进行关键点之间的位姿轨迹插补，3次非有理B样条曲线定义为0≤u≤1，这里{P_i}是控制点，{N_i,p(u)}是定义在非周期的节点矢量U＝{0,0,0,0,u₄,u₅,u₆…u_q+2,1,1,1,1}上的3 次B样条基函数；同时利用公式可以求得对应的B样条曲线矢量节点，在已知控制点和节点矢量便可求出对应的B样条曲线以及控制点，通过映射关系可得到笛卡尔空间B样条曲线的控制顶点di，同时利用德布尔递推公式可以求出di¹，di²，它们分别为笛卡尔空间B样条曲线的一阶、二阶的控制顶点，通过控制di，di¹，di²约束笛卡尔空间的运动轨迹；

步骤3、由雅克比矩阵可得，笛卡尔空间速度与关节空间关节速度的映射关系，由B样条曲线的仿射不变性可以得到关节空间速度曲线的控制顶点di′，经变化后的di′在u从0增加到1的过程中形成新的B样条曲线记为d(u)′，由B样条曲线的强凸性可知，关节空间的运动速度曲线要在d(u)′的取值范围内，同时有曲线的控制顶点小于d(u)′的控制顶点d(u)′_l的最大值且大于d(u)′_l的最小值，联立可得曲线满足且其中i＝1,2…n,l＝1,2…n；0≤u≤1，同理可求得关节空间加速器约束条件；

步骤4、联合上述约束求机器人运动轨迹的最优解，约束条件可以简化为式中，l_e,v_e,a_e分别为笛卡尔空间的路径约束，速度与加速度约束，v¹ _e,a¹ _e分别为关节空间的速度约束与加速度约束，利用遗传算法可求得对应的节点矢量，解除操作空间末端执行器的运动曲线，再根据映射关系求出相应的关节运动曲线，完成轨迹的联合规划。

进一步地，所述步骤1中，对于闭链12关节机器人，其具体实现方法为：

对于闭链12关节机器人，运动学可表示为：

由于虚拟机器人的基座标与实体机器人的基座标重合，则g(a)＝g(0)，式(2) 可化为：

已知机器人的空间位姿，即可得到各虚拟关节的关节变量θ₁，θ₂，θ₃，…θ₆的值，带入式(3)便可得到类似开链6关节机器人的运动表达式，利用开链6 关节机器人的运动学求解方法，便可得到相应的关节序列。

进一步地，所述步骤2中，具体实现过程包括：

控制点可由曲线经过的关键点和节点矢量求出，故在关键点已知的情况下，B样条曲线可由节点矢量来进行控制，随着u从0增长到1，各关节从起点位置运动到终止位置，设开链6关节的运动曲线组成的6维空间B样条曲线记为G (A，u)，6维虚拟关节运动曲线记为H(B，u)有：

设di为H的控制顶点，di¹，di²为曲线H的一阶、二阶曲线的控制顶点，利用B样条曲线的凸包性，通过控制di，di¹，di²可满足笛卡尔空间的轨迹约束，

式中，l_e,v_e,a_e分别为笛卡尔空间的路径约束，速度与加速度约束，di¹,di²可由德布尔递推公式求出：

其中，j＝i-3+l，...i，l＝1,2,...c。

进一步地，所述步骤3中，具体实现过程包括：

设笛卡尔空间速度与关节空间速度的映射关系为g，有：

已知笛卡尔空间速度与关节空间速度之间的关系可由雅克比矩阵表示：

由式可知笛卡尔空间速度与关节空间的速度为线性关系，即g为线性映射；由于实体机器人的每组关节变量总会对应一组相应的位姿，故f为到上的映射，即为满射，且易知f为连续映射，利用B样条的仿射不变性和强凸包性结合映射关系f和g在关节空间进行轨迹规划可间接控制机器人在关节空间的运动轨迹，

由B样条曲线的仿射不变性可知，当经过变换J(B)-¹变为时，对其原曲线的控制点di进行该仿射变换，即可得到变换后的控制点di′，即：

式中，i为控制点的个数，当u从0增加到1时，A(u)形成的曲线为空间6 维B样条曲线，由于原控制点不变，雅克比逆变换为线性变换，故，变换后的控制点di′在u从0到1的过程中形成的曲线为空间6维B样条曲线经仿射变换后形成的新B样条曲线，记为di(u)′，

虽然曲线是由上的每个点经过不同的仿射变换组合而成的曲线，但由B 样条曲线的强凸包性可知，曲线的最大值总在由每个最大新控制点所形成的控制多边形内，有：

对于B样条曲线di(u)′，由B样条曲线的强凸包性可知，di(u)′的最大值在各个控制顶点所包含的控制多边形内，设di(u)′的控制顶点为di(u)′_l，有：

Pi(u)¹≤Max(di(u)′_l) (11)

同理，对于曲线G的最小值有：

Pi(u)¹≥Min(di(u)′_l) (12)

联立式(10)与式(11)可得：

同理，对于曲线G的最小值有：

其中，i＝1,2…n,l＝1,2…n；0≤u≤1；

设笛卡尔空间的加速度与关节空间的加速度曲线分别为和式两边分别对时间求导可得：

令：

关节空间的加速度曲线可由曲线Q₁(u)与Q₂(u)合成得到：

由B样条曲线的连续可微性可知，同为B样条曲线，由式(10)、 (11)、(12)可得到相类似的结果：

Q₁(B,u)≤Max(Q₁di(u)′_l) (19)

Q₂(B,u)≤Max(Q₂di(u)′_l) (20)

Q₁(B,u)≥Min(Q₁di(u)′_l) (21)

Q₂(B,u)≥Min(Q₂di(u)′_l) (22)

联立式，可得：

同理可得：

设关节空间的速度约束与加速度约束分别为v¹ _e,a¹ _e，联立式可对关节空间的速度及加速度进行间接控制，有：

其中，i＝1,2…n,l＝1,2…n；0≤u≤1。

进一步地，所述步骤4中，具体实现过程包括：

轨迹联合规划的时间最优问题的求解，其实质就是在满足关节空间以及笛卡尔空间约束条件的情况下求解与归一化时间节点u对应的总时间最小的时间序列，利用遗传算法求解其最优解，由于该问题为带有约束问题的优化问题，一般采用罚函数的方法来处理，代价函数可写为：

联合轨迹规划需考虑机器人的避障问题，同时使操作空间末端执行器的速度、加速度以及关节空间的关节速度、加速度满足要求，故该问题的约束条件可简化为：

式中，x_i＝△t_i＝t_i+1-t_i，i＝1,2…n,l＝1,2…n，r_g为惩罚系数，φ(x_i)为罚函数，大小为：

采用遗传算法可求解出f(x)的最小值，并得到相对应的时间序列，由式可得到相对应的时间节点矢量，联立式根据可求解出操作空间末端执行器的运动曲线，由映射关系即可求出相对应的关节运动曲线，完成轨迹的联合规划。

与现有技术相比，本发明的有益效果是：本发明提出的一种用于串联机器人的新型轨迹规划方法，研究表明：

(1)在机器人的笛卡尔空间内进行轨迹规划，利用B样条曲线的仿射不变性以及强凸包性，间接控制机器人的关节运动轨迹，实现机器人关节空间与笛卡尔空间的联合规划，使机器人运动轨迹同时满足与笛卡尔空间以及关节空间的约束需求。

(2)该方法的优点是在机器人运动平稳的条件下，减小机器人关节磨损以及机械振动，在UR5机器人上进行实验，结果表明该方法是有效的，可行的。

附图说明

图1为虚拟关节坐标系图；

图2为UR5机械手实验平台；

图3为机器人末端执行器位置；

图4为机器人末端执行器欧拉角；

图5为机器人末端执行器速度曲线；

图6为机器人末端执行器角速度曲线；

图7为机器人末端执行器加速度曲线；

图8为机器人末端执行器角加速度曲线；

图9为机器人关节位置；

图10为机器人关节速度曲线；

图11为机器人关节加速度曲线。

具体实施方式

为了便于本领域普通技术人员理解和实施本发明，下面结合实施例对本发明作进一步的详细减述，应当理解，此处所描述的实施示例仅用于说明和解释本发明，并不用于限定本发明。

本发明涉及一种用于串联机器人在笛卡尔空间和关节空间联合轨迹规划方法，旨在机器人运动平稳的条件下，减小机器人关节磨损以及机械振动。具体实现过程为：

步骤1，为得到机器人各关节变量与机器人空间位姿的映射关系，选取6自由度串联机器人中较为典型的UR5机械臂为例，在机械臂末端处构造虚拟机器人，利用虚拟机器人的虚拟关节变量来表示机器人的空间位姿变化，虚拟机器人的结构模型如图1所示。

如图1所示，虚拟机器人的基座标系与原机器人的基座标系重合，虚拟关节 1，2，3为移动关节，虚拟关节4，5，6为旋转关节，虚拟关节1，2，3的关节轴线方向分别沿基座标系x，y，z方向，其连杆长度确定的活动空间包含原机器人的工作空间；虚拟关节4，5，6其轴线方向分别沿基座标系x，y，z方向，连杆长度设定为0。

可以看出，通过控制虚拟关节1，2，3的关节变量可以控制连杆的末端位置，由于连杆的长度为零，则连杆的末端位置与焊枪的末端位置一致，同理虚拟关节 4，5，6的关节变量可以控制机器人末端的姿态。故机器人的末端位姿可利用虚拟关节的关节变量来表示。

关节运动学求解的机器人为闭链12关节机器人，由于关节较多且部分连杆长度为零，利用传统的D-H参数法较为复杂，故本文采用螺旋理论以及POE公式来进行机器人的运动学反解。

根据旋量理论，机器人各关节的运动由位于关节轴线的运动螺旋产生，由此可得运动学的几何描述。若用g(0)表示刚体的初始位形，用g(a)表示最终位形，则该运动螺旋描述的刚体运动可表示为：

则对于上述建立的闭链12关节机器人，运动学可表示为：

已知机器人的空间位姿，即可得到各虚拟关节的关节变量θ₁，θ₂，θ₃，θ₄，θ₅，θ₆的值，带入式便可得到类似开链6关节机器人的运动表达式，利用开链6关节机器人的运动学求解方法，便可得到相应的关节序列。

步骤2，设B和A为两个集合：B＝(θ₁,θ₂,θ₃,θ₄,θ₅,θ₆)∈R⁶为各虚拟关节关节变量的组合，A＝(θ₇,θ₈,θ₉,θ₁₀,θ₁₁,θ₁₂)∈R⁶为真实关节关节变量的组合，由式(3)得到B 到A的映射关系f，有：

f:B→A,f(B)＝A

采用3次非有理B样条曲线进行关键点之间的位姿轨迹插补，可实现机器人笛卡尔空间的速度，加速度连续，3次非有理B样条曲线的定义为：

这里{P_i}是控制点，{N_i,p(u)}是定义在非周期的节点矢量 U＝{0,0,0,0,u₄,u₅,u₆…u_q+2,1,1,1,1}上的3次B样条基函数。已累计时长的方法对t_i归一化，则求出对应的B样条曲线的节点矢量：

由式可知已知控制点和节点矢量便可求出对应的B样条曲线，而控制点可由曲线经过的关键点和节点矢量求出，故在关键点已知的情况下，B样条曲线可由节点矢量来进行控制。随着u从0增长到1，各关节从起点位置运动到终止位置，设开链6关节的运动曲线组成的6维空间B样条曲线记为G(A，u)，6维虚拟关节运动曲线记为H(B，u)有：

设di为H的控制顶点，di¹，di²为曲线H的一阶、二阶曲线的控制顶点，利用B样条曲线的凸包性，通过控制di，di¹，di²可满足笛卡尔空间的轨迹约束。

其中，j＝i-3+l，...i，l＝1,2,...c；

步骤3，设笛卡尔空间速度与关节空间速度的映射关系为g，有：

由式可知笛卡尔空间速度与关节空间的速度为线性关系，即g为线性映射；由于实体机器人的每组关节变量总会对应一组相应的位姿，故f为到上的映射，即为满射，且易知f为连续映射，利用B样条的仿射不变性和强凸包性结合映射关系f和g在关节空间进行轨迹规划可间接控制机器人在关节空间的运动轨迹。

由B样条曲线的仿射不变性可知，当经过变换J(B)^-1变为时，对其原曲线的控制点di进行该仿射变换，即可得到变换后的控制点di′，即：

式中，i为控制点的个数。当u从0增加到1时，A(u)形成的曲线为空间6 维B样条曲线，由于原控制点不变，雅克比逆变换为线性变换，故不难发现，变换后的控制点di′在u从0到1的过程中形成的曲线为空间6维B样条曲线经仿射变换后形成的新B样条曲线，记为di(u)′。

对于B样条曲线di(u)′，由B样条曲线的强凸包性可知，di(u)′的最大值在各个控制顶点所包含的控制多边形内。设di(u)′的控制顶点为di(u)′_l，有：

Pi(u)¹≤Max(di(u)′_l) (11)

同理，对于曲线G的最小值有：

Pi(u)¹≥Min(di(u)′_l) (12)

联立式(10)与式(11)可得：

同理，对于曲线G的最小值有：

其中，i＝1,2…n,l＝1,2…n；0≤u≤1。

令：

关节空间的加速度曲线可由曲线Q₁(u)与Q₂(u)合成得到：

Q₁(B,u)≤Max(Q₁di(u)′_l) (19)

Q₂(B,u)≤Max(Q₂di(u)′_l) (20)

Q₁(B,u)≥Min(Q₁di(u)′_l) (21)

Q₂(B,u)≥Min(Q₂di(u)′_l) (22)

联立式，可得：

同理可得：

其中，i＝1,2…n,l＝1,2…n；0≤u≤1。

步骤4，轨迹联合规划的时间最优问题的求解，其实质就是在满足关节空间以及笛卡尔空间约束条件的情况下求解与归一化时间节点u对应的总时间最小的时间序列。利用遗传算法求解其最优解，由于该问题为带有约束问题的优化问题，一般采用罚函数的方法来处理，代价函数可写为：

以如图2所示UR5机械手为平台进行实验。选取一系列空间轨迹的关键点，相应的位姿序列如下：

表1机械手位姿

设与表1中6个关键点对应的时间序列为T＝(t₁,t₂,t₃,t₄,t₅,t₆)，利用式可求出对应的时间节点序列进而求解出运动曲线的各个控制点。机器人的关节空间运动学约束为：

表2关节空间约束

笛卡尔空间运动学约束为：

表3笛卡尔空间约束

遗传算法的原理在这里便不再赘述，设置群体M＝40，终止代数T＝300，选择算子为锦标赛法，循环算子采用循环交叉，P_C＝0.8，变异算子取值为0.02，惩罚系数取值为50，利用MATLAB软件编写程序，求出最优解为[5.9375，6.5830， 8.0174，7.0800，6.5586]，总时间T＝34.1765s。

已知时间序列，由式(5)可得B样条曲线的时间节点矢量，结合位置序列pi，便可绘制出串联机器人末端在笛卡尔空间的运行轨迹曲线如图3～4，速度曲线如图5～6、加速度曲线如图7～8所示。图3～6中，曲线平滑连续，且都在机器人姿态约束范围之内，满足所设定的条件；图7～8中，曲线连续但受限于B样条曲线的阶次，成折线的形式，但曲线都在机器人位置约束的范围之内，同样满足所设定的条件。

同时由笛卡尔空间到关节空间的映射关系f，可以求得串联机器人在关节空间中的关节位置、关节速度曲线、关节加速度曲线，如图9～11。图9～10中，关节运动曲线平滑连续，且都在机器人关节运动范围之内，满足所设定的条件；图 11中，关节加速度曲线连续但是受限与B样条曲线的阶次，成折线形式，但曲线都在机器人关节运动约束的范围内，同样满足所设定的条件。

利用本发明的方法，通过将6自由度开链机器人转变为12自由度闭链机器人，利用虚拟机器人的虚拟关节变量表示机器人的末端位姿，得到关节变量与位置姿态之间的直接关系。采用B样条曲线在笛卡尔空间进行轨迹规划，并对机器人各关节运动轨迹进行间接控制使其满足关节空间的速度、加速度要求。利用遗传算法在满足关节空间和笛卡尔空间的条件约束下求解轨迹规划的时间最优解。该方法的优点是使得规划出的轨迹能够同时满足笛卡尔空间以及关节空间的约束需求，从而达到保证机器人运动精度的条件下，能够减小机器人关节磨损以及机械振动。

应当理解的是，本说明书未详细阐述的部分均属于现有技术。

应当理解的是，上述针对较佳实施例的描述较为详细，并不能因此而认为是对本发明专利保护范围的限制，本领域的普通技术人员在本发明的启示下，在不脱离本发明权利要求所保护的范围情况下，还可以做出替换或变形，均落入本发明的保护范围之内，本发明的请求保护范围应以所附权利要求为准。

Claims

1.一种用于串联机器人的新型轨迹规划方法，其特征在于，包括以下步骤：

步骤1、在串联机器人机械臂末端构造虚拟机器人，利用虚拟机器人的空间位姿变化来表示机器人的空间位姿变化，虚拟机器人的基坐标系与串联机器人的基坐标系重合，将开链串联机器人构造成闭链串联机器人，利用螺旋理论以及POE公式，得到其刚体运动公式为其中虚拟关节的关节变量为θ₁，θ₂，θ₃，...θ_n，表示位于关节轴线的运动螺旋，g(0)表示刚体的初始位形，g(a)表示最终位形；

步骤2、在虚拟关节中利用3次非有理B样条曲线进行关键点之间的位姿轨迹插补，3次非有理B样条曲线定义为0≤u≤1，这里{P_i}是控制点，{N_i,p(u)}是定义在非周期的节点矢量U＝{0,0,0,0,u₄,u₅,u₆…u_q+2,1,1,1,1}上的3次B样条基函数；同时利用公式可以求得对应的B样条曲线矢量节点，在已知控制点和节点矢量便可求出对应的B样条曲线以及控制点，通过映射关系可得到笛卡尔空间B样条曲线的控制顶点di，同时利用德布尔递推公式可以求出di¹，di²，它们分别为笛卡尔空间B样条曲线的一阶、二阶的控制顶点，通过控制di，di¹，di²约束笛卡尔空间的运动轨迹；

步骤3、由雅克比矩阵可得，笛卡尔空间速度与关节空间关节速度的映射关系，由B样条曲线的仿射不变性可以得到关节空间速度曲线的控制顶点di′，经变化后的di′在u从0增加到1的过程中形成新的B样条曲线记为d(u)′，由B样条曲线的强凸性可知，关节空间的运动速度曲线要在d(u)′的取值范围内，同时有曲线的控制顶点小于d(u)′的控制顶点d(u)_l′的最大值且大于d(u)_l′的最小值，联立可得曲线满足且其中i＝1,2…n,l＝1,2…n；0≤u≤1，同理可求得关节空间加速器约束条件；

2.如权利要求1所述的用于串联机器人的新型轨迹规划方法，其特征在于，所述步骤1中，对于闭链12关节机器人，其具体实现方法为：

对于闭链12关节机器人，运动学可表示为：

<mrow> <mi>g</mi> <mrow> <mo>(</mo> <mi>a</mi> <mo>)</mo> </mrow> <mo>=</mo> <msup> <mi>e</mi> <mrow> <msub> <mover> <mi>&xi;</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> <msub> <mi>&theta;</mi> <mn>1</mn> </msub> </mrow> </msup> <msup> <mi>e</mi> <mrow> <msub> <mover> <mi>&xi;</mi> <mo>^</mo> </mover> <mn>2</mn> </msub> <msub> <mi>&theta;</mi> <mn>2</mn> </msub> </mrow> </msup> <msup> <mi>e</mi> <mrow> <msub> <mover> <mi>&xi;</mi> <mo>^</mo> </mover> <mn>3</mn> </msub> <msub> <mi>&theta;</mi> <mn>3</mn> </msub> </mrow> </msup> <mo>...</mo> <msup> <mi>e</mi> <mrow> <msub> <mover> <mi>&xi;</mi> <mo>^</mo> </mover> <mn>12</mn> </msub> <msub> <mi>&theta;</mi> <mn>12</mn> </msub> </mrow> </msup> <mi>g</mi> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow>

由于虚拟机器人的基座标与实体机器人的基座标重合，则g(a)＝g(0)，式(1)可化为：

<mrow> <mi>E</mi> <mo>=</mo> <msup> <mi>e</mi> <mrow> <msub> <mover> <mi>&xi;</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> <msub> <mi>&theta;</mi> <mn>1</mn> </msub> </mrow> </msup> <msup> <mi>e</mi> <mrow> <msub> <mover> <mi>&xi;</mi> <mo>^</mo> </mover> <mn>2</mn> </msub> <msub> <mi>&theta;</mi> <mn>2</mn> </msub> </mrow> </msup> <msup> <mi>e</mi> <mrow> <msub> <mover> <mi>&xi;</mi> <mo>^</mo> </mover> <mn>3</mn> </msub> <msub> <mi>&theta;</mi> <mn>3</mn> </msub> </mrow> </msup> <mo>...</mo> <msup> <mi>e</mi> <mrow> <msub> <mover> <mi>&xi;</mi> <mo>^</mo> </mover> <mn>12</mn> </msub> <msub> <mi>&theta;</mi> <mn>12</mn> </msub> </mrow> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow>

已知机器人的空间位姿，即可得到各虚拟关节的关节变量θ₁，θ₂，θ₃，…θ₆的值，带入式(3)便可得到类似开链6关节机器人的运动表达式，利用开链6关节机器人的运动学求解方法，便可得到相应的关节序列。

3.如权利要求1所述的用于串联机器人的新型轨迹规划方法，其特征在于，所述步骤2中，具体实现过程包括：

控制点可由曲线经过的关键点和节点矢量求出，故在关键点已知的情况下，B样条曲线可由节点矢量来进行控制，随着u从0增长到1，各关节从起点位置运动到终止位置，设开链6关节的运动曲线组成的6维空间B样条曲线记为G(A，u)，6维虚拟关节运动曲线记为H(B，u)有：

<mrow> <mi>H</mi> <mrow> <mo>(</mo> <mi>B</mi> <mo>,</mo> <mi>u</mi> <mo>)</mo> </mrow> <mover> <mo>&RightArrow;</mo> <mi>f</mi> </mover> <mi>G</mi> <mrow> <mo>(</mo> <mi>A</mi> <mo>,</mo> <mi>u</mi> <mo>)</mo> </mrow> </mrow>

<mrow> <msup> <mi>p</mi> <mrow> <mo>(</mo> <mi>c</mi> <mo>)</mo> </mrow> </msup> <mrow> <mo>(</mo> <mi>u</mi> <mo>)</mo> </mrow> <mo>=</mo> <munderover> <mo>&Sigma;</mo> <mrow> <mi>j</mi> <mo>=</mo> <mi>i</mi> <mo>-</mo> <mn>3</mn> <mo>+</mo> <mi>c</mi> </mrow> <mi>i</mi> </munderover> <msubsup> <mi>d</mi> <mi>j</mi> <mrow> <mo>(</mo> <mi>c</mi> <mo>)</mo> </mrow> </msubsup> <msub> <mi>N</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mi>c</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>u</mi> <mo>)</mo> </mrow> <mo>,</mo> <mi>c</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>,</mo> <mn>3</mn> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> </mrow>

其中，j＝i-3+l，...i，l＝1,2,...c。

4.如权利要求1所述的用于串联机器人的新型轨迹规划方法，其特征在于，所述步骤3中，具体实现过程包括：

设笛卡尔空间速度与关节空间速度的映射关系为g，有：

<mrow> <mi>g</mi> <mo>:</mo> <mover> <mi>B</mi> <mo>&CenterDot;</mo> </mover> <mo>&RightArrow;</mo> <mover> <mi>A</mi> <mo>&CenterDot;</mo> </mover> <mo>,</mo> <mi>g</mi> <mrow> <mo>(</mo> <mover> <mi>B</mi> <mo>&CenterDot;</mo> </mover> <mo>)</mo> </mrow> <mo>=</mo> <mover> <mi>A</mi> <mo>&CenterDot;</mo> </mover> </mrow>

<mrow> <mover> <mi>B</mi> <mo>&CenterDot;</mo> </mover> <mo>=</mo> <mi>J</mi> <mrow> <mo>(</mo> <mi>A</mi> <mo>)</mo> </mrow> <mover> <mi>A</mi> <mo>&CenterDot;</mo> </mover> <mo>,</mo> <mover> <mi>A</mi> <mo>&CenterDot;</mo> </mover> <mo>=</mo> <mi>J</mi> <msup> <mrow> <mo>(</mo> <mi>A</mi> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mover> <mi>B</mi> <mo>&CenterDot;</mo> </mover> </mrow>

<mrow> <mover> <mi>H</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>B</mi> <mo>,</mo> <mi>u</mi> <mo>)</mo> </mrow> <mover> <mo>&RightArrow;</mo> <mi>g</mi> </mover> <mover> <mi>G</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>A</mi> <mo>,</mo> <mi>u</mi> <mo>)</mo> </mrow> </mrow>

式中，i为控制点的个数，当u从0增加到1时，A(u)形成的曲线为空间6维B样条曲线，由于原控制点不变，雅克比逆变换为线性变换，故，变换后的控制点di′在u从0到1的过程中形成的曲线为空间6维B样条曲线经仿射变换后形成的新B样条曲线，记为di(u)′，

虽然曲线是由上的每个点经过不同的仿射变换组合而成的曲线，但由B样条曲线的强凸包性可知，曲线的最大值总在由每个最大新控制点所形成的控制多边形内，有：

<mrow> <mover> <mi>G</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>B</mi> <mo>,</mo> <mi>u</mi> <mo>)</mo> </mrow> <mo>&le;</mo> <mi>M</mi> <mi>a</mi> <mi>x</mi> <mrow> <mo>(</mo> <mi>d</mi> <mi>i</mi> <msup> <mrow> <mo>(</mo> <mi>u</mi> <mo>)</mo> </mrow> <mo>&prime;</mo> </msup> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>7</mn> <mo>)</mo> </mrow> </mrow>

对于B样条曲线di(u)′，由B样条曲线的强凸包性可知，di(u)′的最大值在各个控制顶点所包含的控制多边形内，设di(u)′的控制顶点为di(u)_l′，有：

Pi(u)¹≤Max(di(u)_l′) (8)

同理，对于曲线G的最小值有：

Pi(u)¹≥Min(di(u)_l′) (9)

联立式(7)与式(8)可得：

<mrow> <mover> <mi>G</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>B</mi> <mo>,</mo> <mi>u</mi> <mo>)</mo> </mrow> <mo>&le;</mo> <mi>M</mi> <mi>a</mi> <mi>x</mi> <mrow> <mo>(</mo> <mi>d</mi> <mi>i</mi> <msubsup> <mrow> <mo>(</mo> <mi>u</mi> <mo>)</mo> </mrow> <mi>l</mi> <mo>&prime;</mo> </msubsup> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>10</mn> <mo>)</mo> </mrow> </mrow>

同理，对于曲线G的最小值有：

<mrow> <mover> <mi>G</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>B</mi> <mo>,</mo> <mi>u</mi> <mo>)</mo> </mrow> <mo>&GreaterEqual;</mo> <mi>M</mi> <mi>i</mi> <mi>n</mi> <mrow> <mo>(</mo> <mi>d</mi> <mi>i</mi> <msubsup> <mrow> <mo>(</mo> <mi>u</mi> <mo>)</mo> </mrow> <mi>l</mi> <mo>&prime;</mo> </msubsup> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>11</mn> <mo>)</mo> </mrow> </mrow>

其中，i＝1,2…n,l＝1,2…n；0≤u≤1；

令：

关节空间的加速度曲线可由曲线Q₁(u)与Q₂(u)合成得到：

<mrow> <mover> <mi>G</mi> <mo>&CenterDot;&CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>u</mi> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>Q</mi> <mn>1</mn> </msub> <mrow> <mo>(</mo> <mi>u</mi> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>Q</mi> <mn>2</mn> </msub> <mrow> <mo>(</mo> <mi>u</mi> <mo>)</mo> </mrow> <mo>;</mo> <mn>0</mn> <mo>&le;</mo> <mi>u</mi> <mo>&le;</mo> <mn>1</mn> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>15</mn> <mo>)</mo> </mrow> </mrow>

由B样条曲线的连续可微性可知，同为B样条曲线，由式(7)、(8)、(9)可得到相类似的结果：

Q₁(B,u)≤Max(Q₁di(u)_l′) (16)

Q₂(B,u)≤Max(Q₂di(u)_l′) (17)

Q₁(B,u)≥Min(Q₁di(u)_l′) (18)

Q₂(B,u)≥Min(Q₂di(u)_l′) (19)

联立式，可得：

<mrow> <mover> <mi>G</mi> <mo>&CenterDot;&CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>B</mi> <mo>,</mo> <mi>u</mi> <mo>)</mo> </mrow> <mo>&le;</mo> <mi>M</mi> <mi>a</mi> <mi>x</mi> <mrow> <mo>(</mo> <msub> <mi>Q</mi> <mn>1</mn> </msub> <mi>d</mi> <mi>i</mi> <msubsup> <mrow> <mo>(</mo> <mi>u</mi> <mo>)</mo> </mrow> <mi>l</mi> <mo>&prime;</mo> </msubsup> <mo>)</mo> </mrow> <mo>+</mo> <mi>M</mi> <mi>a</mi> <mi>x</mi> <mrow> <mo>(</mo> <msub> <mi>Q</mi> <mn>2</mn> </msub> <mi>d</mi> <mi>i</mi> <msubsup> <mrow> <mo>(</mo> <mi>u</mi> <mo>)</mo> </mrow> <mi>l</mi> <mo>&prime;</mo> </msubsup> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>20</mn> <mo>)</mo> </mrow> </mrow>

同理可得：

<mrow> <mover> <mi>G</mi> <mo>&CenterDot;&CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>B</mi> <mo>,</mo> <mi>u</mi> <mo>)</mo> </mrow> <mo>&GreaterEqual;</mo> <mi>M</mi> <mi>i</mi> <mi>n</mi> <mrow> <mo>(</mo> <msub> <mi>Q</mi> <mn>1</mn> </msub> <mi>d</mi> <mi>i</mi> <msubsup> <mrow> <mo>(</mo> <mi>u</mi> <mo>)</mo> </mrow> <mi>l</mi> <mo>&prime;</mo> </msubsup> <mo>)</mo> </mrow> <mo>+</mo> <mi>M</mi> <mi>i</mi> <mi>n</mi> <mrow> <mo>(</mo> <msub> <mi>Q</mi> <mn>2</mn> </msub> <mi>d</mi> <mi>i</mi> <msubsup> <mrow> <mo>(</mo> <mi>u</mi> <mo>)</mo> </mrow> <mi>l</mi> <mo>&prime;</mo> </msubsup> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>21</mn> <mo>)</mo> </mrow> </mrow>

<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>M</mi> <mi>a</mi> <mi>x</mi> <mrow> <mo>(</mo> <mi>d</mi> <mi>i</mi> <msubsup> <mrow> <mo>(</mo> <mi>u</mi> <mo>)</mo> </mrow> <mi>l</mi> <mo>&prime;</mo> </msubsup> <mo>)</mo> </mrow> <mo>&le;</mo> <msub> <msup> <mi>v</mi> <mn>1</mn> </msup> <mi>e</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>M</mi> <mi>a</mi> <mi>x</mi> <mrow> <mo>(</mo> <msub> <mi>Q</mi> <mn>1</mn> </msub> <mi>d</mi> <mi>i</mi> <msubsup> <mrow> <mo>(</mo> <mi>u</mi> <mo>)</mo> </mrow> <mi>l</mi> <mo>&prime;</mo> </msubsup> <mo>)</mo> </mrow> <mo>+</mo> <mi>M</mi> <mi>a</mi> <mi>x</mi> <mrow> <mo>(</mo> <msub> <mi>Q</mi> <mn>2</mn> </msub> <mi>d</mi> <mi>i</mi> <msubsup> <mrow> <mo>(</mo> <mi>u</mi> <mo>)</mo> </mrow> <mi>l</mi> <mo>&prime;</mo> </msubsup> <mo>)</mo> </mrow> <mo>&le;</mo> <msub> <msup> <mi>a</mi> <mn>1</mn> </msup> <mi>e</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>22</mn> <mo>)</mo> </mrow> </mrow>

其中，i＝1,2…n,l＝1,2…n；0≤u≤1。

5.如权利要求1所述的用于串联机器人的新型轨迹规划方法，其特征在于，所述步骤4中，具体实现过程包括：

<mrow> <mi>f</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>m</mi> <mi>i</mi> <mi>n</mi> <munderover> <mo>&Sigma;</mo> <mn>0</mn> <mrow> <mi>n</mi> <mo>-</mo> <mn>1</mn> </mrow> </munderover> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>+</mo> <msub> <mi>r</mi> <mi>g</mi> </msub> <mi>&phi;</mi> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>23</mn> <mo>)</mo> </mrow> </mrow>

<mrow> <mi>s</mi> <mo>.</mo> <mi>t</mi> <mo>.</mo> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>M</mi> <mi>a</mi> <mi>x</mi> <mrow> <mo>(</mo> <mrow> <mi>d</mi> <mi>i</mi> <msubsup> <mrow> <mo>(</mo> <mi>u</mi> <mo>)</mo> </mrow> <mi>l</mi> <mo>&prime;</mo> </msubsup> </mrow> <mo>)</mo> </mrow> <mo>&le;</mo> <msubsup> <mi>v</mi> <mi>e</mi> <mn>1</mn> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>M</mi> <mi>i</mi> <mi>n</mi> <mrow> <mo>(</mo> <mrow> <mi>d</mi> <mi>i</mi> <msubsup> <mrow> <mo>(</mo> <mi>u</mi> <mo>)</mo> </mrow> <mi>l</mi> <mo>&prime;</mo> </msubsup> </mrow> <mo>)</mo> </mrow> <mo>&GreaterEqual;</mo> <mo>-</mo> <msubsup> <mi>v</mi> <mi>e</mi> <mn>1</mn> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>M</mi> <mi>a</mi> <mi>x</mi> <mrow> <mo>(</mo> <mrow> <msub> <mi>Q</mi> <mn>1</mn> </msub> <mi>d</mi> <mi>i</mi> <msubsup> <mrow> <mo>(</mo> <mi>u</mi> <mo>)</mo> </mrow> <mi>l</mi> <mo>&prime;</mo> </msubsup> </mrow> <mo>)</mo> </mrow> <mo>+</mo> <mi>M</mi> <mi>a</mi> <mi>x</mi> <mrow> <mo>(</mo> <mrow> <msub> <mi>Q</mi> <mn>2</mn> </msub> <mi>d</mi> <mi>i</mi> <msubsup> <mrow> <mo>(</mo> <mi>u</mi> <mo>)</mo> </mrow> <mi>l</mi> <mo>&prime;</mo> </msubsup> </mrow> <mo>)</mo> </mrow> <mo>&le;</mo> <msubsup> <mi>a</mi> <mi>e</mi> <mn>1</mn> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>M</mi> <mi>i</mi> <mi>n</mi> <mrow> <mo>(</mo> <mrow> <msub> <mi>Q</mi> <mn>1</mn> </msub> <mi>d</mi> <mi>i</mi> <msubsup> <mrow> <mo>(</mo> <mi>u</mi> <mo>)</mo> </mrow> <mi>l</mi> <mo>&prime;</mo> </msubsup> </mrow> <mo>)</mo> </mrow> <mo>+</mo> <mi>M</mi> <mi>i</mi> <mi>n</mi> <mrow> <mo>(</mo> <mrow> <msub> <mi>Q</mi> <mn>2</mn> </msub> <mi>d</mi> <mi>i</mi> <msubsup> <mrow> <mo>(</mo> <mi>u</mi> <mo>)</mo> </mrow> <mi>l</mi> <mo>&prime;</mo> </msubsup> </mrow> <mo>)</mo> </mrow> <mo>&GreaterEqual;</mo> <mo>-</mo> <msubsup> <mi>a</mi> <mi>e</mi> <mn>1</mn> </msubsup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>24</mn> <mo>)</mo> </mrow> </mrow>