CN105234930A - 基于构形平面的冗余机械臂运动控制方法 - Google Patents

Abstract

本发明提供了一种基于构形平面的冗余机械臂运动控制方法。本发明从组成冗余机械臂构形平面入手，利用空间几何的方法，确定冗余机械臂进行空间运动控制规划的过程，通过空间矢量引导、避障路径的比较、动力学校核，快速找到空间优化的路径，实现多目标轨迹规划方法。本发明解决的冗余机械臂运动规划的通用性问题，不依赖机器人构形，形式简单，降低了求解的难度，减少计算量，能够按需要得到优化解，具有通用性和快速性，满足多目标任务和环境对冗余机械臂的运动规划要求。

Description

基于构形平面的冗余机械臂运动控制方法

技术领域

本发明涉及的是一种机器人控制方法，具体地说是一种基于构形平面的冗余机械臂运动控制轨迹方法，该方法能够保证冗余机械臂在复杂的工作空间中实现空间避障等多目标条件下的运动控制。

背景技术

空间运动物体的自由度是6，因此机械臂操作空间也是6，冗余机械臂具有比操作空间自由度多的关节，意味着在相同的任务中，冗余机械臂具有更多的操作自由度，能够实现更加灵活运动控制，进而满足实际任务的需要，因此冗余机械臂的研究成为机器人运动控制领域的研究热点。

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

在对机器人进行运动控制时，各个关节的角加速度约束、角速度约束和角度约束是涉及的主要约束问题。在一般低速运动情况下，只要保证关节角度不超限即可，这对运动轨迹规划影响很小。但是，当机器人运动速度较快时，关节角加速度和角速度极易超出约束范围，导致驱动电流过大或者超出限位的事故发生。轻则机器人运动出错，重则损坏硬件。此时，必须在机器人运动轨迹规划时综合考虑各种约束条件，最常用的方法是对运动时间进行优化。例如，有研究人员在用机械臂拦截快速飞行的物体时，对机械臂的运动轨迹施加关节速度约束和力矩约束。还有学者推导出了具有特殊关节配置的7R机械臂的解析逆解，并且使所有的关节满足其关节角度限制。工作主要是服务于拟人机械臂工作空间的求解。众多学者对7R机械臂的仿人达点运动规划展开了研究。也可将工作空间划分成一个个小格，得到每个小格内拟人的关节角度，建立查找表，根据要求的末端位置，在查找表中找到冗余角q3，根据解析逆解求出其他关节角。MINH利用运动元的方式实现拟人运动。但这些研究工作的重点是在仿人达点运动，并没有考虑避障。

发明内容

本发明目的在于提供一种不依赖机器人构形，形式简单，计算量少，能够按需要得到优化解，具有通用性和快速性的基于构形平面的冗余机械臂运动控制方法。

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

步骤1：输入冗余机械臂结构参数，运用动力学对冗余机械臂进行构形平面划分；

步骤2：将已划分的构型平面按照构形平面匹配的方式进行冗余机械臂初规划，所述构形平面匹配以加权的空间矢量法进行；

步骤3：对规划的冗余机械臂的每个构形平面进行空间障碍物干涉检测，对产生干涉的构形平面进行重新调整和规划；

步骤4：对调整后的冗余机械臂规划进行关节的速度、加速度的样条曲线平滑处理，保证机械臂在运动过程中的平稳运动；

步骤5：对整个运动过程进行动力学校核，避免冗余机械臂在运动过程出现的过载荷、超速的现象。

本发明总结串联形式的冗余机械臂结构特点以构形平面为基础，针对具体形式的串联冗余机械臂工作构形。该方法形式简洁，具有很高的精度和求解速度，具有很强的实用性和通用性。

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

(1)结合冗余机械臂的结构特点和工作方式，将构形平面引入到冗余机械臂的运动控制中，该方法能够直观快速实现冗余机械臂的运动控制规划。

(2)以构形平面为基础，通过空间矢量引导的方式对构形平面进行空间位置确定，进而确定较为合理的冗余机械臂空间位形。该方法避免了采用传统解析方法依赖机器人构形形式和自由度，也避免数值方法的求解精度和求解速度问题，具有实用性和通用性。

(3)基于构形平面的动力学方法。该方法以构形平面为单位，减少推算和求解步骤，能够快速校核机械臂在规划过程中的关节运动性能，为优化和调整机械臂规划提供依据。

附图说明

图1构形平面划分示意图附图。

图2构形平面内的障碍物干涉检测。

图3空间构形平面位置确定。

图4已知轨迹点的构形平面规划。

图5运动控制流程图。

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

图7各关节的角速度变化曲线。

图8各关节的力矩变化曲线。

图9a-图9d冗余机械臂运动过程中的截图。

具体实施方式

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

结合图5，本发明的方法的整体步骤如下：

步骤1：给定冗余机械臂结构参数，运用动力学对冗余机械臂进行构形平面划分。

步骤2：按照构形平面匹配的方式进行冗余机械臂初规划，构形平面匹配以加权的空间矢量法进行。

步骤3：对规划的冗余机械臂的每个构形平面进行空间障碍物干涉检测，对产生干涉的构形平面进行重新调整和规划。

步骤4：对调整后的冗余机械臂规划进行关节的速度、加速度的样条曲线平滑处理，保证机械臂在运动过程中的平稳运动。

步骤5：对整个运动过程进行动力学校核，避免冗余机械臂在运动过程出现的过载荷、超速的现象。

结合图1，

机械臂的构形平面定义为若干个依次连接的机器人关节的中心轴线所形成的平面。按照空间几何的原理，两个平面相较于线，回转关节的中心轴线作为构形平面的间的分界线。为此可将构形平面的划分为如下几类：

(1)构形平面末端位置不变，姿态不变：此类构形平面所包含模块仅为连接模块，不存在摇摆模块、回转模块、移动模块等存在运动量变化的模块；

(2)构形平面末端位置变化，姿态不变：此类构形平面中包含连接模块和移动模块，不存在摇摆模块和回转模块；

(3)构形平面末端位置不变，姿态变化：此类构形平面中包含回转模块和基本连接模块，若存在角度连接模块，则机器人拓扑结构中，角度连接模块相比基本连接模块更靠近构形平面的中心；

(4)构形平面末端位置变化，姿态变化：根据构形平面的定义，无论组成构形平面的机器人运动模块如何运动，其位置和姿态都是在二维平面能变化。角度连接模块和回转模块成为构形平面分界点。回转关节作为特定的构形平面分界关节，则摇摆关节和移动关节的数量是构形平面中的自由度数目。

结合图2，空间障碍物被构形平面所在二维平面横切成截图图形，这个截面图形可能很不规则，采用较为简单的依次相连的直线段进行包覆，使所包覆图形近似规则图形，所包覆区域不标注为障碍物区域1，结合附图4，考虑到机器人连杆具有外形尺寸和安全距离，因此在障碍物区域1外扩距离k，这样所得障碍物的实际区域标注为区域2。

设安全避障区域由m线段组成，则第i段线段可由下式表示：

$z=\frac{(z_i-z_{i-1})(x-x_{i-1})}{x_i-x_{i-1}}+z_{i-1}---\left(1\right)$

机器人构形由n段线段组成，则第j段线段由下式表示：

$z=\frac{(z_j-z_{j-1})(x-x_{j-1})}{x_j-x_{j-1}}+z_{j-1}---\left(2\right)$

其中：x_i，z_i为第i个关节在空间坐标系下x轴，z轴方向上的坐标值；

当存在障碍干涉状况时，意味着机器人关节部分与组成安全障碍区域的边界线段存在干涉。为了提高整体机器人规划计算效率，节省计算时间，仅计算构形平面中与障碍物区域存在干涉的机械臂关节即可。对于出现干涉情况的机械臂关节，按照构形平面的位形匹配方法重新进行调配，满足规划要求。

平面几何中，两条直线要么平行要么相交，因此，为了提高判断过程的效率，首先比较公式(1)和公式(2)所示的两个线段的斜率，是否相等。若相等，意味着两天线段没有交点，则不会发生干涉，满足规划要求。若不等则求解两条线段所在直线的交点，若交点不在其中任何一个线段上，则机械臂与障碍物不发生干涉，满足规划要求，否则发生干涉。

按照冗余机械臂轨迹规划的过程，构形平面的中心和末端不能够在障碍区域内的，否则冗余机械臂就不能完成工作了。因此存在障碍干涉状况，是机器人关节部分与组成安全障碍区域的边界线段存在干涉。为了提高整体机器人规划计算效率，只需在该构形平面中的干涉关节进行重新调整，在干涉关节的安全障碍区域内并且靠近机器人基座一侧的边界线段之间进行判断。

结合图3，构形平面的空间位置确定采用加权空间矢量法，P_i为机械臂末端轨迹规划中的第i点，O₀为机械臂极坐标系原点，连接P_iO₀，k_j(j＝1,…n)为构形平面i的起点到末端点的连线。

以P_iO₀向量为导向，规划构形平面的k_j(j＝1,…n)。k₁的值与初始关节有关，通常其空间矢量方向是已知，而k_n与机械臂末端关节有关，其空间矢量的方向和大小都是固定的。在保证机械臂工作空间的条件下，规划k_j(j＝1,…n-1)。由于k_j(j＝1,…n-1)不是实际机械臂关节中心轴线，因此k_j(j＝1,…n-1)可能与空间障碍物发生重叠干涉。对构形平面与障碍物发生重叠干涉的部分，运用介绍的方法对该构形平面中机械臂关节与障碍物进行干涉判断检查。

以P_iO₀向量为导向，规划构形平面的k_j(j＝1,…n)。k₁的值与初始关节有关，通常其空间矢量方向是已知，而k_n与机械臂末端关节有关，其空间矢量的方向和大小都是固定的。在保证机械臂工作空间的条件下，规划k_j(j＝1,…n-1)。由于k_j(j＝1,…n-1)不是实际机械臂关节中心轴线，因此k_j(j＝1,…n-1)可能与空间障碍物发生重叠干涉。对构形平面与障碍物发生重叠干涉的部分，运用介绍的方法对该构形平面中机械臂关节与障碍物进行干涉判断检查。

而在实际运算中比较简单，因为构形平面已经简化机械臂的中具有冗余功能的关节，组成机械臂构形平面数量较少，通常在3-5左右，在规划少量构形平面后，剩余的2个构形平面就可通过解析的方式确定其空间位置。

在空间障碍物对机械臂工作空间占用不多的情况下，机械臂的构形平面空间位置有了更多的选择。结合附图4，左侧图和右侧图是在相同的轨迹条件下的两种状态，由于运动轨迹的要求，机械臂最后关节所在的构形平面CPn是确定的，故两种状态下构形平面CPn是相同的。

这种情况下，要对机械臂的空间轨迹规划进行整体考虑。以上一个轨迹点的工作构形为参考，建立评价函数。

$f=\underset{h=1}{\overset{m}{\Σ}}{u}_h({\mathrm{\θ}}_i^h-{\mathrm{\θ}}_i^{,h})---\left(3\right)$

式中u_h(h＝1…m)为加权值，该加权值与第h关节的功率和位置有关，功率越大和靠近基座越近的关节，该加权值越大，在多种选择条件下，通过该加权值可使得功率大的关节运动幅度，提高机械臂的动力学功能；f用来评价当前轨迹点与上一个轨迹点的工作构形的差别，在多种选择条件下力求最小。

为保证机械末端运动平稳，采用B样条曲线的进行轨迹规划。使用B样条曲线的方法，使得机器人的速度、加速度、加加速度以及机器人的起始点和终止点的位置能够稳定且符合要求。

动力学校核方法，

基于构形平面的动力学校核方法分为两个部分：构形平面内动力学分析方法和构形平面间的动力学分析方法。由驱动器施加力矩或者施加在操作臂上的外力使操作臂运动，构型平面内动力学分析方法主要研究本构型平面内第一个关节和最后一个关节力、力矩之间的关系；构型平面间动力学分析方法主要研究本构型平面内最后一个关节和下一个构型平面第一个关节力、力矩之间的关系。

(1)构形平面内动力学分析方法

结合附图1，第k个构形平面的自由度是n，关节a为组成第k个构形平面的第一个关节，由于关节b是回转关节，在考虑角速度，角加速度叠加公式时，为了方便计算，把关节b划分到构形平面k+1中。

1)速度公式

^aω_a为关节a的角速度，设构形平面的自由度是n，已知构形平面的起点的速度^aω_a， 通过速度公式求得构形平面的最后一个关节的速度^a+nω_a+n， ^a+nv_a+n，

${\mathrm{\ω}_{a+n}}_{a+n}=R_a^{a+n}{\mathrm{\ω}_a}_a\underset{j=a+1}{\overset{a+n}{\Σ}}{\stackrel{\·}{q}}_j{e_j}_j---\left(4\right)$

${\stackrel{\·}{\mathrm{\ω}}_{a+n}}_{a+n}=R_a^{a+n}{\stackrel{\·}{\mathrm{\ω}}_a}_{a}R_a^{a+n}{\mathrm{\ω}_a}_a\×\underset{j=a+1}{\overset{a+n}{\Σ}}{\stackrel{\·}{q}}_j{e_j}_j+\underset{j=a+1}{\overset{a+n}{\Σ}}{\stackrel{\·\·}{q}}_j{e_j}_j---\left(5\right)$

结合图1，在构形平面k的质心k_c出建立坐标，设此坐标平行于此构形平面起点处的坐标,表示质心k_c在坐标系a+n下的坐标，则

${\mathrm{\ρ}_{a+n}}_{k_c}\underset{i=a}{\overset{a+n}{\Σ}}{m}_i=\underset{i=a}{\overset{a+n}{\Σ}}R_i^{a+n}{m}_i{\mathrm{\ρ}_i}_i---\left(6\right)$

${\mathrm{\ω}_{a+n}}_{k_c}={\mathrm{\ω}_{a+n}}_{a+n}---\left(7\right)$

${\stackrel{\·}{\mathrm{\ω}}_{a+n}}_{k_c}={\stackrel{\·}{\mathrm{\ω}}_{a+n}}_{a+n}---\left(8\right)$

${\stackrel{\·}{v}_{a+n}}_{k_c}={\stackrel{\·}{\mathrm{\ω}}_{a+n}}_{k_c}\×{\mathrm{\ρ}_{a+n}}_{k_c}+{\mathrm{\ω}_{a+n}}_{k_c}\×\left({\mathrm{\ω}_{a+n}}_{k_c}\×{\mathrm{\ρ}_{a+n}}_{k_c}\right)R_a^{a+n}{\stackrel{\·}{v}_a}_a---\left(9\right)$

${F_{a+n}}_{k_c}=\underset{j=a}{\overset{a+v}{\Σ}}{m}_j{\stackrel{\·}{v}_{a+n}}_{k_c}---\left(10\right)$

设构形平面质心k_c到各个关节的质心的坐标为(j＝a到a+n)，则各个关节的质心的坐标到构形平面质心k_c的坐标为

${I_{k_c}}_{a+n}=\underset{j=a}{\overset{a+n}{\Σ}}R_j^{a+n}\left({I_{c_j}}_j+m_j\left({\mathrm{\ρ}_{k_c}}_j{\mathrm{\ρ}_{k_c}}_j{I_T}_3-{\mathrm{\ρ}_{k_c}}_j{\mathrm{\ρ}_{T_{k_c}}}_j\right)\right){R_j^{a+n}}^T---\left(11\right)$

${M_{a+n}}_{k_C}={I_{k_c}}_{a+n}{\stackrel{\·}{\mathrm{\ω}}_{a+n}}_k+{\mathrm{\ω}_{a+n}}_k\×{I_{k_c}}_{a+n}{\mathrm{\ω}_{a+n}}_k---\left(12\right)$

2)力和力矩公式

已知力^a+nF_a+n，力矩求^aF_a，

${M_a}_{J_a}=R_{a+n}^a\left({M_{a+n}}_{J_{a+n}}+{M_{a+n}}_{k_c}+{\mathrm{\ρ}_{a+n}}_{k_c}\×{F_{a+n}}_{K_C}\right)---\left(13\right)$

${Q_a}_a={M_a}_{J_a}*{e_a}_a---\left(14\right)$

(2)构形平面间动力学分析方法

设关节a为构形平面的最后一个关节，则关节a+1为与之相邻的下一个构形平面的第一个关节

1)速度公式

在式(7)、式(8)中，令^aω_a、为上一个构形平面的最后一个关节的角速度，角加速度，i＝1，则与之相邻的下一个构形平面的第一个关节的角速度就可以求出。关节a所在的构形平面为第一个构形平面，则^aω_a、为基座标的角速度，角加速度，若基座标不动，则 ${\mathrm{\ω}_a}_a={\stackrel{\·}{\mathrm{\ω}}_a}_a=0.$

${\mathrm{\ω}_{a+1}}_{a+1}=R_a^{a+1}{\mathrm{\ω}_a}_a+{\stackrel{\·}{q}}_{a+1}{e_{a+1}}_{a+1}---\left(15\right)$

${\stackrel{\·}{\mathrm{\ω}}_{a+1}}_{a+1}=R_a^{a+1}{\stackrel{\·}{\mathrm{\ω}}_a}_a+{P_{a+1}}_{a+1}R_a^{a+1}{\mathrm{\ω}_a}_a\×{\stackrel{\·}{q}}_{a+1}{e_{a+1}}_{a+1}+{\stackrel{\·\·}{q}}_{a+1}{e_{a+1}}_{a+1}---\left(16\right)$

${\stackrel{\·}{v}_{a+1}}_{a+1}=R_a^{a+1}\left({\stackrel{\·}{\mathrm{\ω}}_a}_a\×{l_a}_{a+1}+{\mathrm{\ω}_a}_a\×\left({\mathrm{\ω}_a}_a\×{l_a}_{a+1}\right)+{\stackrel{\·}{v}_a}_a\right)---\left(17\right)$

${\stackrel{\·}{v}_{a+1}}_{c_{a+1}}={\stackrel{\·}{\mathrm{\ω}}_{a+1}}_{a+1}\×{\mathrm{\ρ}_{a+1}}_{a+1}+{\mathrm{\ω}_{a+1}}_{a+1}\×\left({\mathrm{\ω}_{a+1}}_{a+1}\×{\mathrm{\ρ}_{a+1}}_{a+1}\right)+{\stackrel{\·}{v}_{a+1}}_{c_{a+1}}---\left(18\right)$

${F_{a+1}}_{a+1}=m_{a+1}{\stackrel{\·}{v}_{a+1}}_{c_{a+1}}---\left(19\right)$

${M_{a+1}}_{a+1}={I_{c_{a+1}}}_{a+1}{\stackrel{\·}{\mathrm{\ω}}_{a+1}}_{a+1}+{\mathrm{\ω}_{a+1}}_{a+1}\×{I_{c_{a+1}}}_{a+1}{\mathrm{\ω}_{a+1}}_{a+1}---\left(20\right)$

2)力和力矩公式

最后一个关节(机械臂所有关节数为r)如果末端操作手在空间是自由的，则M_end、F_end等于零。

${F_a}_{J_a}=R_{a+1}^a{F_{a+1}}_{J_{a+1}}+{F_a}_a---\left(21\right)$

${M_a}_{J_a}={M_a}_a+R_{a+1}^a{M_{a+1}}_{J_{a+1}}+{\mathrm{\ρ}_a}_a\×{F_a}_a+{l_a}_{a+1}\×R_{a+1}^a{F_{a+1}}_{J_{a+1}}---\left(22\right)$

${Q_a}_a={M_a}_{J_a}*{e_a}_a---\left(23\right)$

考虑连杆本身重量的影响时，令即机器人基座受到的支撑作用相当向上的重力加速度g。这样处理与各模块重力的影响完全一样。

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

表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

机械臂轨迹规划任务要求机械臂末端点轨迹为一条直线段，机械臂运动过程中，手爪夹持面垂直该直线轨迹，在机械臂工作空间中存在空间障碍物。运动过程为加速-匀速-减速。

线段起始点位姿矩阵为：

$\left[\begin{array}{cccc}0.9511& -0.3089& 0& 0\\ 0& 0& 1& 140.5\\ -0.3089& -0.9511& 0& 35\\ 0& 0& 0& 1\end{array}\right]---\left(24\right)$

线段终点位姿矩阵为：

$\left[\begin{array}{cccc}0.7279& -0.6487& -0.2161& -68.6079\\ 0.5703& 0.4030& 0.7158& 92.2059\\ -0.3773& -0.6456& 0.6640& 93.2544\\ 0& 0& 0& 1\end{array}\right]---\left(25\right)$

根据任务轨迹的起点和终点的空间位置，进行空间轨迹插补计算，整体分成10段，共11个空间轨迹点，轨迹点的空间坐标如表2所示。

表2机械臂插补轨迹点

插补点	px	py	pz
				1	0	140.5	35
2	-6.8608	135.6706	40.8254
				3	-13.7216	130.8412	46.6509
4	-20.5824	126.0118	52.4763
				5	-27.4431	121.1824	58.3017
6	-34.3039	116.353	64.1272
				7	-41.1647	111.5236	69.9526
8	-48.0255	106.6942	75.7781
				9	-54.8863	101.8648	81.6035
10	-61.7471	97.0353	87.429
				11	-68.6079	92.2059	93.2544

根据表2所示的空间轨迹点的坐标，按照构形平面轨迹规划方法，通过构形平面的匹配，同时计算机械臂和空间障碍物的干涉检测，从起点起始，确定冗余机械各关节角的数值，如表3所示。

表3机械臂各关节在插补点的角度

	θ1	θ2	θ3	θ4	θ5	θ6	θ7
								1	0	0	0	0	13.58	0	94.41
2	7.72	-18.55	30.27	-9.1	11.76	-20.3	54.15
								3	10.22	-23.5	39.6	-8.1	6.41	-37.82	47.35
4	14.22	-31.8	48.84	-9.5	6.46	-40.73	47.96
								5	17.22	-34.94	52.05	-6.71	9.5	-49.44	52.9
6	25.05	-37.82	50.37	-4.15	36.61	-52.82	76.45
								7	27.15	-40.02	51.48	-0.78	31.2	-58.19	77.16
8	31.2	-43.03	51.7	-0.83	31	-57.9	80.82
								9	35.15	-45.77	51.16	-1.24	29.94	-55.47	83.94
10	38.91	-48.13	49.69	-1.98	27.87	-50.74	86.01
								11	42.4	-49.75	46.8	-2.91	24.53	-43.69	86.41

运用构形平面动力学方法对整体冗余机械臂空间规划进行动力学校核，所得到的各关节角速度变化曲线见图7所示。运动过程中的力矩曲线见图8所示，整体运动过程见图9a-图9d所示。

Claims (5)

1.一种基于构形平面的冗余机械臂运动控制方法，其特征是包括以下步骤：

步骤1：输入冗余机械臂结构参数，运用动力学对冗余机械臂进行构形平面划分；

步骤2：将已划分的构型平面按照构形平面匹配的方式进行冗余机械臂初规划，所述构形平面匹配以加权的空间矢量法进行；

步骤3：对规划的冗余机械臂的每个构形平面进行空间障碍物干涉检测，对产生干涉的构形平面进行重新调整和规划；

步骤4：对调整后的冗余机械臂规划进行关节的速度、加速度的样条曲线平滑处理；

步骤5：对整个运动过程进行动力学校核，避免冗余机械臂在运动过程出现的过载荷、超速的现象。

2.根据权利要求1所述的基于构形平面的冗余机械臂运动控制方法，其特征是所述构形平面划分包括如下几类：

(1)构形平面末端位置不变，姿态不变：此类构形平面所包含模块仅为连接模块，不存在摇摆模块、回转模块、移动模块存在运动量变化的模块；

(2)构形平面末端位置变化，姿态不变：此类构形平面中包含连接模块和移动模块，不存在摇摆模块和回转模块；

(3)构形平面末端位置不变，姿态变化：此类构形平面中包含回转模块和基本连接模块，若存在角度连接模块，则机器人拓扑结构中，角度连接模块相比基本连接模块更靠近构形平面的中心；

(4)构形平面末端位置变化，姿态变化：根据构形平面的定义，无论组成构形平面的机器人运动模块如何运动，其位置和姿态都是在二维平面能变化，角度连接模块和回转模块成为构形平面分界点；

回转关节作为特定的构形平面分界关节，摇摆关节和移动关节的数量是构形平面中的自由度数目。

3.根据权利要求1所述的基于构形平面的冗余机械臂运动控制方法，其特征是所述的构形平面匹配以加权的空间矢量法具体包括：

P_i为机械臂末端轨迹规划中的第i点，O₀为机械臂极坐标系原点，连接P_iO₀，k_j为构形平面i的起点到末端点的连线、j＝1，…n，以P_iO₀向量为导向，规划构形平面的k_j；k₁的值与初始关节有关，k₁空间矢量方向是已知，k_n与机械臂末端关节有关，k_n空间矢量的方向和大小都是固定的，在保证机械臂工作空间的条件下，规划k_j,j＝1，…n-1，由于k_j,j＝1,…n-1不是实际机械臂关节中心轴线，因此k_j,j＝1,…n-1可能与空间障碍物发生重叠干涉，对构形平面与障碍物发生重叠干涉的部分，对该构形平面中机械臂关节与障碍物进行干涉判断检查。

4.根据权利要求1所述的基于构形平面的冗余机械臂运动控制方法，其特征是所述的空间障碍物干涉检测方法具体包括：

空间障碍物被构形平面所在二维平面横切成截图图形，这个截面图形不规则，采用依次相连的直线段进行包覆，使所包覆图形近似规则图形，所包覆区域不标注为障碍物区域1，在障碍物区域1外扩距离k，所得障碍物的实际区域标注为区域2；

设安全避障区域由m线段组成，则第i段线段由下式表示：

$z=\frac{(z_i-z_{i-1})(x-x_{i-1})}{x_i-x_{i-1}}+z_{i-1}---\left(1\right)$

机器人构形由n段线段组成，则第j段线段由下式表示：

$z=\frac{(z_j-z_{j-1})(x-x_{j-1})}{x_j-x_{j-1}}+z_{j-1}---\left(2\right)$

其中：x_i，z_i为第i个关节在空间坐标系下x轴，z轴方向上的坐标值；

首先比较公式(1)和公式(2)所示的两个线段的斜率是否相等，若相等，意味着两天线段没有交点，则不会发生干涉，满足规划要求；若不等则求解两条线段所在直线的交点，若交点不在其中任何一个线段上，则机械臂与障碍物不发生干涉，满足规划要求，否则发生干涉。

5.根据权利要求1所述的基于构形平面的冗余机械臂运动控制方法，其特征是所述的动力学校核方法具体包括：

基于构形平面的动力学校核方法分为两个部分：构形平面内动力学分析方法和构形平面间的动力学分析方法；

(1)构形平面内动力学分析方法

第k个构形平面的自由度是n，关节a为组成第k个构形平面的第一个关节，关节b是回转关节，把关节b划分到构形平面k+1中；

1)速度公式

^aω_a为关节a的角速度，设构形平面的自由度是n，已知构形平面的起点的速度^aω_a， 通过速度公式求得构形平面的最后一个关节的速度^a+nω_a+n， ^a+nv_a+n，

${\mathrm{\ω}_{a+n}}_{a+n}={R_a^{a+n}}^a{\mathrm{\ω}}_a+\underset{j=a+1}{\overset{a+n}{\Σ}}{{\stackrel{\·}{q}}_j}^j{e}_j---\left(3\right)$

${\stackrel{\·}{\mathrm{\ω}}_{a+n}}_{a+n}={R_a^{a+n}}^a{\stackrel{\·}{\mathrm{\ω}}}_a+{R_a^{a+n}}^a{\mathrm{\ω}}_a\×\underset{j=a+1}{\overset{a+n}{\Σ}}{{\stackrel{\·}{q}}_j}^j{e}_j+\underset{j=a+1}{\overset{a+n}{\Σ}}{{\stackrel{\·\·}{q}}_j}^j{e}_j---\left(4\right)$

在构形平面k的质心k_c出建立坐标，设此坐标平行于此构形平面起点处的坐标,表示质心k_c在坐标系a+n下的坐标，则

${\mathrm{\ρ}_{a+n}}_{k_c}\underset{i=a}{\overset{a+n}{\Σ}}{m}_i=\underset{i=a}{\overset{a+n}{\Σ}}R_i^{a+n}{m_i}^i{\mathrm{\ρ}}_i---\left(5\right)$

${\mathrm{\ω}_{a+n}}_{k_c}={\mathrm{\ω}_{a+n}}_{a+n}---\left(6\right)$

${\stackrel{\·}{\mathrm{\ω}}_{a+n}}_{k_c}={\stackrel{\·}{\mathrm{\ω}}_{a+n}}_{a+n}---\left(7\right)$

${\stackrel{\·}{v}_{a+n}}_{k_c}={\stackrel{\·}{\mathrm{\ω}}_{a+n}}_{k_c}\×{\mathrm{\ρ}_{a+n}}_{k_c}+{\mathrm{\ω}_{a+n}}_{k_c}\×({\mathrm{\ω}_{a+n}}_{k_c}\×{\mathrm{\ρ}_{a+n}}_{k_c})+{\stackrel{\·}{v}_a^{a+n}}_a---\left(8\right)$

${F_{a+n}}_{k_c}=\underset{j=a}{\overset{a+n}{\Σ}}{m_j}^{a+n}{\stackrel{\·}{v}}_{k_c}---\left(9\right)$

设构形平面质心k_c到各个关节的质心的坐标为j＝a到a+n，则各个关节的质心的坐标到构形平面质心k_c的坐标为

${I_{k_c}}_{a+n}=\underset{j=a}{\overset{a+n}{\Σ}}R_j^{a+n}({I_{c_j}}_j+m_j(\mathrm{\ρ}_{k_c}{{}_j}^{k_c}{{\mathrm{\ρ}}_j}^T{I}_3-\mathrm{\ρ}_{k_c}{{{}_j}^T}^{k_c}{\mathrm{\ρ}}_j\left)\right){R_j^{a+n}}^T---\left(10\right)$

${M_{a+n}}_{k_C}={I_{k_c}}_{a+n}{\stackrel{\·}{\mathrm{\ω}}_{a+n}}_k+{\mathrm{\ω}_{a+n}}_k\×{I_{k_c}}_{a+n}{\mathrm{\ω}_{a+n}}_k---\left(11\right)$

2)力和力矩公式

已知力^a+nF_a+n，力矩求^aF_a，

${M_a}_{J_a}=R_{a+n}^a({M_{a+n}}_{J_{a+n}}+{M_{a+n}}_{k_C}+{\mathrm{\ρ}_{a+n}}_{k_c}\×{F_{a+n}}_{k_C})---\left(12\right)$

${Q_a}_a={M_a}_{J_a}*{e_a}_a---\left(13\right)$

(2)构形平面间动力学分析方法

设关节a为构形平面的最后一个关节，则关节a+1为与之相邻的下一个构形平面的第一个关节

1)速度公式

在式(6)、式(7)中，令^aω_a、为上一个构形平面的最后一个关节的角速度，角加速度，i＝1，则求出与之相邻的下一个构形平面的第一个关节的角速度；若关节a所在的构形平面为第一个构形平面，则^aω_a、为基座标的角速度，角加速度，若基座标不动，则

${\mathrm{\ω}_{a+1}}_{a+1}={R_a^{a+1}}^a{\mathrm{\ω}}_a+{{\stackrel{\·}{q}}_{a+1}}^{a+1}{e}_{a+1}---\left(14\right)$

${\stackrel{\·}{\mathrm{\ω}}_{a+1}}_{a+1}={R_a^{a+1}}^a{\stackrel{\·}{\mathrm{\ω}}}_a+{P_{a+1}}_{a+1}{R_a^{a+1}}^a{\mathrm{\ω}}_a\×{\stackrel{\·}{q}}_{a+1}{e_{a+1}}_{a+1}+{\stackrel{\·\·}{q}}_{a+1}{e_{a+1}}_{a+1}---\left(15\right)$

${\stackrel{\·}{v}_{a+1}}_{a+1}=R_a^{a+1}({\stackrel{\·}{\mathrm{\ω}}_a}_a+{l_a}_{a+1}{\mathrm{\ω}_a}_a\×({\mathrm{\ω}_a}_a\×{l_a}_{a+1})+{\stackrel{\·}{v}_a}_a)---\left(16\right)$

${\stackrel{\·}{v}_{a+1}}_{c_{a+1}}={\stackrel{\·}{\mathrm{\ω}}_{a+1}}_{a+1}\×{\mathrm{\ρ}_{a+1}}_{a+1}+{\mathrm{\ω}_{a+1}}_{a+1}\×({\mathrm{\ω}_{a+1}}_{a+1}\×{\mathrm{\ρ}_{a+1}}_{a+1})+{\stackrel{\·}{v}_{a+1}}_{a+1}---\left(17\right)$

${F_{a+1}}_{a+1}=m_{a+1}{\stackrel{\·}{v}_{a+1}}_{c_{a+1}}---\left(18\right)$

${M_{a+1}}_{a+1}={I_{c_{a+1}}}_{a+1}{\stackrel{\·}{\mathrm{\ω}}_{a+1}}_{a+1}+{\mathrm{\ω}_{a+1}}_{a+1}\×{I_{c_{a+1}}}_{a+1}{\mathrm{\ω}_{a+1}}_{a+1}---\left(19\right)$

2)力和力矩公式

最后一个关节如果末端操作手在空间是自由的，则M_end、F_end等于零，

${F_a}_{J_a}={R_{a+2}^a}^{a+1}{F}_{J_{a+1}}+{F_a}_a---20)$

${M_a}_{J_a}={M_a}_a+{R_{a+1}^a}^{a+1}{M}_{J_{a+1}}+{\mathrm{\ρ}_a}_a\×{F_a}_a+{l_a}_{a+1}\×{R_{a+1}^a}^{a+1}{F}_{J_{a+1}}---\left(21\right)$

${Q_a}_a={M_a}_{J_a}*{e_a}_a---\left(22\right)$

考虑连杆本身重量的影响时，令即机器人基座受到的支撑作用相当向上的重力加速度g；

其中：ⁱw_i：第i个关节的角速度在第i个关节坐标系下的表示；

ⁱv_i：第i个关节的线速度在第i个关节坐标系下的表示；

第i个关节的角加速度在第i个关节坐标系下的表示；

第i个关节的线加速度在第i个关节坐标系下的表示；

q_j：第j个关节的角位移量；

第j个关节的自身角加速度；

ⁱe_i：坐标系{O_i}下的Z轴单位分量；

ⁱl_i+1：第i+1个关节的长度在第i个关节坐标系下的表示；

关节i-1对连杆i的作用力在{O_i}下的表示；

关节i-1对连杆i的作用力矩在{O_i}下的表示；

ⁱF_i：连杆i-1对连杆i的作用力在{O_i}下的表示；

ⁱM_i：连杆i-1对连杆i的作用力矩在{O_i}下的表示；

构型平面质心处所受合力；

构型平面质心处所受合力矩；

ⁱQ_i：i关节力矩在其坐标系下的Z轴分量；

m_i：第i个关节的质量；

F_end：机械臂末端所受外力；

M_end：机械臂末端所受外力矩。