CN105563490A - 一种移动机械臂障碍物躲避的容错运动规划方法 - Google Patents

Abstract

本发明提供了一种移动机械臂障碍物躲避的容错运动规划方法，包括如下步骤：1)确定发生故障出现锁死的关节或驱动轮，对移动机械臂，建立二次规划统一的冗余度解析方案，解析方案设计的最小性能指标为移动平台和移动机械臂联合速度向量的二次函数；2)将二次规划统一协调的冗余度解析方案运用二次规划求解器进行求解；3)将求解结果传递给下位机控制器，驱动移动平台和移动机械臂同时协调完成给定的末端任务。本发明通过对移动平台和移动机械臂的统一描述与规划，只需要知道出现故障锁死的关节或驱动轮编号和锁定角度，就可进行自动容错处理，控制移动机械臂的移动平台和移动机械臂同时协调运动，且能躲避障碍物，完成给定末端任务。

Description

一种移动机械臂障碍物躲避的容错运动规划方法

技术领域

本发明涉及移动机械臂的运动规划及控制领域，更具体地，涉及一种移动机械臂障碍物躲避的容错运动规划方法。

背景技术

移动机械臂是由一个移动平台和固定在移动平台上的一个机械臂组成。相比于固定平台的机械臂而言，移动机械臂既拥有移动平台的大范围移动性，又拥有机械臂的可操作能力，因此受到了广泛的关注和研究。然而，移动平台和固定机械臂的结合产生了新的问题：怎么样同时协调控制移动平台和移动机械臂的运动以能够在关节锁定的情况下实现障碍物躲避？当冗余度移动机械臂的某一关节或者驱动轮因故障锁死时(相当于减少了机械臂的自由度)或者遭遇障碍物时，就一般规划方案而言，该移动机械臂末端就无法根据预定的解析方案完成指定的任务；而障碍物躲避的容错型解析方案，就是对出现问题的关节进行容错处理且对整体进行障碍物躲避处理，使得移动机械臂能够完成指定末端轨迹任务。以往，当移动机械臂的某一关节或驱动轮发生故障锁死时，通常需要重新设计方案或利用优化算法不断调节参数以适应所剩的自由度，过程较为繁琐且较难实现障碍物躲避。

发明内容

为解决上述问题，本发明提供了一种计算量小、用于移动机械臂障碍物躲避的移动机械臂障碍物躲避的容错运动规划方法。

为解决上述技术问题，本发明通过以下技术方案予以实现：

一种移动机械臂障碍物躲避的容错运动规划方法，包括如下步骤：

S1.确定发生故障出现锁死的关节或驱动轮，对移动机械臂，建立二次规划统一的冗余度解析方案，所述解析方案设计的最小性能指标为移动平台和移动机械臂的联合速度向量的二次函数，受约束于移动平台和移动机械臂的统一描述方程、障碍物躲避不等式、联合速度极限和联合角度极限；

S2.将步骤S1的二次规划统一协调的冗余度解析方案运用二次规划求解器进行求解；

S3.将步骤S2的求解结果传递给下位机控制器，驱动移动平台和移动机械臂同时协调完成给定的末端任务。

二次规划统一协调的冗余度解析方案设计为：最小化受约束于 q-≤q≤q⁺、其中为欲优化的运动性能指标，表示移动平台和移动机械臂的联合速度向量，上标^T表示矩阵和向量的转置，p为系数向量，且p由欲优化的目标决定，等式约束对应移动平台和移动机械臂的统一描述方程，K表示移动机械臂末端执行器速度向量和移动机械臂联合速度向量的连接矩阵，表示移动机械臂末端执行器的速度向量，C与e表示基于突加度的障碍物障碍物躲避参数，H表示移动机械臂关节的状态故障矩阵，用于在解析方案中确定故障关节，q表示移动平台和移动机械臂的联合角度向量，q^-≤q≤q⁺和分别表示联合角度范围和联合速度范围，q^±表示联合角度上下极限，表示联合速度上下极限。

所述等式约束为移动平台和移动机械臂的统一运动学描述方程，根据给定的移动机械臂，通过对其结构分析，求得具体的连接矩阵K；参数H表示移动机械臂关节的状态故障矩阵(用于在方案解析过程中固定故障关节)，根据发生故障出现锁死的关节编号，调整状态故障矩阵H中的相应元素为1(而H中该行其余元素均为0)。

所述二次规划统一协调的冗余度解析方案转化为一个标准二次规划：最小化x^Tx/2+p^Tx，受约束于Lx＝d，Cx≤e，其中L＝[K^T,H^T]^T， 和分别表示联合速度的双端不等式约束的两个端点，和的第i个元素分别定义为i表示联合角度的序号，正的常数k用来调节和保证联合速度足够大的可行域；通过二次规划求解器求解所述标准二次规划，从而得到移动机械臂障碍物躲避的容错运动规划方法的最优解。

与现有技术相比，本发明具有如下优点：

以往，当移动机械臂的某一关节或驱动轮发生故障锁死时，通常需要重新设计方案或利用优化算法不断调节参数以适应所剩的自由度，过程较为繁琐且较难实现障碍物躲避。本发明只需要知道出现故障锁死的关节或驱动轮编号和锁定角度，就可在躲避障碍物的同时进行自动容错处理，完成给定末端任务，从而避免了重新设计所带来的额外工作量和优化算法不断试探/调整参数的繁琐过程。

附图说明

图1为本发明的流程图。

图2为本发明的具体实施移动机械臂三维模型图。

图3为本发明的具体实施移动平台结构示意图。

具体实施方式

下面结合附图对本发明做进一步的描述，但本发明的实施方式并不限于此。

图1所示的移动机械臂障碍物躲避的容错运动规划方法首先主要建立移动机械臂的二次规划统一协调的冗余度解析方案；然后运用二次规划求解器对二次规划方案进行求解；下位机控制器根据求解结果控制移动机械臂的运动。

具体实施例一

图2展示了实现本发明的移动机械臂为一个三维的二轮驱动的移动机械臂。机械臂由六个连杆所组成，通过第一关节5、第二关节6、第三关节7、第四关节8、第五关节9和关节10连接。移动平台包括两个驱动轮：左驱动轮11和右驱动轮12。在本发明专利示例中，该机械臂关节5-10初始角度设置为[0,π/3,π/6,π,π/2,0]^T弧度，左驱动轮11和右驱动轮12初始角度设置为[0,0]^T弧度，即q(0)＝[0,0,0,π/3,π/6,π,π/2,0]^T，其联合角度上下极限设置为q^±＝[±∞,±∞,±0.5236,±2.3562,±6.2832,±6.2832,±6.2832,±6.2832]^T弧度，其联合速度上下极限设置为 ${\stackrel{\·}{q}}^{\±}={\[\±30.0,\±30.0,\±3.0,\±3.0,\±3.0,\±3.0,\±3.0,\±3.0\]}^T$ 弧度/秒。该空间六自由度冗余度机械臂的前向运动学方程为

$f\left(\mathrm{\θ}\right)=\left[\begin{array}{c}(l_6+l_5)(c_5{s}_{32}{c}_1-s_5{c}_4{c}_{32}{c}_1+s_5{s}_4{s}_1)+(l_4+l_3)s_{32}{c}_1+l_2{s}_2{c}_1\\ (l_6+l_5)(c_5{s}_{32}{s}_1-s_5{c}_4{c}_{32}{s}_1-s_5{s}_4{c}_1)+(l_4+l_3)s_{32}{s}_1+l_2{s}_2{s}_1\\ (l_6+l_5)(s_5{c}_4{s}_{32}+c_5{c}_{32})+(l_4+l_3)c_{32}+l_2{c}_2+l_1+h\end{array}\right],$

其中l_i表示第i个连杆的长度，c_i＝cos(θ_i)，s_i＝sin(θ_i)，θ_i表示第i个关节的角度，i＝1,2,…6，s₃₂＝sin(θ₃+θ₂)，c₃₂＝cos(θ₃+θ₂)，h表示第一个关节到移动平台底部的距离。在该空间六自由度冗余度机械臂中，l＝[0.065,0.555,0.19,0.13,0.082,0.018]^T米，h＝0.698米。

图3为实现本发明的移动平台结构示意图。在这个具体示例中，通过对移动平台的结构分析，求得二轮驱动的移动平台运动学方程为和其中(和分别表示移动平台左右轮的旋转速度，其初始速度设置为零)，表示移动平台航向角的速度(初始速度设置为零)，表示移动平台和移动机械臂连接点的速度(和分别表示它的横坐标和纵坐标的速度)， $A=\frac{r}{2b}\[-1,1\],B=\frac{r}{2}\left[\begin{array}{cc}\cos \mathrm{\φ},& -\sin \mathrm{\φ}\\ \sin \mathrm{\φ},& \cos \mathrm{\φ}\end{array}\right]\left[\begin{array}{cc}1,& 1\\ -d/b,& d/b\end{array}\right]$ (r表示驱动轮的半径，其为0.1025米，d表示两个驱动轮的中点到机械臂和移动平台连接点的距离，其为0.1米，b表示驱动轮到其两中点的距离，其为0.32米，φ表示移动平台的航向角)。根据移动平台和移动机械臂的运动学描述方程，建立移动平台和移动机械臂在速度层上统一的运动学描述方程为其中 $K=\left[\begin{array}{cc}B,& 0\\ 0,& 0\end{array}\right]+J(\mathrm{\θ},\mathrm{\φ})\left[\begin{array}{cc}A,& 0\\ 0,& I\end{array}\right],J(\mathrm{\θ},\mathrm{\φ})=\frac{\∂g(\mathrm{\θ},\mathrm{\φ})}{\∂w},$ $g(\mathrm{\θ},\mathrm{\φ})=\left[\begin{array}{ccc}cos\mathrm{\φ},& -sin\mathrm{\φ},& 0\\ \sin \mathrm{\φ},& \cos \mathrm{\φ},& 0\\ 0,& 0,& 1\end{array}\right]f\left(\mathrm{\θ}\right),w=\left[\begin{array}{c}\mathrm{\φ}\\ \mathrm{\θ}\end{array}\right],$ I为单位矩阵。

下面是本发明关于二次规划统一协调的移动械臂运动描述与规划方法：

最小化 ${\stackrel{\·}{q}}^T\stackrel{\·}{q}/2+p^T\stackrel{\·}{q}---\left(1\right)$

约束条件 $K\stackrel{\·}{q}={\stackrel{\·}{r}}_w---\left(2\right)$

$H\stackrel{\·}{q}=0---\left(3\right)$

$C\stackrel{\·}{q}\≤e---\left(4\right)$

q^-≤q≤q⁺(5)

${\stackrel{\·}{q}}^{-}\≤\stackrel{\·}{q}\≤{\stackrel{\·}{q}}^{+}---\left(6\right)$

其中为欲优化的运动性能指标，表示联合速度向量，p为系数向量，且p由欲优化的目标决定，等式约束对应移动平台和移动机械臂的统一描述方程，K表示机械臂的末端执行器速度向量和移动机械臂联合速度变量的连接矩阵，表示移动机械臂末端执行器速度向量，C与e表示基于突加度的障碍物躲避参数，H表示机械臂关节的状态故障矩阵，用于在解析方案中确定故障关节，q表示移动机械臂的联合角度向量，q^-≤q≤q⁺和分别表示联合角度范围和联合速度范围，q^±表示联合角度上下限，表示联合速度上下限。

考虑到上述优化问题是在速度层上求解，因此需将移动机械臂的联合角度约束(3)、联合速度约束(4)合并，从而得到基于联合速度的双端不等式约束：

其中和的第i个元素分别定义为和正的常数k(如取为4)用来调节联合速度的可行域。进一步地，令L＝[K^T,H^T]^T，将两个等式约束合并为一个等式约束：

$L\stackrel{\·}{q}=d---\left(8\right)$

用x表示移动机械臂的联合速度带物理约束的移动机械臂冗余度解析方案(1)-(4)便可描述为如下的标准二次规划方案：

最小化x^Tx/2+p^Tx(9)

约束条件Lx＝d(10)

其中L＝[K^T,H^T]^T和在本实施例中，p为零向量和q-q(0)的组合，如此运动性能指标可以起到最小轮速和最小关节运动的双重目的。上述标准二次规划用二次规划求解器进行求解，从而得到移动机械臂障碍物躲避的容错运动规划方法的最优解。

以上所述的本发明的实施方式，并不构成对本发明保护范围的限定。任何在本发明的精神原则之内所作出的修改、等同替换和改进等，均应包含在本发明的权利要求保护范围之内。

Claims (4)

1.一种移动机械臂障碍物躲避的容错运动规划方法，其特征在于，包括如下步骤：

S1.确定发生故障出现锁死的关节或驱动轮，对移动机械臂，建立二次规划统一的冗余度解析方案，所述解析方案设计的最小性能指标为移动平台和移动机械臂的联合速度向量的二次函数，所述联合速度向量的二次函数受约束于移动平台和移动机械臂的统一描述方程、障碍物躲避不等式、联合速度极限和联合角度极限；

S2.对步骤S1协调移动机械臂的二次规划统一的冗余度解析方案运用二次规划求解器进行求解，得到求解结果；

S3.将步骤S2的求解结果传递给下位机控制器，下位机控制器驱动移动平台和移动机械臂同时协调完成给定的末端任务。

2.根据权利要求1所述的移动机械臂障碍物躲避的容错运动规划方法，其特征在于：所述步骤S1的二次规划统一协调的冗余度解析方案具体为：最小化受约束于其中，为欲优化的运动性能指标；表示移动平台和移动机械臂的联合速度向量；上标^T表示矩阵和向量的转置；p为系数向量且由欲优化的目标决定；等式约束对应移动平台和移动机械臂的统一描述方程；K表示移动机械臂末端执行器速度向量和移动机械臂联合速度向量的连接矩阵；表示移动机械臂末端执行器的速度向量；C与e表示基于突加度障碍物的障碍物躲避参数；H表示移动机械臂关节的状态故障矩阵，用于在解析方案中确定故障关节；q表示移动平台和移动机械臂的联合角度向量；q^-≤q≤q⁺表示联合角度范围，表示联合速度范围，q^±表示联合角度上下极限，表示联合速度上下极限。

3.根据权利要求2所述的移动机械臂障碍物躲避的容错运动规划方法，其特征在于：所述等式约束为移动平台和移动机械臂的统一运动学描述方程，根据给定的移动机械臂，通过对其结构分析，求得具体的连接矩阵K；参数H表示移动机械臂关节的状态故障矩阵，参数H用于在方案解析过程中确定故障关节，根据发生故障出现锁死的关节编号，调整状态故障矩阵H中的相应元素，将发生故障出现锁死的关节设为1，未发生故障出现锁死的关节设为0。

4.根据权利要求2所述的移动机械臂障碍物躲避的容错运动规划方法，其特征在于：所述步骤S1的二次规划统一协调的冗余度解析方案转化为一个标准二次规划：最小化x^Tx/2+p^Tx，受约束于Lx＝d，Cx≤e，其中L＝[K^T,H^T]^T， 和分别表示联合速度的双端不等式约束的两个端点，和的第i个元素分别定义为i表示联合角度的序号，正的常数k用来调节和保证联合速度足够大的可行域；通过二次规划求解器求解所述标准二次规划，从而得到移动机械臂障碍物躲避的容错运动规划方法的最优解。