一种移动机械臂障碍物躲避的容错运动规划方法
【技术领域】
[0001] 本发明设及移动机械臂的运动规划及控制领域，更具体地，设及一种移动机械臂 障碍物躲避的容错运动规划方法。
【背景技术】
[0002] 移动机械臂是由一个移动平台和固定在移动平台上的一个机械臂组成。相比于固 定平台的机械臂而言，移动机械臂既拥有移动平台的大范围移动性，又拥有机械臂的可操 作能力，因此受到了广泛的关注和研究。然而，移动平台和固定机械臂的结合产生了新的问 题:怎么样同时协调控制移动平台和移动机械臂的运动W能够在关节锁定的情况下实现障 碍物躲避？当冗余度移动机械臂的某一关节或者驱动轮因故障锁死时（相当于减少了机械 臂的自由度)或者遭遇障碍物时，就一般规划方案而言，该移动机械臂末端就无法根据预定 的解析方案完成指定的任务;而障碍物躲避的容错型解析方案，就是对出现问题的关节进 行容错处理且对整体进行障碍物躲避处理，使得移动机械臂能够完成指定末端轨迹任务。 W往，当移动机械臂的某一关节或驱动轮发生故障锁死时，通常需要重新设计方案或利用 优化算法不断调节参数W适应所剩的自由度，过程较为繁琐且较难实现障碍物躲避。
【发明内容】
[0003] 为解决上述问题，本发明提供了一种计算量小、用于移动机械臂障碍物躲避的移 动机械臂障碍物躲避的容错运动规划方法。
[0004] 为解决上述技术问题，本发明通过W下技术方案予W实现：
[000引一种移动机械臂障碍物躲避的容错运动规划方法，包括如下步骤：
[0006] S1.确定发生故障出现锁死的关节或驱动轮，对移动机械臂，建立二次规划统一的 冗余度解析方案，所述解析方案设计的最小性能指标为移动平台和移动机械臂的联合速度 向量的二次函数，受约束于移动平台和移动机械臂的统一描述方程、障碍物躲避不等式、联 合速度极限和联合角度极限；
[0007] S2.将步骤S1的二次规划统一协调的冗余度解析方案运用二次规划求解器进行求 解；
[0008] S3.将步骤S2的求解结果传递给下位机控制器，驱动移动平台和移动机械臂同时 协调完成给定的末端任务。
[0009] 二次规划统一协调的冗余度解析方案设计为：最小化<:川/2 +衣可，受约束于 巧=?,、巧<(J、/句=0、9-<9含9+、每一<奇<()'，其中(巧/2^户每为欲优化的运动性能指 标，f表示移动平台和移动机械臂的联合速度向量，上标T表示矩阵和向量的转置，P为系数 向量，且P由欲优化的目标决定，等式约束巧对应移动平台和移动机械臂的统一描述方 程，Κ表示移动机械臂末端执行器速度向量和移动机械臂联合速度向量的连接矩阵，表示 移动机械臂末端执行器的速度向量，C与e表示基于突加度的障碍物障碍物躲避参数，Η表示 移动机械臂关节的状态故障矩阵，用于在解析方案中确定故障关节，q表示移动平台和移动 机械臂的联合角度向量，q^q<q+和杳分别表示联合角度范围和联合速度范围，q ±表示联合角度上下极限，r表示联合速度上下极限。
[0010] 所述等式约束辟为移动平台和移动机械臂的统一运动学描述方程，根据给定 的移动机械臂，通过对其结构分析，求得具体的连接矩阵K;参数Η表示移动机械臂关节的状 态故障矩阵（用于在方案解析过程中固定故障关节），根据发生故障出现锁死的关节编号， 调整状态故障矩阵Η中的相应元素为1(而Η中该行其余元素均为0)。
[0011] 所述二次规划统一协调的冗余度解析方案转化为一个标准二次规划:最小化χΤχ/ 2+ρΤχ，受约束于Lx = d，Cx < e，$-全龙含扛-+，其中乂 = 4.，L=[KT,hT]t，抹=抗Τ，冲，和分 别表示联合速度的双端不等式约束的两个端点，r和$+惭第i个元素分别定义为 狂，扭咕_g,)，乐-}，i表示联合角度的序号，正的常数k用来调节 和保证联合速度足够大的可行域;通过二次规划求解器求解所述标准二次规划，从而得到 移动机械臂障碍物躲避的容错运动规划方法的最优解。
[0012] 与现有技术相比，本发明具有如下优点：
[0013] W往，当移动机械臂的某一关节或驱动轮发生故障锁死时，通常需要重新设计方 案或利用优化算法不断调节参数W适应所剩的自由度，过程较为繁琐且较难实现障碍物躲 避。本发明只需要知道出现故障锁死的关节或驱动轮编号和锁定角度，就可在躲避障碍物 的同时进行自动容错处理，完成给定末端任务，从而避免了重新设计所带来的额外工作量 和优化算法不断试探/调整参数的繁琐过程。
【附图说明】
[0014] 图1为本发明的流程图。
[0015] 图2为本发明的具体实施移动机械臂Ξ维模型图。
[0016] 图3为本发明的具体实施移动平台结构示意图。
【具体实施方式】
[0017] 下面结合附图对本发明做进一步的描述，但本发明的实施方式并不限于此。
[0018] 图1所示的移动机械臂障碍物躲避的容错运动规划方法首先主要建立移动机械臂 的二次规划统一协调的冗余度解析方案;然后运用二次规划求解器对二次规划方案进行求 解;下位机控制器根据求解结果控制移动机械臂的运动。
[0019] 具体实施例一
[0020] 图2展示了实现本发明的移动机械臂为一个Ξ维的二轮驱动的移动机械臂。机械 臂由六个连杆所组成，通过第一关节5、第二关节6、第Ξ关节7、第四关节8、第五关节9和关 节10连接。移动平台包括两个驱动轮:左驱动轮11和右驱动轮12。在本发明专利示例中，该 机械臂关节5-10初始角度设置为[0,31/3,31/6,31，31/2,0^弧度，左驱动轮11和右驱动轮12初 始角度设置为[0,0]τ弧度，即9(0) = [0,0,0,31/3，^6,31，31/2,0]了，其联合角度上下极限设置 为q±= [ ± W，± W，±0.5236，± 2.3562，±6.2832，±6.2832，±6.2832，±6.2832]τ弧度， 其联合速度上下极限设置为?/ =[±30.()，±30.0.,±3.()，±3.0，±3.0，±3.()，±3.化巧.0]|弧度/秒。该 空间六自由度冗余度机械臂的前向运动学方程为
[0021]
[0022] 其中li表示第i个连杆的长度，ci = cos(0i)，si = sin(0i)，θ?表示第i个关节的角 度，i = l ,2,…6，S32 = sin (目3+目2)，C32 = COS (目3+目2)，h表示第一个关节到移动平台底部的距 离。在该空间六自由度冗余度机械臂中，1 = [0.065,0.555,0.19,0.13,0.082，0.018]τ米，h = 0.698米。
[0023] 图3为实现本发明的移动平台结构示意图。在运个具体示例中，通过对移动平台的