结构分析，求得二轮驱动的移动平台运动学方程为#=如口蹲=知，其中=[病，喪]T (病 和喪分别表示移动平台左右轮的旋转速度，其初始速度设置为零），^表示移动平台航向角 的速度(初始速度设置为零）
衰示移动平台和移动机械臂连接点的速度（拓 和知分别表示它的横坐标和纵坐标的速度）
(r表示驱动轮的半径，其为0.1025米，d表示两个驱动轮的中点到机械臂和移动平台连接点 的距离，其为0.1米，b表示驱动轮到其两中点的距离，其为0.32米，Φ表示移动平台的航向 角）。根据移动平台和移动机械臂的运动学描述方程，建立移动平台和移动机械臂在速度层 上统一的运动学描述方程为码=4，其中
，1为单位矩阵。
[0024] 下面是本发明关于二次规划统一协调的移动械臂运动描述与规划方法：
[0031]其中?????/2 + /.，ι句为欲优化的运动性能指标，夺表示联合速度向量，P为系数向量， 且Ρ由欲优化的目标决定，等式约束足= 对应移动平台和移动机械臂的统一描述方程，Κ 表示机械臂的末端执行器速度向量和移动机械臂联合速度变量的连接矩阵，表示移动机 械臂末端执行器速度向量，c与e表示基于突加度的障碍物躲避参数，Η表示机械臂关节的状 态故障矩阵，用于在解析方案中确定故障关节，q表示移动机械臂的联合角度向量，q^<q< 和弓 <()< 与i分别表示联合角度范围和联合速度范围，q±表示联合角度上下限，沪表示联 合速度上下限。
[0032] 考虑到上述优化问题是在速度层上求解，因此需将移动机械臂的联合角度约束 (3) 、联合速度约束(4)合并，从而得到基于联合速度3的双端不等式约束：
[0033]
(7)
[0034] 其中?-和的第i个元素分别定义为片=m昭斯免Γ -化)，4,巧临=rain秋献-&)，扔:。 正的常数k(如取为4)用来调节联合速度的可行域。进一步地，令L=[KT，HT]T，i/=[/；,i，0]i， 将两个等式约束合并为一个等式约束：
[003引 14 二 d (8)
[0036]用X表示移动机械臂的联合速度f，带物理约束的移动机械臂冗余度解析方案(1)- (4) 便可描述为如下的标准二次规划方案：
[0040] 其中义叫，L = [ΚΤ，护]T和J二[?,.ι，(印。在本实施例中，P为零向量和q-q(0)的组 合，如此运动性能指标可W起到最小轮速和最小关节运动的双重目的。上述标准二次规划 用二次规划求解器进行求解，从而得到移动机械臂障碍物躲避的容错运动规划方法的最优 解。
[0041] W上所述的本发明的实施方式，并不构成对本发明保护范围的限定。任何在本发 明的精神原则之内所作出的修改、等同替换和改进等，均应包含在本发明的权利要求保护 范围之内。
【主权项】
1. 一种移动机械臂障碍物躲避的容错运动规划方法，其特征在于，包括如下步骤：51. 确定发生故障出现锁死的关节或驱动轮，对移动机械臂，建立二次规划统一的冗余 度解析方案，所述解析方案设计的最小性能指标为移动平台和移动机械臂的联合速度向量 的二次函数，所述联合速度向量的二次函数受约束于移动平台和移动机械臂的统一描述方 程、障碍物躲避不等式、联合速度极限和联合角度极限；52. 对步骤S1协调移动机械臂的二次规划统一的冗余度解析方案运用二次规划求解器 进行求解，得到求解结果；53. 将步骤S2的求解结果传递给下位机控制器，下位机控制器驱动移动平台和移动机 械臂同时协调完成给定的末端任务。2. 根据权利要求1所述的移动机械臂障碍物躲避的容错运动规划方法，其特征在于:所 述步骤S1的二次规划统一协调的冗余度解析方案具体为：最小化+ ，受约束于 句蹲=0、与％ 4 其中，<^八)/'2+尸I-为欲优化的运动性 能指标；f表示移动平台和移动机械臂的联合速度向量;上标T表示矩阵和向量的转置;P为 系数向量且由欲优化的目标决定;等式约束聯=4对应移动平台和移动机械臂的统一描述 方程;K表示移动机械臂末端执行器速度向量和移动机械臂联合速度向量的连接矩阵；?ν表 示移动机械臂末端执行器的速度向量;C与e表示基于突加度障碍物的障碍物躲避参数;Η表 示移动机械臂关节的状态故障矩阵，用于在解析方案中确定故障关节;q表示移动平台和移 动机械臂的联合角度向量;q- < q < 表示联合角度范围，^ ^句'表示联合速度范围，q± 表示联合角度上下极限，r表示联合速度上下极限。 3 .根据权利要求2所述的移动机械臂障碍物躲避的容错运动规划方法，其特征在于:所 述等式约束@ = t为移动平台和移动机械臂的统一运动学描述方程，根据给定的移动机械 臂，通过对其结构分析，求得具体的连接矩阵K;参数Η表示移动机械臂关节的状态故障矩 阵，参数Η用于在方案解析过程中确定故障关节，根据发生故障出现锁死的关节编号，调整 状态故障矩阵Η中的相应元素，将发生故障出现锁死的关节设为1，未发生故障出现锁死的 关节设为0。4.根据权利要求2所述的移动机械臂障碍物躲避的容错运动规划方法，其特征在于:所 述步骤S1的二次规划统一协调的冗余度解析方案转化为一个标准二次规划:最小化χΤχ/化 ρΤχ，受约束于Lx = d，Cx < e，与-一 < X < ζ·4，其中 J = #，L =比Τ，护]Τ，(/ = [/;Τ，〇]τ ; f -和各+ 分别 表示联合速度f的双端不等式约束的两个端点，Γ和户的第i个元素分别定义为 耗=max W如-ι?,)，α?，扭-mill例如-如，訂，i表示联合角度的序号，正的常数k用来调节 和保证联合速度足够大的可行域;通过二次规划求解器求解所述标准二次规划，从而得到 移动机械臂障碍物躲避的容错运动规划方法的最优解。
【专利摘要】本发明提供了一种移动机械臂障碍物躲避的容错运动规划方法，包括如下步骤：1)确定发生故障出现锁死的关节或驱动轮，对移动机械臂，建立二次规划统一的冗余度解析方案，解析方案设计的最小性能指标为移动平台和移动机械臂联合速度向量的二次函数；2)将二次规划统一协调的冗余度解析方案运用二次规划求解器进行求解；3)将求解结果传递给下位机控制器，驱动移动平台和移动机械臂同时协调完成给定的末端任务。本发明通过对移动平台和移动机械臂的统一描述与规划，只需要知道出现故障锁死的关节或驱动轮编号和锁定角度，就可进行自动容错处理，控制移动机械臂的移动平台和移动机械臂同时协调运动，且能躲避障碍物，完成给定末端任务。
【IPC分类】B25J9/16
【公开号】CN105563490
【申请号】CN201610121909
【发明人】廖柏林, 刘梅, 肖林
【申请人】吉首大学
【公开日】2016年5月11日
【申请日】2016年3月3日