CN105183009A - 一种冗余机械臂轨迹控制方法 - Google Patents

Abstract

本发明提供了一种冗余机械臂轨迹控制方法。本发明以构形平面的冗余机械臂运动学求解方法为基础，推导出构形平面的速度分配方法，进而确定了基于构形平面间速度分配的冗余机械臂运动轨迹规划过程，对运动规划中面临的工作位形确定、空间避障、构形平面内速度分配等关键步骤进行了推导，能够满足复杂的空间避障要求，解决空间插值点之间的机械臂末端点轨迹的不确定性，能够实现良好的运动轨迹。本发明解决了冗余机械臂逆解的难题，具有计算速度快、解算精度高的优点，满足实际任务和环境对冗余机械臂的轨迹运动作业需要。

Description

一种冗余机械臂轨迹控制方法

技术领域

本发明涉及的是一种冗余机械臂运动规划方法，该方法能够实现冗余机械臂运动轨迹的可控性，减少运动误差。

背景技术

冗余机械臂具有多余的空间运动自由度，在空间避障和运动规划方面具有很大的优点，成为机器人研究领域的热点。国内外许多研究机构对此展开了研究，得到了很多研究方法。许多专家在冗余机械臂的运动控制方面提出了很多方法。基于C空间的自由空间法以机械臂的关节坐标系建立C空间，将障碍物映射到C空间，形成空间构型障碍，从而求得C空间的补集，即自由空间。在此基础上，利用启发式搜索算法在机械臂的自由空间内寻找机械臂的运动路径。该方法虽然能够实现机械臂无碰撞路径规划，但是由于将障碍物映射到C空间方法较为复杂，对于复杂环境难以满足实时性的要求。为了实现空间避障，对障碍定义一个排斥势场，目标点处定义一个吸引势场，机械臂的运动由两个势场共同作用力来决定，由此来保证机械臂在避障的同时顺利到达最终目标点。该方法对于处理全局路径规划中的动态避障非常有效，但容易陷入局部最小点处。

在对机器人进行运动控制时，各个关节的角加速度约束、角速度约束和角度约束是涉及的主要约束问题。在一般低速运动情况下，只要保证关节角度不超限即可，这对运动轨迹规划影响很小。但是，当机器人运动速度较快时，关节角加速度和角速度极易超出约束范围，导致驱动电流过大或者超出限位的事故发生。轻则机器人运动出错，重则损坏硬件。此时，必须在机器人运动轨迹规划时综合考虑各种约束条件，最常用的方法是对运动时间进行优化。在冗余机械臂轨迹运动方面，常规的方法是：根据预定轨迹建立机械臂运动插补点，根据插补点求解合理的机械臂运动关节量，驱动关节运动实现预定轨迹运动。

但这种方法存在以下缺点:

(1)该轨迹规划方法基础是冗余机械臂的逆运动学求解方法，因此该方法依赖冗余机械臂的拓扑结构形式，在规划的过程中需要对算法重新调整，不具有一般性。

(2)采用目标函数的作为优化目标进行遗传方式的优化，忽略了一些局部轨迹规划的动力学效果，会造成一段区域运动不合理，某关节承担的整体载荷不平均，满载或满速工作，使得整个轨迹运动过程存在局部缺陷。

(3)运动轨迹在插补点间的运动不具有可控性，运动超调量和误差量都比较大。

本发明采用构形平面的速度分配和位形确定的方法，解决冗余机械臂中运动连续、空间避障、空间多约束和插补点间的运动不可控的问题。

发明内容

本发明目的在以提供一种适用于具有多种运动约束、空间避障、运动连续控制等多种要求运动轨迹控制的冗余机械臂轨迹控制方法。

本发明的目的是这样实现的：

步骤1：运动规划初始，输入机械臂运动轨迹方程，输入机械臂的结构参数，建立机械臂运动学模型，同时输入空间障碍物得到外形尺寸和空间位置；

步骤2：以空间机械臂运动轨迹方程为约束，建立多个路标式的空间插补点，建立具有躲避空间障碍物的机械臂运动规划路径；

步骤3：建立第一个轨迹插补点的运动速度方向和大小，利用构形平面的速度分配方法，求解出组成冗余机械臂的各构形平面的速度，进而应用构形平面内的速度分解，求解各关节的运动速度，将这些运动速度输入到机械臂关节驱动器驱动关节运动；

步骤4：在运动Δt时间后，对该时刻的机械臂末端点的位置和姿态进行检测，计算与预定的运动轨迹点进行误差比较，驱动机械臂关节运动消除位置和姿态点误差；

步骤5：对机械臂的运动轨迹点进行判断，是否到达下一个插补点，若未到插补点，继续机械臂末端的运动速度方向和大小，计算各关节运动速度，驱动机械臂运动，若到达插补点，判断是否到达最后一个轨迹插补点，若未到达，重复前面的运动过程，若到达则结束运动轨迹过程。

本发明适用于具有多种运动约束、空间避障、运动连续控制等多种要求运动轨迹控制。

运动规划初始，根据具体任务需要输入机械臂运动轨迹方程，输入机械臂的结构参数，建立机械臂运动学模型，同时输入空间障碍物得到外形尺寸和空间位置；以空间机械臂运动轨迹方程为约束，建立多个路标式的空间插补点，建立具有躲避空间障碍物的机械臂运动规划路径；建立第一个轨迹插补点的运动速度方向和大小，利用构形平面的速度分配方法，求解出组成冗余机械臂的各构形平面的速度，进而应用构形平面内的速度分解，求解各关节的运动速度，将这些运动速度输入到机械臂关节驱动器驱动关节运动；在运动Δt时间后，对该时刻的机械臂末端点的位置和姿态进行检测，计算与预定的运动轨迹点进行误差比较，驱动机械臂关节运动消除位置和姿态点误差；对机械臂的运动轨迹点进行判断，是否到达下一个插补点，若未到插补点，继续机械臂末端的运动速度方向和大小，计算各关节运动速度，驱动机械臂运动，若到达插补点，判断是否到达最后一个轨迹插补点，若未到达，重复前面的运动过程，若到达则结束运动轨迹过程。

本发明解决了冗余机械臂逆解的难题，具有计算速度快、解算精度高的优点，满足实际任务和环境对冗余机械臂的轨迹运动作业需要。

本发明与现有技术相比，具有以下突出实质性的优点和有益效果：

(1)总结串联形式的机器人结构特点，运用构形平面的基本模型，在此基础上推导了构形平面速度分配方法，为后面的冗余机械臂规划工作奠定基础。

(2)对于空间运动的多种约束要求，通过机械臂的构形平面方法能够使快速确定合理的机械臂位形，该方法简单明了，同遗传算法、迭代算法等数值方法相比具有计算量较小，且具有通用性，适用于大多数形式的冗余机械臂。

(3)针对运动规划中插补点间运动不连续和可控性差问题，提出构形平面的速度分配方法。该方法利用短时间内机械臂末端线速度的平滑特点，保证运动的连续性，同时对于构形平面内某些关节接近障碍物或者空间其他的约束条件，分配构形平面的速度，避免空间干涉，增加冗余机械臂在插补点间的可控性。

附图说明

图1运动规划过程示意图。

图2构形平面速度迭代示意图。

图3冗余机械臂空间避障示意图。

图4构形平面与障碍物的干涉检查示意图。

图5插补点间的速度规划过程。

图6冗余机械臂运动模型图。

图7轨迹运动简图。

具体实施方式

面结合附图对本发明做更详细的描述。

结合图1，本发明的方法包括如下几个步骤：

步骤1：运动规划初始，根据具体任务需要输入机械臂运动轨迹方程，输入机械臂的结构参数，建立机械臂运动学模型，同时输入空间障碍物得到外形尺寸和空间位置；

步骤2：以空间机械臂运动轨迹方程为约束，建立多个路标式的空间插补点，建立具有躲避空间障碍物的机械臂运动规划路径；

步骤3：建立第一个轨迹插补点的运动速度方向和大小，利用构形平面的速度分配方法，求解出组成冗余机械臂的各构形平面的速度，进而应用构形平面内的速度分解，求解各关节的运动速度，将这些运动速度输入到机械臂关节驱动器驱动关节运动；

步骤4：在运动Δt时间后，对该时刻的机械臂末端点的位置和姿态进行检测，计算与预定的运动轨迹点进行误差比较，驱动机械臂关节运动消除位置和姿态点误差；

步骤5：对机械臂的运动轨迹点进行判断，是否到达下一个插补点，若未到插补点，继续机械臂末端的运动速度方向和大小，计算各关节运动速度，驱动机械臂运动，若到达插补点，判断是否到达最后一个轨迹插补点，若未到达，重复前面的运动过程，若到达则结束运动轨迹过程。

结合图2，构形平面末端的速度求解公式为

$\stackrel{\·}{p}=J\stackrel{\·}{q}---\left(1\right)$

式中，为构形平面末端P点相对构形平面中心的速度,υ_x,υ_y,υ_z,ω_x,ω_y,ω_z分别为构形平面末端相对构形平面中心的x轴线速度、y轴线速度、z轴线速度、x轴角速度、y轴角速度、z轴角速度；J∈R^6×n为构形平面的一阶影响系数矩阵，即Jacobian矩阵，当第kk的关节是摆动关节时，表达式见公式(2),若第kk个关节是移动关节时，表达式见公式(3)；为广义输入速度矢量，包含回转关节、摆动关节和移动关节的运动速度量。

$J=\left[\begin{array}{ccccc}-\sin \mathrm{\θ}\underset{i=2}{\overset{n}{\Σ}}{h}_i\cos \left(\underset{j=2}{\overset{i}{\Σ}}{\mathrm{\β}}_j\right)& \mathrm{...}& -\cos \mathrm{\θ}\underset{i=kk}{\overset{n}{\Σ}}{h}_i\sin \left(\underset{j=2}{\overset{i}{\Σ}}{\mathrm{\β}}_j\right)& \mathrm{...}& -h_n\cos \mathrm{\θ}\sin \left(\underset{i=2}{\overset{n}{\Σ}}{\mathrm{\β}}_i\right)\\ \cos \mathrm{\θ}\underset{i=2}{\overset{n}{\Σ}}{h}_i\cos \left(\underset{j=2}{\overset{i}{\Σ}}{\mathrm{\β}}_j\right)& \mathrm{...}& -\sin \mathrm{\θ}\underset{i=kk}{\overset{n}{\Σ}}{h}_i\sin \left(\underset{j=2}{\overset{i}{\Σ}}{\mathrm{\β}}_j\right)& \mathrm{...}& -h_n\sin \mathrm{\θ}\sin \left(\underset{j=2}{\overset{n}{\Σ}}{\mathrm{\β}}_j\right)\\ 0& \mathrm{...}& \underset{i=kk}{\overset{n}{\Σ}}{h}_i\cos \left(\underset{j=2}{\overset{i}{\Σ}}{\mathrm{\β}}_j\right)& \mathrm{...}& h_n\sin \left(\underset{j=2}{\overset{n}{\Σ}}{\mathrm{\β}}_j\right)\\ 0& \mathrm{...}& \cos \mathrm{\θ}\sin \left(\underset{j=2}{\overset{kk-1}{\Σ}}{\mathrm{\β}}_j\right)& \mathrm{...}& \cos \mathrm{\θ}\sin \left(\underset{j=2}{\overset{n-1}{\Σ}}{\mathrm{\β}}_j\right)\\ 0& \mathrm{...}& \sin \mathrm{\θ}\sin \left(\underset{j=2}{\overset{kk-1}{\Σ}}{\mathrm{\β}}_j\right)& \mathrm{...}& \sin \mathrm{\θ}\sin \left(\underset{j=2}{\overset{n-1}{\Σ}}{\mathrm{\β}}_j\right)\\ 1& \mathrm{...}& \cos \left(\underset{j=2}{\overset{kk-1}{\Σ}}{\mathrm{\β}}_j\right)& \mathrm{...}& \cos \left(\underset{j=2}{\overset{n-1}{\Σ}}{\mathrm{\β}}_j\right)\end{array}\right]---\left(2\right)$

$J=\left[\begin{array}{ccccc}-\sin \mathrm{\θ}\underset{i=2}{\overset{n}{\Σ}}{h}_i\cos \left(\underset{j=2}{\overset{i}{\Σ}}{\mathrm{\β}}_j\right)& \mathrm{...}& \cos \mathrm{\θ}\cos \left(\underset{j=2}{\overset{kk}{\Σ}}{\mathrm{\β}}_j\right)& \mathrm{...}& -h_n\cos \mathrm{\θ}\sin \left(\underset{i=2}{\overset{n}{\Σ}}{\mathrm{\β}}_i\right)\\ \cos \mathrm{\θ}\underset{i=2}{\overset{n}{\Σ}}{h}_i\cos \left(\underset{j=2}{\overset{i}{\Σ}}{\mathrm{\β}}_j\right)& \mathrm{...}& \sin \mathrm{\θ}\cos \left(\underset{j=2}{\overset{kk}{\Σ}}{\mathrm{\β}}_j\right)& \mathrm{...}& -h_n\sin \mathrm{\θ}\sin \left(\underset{j=2}{\overset{n}{\Σ}}{\mathrm{\β}}_j\right)\\ 0& \mathrm{...}& \sin \left(\underset{j=2}{\overset{kk}{\Σ}}{\mathrm{\β}}_j\right)& \mathrm{...}& h_n\sin \left(\underset{j=2}{\overset{n}{\Σ}}{\mathrm{\β}}_j\right)\\ 0& \mathrm{...}& 0& \mathrm{...}& \cos \mathrm{\θ}\sin \left(\underset{j=2}{\overset{n-1}{\Σ}}{\mathrm{\β}}_j\right)\\ 0& \mathrm{...}& 0& \mathrm{...}& \sin \mathrm{\θ}\sin \left(\underset{j=2}{\overset{n-1}{\Σ}}{\mathrm{\β}}_j\right)\\ 1& \mathrm{...}& 0& \mathrm{...}& \cos \left(\underset{j=2}{\overset{n-1}{\Σ}}{\mathrm{\β}}_j\right)\end{array}\right]---\left(3\right)$

式中：h_i(i＝2,…n)为摆动关节间的连杆长度，若该关节是移动关节，则h_i(i＝2,…n)为移动关节固定长度和变化长度和，β_i(i＝2…n)为第i个摆动关节转动角度，若第i个关节是移动关节，θ为构形平面中包含的回转关节转动角度。

由于机械臂的拓扑关系，组成机械臂的构形平面间存在速度的耦合，靠近基座的构形平面输出的速度影响靠近机械臂末端的构形平面的速度。

设前i-1个构形平面关节运动产生复合速度在第i个构形平面中心为其在基坐标系的产生的分量为其中υ_x ⁱ,υ_y ⁱ,υ_z ⁱ,ω_x ⁱ,ω_y ⁱ,ω_z ⁱ分别为相对基坐标系的x轴线速度、y轴线速度、z轴线速度、x轴角速度、y轴角速度、z轴角速度；第i个构形平面中各关节运动产生的速度在该构形平面坐标系中的分量为其中υ_cpx ⁱ,υ_cpy ⁱ,υ_cpz ⁱ,ω_cpx ⁱ,ω_cpy ⁱ,ω_cpz ⁱ分别为该关节产生的素的在该构型平面坐标系中的x轴线速度、y轴线速度、z轴线速度、x轴角速度、y轴角速度、z轴角速度；在第i个构形平面末端产生的复合速度相对基坐标系的速度分量为其中υ_x ⁱ⁺¹,υ_y ⁱ⁺¹,υ_z ⁱ⁺¹,ω_x ⁱ⁺¹,ω_y ⁱ⁺¹,ω_z ⁱ⁺¹分别为第i个构型平面末端产生的符合素的相对于极坐标系的x轴线速度、y轴线速度、z轴线速度、x轴角速度、y轴角速度、z轴角速度；r_i ⁱ⁺¹为第i构形平面末端相对其中心的空间矢量,其在构形平面坐标系的分量为设第i个构形平面中心的坐标系相对基坐标系的姿态变换矩阵为R_i，该矩阵为3×3矩阵。则其在基座标系的分量为：

$\left[\begin{array}{c}{r}_x^i\\ r_y^i\\ r_z^i\end{array}\right]=R_i\left[\begin{array}{c}{r}_{cpx}^i\\ r_{cpy}^i\\ r_{cpz}^i\end{array}\right]---\left(4\right)$

则各构形平面的线速度和角速度分量可表示为:

$\left[\begin{array}{c}{\mathrm{\υ}}_x^{i+1}\\ {\mathrm{\υ}}_y^{i+1}\\ {\mathrm{\υ}}_z^{i+1}\end{array}\right]=\left[\begin{array}{c}{r}_x^i{\mathrm{\ω}}_x^i\\ r_y^i{\mathrm{\ω}}_y^i\\ r_z^i{\mathrm{\ω}}_z^i\end{array}\right]+R_i\left[\begin{array}{c}{\mathrm{\υ}}_{cpx}^i\\ {\mathrm{\υ}}_{cpy}^i\\ {\mathrm{\υ}}_{cpz}^i\end{array}\right]+\left[\begin{array}{c}{\mathrm{\υ}}_x^i\\ {\mathrm{\υ}}_y^i\\ {\mathrm{\υ}}_z^i\end{array}\right]---\left(5\right)$

$\left[\begin{array}{c}{\mathrm{\ω}}_x^{i+1}\\ {\mathrm{\ω}}_y^{i+1}\\ {\mathrm{\ω}}_z^{i+1}\end{array}\right]=R_i\left[\begin{array}{c}{\mathrm{\ω}}_{cpx}^i\\ {\mathrm{\ω}}_{cpy}^i\\ {\mathrm{\ω}}_{cpz}^i\end{array}\right]+\left[\begin{array}{c}{\mathrm{\ω}}_x^i\\ {\mathrm{\ω}}_y^i\\ {\mathrm{\ω}}_z^i\end{array}\right]---\left(6\right)$

结合图3，冗余机械臂的工作位形确定实际上是冗余机械臂的逆运动学求解，设t_i时刻空间轨迹点的位姿矩阵为TP(t_i),冗余机械臂有ncp个构形平面组成，则根据构形平面表达式则有：

TP(t_i)＝CP₁(t_i)*CP₂(t_i)*…*CP_ncp(t_i)(7)

常用形式冗余机械臂由2-3个构形平面组成，每个构形平面的姿态部分表达式有两个参数，通过空间姿态要求用解析的方式易求出构形平面中姿态部分的参数，然后用冗余自由度关节与满足为空间轨迹点的位置和空间避障等要求；对于包含4个构形平面的冗余机械臂，某个构形平面的姿态中包含确定的参数，如构形平面中具有多角度连接形式的连杆或者构形平面中仅包含回转关节的构形平面。对于包含4个以上构形平面的机械臂，至少要有8个以上的自由度，属于超冗余机械臂，这里不作为讨论。

通过姿态的解析的方法，确定部分关节的运动量，其余未求解出运动量关节通过空间矢量投影法能够确定初步工作位形。

结合图4，空间避障首先要进行冗余机械臂与空间障碍物进行的干涉检查，由于采用冗余机械臂已经分解成有限个构形平面，冗余机械臂的初步工作位形已知，因此构形平面在空间的表达式也是已知的，通过空间几何解析的方法易知空间障碍物在每个构形平面内的表达式。构形平面内的连杆与障碍物进行一一解算，如图附图4所示，K为机械臂连杆与障碍物的空间安全距离。障碍物空间检测后，发生干涉的部分重新调整，如图4所示，采用基于矢量投影的方法对发生障碍物干涉的连杆进行调整。通过公式(8)调整构形平面内连杆。

$\left|\right|p\left|\right|=\underset{i=0}{\overset{n}{\Σ}}{a}_i{h}_i---\left(8\right)$

式中，||p||为构形平面末端相对其中心矢量的范数；a_i为第i个关节杆臂在矢量上投影与该关节杆臂的比值，由于矢量投影具有方向性，因此a_i具有正负号，其表达式为：

a_i＝h_icos(θ_i')(9)

式中，θ_i'为第i个关节杆臂与目标点矢量的夹角。

结合图5，机械臂末端在两个插补点S1和S2间运动，S1和S2间的运动轨迹是与时间相关的连续线段，设为：

S＝f(x,y,z,t)(10)

公式(10)中的具体表达式与实际运动规划中的轨迹有关，S1作为起始点，S2是终点。

根据公式(10),容易计算出机器人末端在S1点的速度，按照上一节的构形平面速度分配，规划该时刻构形平面的运动速度。机械臂在运行Δt时间后，到达b1点，这里Δt称为补偿周期。由于冗余机械臂运动的非线性和高耦合性的特点，当机械臂末端与预定的轨迹存在空间偏差，为此进行补偿调整，将驱动机械臂末端由b1点运动到a1点，依次规划，直到完全运动到S2点。

在这种运动规划过程中，S1点b1点之间并非直线，而是依赖构形平面的速度分配实现短时间内的线型运动。

采用这种规划方式，由于机械臂末端的轨迹变化较大，b1点和实际运动轨迹点a1的差值会比较大，为此对公式(10)求解轨迹中任意一点的加速度，根据加速度值调整机械臂末端在运动起始点的速度运动方向和大小，这种方式极大减小运动误差量。

插补点间的轨迹规划依赖的是以构形平面为基础的具有空间避障的逆运动学求解和速度分配方式，对于大多数冗余机械臂和超冗余机械臂结构来说，一般情况下其工作构形由2-4构形平面组成；而插补点的间的轨迹规划也是为了消除插补点间的运动的不可控性，减少轨迹运动误差，因此补偿周期较少。综上两个方面的因素，这种运动规划方式不仅运算量极大减少，而且减少规划运动过程中的运动偏差，增加运动的可控性。

结合图6，冗余机械臂的关节参数如表1所示，冗余机械臂的运动学模型如图6所示。

表1关节参数

关节名称	运动范围	关节参数	尺寸
				关节1	-180°≤θ1≤180°	d1	350mm
关节2	-190°≤θ2≤10°	d2	450mm
				关节3	-100°≤θ3≤100°	d3	350mm
关节4	-100°≤θ4≤100°	d4	150mm
				关节5	-150°≤θ5≤150°	d5	145mm
关节6	-100°≤θ6≤100°	d6	150mm
				关节7	-180°≤θ7≤180°	d7	160mm

表2空间插补点的坐标

插补点	px	py	pz
				0	-115.4661	222.7550	142.4795
45	-90.0399	258.5391	94.6887
				90	-29.8487	272.5428	74.8931
135	29.8483	256.5630	94.6887
				180	54.0814	219.9605	142.4795
225	28.6552	184.1764	190.2703
				270	-31.5360	170.1727	210.0659
315	-91.2330	186.1525	190.2703

表3对应插补点的关节运动量

插补点

关节1

关节2

关节3

关节4

关节5

关节6

关节7

0°

14.82°

-22.44°

8.25°

5.08°

86.64°

47.9°

73.02°

45°

14.32°

-17.6°

13.44°

7.52°

26.05°

16.77°

78.9°

90°

5.34°

-15.48°

14.87°

0.89°

6.87°

20.05°

92.85°

135°

11.66°

-16.02°

12.01°

-40.43°

-21.48°

11.21°

124.09°

180°

3.55°

-53.55°

44.1°

-26.34°

-37.44°

32.79°

81.26°

225°

26.44°

-67.21°

53.74°

-68.99°

-49.9°

-12.97°

87.41°

270°

37.08°

-87.65°

75.93°

-48.34°

-38°

-22.13°

70.4°

315°

40.5°

-66.85°

60.77°

-46.9°

-61.64°

-42.25°

88.98°

最后运动轨迹如图7轨迹运动简图所示。

Claims (4)

1.一种冗余机械臂轨迹控制方法，其特征是包括如下步骤：

步骤1：运动规划初始，输入机械臂运动轨迹方程，输入机械臂的结构参数，建立机械臂运动学模型，同时输入空间障碍物得到外形尺寸和空间位置；

步骤2：以空间机械臂运动轨迹方程为约束，建立多个路标式的空间插补点，建立具有躲避空间障碍物的机械臂运动规划路径；

步骤3：建立第一个轨迹插补点的运动速度方向和大小，利用构形平面的速度分配方法，求解出组成冗余机械臂的各构形平面的速度，进而应用构形平面内的速度分解，求解各关节的运动速度，将这些运动速度输入到机械臂关节驱动器驱动关节运动；

步骤4：在运动Δt时间后，对该时刻的机械臂末端点的位置和姿态进行检测，计算与预定的运动轨迹点进行误差比较，驱动机械臂关节运动消除位置和姿态点误差；

步骤5：对机械臂的运动轨迹点进行判断，是否到达下一个插补点，若未到插补点，继续机械臂末端的运动速度方向和大小，计算各关节运动速度，驱动机械臂运动，若到达插补点，判断是否到达最后一个轨迹插补点，若未到达，重复前面的运动过程，若到达则结束运动轨迹过程。

2.根据权利要求1所述的冗余机械臂轨迹控制方法，其特征是所述的构形平面速度分配方法具体包括：

构形平面末端的速度求解公式为

$\stackrel{\·}{p}=J\stackrel{\·}{q}---\left(1\right)$

式中，为构形平面末端P点相对构形平面中心的速度,υ_x,υ_y,υ_z,ω_x,ω_y,ω_z分别为构形平面末端相对构形平面中心的x轴线速度、y轴线速度、z轴线速度、x轴角速度、y轴角速度、z轴角速度；J∈R^6×n为构形平面的一阶影响系数矩阵，即Jacobian矩阵，当第kk的关节是摆动关节时，表达式为公式(2)，若第kk个关节是移动关节时，表达式为公式(3)；为广义输入速度矢量，包含回转关节、摆动关节和移动关节的运动速度量；

$J=\left[\begin{array}{ccccc}-\sin \mathrm{\θ}\underset{i=2}{\overset{n}{\Σ}}{h}_i\cos \left(\underset{j=2}{\overset{i}{\Σ}}{\mathrm{\β}}_j\right)& ...& -\cos \mathrm{\θ}\underset{i=kk}{\overset{n}{\Σ}}{h}_i\sin \left(\underset{j=2}{\overset{i}{\Σ}}{\mathrm{\β}}_j\right)& ...& -h_n\cos \mathrm{\θ}\sin \left(\underset{i=2}{\overset{n}{\Σ}}{\mathrm{\β}}_i\right)\\ \cos \mathrm{\θ}\underset{i=2}{\overset{n}{\Σ}}{h}_i\cos \left(\underset{j=2}{\overset{i}{\Σ}}{\mathrm{\β}}_j\right)& ...& -\sin \mathrm{\θ}\underset{i=kk}{\overset{n}{\Σ}}{h}_i\sin \left(\underset{j=2}{\overset{i}{\Σ}}{\mathrm{\β}}_j\right)& ...& -h_n\sin \mathrm{\θ}\sin \left(\underset{j=2}{\overset{n}{\Σ}}{\mathrm{\β}}_j\right)\\ 0& ...& \underset{i=kk}{\overset{n}{\Σ}}{h}_i\cos \left(\underset{j=2}{\overset{i}{\Σ}}{\mathrm{\β}}_j\right)& ...& h_n\sin \left(\underset{j=2}{\overset{n}{\Σ}}{\mathrm{\β}}_j\right)\\ 0& ...& \cos \mathrm{\θ}\sin \left(\underset{j=2}{\overset{kk-1}{\Σ}}{\mathrm{\β}}_j\right)& ...& \cos \mathrm{\θ}\sin \left(\underset{j=2}{\overset{n-1}{\Σ}}{\mathrm{\β}}_j\right)\\ 0& ...& \sin \mathrm{\θ}\sin \left(\underset{j=2}{\overset{kk-1}{\Σ}}{\mathrm{\β}}_j\right)& ...& \sin \mathrm{\θ}\sin \left(\underset{j=2}{\overset{n-1}{\Σ}}{\mathrm{\β}}_j\right)\\ 1& ...& \cos \left(\underset{j=2}{\overset{kk-1}{\Σ}}{\mathrm{\β}}_j\right)& ...& \cos \left(\underset{j=2}{\overset{n-1}{\Σ}}{\mathrm{\β}}_j\right)\end{array}\right]---\left(2\right)$

$J=\left[\begin{array}{ccccc}-\sin \mathrm{\θ}\underset{i=2}{\overset{n}{\Σ}}{h}_i\cos \left(\underset{j=2}{\overset{i}{\Σ}}{\mathrm{\β}}_j\right)& ...& \cos \mathrm{\θ}\cos \left(\underset{j=2}{\overset{kk}{\Σ}}{\mathrm{\β}}_j\right)& ...& -h_n\cos \mathrm{\θ}\sin \left(\underset{i=2}{\overset{n}{\Σ}}{\mathrm{\β}}_i\right)\\ \cos \mathrm{\θ}\underset{i=2}{\overset{n}{\Σ}}{h}_i\cos \left(\underset{j=2}{\overset{i}{\Σ}}{\mathrm{\β}}_j\right)& ...& \sin \mathrm{\θ}\cos \left(\underset{j=2}{\overset{kk}{\Σ}}{\mathrm{\β}}_j\right)& ...& -h_n\sin \mathrm{\θ}\sin \left(\underset{j=2}{\overset{n}{\Σ}}{\mathrm{\β}}_j\right)\\ 0& ...& \sin \left(\underset{j=2}{\overset{kk}{\Σ}}{\mathrm{\β}}_j\right)& ...& h_n\sin \left(\underset{j=2}{\overset{n}{\Σ}}{\mathrm{\β}}_j\right)\\ 0& ...& 0& ...& \cos \mathrm{\θ}\sin \left(\underset{j=2}{\overset{n-1}{\Σ}}{\mathrm{\β}}_j\right)\\ 0& ...& 0& ...& \sin \mathrm{\θ}\sin \left(\underset{j=2}{\overset{n-1}{\Σ}}{\mathrm{\β}}_j\right)\\ 1& ...& 0& ...& \cos \left(\underset{j=2}{\overset{n-1}{\Σ}}{\mathrm{\β}}_j\right)\end{array}\right]---\left(3\right)$

式中：h_i为摆动关节间的连杆长度，若该关节是移动关节，则h_i为移动关节固定长度和变化长度和，β_i为第i个摆动关节转动角度，若第i个关节是移动关节，θ为构形平面中包含的回转关节转动角度，i＝2,…n；

由于机械臂的拓扑关系，组成机械臂的构形平面间存在速度的耦合，靠近基座的构形平面输出的速度影响靠近机械臂末端的构形平面的速度；

设前i-1个构形平面关节运动产生复合速度在第i个构形平面中心为其在基坐标系的产生的分量为其中υ_x ⁱ，υ_y ⁱ，υ_z ⁱ，ω_x ⁱ，ω_y ⁱ，ω_z ⁱ分别为相对基坐标系的x轴线速度、y轴线速度、z轴线速度、x轴角速度、y轴角速度、z轴角速度；第i个构形平面中各关节运动产生的速度在该构形平面坐标系中的分量为其中υ_cpx ⁱ,υ_cpy ⁱ,υ_cpz ⁱ,ω_cpx ⁱ,ω_cpy ⁱ,ω_cpz ⁱ分别为该关节产生的素的在该构型平面坐标系中的x轴线速度、y轴线速度、z轴线速度、x轴角速度、y轴角速度、z轴角速度；在第i个构形平面末端产生的复合速度相对基坐标系的速度分量为其中υ_x ⁱ⁺¹,υ_y ⁱ⁺¹,υ_z ⁱ⁺¹,ω_x ⁱ⁺¹,ω_y ⁱ⁺¹,ω_z ⁱ⁺¹分别为第i个构型平面末端产生的符合素的相对于极坐标系的x轴线速度、y轴线速度、z轴线速度、x轴角速度、y轴角速度、z轴角速度；为第i构形平面末端相对其中心的空间矢量,其在构形平面坐标系的分量为设第i个构形平面中心的坐标系相对基坐标系的姿态变换矩阵为R_i，该矩阵为3×3矩阵，则其在基座标系的分量为：

$\left[\begin{array}{c}{r}_x^i\\ r_y^i\\ r_z^i\end{array}\right]=R_i\left[\begin{array}{c}{r}_{cpx}^i\\ r_{cpy}^i\\ r_{cpz}^i\end{array}\right]---\left(4\right)$

则各构形平面的线速度和角速度分量表示为:

$\left[\begin{array}{c}{\mathrm{\υ}}_x^{i+1}\\ {\mathrm{\υ}}_y^{i+1}\\ {\mathrm{\υ}}_z^{i+1}\end{array}\right]=\left[\begin{array}{c}{r}_x^i{\mathrm{\ω}}_x^i\\ r_y^i{\mathrm{\ω}}_y^i\\ r_z^i{\mathrm{\ω}}_z^i\end{array}\right]+R_i\left[\begin{array}{c}{\mathrm{\υ}}_{cpx}^i\\ {\mathrm{\υ}}_{cpy}^i\\ {\mathrm{\υ}}_{cpz}^i\end{array}\right]+\left[\begin{array}{c}{\mathrm{\υ}}_x^i\\ {\mathrm{\υ}}_y^i\\ {\mathrm{\υ}}_z^i\end{array}\right]---\left(5\right)$

$\left[\begin{array}{c}{\mathrm{\ω}}_x^{i+1}\\ {\mathrm{\ω}}_y^{i+1}\\ {\mathrm{\ω}}_z^{i+1}\end{array}\right]=R_i\left[\begin{array}{c}{\mathrm{\ω}}_{cpx}^i\\ {\mathrm{\ω}}_{cpy}^i\\ {\mathrm{\ω}}_{cpz}^i\end{array}\right]+\left[\begin{array}{c}{\mathrm{\ω}}_x^i\\ {\mathrm{\ω}}_y^i\\ {\mathrm{\ω}}_z^i\end{array}\right]---\left(6\right).$

3.根据权利要求2所述的冗余机械臂轨迹控制方法，其特征是构形平面间的速度求解方法为：

对每一空间轨迹点对应的工作位形进行各关节的运动速度分配，采用构形平面的方法进行速度分配，

按照构形平面间的速度迭代公式(5)和(6)进行分解，式中的和是已知的，仅需要分配线速度和角速度，

设在上一个轨迹点的各关节运动角度为q_i(t_j)，i＝1,2,…m,t_j为运动起始后的时间节点，则当前轨迹点的各关节运动角度为q_i(t_j+1)，时间间隔为Δt＝t_j+1-t_j，则各关节在两个轨迹点之间平均运动速度为：

${\mathrm{qs}}_i\left(t_{j+1}\right)=\frac{q_i\left(t_{j+1}\right)-q_i\left(t_j\right)}{\mathrm{\Δ}t}---\left(7\right)$

利用公式(6)求出的速度，以当前机械臂工作位形，运用构形平面内的速度求解和构形平面间的速度迭代能够求解出机械臂末端速度,该速度在基坐标系的分量为：{υs_x,υs_y,υs_z,ωs_x,ωs_y,ωs_z}，其中υs_x,υs_y,υs_z,ωs_x,ωs_y,ωs_z分别为该速度相对于极坐标系的x轴线速度、y轴线速度、z轴线速度、x轴角速度、y轴角速度、z轴角速度；而当前轨迹点的目标速度分量为{υ_x(t_j+1),υ_y(t_j+1),υ_z(t_j+1),ω_x(t_j+1),ω_y(t_j+1),ω_z(t_j+1)}，则两者存在的误差为：

$\left[\begin{array}{c}{\mathrm{\Δ\υ}}_x\\ {\mathrm{\Δ\υ}}_y\\ {\mathrm{\Δ\υ}}_z\\ {\mathrm{\Δ\ω}}_x\\ {\mathrm{\Δ\ω}}_y\\ {\mathrm{\Δ\ω}}_z\end{array}\right]=\left[\begin{array}{c}{\mathrm{\υ}}_x\left(t_{j+1}\right)-{\mathrm{\υs}}_x\\ {\mathrm{\υ}}_y\left(t_{j+1}\right)-{\mathrm{\υs}}_y\\ {\mathrm{\υ}}_z\left(t_{j+1}\right)-{\mathrm{\υs}}_z\\ {\mathrm{\ω}}_x\left(t_{j+1}\right)-{\mathrm{\ωs}}_x\\ {\mathrm{\ω}}_y\left(t_{j+1}\right)-{\mathrm{\ωs}}_y\\ {\mathrm{\ω}}_z\left(t_{j+1}\right)-{\mathrm{\ωs}}_z\end{array}\right]---\left(8\right)$

由公式(5)和公式(6)的迭代关系，构形平面末端相对基坐标系速度是由两部分复合而成，构形平面内的产生的运动量和上一个构形平面传递到当前构形平面中心的附加运动量，将公式(8)产生的运动速度偏差分配到每一个构形平面的关节中，

假设第i个构形平面末端速度增量为

${\left[\begin{array}{cccccc}{\mathrm{\Δ\υ}}_x^{i+1}& {\mathrm{\Δ\υ}}_y^{i+1}& {\mathrm{\Δ\υ}}_z^{i+1}& {\mathrm{\Δ\ω}}_x^{i+1}& {\mathrm{\Δ\ω}}_y^{i+1}& {\mathrm{\Δ\ω}}_z^{i+1}\end{array}\right]}^T,$ 第i个构形平面中心的速度增量为 ${\left[\begin{array}{cccccc}{\mathrm{\Δ\υ}}_x^i& {\mathrm{\Δ\υ}}_y^i& {\mathrm{\Δ\υ}}_z^i& {\mathrm{\Δ\ω}}_x^i& {\mathrm{\Δ\ω}}_y^i& {\mathrm{\Δ\ω}}_z^i\end{array}\right]}^T,$ 则由公式(5)和公式(6)，则构形平面的线速度和角速度分量表示为:

$\left[\begin{array}{c}{\mathrm{\υ}}_x^{i+1}+{\mathrm{\Δ\υ}}_x^{i+1}\\ {\mathrm{\υ}}_y^{i+1}+{\mathrm{\Δ\υ}}_y^{i+1}\\ {\mathrm{\υ}}_z^{i+1}+{\mathrm{\Δ\υ}}_z^{i+1}\end{array}\right]=\left[\begin{array}{c}{r}_x^i\left({\mathrm{\ω}}_x^i+{\mathrm{\Δ\ω}}_x^i\right)\\ r_y^i\left({\mathrm{\ω}}_y^i+{\mathrm{\Δ\ω}}_y^i\right)\\ r_z^i\left({\mathrm{\ω}}_z^i+{\mathrm{\Δ\ω}}_z^i\right)\end{array}\right]+R_i\left[\begin{array}{c}{\mathrm{\υ}}_{cpx}^i\\ {\mathrm{\υ}}_{cpy}^i\\ {\mathrm{\υ}}_{cpz}^i\end{array}\right]+\left[\begin{array}{c}{\mathrm{\υ}}_x^i+{\mathrm{\Δ\υ}}_x^i\\ {\mathrm{\υ}}_y^i+{\mathrm{\Δ\υ}}_y^i\\ {\mathrm{\υ}}_z^i+{\mathrm{\Δ\υ}}_z^i\end{array}\right]---\left(9\right)$

$\left[\begin{array}{c}{\mathrm{\ω}}_x^{i+1}+{\mathrm{\Δ\ω}}_x^{i+1}\\ {\mathrm{\ω}}_y^{i+1}+{\mathrm{\Δ\ω}}_y^{i+1}\\ {\mathrm{\ω}}_z^{i+1}+{\mathrm{\Δ\ω}}_z^{i+1}\end{array}\right]=R_i\left[\begin{array}{c}{\mathrm{\ω}}_{cpx}^i\\ {\mathrm{\ω}}_{cpy}^i\\ {\mathrm{\ω}}_{cpz}^i\end{array}\right]+\left[\begin{array}{c}{\mathrm{\ω}}_x^i+{\mathrm{\Δ\ω}}_x^i\\ {\mathrm{\ω}}_y^i+{\mathrm{\Δ\ω}}_y^i\\ {\mathrm{\ω}}_z^i+{\mathrm{\Δ\ω}}_z^i\end{array}\right]---\left(10\right)$

对公式(9)和公式(10)进行处理，得到第i个构形平面末端相对其中心坐标系的速度量，如公式(11)和公式(12)所示；

$\left[\begin{array}{c}{\mathrm{\υ}}_{cpx}^i\\ {\mathrm{\υ}}_{cpy}^i\\ {\mathrm{\υ}}_{cpz}^i\end{array}\right]=R_i^{-1}\left(\left[\begin{array}{c}{\mathrm{\υ}}_x^{i+1}+{\mathrm{\Δ\υ}}_x^{i+1}\\ {\mathrm{\υ}}_y^{i+1}+{\mathrm{\Δ\υ}}_y^{i+1}\\ {\mathrm{\υ}}_z^{i+1}+{\mathrm{\Δ\υ}}_z^{i+1}\end{array}\right]-\left[\begin{array}{c}{r}_x^i\left({\mathrm{\ω}}_x^i+{\mathrm{\Δ\ω}}_x^i\right)\\ r_y^i\left({\mathrm{\ω}}_y^i+{\mathrm{\Δ\ω}}_y^i\right)\\ r_z^i\left({\mathrm{\ω}}_z^i+{\mathrm{\Δ\ω}}_z^i\right)\end{array}\right]-\left[\begin{array}{c}{\mathrm{\υ}}_x^i+{\mathrm{\Δ\υ}}_x^i\\ {\mathrm{\υ}}_y^i+{\mathrm{\Δ\υ}}_y^i\\ {\mathrm{\υ}}_z^i+{\mathrm{\Δ\υ}}_z^i\end{array}\right]\right)---\left(11\right)$

$\left[\begin{array}{c}{\mathrm{\ω}}_{cpx}^i\\ {\mathrm{\ω}}_{cpy}^i\\ {\mathrm{\ω}}_{cpz}^i\end{array}\right]=R_i^{-1}\left(\left[\begin{array}{c}{\mathrm{\ω}}_x^{i+1}+{\mathrm{\Δ\ω}}_x^{i+1}\\ {\mathrm{\ω}}_y^{i+1}+{\mathrm{\Δ\ω}}_y^{i+1}\\ {\mathrm{\ω}}_z^{i+1}+{\mathrm{\Δ\ω}}_z^{i+1}\end{array}\right]-\left[\begin{array}{c}{\mathrm{\ω}}_x^i+{\mathrm{\Δ\ω}}_x^i\\ {\mathrm{\ω}}_y^i+{\mathrm{\Δ\ω}}_y^i\\ {\mathrm{\ω}}_z^i+{\mathrm{\Δ\ω}}_z^i\end{array}\right]\right)---\left(12\right)$

再由公式(1),求得各关节的运动速度，有下式：

$\left[\begin{array}{c}{\mathrm{\θ}}^i\\ {\mathrm{\β}}_2^i\\ \begin{array}{c}.\\ .\\ .\end{array}\\ {\mathrm{\β}}_n^i\end{array}\right]=J_i^{-1}\left[\begin{array}{c}{\mathrm{\υ}}_{cpx}^i\\ {\mathrm{\υ}}_{cpy}^i\\ {\mathrm{\υ}}_{cpz}^i\\ {\mathrm{\ω}}_{cpx}^i\\ {\mathrm{\ω}}_{cpy}^i\\ {\mathrm{\ω}}_{cpz}^i\end{array}\right]---\left(13\right).$

4.根据权利要求3所述的冗余机械臂轨迹控制方法，其特征是运动轨迹插补点间的规划方法为：机械臂末端在两个插补点S1和S2间运动，S1和S2间的运动轨迹是与时间相关的连续线段，设为：

S＝f(x,y,z,t)(14)

其中x,y,z为改点的坐标值而t为运动时间，

S1作为起始点，S2是终点，

根据公式(14),计算出机器人末端在S1点的速度，按照构形平面速度分配，规划该时刻构形平面的运动速度，机械臂在运行Δt时间后，到达b1点，Δt为补偿周期，由于冗余机械臂运动的非线性和高耦合性的特点，进行补偿调整，将驱动机械臂末端由b1点运动到a1点，依次规划，直到完全运动到S2点。