一种机器人笛卡尔空间轨迹的规划方法
【技术领域】
[0001] 本发明设及一种机器人笛卡尔空间轨迹的规划方法，设及逆运动学和姿态规划。
【背景技术】
[0002] 工业机器人应用的深度与广度已成为衡量一个国家制造业水平和科技水平的重 要标志。自从第一台工业机器人于1962年应用于GeneralMotor公司生产线，机器人技术 获得快速的发展，尤其，目前在人工成本不断提高和"工业4. 0"概念提出的背景下，机器人 革命有望成为"第=次工业革命"的重要切入点和增长点，而目前我国已经成为全球最大的 机器人市场，2015年5月国务院印发《中国制造2025》，明确提出要重点支持机器人技术的 发展和应用，将其列为十大重点领域之一。
[0003] 运动学和轨迹规划是机器人技术实际应用的基础，其中轨迹规划包括位置和姿 态两方面，尤其在笛卡尔空间的轨迹规划。工业实际应用中，当前在笛卡尔空间下进行轨迹 规划多是在关节空间中进行，末端实际运动轨迹复杂，同时，实际应用中常要求末端W不同 的姿态进行，而当前的规划方法存在易导致奇异性等弊端。其中，运动学问题是机器人运动 控制和轨迹规划的基础，解析形式的机器人逆运动学问题是机器人研究中的难题，也是机 器人研究领域的热点问题。
[0004] 位置逆解问题是机器人运动规划和轨迹规划的基础，只有通过逆运动学把空间位 姿转换为关节变量，才能实现对机械臂末端执行器的控制；而且机器人逆运动学求解的效 率直接影响运动轨迹控制效果和作业效率。由于机器人正运动学是在已知各个关节角度求 解末端的位置和姿态，相对简单且解唯一；逆运动学是是正运动学问题的逆过程，是已知机 械臂末端的位置和姿态，计算各个关节转过的角度值，串联机器人逆向运动学的求解过程 相对复杂，一般会出现多解或者甚至无解的情况。目前逆运动学求解算法分为两大类；封闭 解/解析法和数值解法，其中，数值解法中的迭代过程会降低求解速度，不利于现代工业机 械臂的实时控制；目前常用的数值法有如基于雅克比矩阵的牛顿-拉普松迭代法，根据建 立的正运动模型进行迭代计算，得出机器人逆运动学结果，但是在计算时间和结果的精确 性上很难同时保证，W及拟牛顿共辆梯度法，其存在雅克比矩阵奇异性，且算法初始状态的 取值直接将影响算法的收敛度和求解精度，在计算时间和精确上很难同时保证；神经网络 法是通过学习网络权、阔值参数建立笛卡尔空间下连杆构型同各个关节角度映射关系，而 网络学习需要大量样本数据，在实际中却较难获取，同时，网络学习所需要时间较长，不适 宜实时控制；另外，遗传算法是利用全局并行捜索特性，常规遗传算法在解决优化问题存在 早熟即收敛速度慢等缺陷，降低优化性能，影响求解精度；神经网络、遗传算法和专家系统、 模糊逻辑联合起来进行混合求解，在解决多自由度机器人逆运动学是需要复杂的算法程序 和高性能的配置，所W，数值法计算耗时长，求解精度不高，不适宜高精度和实时性作业任 务；遗传算法和神经网络方法可对求解进行优化，避免局部收敛，得出逆运动学结果，但与 迭代方法一样，在计算时间和计算精度上存在制约性。
[0005] 封闭解是实际应用中需要的，可W直接计算各关节角度理论值，不需要迭 代捜索寻优，具有计算速度快，精度高，包含几何法和代数法，其中几何法被视为仅 适用于结构简单的少自由度机械臂；代数法包括通过D-H建模结合逆矩阵方式和 利用旋量结合指数积通过转化为化den-K址an子问题求解等。前者是最常用的方 法，即先将机器人各关节转动角度设为01，0 2, 0 3... 0。，然后代入正运动学方程 巧=巧倘）*巧㈱*巧裤)*…*巧''''1供.)，推导出
[0006]
[0007] 方程左边的数据已知，采取逆矩阵的方式分离变量，寻找其中姿态和位置间存在 的等量关系，如；（巧(0!)) -，*巧=巧(&) *巧倘）*…巧-1供,.)，之后进行逐级分离变量， 从而求得有关角度，但任意构造的方程可能导致其中的求解过程异常复杂W及后续需要进 行有效解的寻值和匹配，需要更复杂的算法，所W要得到其中的有效解析解表达式，求解过 程复杂，解算效率不高。欠自由度机械臂不能实现任意姿态和位置，当任意给定时，往往无 解；国内外学者曾使用螺旋理论推导5R解析解的框架，几何法或代数法也曾分别被用来求 解5R机械臂逆运动学解析解，一般无法或很难在执行具体任务前预先判定操作点是否在 工作空间内，需要采用查表法等，而且，传统的求解算法通常存在解算范围小，需要匹配或 通过其他算法选择最优解、特殊位置处需单独讨论、最优解不易直观确定等问题，严重影响 机器人的响应速度。因此，位置逆解问题作为机器人运动学中最基础、最重要的研究问题之 一，直接关系到运动分析、轨迹规划和实时控制等，甚至后续的速度和加速度分析。而且，逆 运动学的求解速度和准确度直接影响工业机器人的实时控制的难W程度，也将直接决定机 械臂执行复杂任务的能力。
[0008] 而轨迹规划是逆运动学的实际应用，工业应用中运动轨迹规划的好坏直接影响机 器人作业质量。机器人末端运动是由关节变量直接决定的，由于关节坐标空间和直角坐标 空间转换关系复杂，同时在关节空间进行路径规划，计算比较简单，且不会产生机构奇异问 题，即可W避免雅克比矩阵奇异时形成的速度失控现象，所W除了示教再现法W外，目前轨 迹规划多数采取关节空间的规划方案，但其在直角坐标空间的最终轨迹，路径点之间的轨 迹形状往往是十分复杂的，因此只有对初始点和最终期望点的位姿有要求时而对中间路径 无要求的任务，才可W在关节空间直接进行路径规划。当末端轨迹形状有一定要求，在关 节空间规划则很难或无法达到要求，比如，当需要进行连续轨迹的作业任务，则必须在直角 坐标空间中规划出需要的轨迹。目前存在的直角坐标空间规划方法计算工作量大，W及姿 态规划问题复杂，导致所规划的轨迹有可能接近或通过奇异点，造成无法解算的后果。尤其 针对少自由度机械臂，当姿态规划不成功时，往往通过牺牲末端位姿精度来达到回避奇异 位置的目标，然而当进行目标捕获、精细操作等时，末端位姿精度的牺牲影响任务的成功执 行。
[0009] 因此，在路径要求不高的抓取操作时在关节空间中进行规划尚可，但当有严格轨 迹要求时，如焊接等，需要复杂的算法来保证精度，而在笛卡尔空间进行轨迹规划的精确性 是基于关节空间规划的该种方法所无法比拟的。但在姿态规划方面，由于其所具有的非线 性和禪合性，目前存在的算法多数采取插补方案进行，灵活性差，尤其在五自由度等少自由 度机器人中，存在诸多重大缺陷，容易造成规划路径的奇异性，甚至无法求解的局面。
【发明内容】
[0010] 针对现有技术中存在的技术问题，本发明的目的是；提供一种机器人空间轨迹的 规划方法。
[0011] 本发明通过W下步骤实现：
[0012] 步骤1 ;建立连杆坐标系；将机器人连杆中基坐标系固连于基座，依次建立坐标 系，并命名各个关节轴的转动角度分别为0 1、0 2、0 0 1;工业中各个厂家生产的机器人 的初始状态及关节运动范围有所不同，但其理论模型一致；
[001引步骤2 ;由运动学建模分析方法得正运动学方程；巧=巧*巧*巧...巧^1;
[0014]W下为各个关节转动角度的求解过程：
[0015] 步骤3;主控关节C转动角度日。的求解
[0016]令底部靠近基座的竖直关节为主控关节（controllingjoint)，一般为靠近基座 的第一个关节位置，距离末端执行器最近的具有水平关节轴关节th，其位置记为Pth，由末 端执行器（endeffector)位置通过接近矢量（approachingvector)计算得到；当关节C 处存在连杆偏距a。时，在底部具有水平轴关节化附近建立虚关节（virtualjoint),其位 置记为巧心。.，即
[0017]
如五关节型KUKAyouBot机械臂，当关节C处不存在连杆偏距a。时， 即连杆偏距3。= 0,则巧b=巧也如五关节型Katana机械臂；令二者中间具有水平轴关 节mh位置为IV;由P th、P_*、巧ft.,,.,.空间S点构成空间S角形，其中Pth、空间两点得向量 巧/^1巧M利用向量几何投影性质求得关节C转动角度0当存在沿或绕基坐标系X、y、Z 方向移动或转动时，投影位置受到来自沿轴向移动的影响，需结合末端姿态W求解0。。
[001引因此，采用几何投影法和atan2函数，可求解实际机械臂在[-31，31 ]的转动的范 围。
[001引 新^。！=渝?姑(巧巧h(2)，巧、巧A.(l));非高空顶部作业下，也。a;=a.ton.2仍，巧?)； [0020] 此解由向量几何投影方式直接求得关节C的解，称之为0。的投影解；
[00引]免['此'",二聲。a- '站促。U) *开；
[0022] 此解为由投影解通过相位转换（二者相对坐标系原点中屯、对称）取得关节C的 解，称之为9。的相位解；
[0023] 针对进行高空顶部大幅度作业时，末端多次出现在不同象限，主控关节转动角度 会在不同相位跳动，可对其进行优化。
[0024] 步骤4 ;中间关节血转动角度0 mid的求解
[00巧]求得关节C转动角度0。后，若关节C处存在连杆偏距a。，则由投影解0CW所得底 部具有水平轴关节化的实际所处位置Pbh为
[002引则关节化与关节th位置间的向量关系站:化二巧^巧h，取模求得长度lPl>Mh= \Pbhbh\
[0027]由相位片得关节化的实际所处位置Pbh为
[002引则关节化与关节th的向量关系巧hif。=巧h -巧，取模求得长度 IPbMhv",二\P村比h.r,_,,入;
[0029] 设关节化与关节血的连杆长度记为Ibm，关节血与关节th的连杆长度记为Imt; 当关节恤、mh和th间存在移动关节时，Ibm和1 mt为含移动关节在内的各关节间的连杆长 度。由Pbh、Pmh、Pt适间S点构成空间S角形，由S角形的边角关系余弦定理，求解出中间关 节mh转动角度0mw即相应于9。的投影解情形下
[0032] 同理，可求相应于0。相位解的0 mid其他两个解；
[003引步骤5 ;靠近基座具有水平轴关节化转动角度0 bh的求解
[0034] 针对机器人的非线性强禪合特点，相对于禪合问题的解禪处理方式，留元法采取 先引入多变量，充分利用禪合关系，寻求相应关系的函数算式，避免可能带来的增根剔除、 真解筛选、最优解寻值匹配等。通过Matl油等计算机代数系统的符号计算处理，可寻求关 于0bh的函数算式，在处理过程，将0bh描述为0C和0mJ勺函数，即0bh=f2(ec，0mJ。 令k = k(0)，0及0j表示已求解的关节转动角度，有ki=ki(0)，k2=k2(9)，则
[00；35] ki= b*sin(日1+日J)+a*sin日i;k2= b*cos(日1+日J)+a*cos白i
[0036] 其他算法通过消元求得0 1，割裂了中间的禪合关系，后续获取组解需要寻值匹 配；本发明提出留元法，即保留0J，不轻易消去0J，应用辅角公式，为解决禪合问题提供思 路。方法一应用辅角公式，即留元S角法：
[00；37] ki= (b*cos白j+a)*sin白i+b*sin白j*cos白askg二 〇3*cos日j+a)*cos日i-b*sin日j*sin白i
[0038]令M=b*cos日j+a、N=b*sin日j，则=姑倘。(兴，M)。
[0039] 因为机器人各连杆长度不等，则M2+N2恒大于零，所W分母恒不等于0,即 「00401
[0042]
[004引因为0郝的取值范围，令5打=|巧-脚，则关节1取值为日1 = 白i+ (1-sign (sn)) * 31；
[0044] W上步骤中采用S角辅角公式，其求解范围为[-31/2, 31/2]，虽然目前绝大多数 工业机械臂节活动范围均在上述求解范围内，也存在某种机械臂会略超出该范围，方法二 通过联立方程法求解算法，求解范围为