CN104965517B - 一种机器人笛卡尔空间轨迹的规划方法 - Google Patents

Abstract

本发明公开了一种机器人笛卡尔空间轨迹的规划方法，其过程如下：建立连杆坐标系，由运动学建模分析方法得正运动学方程；通过机器人所具有的向量几何性质和轨迹规划的形位要求，求解主控关节和中间关节的转动角度；利用运动学建模分析方法，利用已求解的关节转动角度，寻求含有已引入变量的关系算式，求解相应关节的转动角度；当任务空间存在障碍时进行轨迹规划，通过向量几何法判定其是否为可达位姿；规划耦合位置信息的可连续时变姿态，进而完成规划任务。本发明避免了产生增根、筛选匹配有效解；可以有效规避奇异路径，同时可以避免和优化由关节空间规划出的末端轨迹复杂的缺陷等。

Description

一种机器人笛卡尔空间轨迹的规划方法

技术领域

本发明涉及一种机器人笛卡尔空间轨迹的规划方法，涉及逆运动学和姿态规划。

背景技术

工业机器人应用的深度与广度已成为衡量一个国家制造业水平和科技水平的重要标志。自从第一台工业机器人于1962年应用于General Motor公司生产线，机器人技术获得快速的发展，尤其，目前在人工成本不断提高和“工业4.0”概念提出的背景下，机器人革命有望成为“第三次工业革命”的重要切入点和增长点，而目前我国已经成为全球最大的机器人市场，2015年5月国务院印发《中国制造2025》，明确提出要重点支持机器人技术的发展和应用，将其列为十大重点领域之一。

运动学和轨迹规划是机器人技术实际应用的基础，其中轨迹规划包括位置和姿态两方面，尤其在笛卡尔空间的轨迹规划。工业实际应用中，当前在笛卡尔空间下进行轨迹规划多是在关节空间中进行，末端实际运动轨迹复杂，同时，实际应用中常要求末端以不同的姿态进行，而当前的规划方法存在易导致奇异性等弊端。其中，运动学问题是机器人运动控制和轨迹规划的基础，解析形式的机器人逆运动学问题是机器人研究中的难题，也是机器人研究领域的热点问题。

位置逆解问题是机器人运动规划和轨迹规划的基础，只有通过逆运动学把空间位姿转换为关节变量，才能实现对机械臂末端执行器的控制；而且机器人逆运动学求解的效率直接影响运动轨迹控制效果和作业效率。由于机器人正运动学是在已知各个关节角度求解末端的位置和姿态，相对简单且解唯一；逆运动学是是正运动学问题的逆过程，是已知机械臂末端的位置和姿态，计算各个关节转过的角度值，串联机器人逆向运动学的求解过程相对复杂，一般会出现多解或者甚至无解的情况。目前逆运动学求解算法分为两大类：封闭解/解析法和数值解法，其中，数值解法中的迭代过程会降低求解速度，不利于现代工业机械臂的实时控制；目前常用的数值法有如基于雅克比矩阵的牛顿-拉普松迭代法，根据建立的正运动模型进行迭代计算，得出机器人逆运动学结果，但是在计算时间和结果的精确性上很难同时保证，以及拟牛顿共轭梯度法，其存在雅克比矩阵奇异性，且算法初始状态的取值直接将影响算法的收敛度和求解精度，在计算时间和精确上很难同时保证；神经网络法是通过学习网络权、阙值参数建立笛卡尔空间下连杆构型同各个关节角度映射关系，而网络学习需要大量样本数据，在实际中却较难获取，同时，网络学习所需要时间较长，不适宜实时控制；另外，遗传算法是利用全局并行搜索特性，常规遗传算法在解决优化问题存在早熟即收敛速度慢等缺陷，降低优化性能，影响求解精度；神经网络、遗传算法和专家系统、模糊逻辑联合起来进行混合求解，在解决多自由度机器人逆运动学是需要复杂的算法程序和高性能的配置，所以，数值法计算耗时长，求解精度不高，不适宜高精度和实时性作业任务；遗传算法和神经网络方法可对求解进行优化，避免局部收敛，得出逆运动学结果，但与迭代方法一样，在计算时间和计算精度上存在制约性。

封闭解是实际应用中需要的，可以直接计算各关节角度理论值，不需要迭代搜索寻优，具有计算速度快，精度高，包含几何法和代数法，其中几何法被视为仅适用于结构简单的少自由度机械臂；代数法包括通过D-H建模结合逆矩阵方式和利用旋量结合指数积通过转化为 Paden-Kahan子问题求解等。前者是最常用的方法，即先将机器人各关节转动角度设为θ₁，θ₂，θ₃...θ_n，然后代入正运动学方程推导出

方程左边的数据已知，采取逆矩阵的方式分离变量，寻找其中姿态和位置间存在的等量关系，如：之后进行逐级分离变量，从而求得有关角度，但任意构造的方程可能导致其中的求解过程异常复杂以及后续需要进行有效解的寻值和匹配，需要更复杂的算法，所以要得到其中的有效解析解表达式，求解过程复杂，解算效率不高。欠自由度机械臂不能实现任意姿态和位置，当任意给定时，往往无解；国内外学者曾使用螺旋理论推导5R解析解的框架，几何法或代数法也曾分别被用来求解5R机械臂逆运动学解析解，一般无法或很难在执行具体任务前预先判定操作点是否在工作空间内，需要采用查表法等，而且，传统的求解算法通常存在解算范围小，需要匹配或通过其他算法选择最优解、特殊位置处需单独讨论、最优解不易直观确定等问题，严重影响机器人的响应速度。因此，位置逆解问题作为机器人运动学中最基础、最重要的研究问题之一，直接关系到运动分析、轨迹规划和实时控制等，甚至后续的速度和加速度分析。而且，逆运动学的求解速度和准确度直接影响工业机器人的实时控制的难以程度，也将直接决定机械臂执行复杂任务的能力。

而轨迹规划是逆运动学的实际应用，工业应用中运动轨迹规划的好坏直接影响机器人作业质量。机器人末端运动是由关节变量直接决定的，由于关节坐标空间和直角坐标空间转换关系复杂，同时在关节空间进行路径规划，计算比较简单，且不会产生机构奇异问题，即可以避免雅克比矩阵奇异时形成的速度失控现象，所以除了示教再现法以外，目前轨迹规划多数采取关节空间的规划方案，但其在直角坐标空间的最终轨迹，路径点之间的轨迹形状往往是十分复杂的，因此只有对初始点和最终期望点的位姿有要求时而对中间路径无要求的任务，才可以在关节空间直接进行路径规划。当末端轨迹形状有一定要求，在关节空间规划则很难或无法达到要求，比如，当需要进行连续轨迹的作业任务，则必须在直角坐标空间中规划出需要的轨迹。目前存在的直角坐标空间规划方法计算工作量大，以及姿态规划问题复杂，导致所规划的轨迹有可能接近或通过奇异点，造成无法解算的后果。尤其针对少自由度机械臂，当姿态规划不成功时，往往通过牺牲末端位姿精度来达到回避奇异位置的目标，然而当进行目标捕获、精细操作等时，末端位姿精度的牺牲影响任务的成功执行。

因此，在路径要求不高的抓取操作时在关节空间中进行规划尚可，但当有严格轨迹要求时，如焊接等，需要复杂的算法来保证精度，而在笛卡尔空间进行轨迹规划的精确性是基于关节空间规划的这种方法所无法比拟的。但在姿态规划方面，由于其所具有的非线性和耦合性，目前存在的算法多数采取插补方案进行，灵活性差，尤其在五自由度等少自由度机器人中，存在诸多重大缺陷，容易造成规划路径的奇异性，甚至无法求解的局面。

发明内容

针对现有技术中存在的技术问题，本发明的目的是：提供一种机器人空间轨迹的规划方法。

本发明通过以下步骤实现：

步骤1：建立连杆坐标系：将机器人连杆中基坐标系固连于基座，依次建立坐标系，并命名各个关节轴的转动角度分别为θ₁、θ₂、θ₃…θ_i；工业中各个厂家生产的机器人的初始状态及关节运动范围有所不同，但其理论模型一致；

步骤2：由运动学建模分析方法得正运动学方程：

以下为各个关节转动角度的求解过程：

步骤3：主控关节c转动角度θ_c的求解

令底部靠近基座的竖直关节为主控关节(controlling joint)，一般为靠近基座的第一个关节位置，距离末端执行器最近的具有水平关节轴关节th，其位置记为P_th，由末端执行器 (end effector)位置通过接近矢量(approaching vector)计算得到；当关节c处存在连杆偏距 a_c时，在底部具有水平轴关节bh附近建立虚关节(virtual joint)，其位置记为即如五关节型KUKAyouBot机械臂，当关节c处不存在连杆偏距a_c时，即连杆偏距 a_c＝0，则如五关节型Katana机械臂；令二者中间具有水平轴关节mh位置为 P_mh；由P_th、P_mh、空间三点构成空间三角形，其中P_th、P_mh空间两点得向量利用向量几何投影性质求得关节c转动角度θ_c；当存在沿或绕基坐标系x、y、z方向移动或转动时，投影位置受到来自沿轴向移动的影响，需结合末端姿态以求解θ_c。

因此，采用几何投影法和atan2函数，可求解实际机械臂在[-π，π]的转动的范围。

非高空顶部作业下，

此解由向量几何投影方式直接求得关节c的解，称之为θ_c的投影解；

此解为由投影解通过相位转换(二者相对坐标系原点中心对称)取得关节c的解，称之为θ_c的相位解；

针对进行高空顶部大幅度作业时，末端多次出现在不同象限，主控关节转动角度会在不同相位跳动，可对其进行优化。

步骤4：中间关节mh转动角度θ_mid的求解

求得关节c转动角度θ_c后，若关节c处存在连杆偏距a_c，则由投影解所得底部具有水平轴关节bh的实际所处位置P_bh为

则关节bh与关节th位置间的向量关系取模求得长度

由相位解所得关节bh的实际所处位置

则关节bh与关节th的向量关系取模求得长度

设关节bh与关节mh的连杆长度记为l_bm，关节mh与关节th的连杆长度记为l_mt；当关节 bh、mh和th间存在移动关节时，l_bm和l_mmt为含移动关节在内的各关节间的连杆长度。由P_bh、P_mh、P_th空间三点构成空间三角形，由三角形的边角关系余弦定理，求解出中间关节 mh转动角度θ_mid，即相应于θ_c的投影解情形下

∠θ_mid＝arccos(cos∠θ_mid)

同理，可求相应于θ_c相位解的θ_mid其他两个解；

步骤5：靠近基座具有水平轴关节bh转动角度θ_th的求解

针对机器人的非线性强耦合特点，相对于耦合问题的解耦处理方式，留元法采取先引入多变量，充分利用耦合关系，寻求相应关系的函数算式，避免可能带来的增根剔除、真解筛选、最优解寻值匹配等。通过Matlab等计算机代数系统的符号计算处理，可寻求关于θ_bh的函数算式，在处理过程，将θ_th描述为θ_c和θ_mid的函数，即θ_bh＝f₂(θ_c，θ_mid)。令k＝k(θ)，θ及θ_j表示已求解的关节转动角度，有k₁＝k₁(θ)，k₂＝k₂(θ)，则

k₁＝b*sin(θ_i+θ_j)+a*sinθ_i；k₂＝b*cos(θ_i+θ_j)+a*cosθ_i

其他算法通过消元求得θ_i，割裂了中间的耦合关系，后续获取组解需要寻值匹配；本发明提出留元法，即保留θ_i，不轻易消去θ_j，应用辅角公式，为解决耦合问题提供思路。方法一应用辅角公式，即留元三角法：

k₁＝(b*cosθ_j+a)*sinθ_i+b*sinθ_j*cosθ_i；k₂＝(b*cosθ_j+a)*cosθ_i-b*sinθ_j*sinθ_i令M＝b*cosθ_j+a、N＝b*sinθ_j，则

因为机器人各连杆长度不等，则M²+N²恒大于零，所以分母恒不等于0，即

a² +b² +2*a*b*cos θ _j ≢ 0

因为θ_i和的取值范围，令则关节i取值为θ_i＝θ_i+(1-sign(sn))*π；

以上步骤中采用三角辅角公式，其求解范围为[-π/2，π/2]，虽然目前绝大多数工业机械臂节活动范围均在上述求解范围内，也存在某种机械臂会略超出该范围，方法二通过联立方程法求解算法，求解范围为[-π，π]，即留元方程组法，联立以下二式求解：

k₁＝(b*cosθ_j+a)*sinθ_i+b*sinθ_j*cosθ_i；

k₂＝(b*cosθ_j+a)*cosθ_i-b*sinθ_j*sinθi可得：

则θ_i＝atan2(sinθ_i，cosθ_i)。

步骤6：调整关节ft转动角度θ_ft的求解

调整关节(fine-tune joint)相对自优关节(self-optimizing joint)，采取微调方式，寻求关系算式得到关节ft转动角度在[-π/2，π/2]内的取值，使得调整关节活动范围控制为能小不大，然后由自优关节so自优匹配，使之运动协调、节省能量并完成规划任务。因为当关节ft的转动范围超出[-π/2.π/2]时，采用atan2函数可以准确地计算出关节ft所应转动的角度值，但在实际生产轨迹规划中，会造成调整关节呈现大幅度范围内运动，因此当存在自优关节时，调整关节.ft转动角度为θ_ft＝atan(tanθ_ft)和

θ′_ft＝atan(tanθ_ft)-sign(θ_ft)*π；

步骤7：自优关节so转动角度θ_so的求解

当末端执行器执行具体任务时，目标的位置和姿态是已知的，此时末端执行器的位置姿态与其相对应，可得二者之间的关系算式。

实际任务操作时，目标物体的具体位置和姿态是已知的；

抓取目标物的方法具有以下两种：

抓取目标物方法一：

通过步骤1-7已求出除自优关节外的其他关节转动角度值，则此时末端执行器的位置姿态信息可由步骤2的正运动学求得，即末端执行器的位置姿态可表示为含有自优关节θ_so的函数，即

此时，末端接近矢量和目标物体的向量方向相同或相反，以及引目标物体的的方向相同或相反，由此可获取含有自优关节角的方程，联立求解，即可进行抓取操作；

抓取目标物的方法二：

通过步骤1-7除自优关节外的其他关节转动角度值已求得，令自优关节转动角度为0，即θ_so＝0，通过正运动学求得此时末端执行器的位姿信息为

则同时由目标位置姿态信息得

由向量的点积公式求得则θ_so＝arccos(cosθ_so)

通过在前添加符号或所求自优关节转动角度±π获得其他姿态的操作。

步骤8：轨迹规划时可达位姿简易判定方法——向量几何法的应用

轨迹规划时，末端执行器的位置处于机械臂工作空间内，以某特定末端姿态是否可达，尤其对于欠自由度机械臂，可通过P_th、P_mh、P_bh空间三点是否可以构成空间三角形为依据进行操作前的判定，即计算出或取模得到其长度lP_bhth或以lPb_hth、l_bm、l_mt或l_bm、l_mt三者作为空间三角形三边，通过三角形的边角关系判定所规划的末端轨迹是否可达；即在实际任务规划中，在θ_c投影解情形下判定方式为

l_bm+l_mt＝＞lP_bhth，而在θ_c相位解情形下

步骤9：面向可连续时变姿态规划方法

机器人领域表示位置和姿态有多种不同参数化的数学形式，如变换矩阵法、矢量法、旋量法、四元数法、李群法和罗德里格斯参数及上述混合形式的对偶矩阵、四元数向量对、欧拉角向量对等，各方法之间存在转化关系。此处本着简单易懂易用，通过以欧拉角表述末端执行器姿态的变换矩阵法具体详述末端执行器的姿态规划为位置P(P_x，P_y，P_z)的函数f(P，t) 的过程。

设基坐标系xyz固连于基座，末端执行器坐标系的接近矢量方向为Z轴，取Y轴平行于基坐标系xoy面，由右手定则确定X轴。将末端执行器接近目标的接近矢量所在平面规划为以基坐标系z轴为母线，沿以末端执行器位置在基坐标系中的投影与基坐标系原点的连线为准线所形成的柱面及其相交面，将末端执行器的姿态规划为末端执行器位置P(P_x，P_y，_Pz)的函数f(P，t)；当机器人存在沿或绕基坐标系x、y、z方向移动或转动时，将末端执行器的接近矢量平移至基坐标系原点处理。

1)末端执行器接近目标的接近矢量所在柱面可由以基坐标系z轴为母线，沿以末端执行器位置投影与基坐标系原点的连线为准线所形成的。

准线方程为

则母线平行于Z轴的柱面方程为cos(atan2(P_y，P_x))*y-sin(atan2(P_y，P_x))*x＝0

所规划的Z轴在基坐标系中的表示为：

其中，α＝atan2(P_y，P_x)

结合欧拉角进行姿态规划，绕Y轴旋转β角其相应转换矩阵为

则最终规划姿态矩阵为O_Z*R_Y；

其中由姿态矩阵中接近矢量和位置矢量组成矩阵：

其中β＝β(t)可为变量或常量；

2)末端执行器接近目标的接近矢量亦可位于规划投影柱面相交面时，其相应矩阵为

基于(1)则最终所规划的姿态为O_Z*R_Y*R_x；

其中由姿态矩阵中接近矢量和位置矢量组成矩阵：

其中α＝atan2(P_y，P_x)且β＝β(t)和γ＝γ(t)可为变量或常量；

当γ＝0时，投影柱面相交面与投影柱面重合，因此，(1)所规划姿态规划是本姿态规划的子集。

3)工业生产中常用的数控机床、人形机器人及并联机器人的关节中存在沿或绕基坐标系 x、y、z方向移动或转动，将接近矢量以原点处矢量处理，替代沿以末端执行器位置的投影线为准线的规划方式：设以竖直为初始位形，末端坐标系与设定的基坐标系相同，则规划所在柱面所取的母线可选取为x正向(一般数控机床中z向为刀轴方向，仿人机器人中x为前行方向)，其他情形下不同的定义方案，可进行简单的转换获得。

初始末端姿态相应于设定基坐标系为：

结合欧拉角进行姿态规划，绕Y轴旋转β角其相应转换矩阵为

则所规划姿态矩阵为O_Z*R_Y；

当其位于规划柱面相交面时，绕X轴旋转α角，其相应矩阵为则所规划的姿态为O_Z*R_Y*R_Z；

步骤10：由步骤9所规划出的位置信息及其耦合位置的可连续时变姿态，依据具体任务所需形位选出步骤1-8得出的机器人各个关节转动角度的最优解，完成笛卡尔空间轨迹的规划任务。

本发明相对于现有技术具有如下的优点及效果：

1、本发明明确提出和划分主控关节、调整关节、自优关节，给今后依据具体的任务需求设计新型工业机械臂提供了有益参考，如一般工业机械臂最好具有主控关节和自优关节，当需要更佳操作效果或面临较复杂的任务要求，则设计为兼具主控关节、调整关节、自优关节的六自由度机械臂；同时，保证中间形位可解，为后续解算和轨迹规划带来方便；

2、本发明优化主控关节θ_c的求解过程，目前多采用的或前者解算范围较窄，后者虽然扩大了解算范围，但当机械臂进行高空顶部大幅度作业，如天花板粉饰、抹灰等，末端执行器的位置多次出现在不同的象限时，若仅仅采用末端位置，容易造成θ_c在不同相位跳动，本发明优化这一缺陷，尤其对于主控关节处不存在连杆偏距的情形；

3、针对目前理论界认为几何法仅适用于计算少自由度串联机构(如：二轴、三轴机械臂)，本发明提出中间形位，将其拓展用于多关节串联机械臂的解算应用中，通过选取最佳形位(多以中间关节呈上拱状态为佳)，避免了其他算法需要后续寻值和匹配才可得到有效解或找到最优解，提高了解算效率；

4、本发明充分利用耦合性质，不盲目解耦消元，首次完整系统地提出留元法处理思路，引入变量，结合三角函数辅角公式或联立方程组等，相较于常用的代数消元等其他解耦处理得出含有一个变量的高吸方程的方式，求解更为简便，避免任意构造方程带来的二次方程或其他复杂的解算过程；同时，姿态规划也体现留元法，如将位置信息耦合到姿态信息中；

5、当多关节型机械臂兼具调整关节、自优关节，本发明采取微调方式，关节转动范围能小不大，然后由自优关节实现自优匹配，使之运动协调、能量最优；

6、针对自优关节，本发明提出的处理方式对于动态目标的实时分析和处理，可以在操作过程依据目标对象进行瞬时自优，控制方便；

7、当进行轨迹规划时，可达位姿简易判定方法——向量几何法，简单易用，不仅可用于解决主控关节的转动角度，而且在最终执行任务的前即可进行施加判断，具有较好超前处理优势，而非滞后判断，提高了处理异常情况的能力；

8、本发明不仅可规避路径的奇异性缺陷，还可以更好地满足轨迹的精确性要求，通过提出的“投影倾角法”，可有效规避奇异性点，规划出无奇异点路径，在笛卡尔空间内实现耦合位置信息的可连续时变随动姿态，达到位置和姿态同时规划，解决插补算法因姿态的强耦合非线性问题可能无法避免机构奇异性的问题，而且理论轨迹与期望轨迹吻合，避免由关节空间插补所产生末端理论轨迹与期望轨迹间的偏差；

9、本发明可在明确轨迹位置后，通过合理姿态规划更好地规划机器人在三维空间运动所需指定位姿，同时在工业生产中达到防止因整体位姿的不连续变化造成关节和机械连接部件的冲击，提高诸如焊接、喷涂等作业质量；

10、本发明中的逆运动学新解兼具直观和高效解算，更好地满足机器人控制中的实时性需求。

总之，本发明针对目前存在的问题，可以在明确轨迹位置后，合理规划姿态，通过提出逆运动学所得解，无需通过关节运动行程最短、能量最优等寻优条件即可得到最优解，无需关节空间的关节角插补算法，高效精准地实现笛卡尔空间轨迹的变姿态轨迹规划，解算巧妙，算法简单，解算速度快，兼具几何直观性和实时性的特点；同时，对于各个关节的活动范围均可实现360度的解算，而且充分利用几何形位和留元法的优势，不仅将几何法引入多自由度机械臂运动分析中，而且避免严重影响算法效率的增根剔除、真解筛选匹配、最优解寻优等后续算法，同时在后续解算中也避免了后续复杂繁琐的寻值和匹配过程，解算范围大、解算精度和效率高；另外，可以简易判定位置点是否位于可达空间内，所有算式在实数空间内可解，且可以直观预测和规划机械臂作业过程中整体形位状态，更好地满足工业机器人的高精度、实时性作业要求，如串联机械臂用于空间3D打印等。姿态规划可用于欠自由度机械臂、数控机床、人形机器人、并联机构，可以有效规避奇异路径、保证姿态连续变化等。

附图说明

图1是常用机器人末端轨迹规划流程图。

图2是本发明机器人末端轨迹规划流程图。

图3表示存在坐标轴垂直纸面向外。

图4表示存在坐标轴垂直纸面向内。

图5表示基座。

图6表示竖直旋转关节，虚线表示旋转轴。

图7表示水平旋转关节，虚线表示旋转轴。

图8-9是使用图3-图7组合表示实施例一的连杆坐标系。

图10-11是使用图3-图7组合表示实施例二的连杆坐标系。

图12-13是使用图3-图7组合表示实施例三的连杆坐标系。

具体实施方式

在当前机器人领域，关于运动学问题分析的表述方法多种多样，比如：D-H参数变换矩阵法、李群李代数方法、螺旋理论的旋量结合指数积法、几何代数方法、对偶四元数方法等，甚至还有应用于拓扑结构分析的图论，比李群李代数方法条件更弱、应用更广的微分流形理论，Walker的ε-代数，以及对偶矩阵法等等，以及上述表述方法的混合形式，各种方式方法存在一定的转化关系，比如：以旋量为例，旋量可以通过转化为对偶四元数，旋量通过指数映射，是连接李群和李代数的纽带，且螺旋理论也是李群李代数理论和几何代数理论的重要数学工具等，旋量亦有矩阵等不同表述方式。同时，在机器人领域表示位置和姿态有多种不同参数化的数学形式，如：变换矩阵法、矢量法、旋量法、四元数法、李群法和罗德里格斯参数及上述方法的混合形式，如对偶矩阵、四元数向量对、欧拉角向量对等，以上方法之间存在一定的转化关系。

基于D-H参数的变换矩阵法是目前机器人界最通用的方法，及以简单易懂易用为原则选取以欧拉角表述末端执行器姿态的变换矩阵法，以下为清楚地说明本发明采取变换矩阵法进行详细描述，同时，依据D-H方法可以有不同的建系方式，解算式会略有差别，以下计算中所依据坐标系仅起到示范性，而非唯一和限制性。以下结合5种典型的实施例进行详细描述，分别是瑞士Neuronics AG公司的Katana 400s_s机械臂、德国KUKA公司的youBot移动机械臂、美国MobileRobots公司的Pioneer-arm机械臂、美国Unimation公司的PUMA560机器人、韩国Robotis公司DARwIn-OP仿人机器人的人形腿及其拓展冗余腿。显然，以下描述仅仅是针对具有代表性个案进行描述，不是全部的实施例，本领域的技术人员完全可以基于本发明中的实施例，获得在其他实施例中的应用，所以本发明决不限以下表述方式：

实施例一：串联五关节型Katana和youBot机械臂

机械臂特点：二者主控关节c为关节1，自优关节so为关节5，不存在调整关节ft，则th＝4，mh＝3，bh＝2，c＝1，参见图8和图9。

步骤1：建立连杆坐标系：将机器人连杆依据D-H方法确立Z轴和X轴，其中基坐标系固连于基座，依次往后建立各个关节坐标系，并命名各个关节轴的转动角度分别为θ₁、θ₂、θ₃、θ₄、θ₅；设以竖直状态为初始形位。

步骤2：以KUKAyouBot为例，当a₁＝0为Katana臂，正运动学求解过程

表1.机械臂D-H连杆参数表

通过正运动学，得：

以下为各个关节转动角度的求解过程：

步骤3：主控关节c转动角度θ_c的求解

当实际作业任务和轨迹规划时，其末端的位置和姿态是已知的；

n₅＝[n_x；n_y；n_z]；o₅＝[o_x；o_y；o_z]；a₅＝[a_x；a_y；az]；

P₅＝[P_x；P_y；P_z]；P₅为末端位置

由末端执行器位置通过接近矢量计算出关节th位置，则关节4的位置为 P₄＝P₅-l₄*a₅，此时存在连杆偏距则结合虚关节(当a₁＝0时，即为Katana臂，且所以统一求解)；

虚关节

虚关节2和关节4间的向量关系：

采用几何投影法和atan2函数，完全求解实际机械臂的转动的范围；

此解为无障碍环境下由向量几何投影直接求得的关节1所对应的解，称之为θ₁的投影解；

此解为由投影解通过相位转换(二者相对xoy原点中心对称)而取得关节1的解，称之为θ₁的相位解；

步骤4：中间关节mh转动角度θ_mid的求解

求得关节角θ₁，由于存在连杆偏距a₁，则关节2的实际位置P₂为

由投影解直接解算：

关节2和关节4间的向量关系：P₂₄＝P₄-P₂；取模求得长度，得：lP₂₄＝|P₂₄|

由相位解解算得：

关节2和关节4间的向量关系：取模求得长度，得：

当存在障碍时，末端执行器的位置处于机械臂工作空间内，以某特定末端姿态是否可达，由关节2、关节3、关节4的位置点通过三角形的边角关系判定所规划的末端轨迹是否可达，即在θ₁投影解条件下l₂+l₃＞＝lP₂₄或在θ₁相位解条件下相对其他算法简单明了。

由关节2、关节3、关节4的位置点构成空间三角形，即中间形位，由三角形的边角关系余弦定理；

∠2_cal＝arccos(cos∠2)

同理可求得相应于θ₁相位解的和

步骤5：关节bh转角角度θ_bh的求解

其中求解中间变量θ₂₃₄，可由得

cosθ₂₃₄＝a_z

实际应用中OP_ee是已知量，则

通过Matlab等计算代数系统的符号计算处理，可得

P_x/cosθ₁l₄*sinθ₂₃₄+a₁＝l₃*sinθ₂₃+l₂*sinθ₂

-P_y/sinθ₁-l₄*sinθ₂₃₄+a₁＝l₃*sinθ₂₃+l₂*sinθ₂

P_z-d₁-l₄*cosθ₂₃₄＝l3*cosθ₂₃+l₂*cosθ₂

相对于其他算法的处理方式，通过消去θ₂₃，求得θ₂，割裂了中间的耦合关系，后续获取组解需要寻值匹配；本发明提出留元法，即保留θ₃，不轻易消去θ₃，应用辅角公式，从而为解决耦合问题提供思路。整理上述三式，得

令

则

和的选择使用

选用因为在实际任务规划情形下，遇到θ₁＝π/2的情形几率很小；另外可在当θ₁＝π/2时，选择因为目前机械臂上臂长度大于前壁长度，则恒大于零，即

同理可求相应于θ₁投影解的和相应于θ₁相位解和

θ₂较大范围的求解技巧——留元方程法

以上采用三角函数辅角公式的求解范围为[-π/2，π/2]，虽然目前绝大多数工业机械臂第二关节活动范围均在上述求解范围内，但鉴于某种机械臂会略超出该范围，则通过联立方程组的方法，关节转动范围扩展至[-π，π]。

通过方程组解法

θ₂＝atan2(sinθ₂，cosθ₂)

步骤6：关节th的转动角度θ_th的求解

相应于θ₁的投影解情形下的θ₄：第一种情况：

同理可求相应于θ₁投影解的及相应于θ₁相位解的和

步骤七：自优关节so转动角度的求解

常规求解同时从求解中间变量θ₂₃₄所示矩阵方程得

同理可求其他解。

当执行抓取目标物体时，末端执行器的控制方法采取自优，此时可以依据目标物体进行自我调整的优势，即由当前求出的4个关节转动的角度，所得的末端姿态为

令

则：o_x＝-cosθ₅*sinθ₁-cosθ₂₃₄*cosθ₁*sinθ₅

o_y＝cosθ₁*cosθ₅-cosθ₂₃₄*sinθ1*sinθ₅

o_z＝-sinθ₂₃₄*sinθ₅

实际操作时，目标物体的具体位置和姿势是已知的，所以可以依据具体的目标位姿进行调整手抓姿态，称为自优。

抓取目标物方法一：

即末端接近矢量和目标物体的向量方向相同或相反；然后由与目标物体的的方向相同或相反，即可进行抓取操作；此处选择令即o_x＝O_x和o_y＝O_y以及o_z＝O_z进行联立求得：

或

θ₅＝atan2(sinθ₅，cosθ₅)

其中，sinθ₅和cosθ₅的选取，可以依据避开分母为0的情形进行选择；同时，θ₁有不同的取值，此时θ₅相应取值；同时，若需要不同姿势，按照相反方向亦可求解，方式同上；

抓取目标物的方法二：

经由以上四个关节的运动后，最后末端的位姿描述为

此时其中

由目标位置姿态信息得

由向量的点积公式可以求得则θ₅＝arccos(cosθ₅)

通过在前添加符号或所求自优关节转动角度±π获得其他姿态的操作。

实施例二：串联五关节型Pioneer-arm臂及串联六关节型PUMA560臂

机械臂特点：二者主控关节c为关节1，调整关节ft为4，后者具有自优关节so为关节 6，前者不存在，则th＝4，mh＝3，bh＝2，c＝1，参见图10和图11。其中，串联六关节型PUMA560型可视为串联五关节型Pioneer型由欠自由度机械臂的拓展，达到以五解六。

步骤1：建立连杆坐标系：将机器人连杆依据D-H方法确立Z轴和X轴，其中基坐标系固连于基座，依次往后建立各个关节坐标系，并命名各个关节轴的转动角度分别为θ₁、θ₂、θ₃、θ₄、θ₅；设以竖直状态为初始形位。

步骤2：以Pioneer-arm为例，正运动学求解过程

表2.机械臂D-H连杆参数表

通过正运动学，得：

以下为各个关节转动角度的求解过程：

步骤3：主控关节c转动角度θ_c的求解

当实际作业任务和轨迹规划时，其末端的位置和姿态是已知的；

n₆＝[n_x；n_y；n_z]；o₆＝[o_x；o_y；o_z]；a₆＝[a_x；a_y；a_z]

P₆＝[P_x；P_y；P_z]；P₆为末端位置

由末端执行器位置通过接近矢量计算出关节th位置，则关节5的位置为 P₅＝P₆-l₅*a₆；此时存在连杆偏距则结合虚关节(不存在连杆偏距时，即a1＝0，所以统一求解)；

虚关节

虚关节2和关节5间的向量关系：

采用几何投影法和atan2函数，完全求解实际机械臂的转动的范围；

此解为由几何投影直接求得的关节1所对应的解，称之为θ₁的投影解；

此解为由投影解通过相位转换(二者相对xoy原点中心对称)而取得关节1的解，称之为θ₁的相位解；

步骤4：中间关节mh转动角度θ_mid的求解

求得关节角θ₁，此时存在连杆偏距a₁，则关节2的实际位置P₂为

由投影解得

关节2和关节4间的向量关系P₂₅＝P₅-P₂，取模求得长度lP₂₅＝|P₂5|

由相位解解算得到

关节2和关节4间的向量关系：取模求得长度

轨迹规划时，末端位置处于机械臂工作空间内，以某特定末端姿态是否可达，由关节 2、关节3、关节5的位置点通过三角形的边角关系判定所规划的末端轨迹是否可达，即在θ₁投影解条件下为l₂+(l₃+l₄)＞＝lP₂₅及在θ₁相位解条件下为相对其他算法简单明了。

由关节2、关节3、关节5的位置点构成空间三角形，即中间形位，由三角形的边角关系余弦定理；

∠2_cal＝arccos(cos∠2)：

同理可求相应于θ₁相位解的和

步骤5：关节bh转角角度θ_bh的求解

实际应用中OP_ee是已知量，则

通过Matlab等计算代数系统的符号计算处理，由上式可知和得：

P_z-d₁-l₅*a_z＝(l₃+l₄)*cosθ₂₃+l₂*cosθ₂

相对于其他算法的处理方式，通过消去θ₂₃，求得θ₂，割裂了中间的耦合关系，后续获取组解需要寻值匹配；本发明提出留元法，即保留θ₃，不轻易消去θ₃，应用辅角公式，从而为解决耦合问题提供思路。整理上述三式得

令

则

和的选择使用

选用因为在实际任务规划情形下，遇到θ₁＝π/2的情形几率很小；另外可在当θ₁＝π/2时，选择因为目前机械臂上臂长度大于前壁长度，则恒大于

零，即

相应于θ₁相位解，第一种情况：应用辅角公式，

令

同理可求相应于θ₁投影解的和相应于θ₁相位解和

θ₂较大范围的求解——留元方程法

上述采用三角函数辅角公式的求解范围为[-π/2，π/2]，虽然目前绝大多数工业机械臂第二关节活动范围均在上述求解范围内，但鉴于某种机械臂会略超出该范围，以下提供通过联立方程法求解算法，求解范围扩展至[-π，π]。

通过方程组解法

θ₂＝atan2(sinθ₂，cosθ₂)

步骤6和7：调整关节ft和关节th转动角度的求解

步骤8：当为串联六关节型PUMA560臂，自优关节so转动角度θ_so的求解。

当为串联五关节型为Pioneer-arm臂，其步骤6调整关节ft转动角度θ_ft的求解：

即

相应于θ₁投影解的θ₄第一种情况：

同理可求相应于θ₁投影解及相应于θ₁相位解的和

步骤7关节th转动角度的求解：

即

相应于θ₁相位解的θ₅第一种情况：

同理可求相应于θ₁投影解及相应于θ₁相位解和

串联六关节型PUMA560臂，步骤6调整关节ft转动角度θ_ft的求解：

串联五关节型Pioneer-arm臂θ₄的运动范围为[-π，π]，虽然转动范围精确，但对于存在自优关节的PUMA560型机械臂而言，在实际生产轨迹规划中，造成调整关节运动幅度大范围调整，对于大多数场合并不合适，由此，提出将θ₄作为调整关节，将其活动范围控制原则定为能小不大。所以，调整关节f相对于靠近末端的自优关节，其运动控制方式采取微调方式，即小幅度范围进行调整而能够达到预期效果，关节转动范围尽力保证能小不大，然后由自优关节实现自优匹配，使之运动协调、节省能量；

即

相应于θ₁投影解θ₅的第一种情况的两个解：

同理可求相应于θ₁投影解的及相应于θ₁相位解和与和

步骤7关节th转动角度的求解：

同理可求相应于θ₁投影解的和受相应于θ₁相位解情和与和

步骤8：当为串联六关节型PUMA560臂，自优关节so转动角度θ_so的求解

当执行抓取目标物体时，末端执行器的控制方法采取自优，依据目标物体进行自我调整的优势，即由当前求出的5个关节转动的角度，所得的末端姿态为

令

实际操作时，目标物体的具体位置和姿势是已知的，则依据具体的目标位姿进行调整手抓姿态，称之为自优。

抓取目标物方法一：

即末端接近矢量和目标物体的向量方向相同或相反；然后由与目标物体的的方向相同或相反，即可进行抓取操作；此处选择令即o_x＝O_x和o_y＝O_y以及o_z＝O_z进行联立求解。

o_x＝sinθ₆*(cosθ₅*(sinθ₁*sinθ₄-cosθ₄*cosθ₁*cosθ₂₃)+sinθ₅*cosθ₁*sinθ₂₃)

-cosθ₆*(cosθ₄*sinθ₁+sinθ₄*cosθ₁*cosθ₂₃))

o_y＝cosθ₆*(cosθ₁*cosθ₄-sinθ₄*sinθ₁*cosθ₂₃)

-sinθ₆*(cosθθ₅*(cosθ₁*sinθ₄+cosθ₄*sinnθ₁*cosθθ₂₃)sinθ₅*sinθ₁*sinθ₂₃)

o_z＝-sinθ₆*(cosθ₂₃*sinθ₅+sinθ₂₃*cosθ₄*cosθ₅)-sinθ₂₃*cosθ₆*sinθ₄

同时sinθ₆和cosθ₆的选取，依据避开分母为0的情形进行选择；

抓取目标物的方法二：

经由以上五个关节的运动后，最后末端的位姿描述

此时

由目标位置姿态信息得

由向量的点积公式求得则θ₆＝arccos(cosθ₆)

通过在前添加符号或所求自优关节转动角度±π获得其他姿态的操作。

实施例三：串联六关节型仿人机器人人形腿及其拓展型冗余腿

作为存在可分别沿或绕基坐标系x、y、z方向移动或转动的情形，投影位置受到来自沿轴向移动的影响，此时需结合末端姿态达到确切求解θ_c的目的。而五轴联动数控机床可视为串联机械臂，存在沿基坐标系x、y、z方向移动的三个关节。

特点：对于仿人机器人人形腿，主控关节c为关节1，自优关节so为冗余腿末端关节，不存在调整关节ft，其中关节2和3及关节5和6的关节位置分别重合，则th＝6，mh＝4，bh＝2，c＝1，参见图12和图13。

步骤1：建立连杆坐标系：将机器人连杆依据D-H方法确立Z轴和X轴，其中基坐标系固连于基座，依次往后建立各个关节坐标系，并命名各个关节轴的转动角度分别为θ₁、θ₂、θ₃、θ₄、θ₅、θ₆或θ₇(冗余腿)；设以竖直状态为初始形位。

步骤2：以Darwin仿人机器人人形腿为例，正运动学求解过程

表3.仿人机器人人形腿D-H连杆参数表

通过正运动学，得：

以下为各个关节转动角度的求解过程：

步骤3：主控关节c转动角度θ_c的求解

当实际步行轨迹规划时，其脚部位置和姿态是已知的；

由末端执行器位置通过接近矢量，计算出关节tf位置，末端(脚部)位置P₇，则踝关节的位置为P₆＝P₇+l₆*a₇；股关节的位置为P₂＝[0；0；-d₀-l₁]；

股关节和踝关节间的向量关系P₂₆＝P₆-P₂，取模求得长度lP₂₆＝|P₂₆|

由上式得sinθ₁*cosθ₂*n_x-cosθ₁*cosθ₂*n_y-sinθ₂*n_z＝0

因为关节5和6的关节位置，由关节6可知关节5的位置信息，将人形腿退化为欠自由度机械臂处理，则

由位置信息得

联立二式，消去sinθ₂和cosθ₂整理得：

则

步骤4：中间关节mh转动角度θ_mid的求解

由关节2和3、关节4、关节5和6的位置点构成空间三角形，即中间形位，由三角形的边角关系余弦定理：

步骤5.1：关节bh转角角度θ_bh的求解

通过步骤三中由位置信息得

联立以上三式，替换因子l₄*cosθ₃₄+l₃*cosθ₃整理得

所以

步骤5.2：关节bh转角角度θ_bh的求解

由式子左边已知，可得以下两式

uar_3x＝(l₄*cosθ₄+l₃)*cosθ₃-l₄*sinθ₄*sinθ₃

uar_3z＝l₄*sinθ₄*cosθ₃+(l₄*cosθ₄+l₃)*sinθ₃

采取留元方程法联立，得

步骤6.1：关节th的转动角度θ_th的求解

得

步骤6.2：关节th转动角度θ_th的求解

串联六关节型仿人机器人人形腿的逆运动学求解完成，如果将其拓展成冗余腿，即在末端增添自优关节，则自优步骤如下；

步骤7：冗余关节转动角度的求解

由此可以更加灵活的控制脚部的动作及其姿态，完成更加复杂和优美的姿势，在实际步态轨迹规划，脚部位置和姿态是已知的，即由当前求出的六个关节转动的角度，所得的脚部姿态为：

可知可求o_x、o_y、o_z或n_x、n_y、n_z；

依据规划要求进行自优调整，即依据步态规划要求进行调整脚部的姿态；

方法一：即末端接近矢量和目标物体的向量方向相同或相反，然后由与目标物体的的方向相同或相反，即可进行抓取操作；此处选择令即o_x＝O_x和o_y＝O_y以及o_z＝O_z进行联立求得。

方法二：经由以上各个关节的运动后，最后末端的位姿可由求得，

由向量的点积公式求得则θ₇＝arccos(cosθ₇)

实际应用中，一般不会规划脚部的后转动作，所以不必如前两例进行是否添加负号调整的步骤。同时，本实施例解算步骤仅起到范性作用，其中，也可依据本发明利用其中存在的其他算式解算，即，不可将本实施例作为本发明处理类似问题的唯一表现形式，实施例一和实施例二亦如此，如：关节6的转动角度θ₆除本实施例所展示的之外，依然存在多处的关系算式进行解算，适当调整解算顺序亦可以得出解算结果。

步骤8：当进行末端轨迹规划时，末端在工作空间内以某特定末端姿态是否可达，尤其对于欠自由度机械臂，可由所构成的三角形三边关系进行操作前的判定，具体如下：针对 KUKAyouBot和katana型臂，通过几何向量法计算出求模得到lP₂₄的长度，lP₂₄、l₂、l₃，三者是否能够构成有效的三角形来判定所规划的末端轨迹是否可达；针对变形机构P-arm 或PUMA560型，则为然后求模得lP₂₅的长度，lP₂₅、l₂、l₃+l₄，三者是否能够构成有效的三角形进行判定。

步骤9：面向可连续时变姿态规划方法

设末端执行器坐标系的接近矢量方向为Z轴，取Y轴平行于基坐标系xoy面，由右手定则确定X轴；将末端执行器接近目标的接近矢量所在平面规划为以基坐标系z轴为母线，沿以末端执行器位置在基坐标系中的投影与基坐标系原点的连线为准线所形成的柱面及其相交面，则末端执行器的姿态规划为末端执行器位置P(P_x，P_y，P_z)的函数f(P，t)；当存在沿或绕基坐标系x、y、z方向移动或转动时，将接近矢量平移至基坐标系原点处理。

机器人领域表示位置和姿态有多种不同参数化的数学形式，以下为通过以欧拉角表述末端执行器姿态的变换矩阵法。

1)对于KUKAyouBot和Katana，其末端执行器接近目标的接近矢量所在柱面由以基坐标系 z轴为母线，沿以末端执行器位置的投影与基坐标系原点的连线为准线所形成的。具体规划步骤如下：

准线方程为

则母线平行于Z轴的柱面方程为cos(atan2(P_y，P_x))*y-sin(atan2(P_y，P_x))*x＝0

所规划的Z轴在基坐标系中的表示为：

其中，α＝atan2(P_y，P_x)

结合欧拉角进行姿态规划，绕Y轴旋转β角其相应转换矩阵为

则最终规划姿态矩阵为O_Z*R_Y；

其中由姿态矩阵中接近矢量和位置矢量组成矩阵：

其中β＝β(t)可为变量或常量；

2)对于Pioneer-arm和PUMA560，除(1)外，末端执行器接近目标的接近矢量亦可以位于规划柱面相交面上，其相应矩阵为则最终所规划的姿态为

O_Z*R_Y*R_X；

其中由姿态矩阵中接近矢量和位置矢量组成矩阵：

其中α＝atan2(P_y，P_x)且β＝β(t)和γ＝γ(t)可为变量或常量；

3)对于工业生产中常用的数控机床、人形机器人及并联机器人的关节中存在沿基坐标系 x、y、z方向移动或转动，将接近矢量以原点处矢量处理，替代沿以末端投影线为准线的规划方式：设以竖直为初始位形，以及末端坐标系与设定的基坐标系相同，则规划所在柱面所取的母线可选取为x正向(一般数控机床中z向为刀轴方向，仿人机器人中x为前行方向)，其他情形下不同的定义方案，可进行简单的转换获得。

初始末端姿态相应于设定基坐标系为：

结合欧拉角进行姿态规划，绕Y轴旋转β角其相应转换矩阵为

则所规划姿态矩阵为O_Z*R_Y；

当其位于规划柱面相交面时，绕X轴旋转α角，其相应矩阵为则所规划的姿态为O_Z*R_Y*R_Z；

步骤10：由步骤9所规划出的位置信息及其耦合位置的可连续时变姿态，依据具体任务所需最佳形位选出通过实施例一中步骤1-7，实施例二中步骤1-8，实施例三中步骤1-7得出的机器人运动规划所需各个关节运动的最优解，完成笛卡尔空间轨迹的规划任务。

本发明在执行过程中会涉及到关节转动速度、转动幅度以及位移等因素的影响，各关节运动时间可能存在不同；同时，由于机械设备、工厂环境等客观原因的存在，在开始或结束时刻，电机存在冲击等，完全可以基于本发明添加PID控制等其他优化措施；以及考虑机器人动力学因素的轨迹优化，如以机器人系统的动能为指标，应用拉格朗日乘子法进行动能最小的轨迹规划等等。另外，各个生产厂家对关节初始状态的设置及运动起始点的位置和转动范围设定会有不同，具体实施时需进行必要的试机操作。由于加工、装配或者磨损带来的结构误差是常见的，当机械结构不完全满足Pieper准则时，可以采用本发明求解出无几何误差情形下的封闭解法，作为初始值，然后以牛顿-拉夫森迭代算法修正结构误差对逆运动学解的影响，从而得到精确的逆运动学解等。

本发明包括但不限于以上示范性实施例的细节，上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (10)

1.一种机器人笛卡尔空间轨迹的规划方法，其特征在于包括以下步骤：

步骤1：建立连杆坐标系：设机器人连杆中基坐标系固连于基座，依次建立坐标系，并设各关节轴的转动角度分别为θ₁、θ₂、θ₃…θ_i；

步骤2：由运动学建模分析方法得正运动学方程，然后通过逆运动学的求解得到机器人各个关节的转动角度；

步骤3：通过末端执行器位置和姿态的数学表述方式，将末端执行器的接近矢量所在平面规划为以基坐标系z轴为母线，以末端执行器位置在基坐标系中的投影与基坐标系原点的连线为准线所形成的柱面及其相交面，将末端执行器的姿态规划为位置P(P_x，P_y，P_z)的函数f(P，t)；当机器人存在沿或绕基坐标系x、y、z方向移动或转动时，将末端执行器的接近矢量平移至基坐标系原点处理；

步骤4：通过步骤3规划出位置及耦合位置信息的可连续时变姿态，由步骤2逆运动学得出各个关节的转动角度，完成笛卡尔空间轨迹的规划任务。

2.根据权利要求1所述的一种机器人笛卡尔空间轨迹的规划方法，其特征在于：

步骤2中所述逆运动学的求解包括：由运动学建模分析方法得正运动学方程通过机器人所具有的向量几何性质和轨迹规划的形位要求，求解主控关节和中间关节的转动角度；利用运动学建模分析方法以及已求解的关节转动角度，寻求含有已引入变量的关系算式，求解其他相应关节的转动角度；当存在调整关节或自优关节，由运动学建模分析方法寻求关系算式分别采用微调和自优方式求解相应关节的转动角度；当机器人通过逆运动学求得关节转动角度执行所规划的轨迹时，采取向量几何法判定所规划的轨迹是否可达。

3.根据权利要求2所述的一种机器人笛卡尔空间轨迹的规划方法，其特征在于：

求解主控关节c转动角度θ_c的过程如下：令底部靠近基座的竖直关节为主控关节c；距离末端执行器最近的具有水平关节轴关节为关节th，其位置记为P_th，由末端执行器位置通过接近矢量计算得到；底部具有水平轴关节为关节bh，其位置记为P_bh，在其附近建立虚关节,其位置记为当关节c处存在连杆偏距a_c时，即当关节c处不存在连杆偏距a_c时，即连杆偏距a_c＝0，则关节th和关节mh间具有水平轴关节为关节mh，其位置记为P_mh；由P_th、P_mh、空间三点构成空间三角形，其中P_th、P_mh空间两点得向量利用向量几何投影性质求得关节c转动角度θ_c；当机器人存在沿或绕基坐标系x、y、z方向移动或转动时,投影位置受到其影响，结合末端姿态求解θ_c。

4.根据权利要求3所述的一种机器人笛卡尔空间轨迹的规划方法，其特征在于：

求解中间关节mh转动角度θ_mid的过程如下：通过向量几何投影法直接求得关节c的解，定义为θ_c的投影解，由投影解通过相位转换取得关节c的解，定义为θ_c的相位解；求得关节c转动角度θ_c后，若关节c处存在连杆偏距a_c，则由投影解所得底部具有水平轴关节bh的实际所处位置P_bh为则关节bh与关节th位置间的向量关系取模求得长度由相位解所得关节bh的实际位置P_bh为则关节bh与关节th的向量关系取模求得长度设关节bh与关节mh间的连杆长度记为l_bm，关节mh与关节th间的连杆长度记为l_mt；当关节bh、mh和th间存在移动关节时，l_bm和l_mt为含移动关节在内的关节间的连杆长度，由P_bh、P_mh、P_th空间三点构成空间三角形，由三角形的边角关系，求解出中间关节mh转动角度θ_mid。

5.根据权利要求3所述的一种机器人笛卡尔空间轨迹的规划方法，其特征在于：

求解关节bh转动角度θ_bh的过程如下：通过计算机代数系统的符号计算处理，可寻求有关θ_bh的函数算式,在处理过程中，将θ_bh描述为θ_c和θ_mid的函数；

令k＝k(θ)，θ及θ_j表示已求解的关节转动角度，有k₁＝k₁(θ)，k₂＝k₂(θ)，则k₁＝b*sin(θ_i+θ_j)+a*sinθ_i；k₂＝b*cos(θ_i+θ_j)+a*cosθ_i，整理得k₁＝(b*cosθ_j+a)*sinθ_i+b*sinθ_j*cosθ_i；

k₂＝(b*cosθ_j+a)*cosθ_i-b*sinθ_j*sinθ_i，联立二式得 则θ_i＝atan2(sinθ_i，cosθ_i)。

6.根据权利要求2所述的一种机器人笛卡尔空间轨迹的规划方法，其特征在于：

求解调整关节ft的转动角度θ_ft的过程如下：当距离末端执行器水平轴关节th下方存在具有竖直轴关节，记为调整关节ft，利用运动学建模分析方法寻求关系算式时采取微调方式，使调整关节活动范围控制为能小不大，然后由自优关节so自优匹配。

7.根据权利要求2所述的一种机器人笛卡尔空间轨迹的规划方法，其特征在于：

求解自优关节so的转动角度θ_so的过程如下：当距离末端执行器具有竖直轴关节，记为自优关节so；末端执行器执行具体任务时，其目标的位置和姿态是已知的，则末端执行器的位置姿态应与其相对应，可得二者之间的关系算式；实际任务操作时，目标的位置和姿态

抓取目标物的方法为两种之一：

抓取目标物方法一：

通过除自优关节外已求得的其他关节转动角度值，此时末端执行器的位置姿态信息可由正运动学求得，即末端执行器的位置姿态可表示为含有自优关节θ_so的函数，即此时，末端接近矢量和目标物体的向量方向相同或相反，以及与目标物体的的方向相同或相反，寻求含有自优关节角的方程，联立求解自优关节转动角度；

抓取目标物的方法二：

通过除自优关节外已求得的其他关节转动角度，令自优关节转动角度为0，通过正运动学可求得此时末端执行器的位姿信息为则同时由目标位置姿态信息得由向量的点积公式求得则θ_so＝arccos(cosθ_so)。

8.根据权利要求3所述的一种机器人笛卡尔空间轨迹的规划方法，其特征在于：

进行轨迹规划时，末端执行器的位置处于机械臂工作空间内，以某特定末端姿态是否可达，利用P_th、P_mh、P_bh空间三点是否可以构成空间三角形进行操作前的判定，通过三角形的边角关系判定所规划的末端轨迹是否可达。

9.根据权利要求1所述的一种机器人笛卡尔空间轨迹的规划方法，其特征在于：

可连续时变姿态规划的过程如下：设基坐标系xyz固连于基座上,将末端执行器接近目标的接近矢量所在平面规划为以基坐标系z轴为母线，沿以末端执行器位置在基坐标系中的投影与基坐标系原点的连线为准线所形成的柱面及其相交面，则末端执行器的姿态规划为末端执行器位置P(P_x，P_y，P_z)的函数f(P，t)；当存在相对于基坐标系x、y、z方向移动或转动时，将接近矢量平移至基坐标系原点处理；

设末端执行器坐标系的接近矢量方向为Z轴，取Y轴平行于基坐标系xoy面，由右手定则确定X轴，从而得到末端执行器接近矢量的初始状态，当末端执行器接近目标的接近矢量在投影柱面上时，由末端执行器位置在基坐标系中的投影点和基坐标系原点得到准线方程，然后求出母线平行于基坐标系z轴的柱面方程，然后绕末端坐标系Y轴或基坐标系选定轴旋转β角，其中β＝β(t)可为变量、常量，完成接近矢量在投影柱面上时末端执行器的姿态规划；当末端执行器接近目标的接近矢量位于规划投影柱面相交面时，由以上取得在投影柱面上的末端执行器的姿态，然后绕末端坐标系X轴或基坐标系选定轴旋转γ角，其中γ＝γ(t)可为变量、常量，完成接近矢量在投影柱面相交面上时末端执行器的姿态规划；当存在沿或绕基坐标系x、y、z方向移动或转动时，末端执行器接近目标的接近矢量平移至基坐标系原点处理，由末端执行器的初始姿态，依次绕基坐标系坐标轴旋转，完成接近矢量在柱面及规划柱面相交面上时末端执行器的姿态规划。

10.根据权利要求5所述的一种机器人笛卡尔空间轨迹的规划方法，其特征在于：

应用三角函数辅角公式保留变量θ_j：k₁＝b*sin(θ_i+θ_j)+a*sinθ_i，k₂＝b*cos(θ_i+θ_j)+a*cosθ_i，整理得k₁＝(b*cosθ_j+a)*sinθ_i+b*sinθ_j*cosθ_i，k₂＝(b*cosθ_j+a)*cosθ_i-b*sinθ_j*sinθ_i；

令M＝b*cosθ_j+a、N＝b*sinθ_j，则

令则θ_i＝θ_i+(1-sign(sn))*π。