一种基于突加度的障碍物躲避运动规划方法
【技术领域】
[0001] 本发明涉及冗余度机械臂运动规划及控制领域，具体涉及一种基于突加度（加速 度的导数，Jerk)的障碍物躲避运动规划方法。
【背景技术】
[0002] 冗余度机械臂是一种所具有的自由度数量大于执行任务所需最小自由度数量的 机械臂，广泛地应用于各种国民经济生产活动中。冗余度解析是操作冗余度机械臂中的一 个重要的问题，即是依据不同的优化性能指标和二级任务，通过冗余度机械臂的末端位置 和姿态来确定机械臂各个关节最优角度的问题。冗余度机械臂的二级任务包括障碍物躲 避、关节极限角躲避和重复运动，通常被描述成等式约束、不等式约束或者双端约束。障碍 物躲避是冗余度解析问题中的一个重要的问题。目前有效的障碍物躲避运动规划方法都是 在速度层或者加速度层上进行解析的。然而，这不能满足某些采用突加度控制冗余度机械 臂的要求，而且不能考虑到突加度极限，从而可能导致机械臂在运动过程中超出突加度极 限。
【发明内容】
[0003] 本发明所要解决的技术问题是提供一种基于突加度的障碍物躲避运动规划方法， 该方法具有计算量小的特点。
[0004] 为解决上述技术问题，本发明通过以下技术方案予以实现：
[0005] -种基于突加度的障碍物躲避运动规划方法，包括如下步骤：
[0006] 1)设计基于突加度的障碍物躲避运动性能指标，生成二次型优化冗余度解析方 案；其中基于突加度的障碍物躲避运动性能指标受约束于突加度的雅可比矩阵等式、基于 突加度的障碍物躲避不等式、关节角度极限、关节速度极限、关节加速度极限和关节突加度 极限；
[0007] 2)将步骤1)中所生成的基于突加度的障碍物躲避二次型优化冗余度解析方案转 化为二次规划问题；
[0008] 3)将步骤2)中的二次规划问题运用二次规划求解器进行求解；
[0009] 4)将步骤3)的求解结果传递给下位机控制器驱动机械臂运动。
[0010] 基于突加度的障碍物躲避运动性能指标设计为：最小化纩2，受约束于基于 突加度的雅可比矩阵等式= ^、基于突加度的障碍物躲避不等式*// + 2人奋+ )'^<〇, 关节角度极限θ < Θ +、关节速度极限，关节加速度极限f ,关节 突加度极限# 谷'其中，谷_TF#/2表示优化性能指标，故表示机械臂关节突加度，上 标T表示矩阵和向量的转置，W为单位矩阵；等式约束J/= ξ表示机械臂末端运动轨迹，上 表示机械臂末端执行器的雅可比矩阵，匕=>v-2j/- ,·/?表示机械臂末端执行器突 加度向量，je是上的时间导数，Je是·^的时间导数，Θ表示机械臂的关节角度，g表示机 械臂的关节速度，卢表示机械臂的关节加速度+ 表示基于突加度的障碍 物躲避不等式约束，Jz、之和J2分别定义如下：
[0011] Jz^-T0UM)
[0012] 人(()）
[0013] j2=-f ,JΛΘ)
[0014] 其中，re =[Jtc-X。,少e -少。,ze-ZtJ , (Xc，yc，Zc)和（X。，y。，Z。）分别是判据点 C 和障碍物点0相对于机械臂基座的坐标，上（Θ)表示判据点c处的雅可比矩阵，Jc(60是 上（Θ)的时间导数，义(60是,人(0)的时间导数，向量与矩阵乘法操作?定义如下：
[0015] s ? V = [S1V1, S2V2，…S1VJt
[0016] 其中，列向量s = [S1, S1，…，sjlp行向量\^表示矩阵V的第i行，i = 1，2,…，1 ; Θ *表示关节角上下限，6^表示关节速度上下限，歹表示关节加速度上下限^^表示关节 突加度上下限。
[0017] 上述的障碍物躲避二次型优化冗余度解析方案转化为二次规划问题，设计其性能 指标为最小化x TWx/2,受约束于Ax = b，Cx < d，ζ X < ζ +，其中，X =谷'，W为单位矩 阵，A = Je，办=匕，C = Jz，j = -2U· - Λ0，ζ _和ζ +分别表示合成双端约束的上下 极限，它们的第j个元素分别定义为：
[0018] Cs. = max + ^ - Θj),kv{0j + δ - 0f -Oj),Oj }
[0019] ζ ： = min [λ'ρ (θj - θ - θj), Α:ν {0：! - δ - θj),k.t (Oj - Oj),Oj }
[0020] 其中，j表示关节序号，j = 1, 2, . . . , η，η为关节数目，〇 > 0和ε >〇为关节转换 裕度，关节极限转换参数kp>0, kv>0和ka>0用来调节和保证关节突加度够大的可行域。
[0021] 上述的二次规划问题运用二次规划求解器进行求解。
[0022] 将上述的二次规划求解器求解结果转化为电机驱动所需要的控制信号，从而驱动 各关节电机使机械臂完成障碍物躲避运动。
[0023] 与现有技术相比，本发明有如下优点：现有有效的障碍物躲避运动规划方法都是 在速度层或者加速度层上进行解析的，这不能满足某些采用突加度控制的冗余度机械臂的 要求，而且不能考虑到突加度极限，从而可能导致机械臂在运动过程中超出突加度极限。本 发明是一种计算量小的基于突加度的障碍物躲避运动规划方法。
【附图说明】
[0024] 图1为本发明的流程图。
[0025] 图2为实现本发明的机械臂模型图。
【具体实施方式】
[0026] 下面结合附图对本发明做进一步的描述，但本发明的实施方式并不限于此。
[0027] 图1所示的一种基于突加度的障碍物躲避运动规划方法，首先提出基于突加度的 障碍物躲避运动性能指标与约束，生成二次型优化冗余度解析方案；然后将二次型优化冗 余度解析方案转化为二次规划问题；接着运用二次规划求解器求解；最后下位机控制器根 据求解结果控制机械臂的运动。
[0028] 图2所示实现本发明的机械臂为一个七自由度的机械臂。该机械臂由七个连杆组 成，通过关节1、关节2、关节3、关节4、关节5、关节6和关节7组成。
[0029] 本发明基于突加度的障碍物躲避运动规划方法的二次型优化冗余度解析方案设 计为：
[0030] 最小化！Ρ μ?/] (1)
[0031] 约束条件：·// = >:; (2)
[0032] JB + 2)β + Jβ < 0 (3)
[0033] θ ^ θ + (4)
[0034] θ <θ<θ' (5)
[0035] θ <θ< θ+ (6)
[0036] θ <θ<θ' (7)
[0037] 其中，#Tr#/2表示优化性能指标，泛表示机械臂关节突加度，上标τ表示矩阵和 向量的转置，W为单位矩阵；等式约束J/ = /;:表示机械臂末端运动轨迹，表示机械臂末 端执行器的雅可比矩阵，& =F-2j/-F表示机械臂末端执行器突加度向量，Je 是上的时间导数，X是Je的时间导数，Θ表示机械臂的关节角度，g表示机械