一种基于突加度的冗余度机械臂重复运动规划方法
【技术领域】
[0001] 本发明涉及冗余度机械臂运动规划及控制领域，具体涉及一种基于突加度(加速 度的导数，Jerk)的重复运动规划方法。
【背景技术】
[0002] 冗余度机械臂具有自由度大于任务空间所需最少自由度的特性，广泛应用于装备 制造、产品加工、机器作业等国民经济生产活动中。冗余度机械臂的逆运动学问题是指已知 机械臂末端位姿，确定机械臂的关节角问题。当冗余度机械臂末端任务为一个封闭曲线时， 其各个关节可能回不到初始位置，这种现象叫做关节角偏差现象，或称非重复运动问题，如 果冗余度机械臂不能实现重复运动，则在运行过程中会产生不能预料甚至是危险有损机械 臂与周围人员安全的问题;而重复运动规划方案就是要设计适当的性能指标，使得机械臂 末端执行完封闭曲线任务时，各个关节角都能够回到其初始位置。以往的重复运动解析方 法大多都是在速度层(或加速度层)上进行，得到的结果为速度值(或加速度值），这不能满 足某些采用非速度(或非加速度)控制的冗余度机械臂的要求，且它们的求解方法并不考虑 突加度极限，机械臂在运动过程中就有可能因超越突加度极限而产生物理损坏。本发明专 利提出一种基于突加度的冗余度机械臂重复运动规划方法，有效地克服了以往速度层(或 加速度层)重复运动规划中的局限，具有新颖性与实用性。
【发明内容】
[0003] 本发明所要解决的技术问题是提供一种新型的、计算量小、普遍用于突加度控制 的冗余度机械臂重复运动规划方法。
[0004] 为解决上述技术问题，本发明通过以下技术方案予以实现：
[0005] -种突加度的冗余度机械臂重复运动规划方法，包括如下步骤：
[0006] 1)通过上位机采用二次型优化在突加度层上对机械臂的重复运动进行逆运动学 解析，得到基于突加度的重复运动二次型优化冗余度解析方案;具体步骤为:设计的最小化 性能指标为重复运动，重复运动受约束于基于突加度的雅可比矩阵等式、基于突加度的障 碍物躲避不等式、关节角度极限、关节速度极限、关节加速度极限和关节突加度极限；
[0007] 2)将步骤1)中所生成的基于突加度的重复运动二次型优化冗余度解析方案转化 为标准二次规划；
[0008] 3)将步骤2)中的标准二次规划运用二次规划求解器进行求解；
[0009] 4)将步骤3)的求解结果传递给下位机控制器驱动机械臂运动。
[0010] 基于突加度的重复运动性能指标设计为:最小化性能指标? / 2+ />1 ，受约束 于基于突加度的雅可比矩阵等式.// = ?、基于突加度的障碍物躲避不等式、关节 角度极限θ < θ+、关节速度极限合d 关节加速度极限r d 和关节突加度 极限获-歹+ ;其中，PW/2 + //5表示优化性能指标，谷表示机械臂关节突加度，上 标T表示矩阵和向量的转置，W为单位矩阵，/;> = /》+/>乂 +却/⑷-6(0))，α、β、x为用来控制 关节位移幅值和收敛率的正设计参数，Θ表示机械臂的关节角度，^表示机械臂的关节速 度，沒表示机械臂的关节加速度，θ(0)表示机械臂的初始关节角度;等式约束= ?表示 机械臂末端运动轨迹，Je表示机械臂末端执行器的雅可比矩阵，·ξ_ =F-2jj-Jj, F 表示机械臂末端执行器突加度向量，尤是1的时间导数，1是《^的时间导数，C与d表示基 于突加度的障碍物躲避参数，9±表示关节角度上下限，6^表示关节速度上下限，表示关 节加速度上下限，故"表示关节突加度上下限；
[0011] 上述的重复运动二次型优化冗余度解析方案转化为标准二次规划，设计其性能指 标为最小化xTWx/2+p Tx，受约束于Ax = b，Cx < (1，ζ- < X < ζ+，其中，X _=纩为单位矩阵，A = Je，，ζ?Ρζ+分别表示合成双端约束的上下极限，它们的第j个元素分别定义为：
[0012] ζ; .= m猫私p($ .+.θ - ),/'\ (0,. + 沒一為:):人成：一見.）為-
[0013] ζ! = minll：^(&J - 0 - Θf),kx (θ? - δ - (Θt }
[0014] 其中，j表示关节序号，j = l,2, . . . ,η，η为关节数目，咎>0和δ>〇为关节转换裕 度，关节极限转换参数kP>0，kv>(^Pka>0用来调节和保证关节突加度够大的可行域。
[0015] 上述的标准二次规划运用二次规划求解器进行求解。
[0016] 将上述的二次规划求解器求解结果转化为电机驱动所需要的控制信号，从而驱动 各关节电机使机械臂完成障碍物躲避运动。
[0017] 与现有技术相比，本发明有如下优点：现有有效的重复运动规划方法都是在速度 层或者加速度层上进行解析的，这不能满足某些采用突加度控制的冗余度机械臂的要求， 而且不能考虑到突加度极限，从而可能导致机械臂在运动过程中超出突加度极限。本发明 是一种计算量小的基于突加度的重复运动规划方法。
【附图说明】
[0018] 图1为本发明的流程图。
[0019] 图2为实现本发明的机械臂模型图。
【具体实施方式】
[0020] 下面结合附图对本发明做进一步的描述，但本发明的实施方式并不限于此。
[0021] 图1所示的一种基于突加度的冗余度机械臂重复运动规划方法，首先提出基于突 加度的重复运动性能指标与约束，生成二次型优化冗余度解析方案;然后将二次型优化冗 余度解析方案转化为标准二次规划;接着运用二次规划求解器求解;最后下位机控制器根 据求解结果控制机械臂的运动。
[0022] 图2所示实现本发明的机械臂为一个六自由度的机械臂。该机械臂由六个连杆组 成，通过第一关节1，第二关节2，第三关节3，第四关节4，第五关节5，第六关节6组成，机械臂 连接在基座7上，关节6的末端连接有末端执行器8。
[0023] 本发明基于突加度的障碍物躲避运动规划方法的二次型优化冗余度解析方案设 计为：
[0024] 最小化⑴
[0025] 约束条件：《7/ = ^ (2)
[0026] CB <d (3)
[0027] θ"< θ < θ+(4)
[0028] 0- <Θ<Θ'· (5)
[0029] 0- < § < β+ (6)
[0030] 〇 <〇<〇' (7)
[0031 ]其中，歹τ| " 2备表示优化性能指标，歹表示机械臂关节突加度，上标Τ表示 矩阵和向量的转置，W为单位矩阵；ρ = + + ，α、β、Χ为用来控制关节位 移幅值和收敛率的正设计参数，Θ表示机械臂的关节角度，^表示机械臂的关节速度，#表 示机械臂的关节加速度，Θ (0)表示机械臂的初始关节角度;等式约束= _ξ表示机械臂末 端运动轨迹，Je表示机械臂末端执行器的雅可比矩阵，，丨:2.人j>，F表示机械 臂末端执行器突加度向量，之是1的时间导数，1是&的时间导数，θ表示机械臂的关节 角度，表示机械臂的关节速度，沒表示机械臂的关节加速度·，d表示基于突加度的 障碍物躲避不等式约束;θ±表示关节角度上下限，# +表示关节速度上下限，合±表示关节加 速度上下限，歹