CN103144111B - 一种二次规划统一协调的移动机械臂运动描述与规划方法 - Google Patents

Abstract

本发明提供了一种QP（二次规划）统一协调的移动机械臂运动描述与规划方法，包括如下步骤：1）对移动机械臂，建立QP统一协调的冗余度解析方案，所述解析方案设计的最小性能指标为移动平台和机械臂的联合速度向量的二次函数，受约束于移动平台和机械臂的统一描述方程、联合速度极限和联合角度极限；2）将步骤1）的QP统一协调的冗余度解析方案运用QP求解器进行求解；3）将步骤2）的求解结果传递给下位机控制器，驱动移动平台和机械臂同时协调完成给定的末端任务。本发明特点在于，通过对移动平台和机械臂的统一描述与规划，控制移动机械臂的移动平台和机械臂同时协调运动，完成给定末端任务。

Description

一种二次规划统一协调的移动机械臂运动描述与规划方法

技术领域

本发明涉及移动机械臂的运动规划及控制领域，更具体地，涉及一种二次规划统一协调的移动机械臂运动描述与规划方法。

背景技术

移动机械臂是由一个移动平台和固定在移动平台上的一个机械臂组成。相比于固定平台的机械臂而言，移动机械臂既拥有移动平台的大范围移动性，又拥有机械臂的可操作能力，因此受到了广泛的关注和研究。然而，移动平台和固定机械臂的结合产生了新的问题：怎么样同时协调控制移动平台和机械臂的运动？就一般规划方案而言，要么是先控制移动平台的运动，后控制机械臂的运动；要么是先控制机械臂的运动，后控制移动平台的运动，从而无法对移动平台和机械臂同时进行协调控制。而基于二次规划统一协调的移动机械臂运动描述与规划方法，就是对移动平台和机械臂同时进行协调控制运动，从而使得移动机械臂更加有效地完成给定的末端任务。

发明内容

本发明所要解决的技术问题是提供一种计算量小、用于移动机械臂对移动平台和机械臂同时进行协调控制运动的二次规划统一协调的移动机械臂运动描述与规划方法。

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

一种二次规划统一协调的移动机械臂运动描述与规划方法，包括如下步骤：

S1.对移动机械臂，建立二次规划统一协调的冗余度解析方案，所述解析方案设计的最小性能指标为移动平台和机械臂的联合速度向量的二次函数，受约束于移动平台和机械臂的统一描述方程、联合速度极限和联合角度极限；

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

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

优选地，二次规划统一协调的冗余度解析方案设计为：最小化受约束于 $K\stackrel{\·}{q}={\stackrel{\·}{r}}_w,$ q^-≤q≤qw、 ${\stackrel{\·}{q}}^{-}\≤\stackrel{\·}{q}\≤{\stackrel{\·}{q}}^{+},$ 其中 ${\stackrel{\·}{q}}^{T}W\stackrel{\·}{q}/2+p^T\stackrel{\·}{q}$ 为需优化的运动性能指标，表示移动平台和机械臂的联合速度向量，上标^T表示矩阵和向量的转置，W为非零系数矩阵，p为系数向量，且W和p由欲优化的目标决定，等式约束对应移动平台和机械臂的统一描述方程，K表示机械臂末端执行器速度向量和移动机械臂联合速度向量的连接矩阵，表示移动机械臂末端执行器的速度向量，q表示移动平台和机械臂的联合角度向量，q^-≤q≤q⁺和分别表示联合角度范围和联合速度范围，q^±表示联合角度上下极限，表示联合速度上下极限。

优选地，所述等式约束为移动平台和机械臂的统一运动学描述方程，根据给定的移动机械臂，通过对其结构分析，求得具体的连接矩阵K。

优选地，所述二次规划统一协调的冗余度解析方案转化为一个标准二次规划：最小化x^TWx/2+p^Tx，受约束于Kx＝d，ζ^-≤x≤ζ⁺，其中 正的常数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}[-\mathrm{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/d\end{array}\right]$ (r表示驱动轮的半径，其为0.1025米，d表示两个驱动轮的中点到机械臂和移动平台连接点的距离，其为0.1米，b表示驱动轮到其两中点的距离，其为0.32米，φ表示移动平台的航向角)。根据移动平台和机械臂的运动学描述方程，建立移动平台和机械臂在速度层上统一的运动学描述方程为 $K\stackrel{\·}{q}={\stackrel{\·}{r}}_w,$ 其中 $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}W\stackrel{\·}{q}/2+p^T\stackrel{\·}{q}$

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

          q^-≤q≤q⁺              (3)

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

其中为欲优化的运动性能指标，表示联合速度向量，W为非零系数矩阵，p为系数向量，且W和p由欲优化的目标决定，等式约束对应移动平台和机械臂的统一描述方程，K表示机械臂的末端执行器速度向量和移动机械臂联合速度变量的连接矩阵，表示移动机械臂末端执行器速度向量，q表示移动机械臂的联合角度向量，q^-≤q≤q⁺和分别表示联合角度范围和联合速度范围，q^±表示联合角度上下限，表示联合速度上下限。

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

${\mathrm{\ζ}}^{-}\≤\stackrel{\·}{q}\≤{\mathrm{\ζ}}^{+}---\left(5\right)$

其中ζ^-和ζ⁺的第i个元素分别定义为 ${\mathrm{\ζ}}_i^{-}=\max \{k(q_i^{-}-q_i),{\stackrel{\·}{q}}_i^{-}\}$ 和 ${\mathrm{\ζ}}_i^{+}=\min \{k(q_i^{+}-q_i),{\stackrel{\·}{q}}_i^{+}\}.$ 正的常数k(如取为4)用来调节联合速度的可行域。用x表示移动机械臂的联合速度带物理约束的移动机械臂冗余度解析方案(1)-(4)便可描述为如下的标准二次规划方案：

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

约束条件  Kx＝d                 (7)

         ζ^-≤x≤ζ⁺             (8)

其中和在本实施例中，W为单位矩阵，p为零向量和q-q(0)的组合，如此运动性能指标可以起到最小轮速和最小关节运动的双重目的。上述标准二次规划用二次规划求解器进行求解，进而得到二次规划统一协调的移动机械臂描述与运动规划方法的最优解。

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

Claims (3)

1.一种二次规划统一协调的移动机械臂运动描述与规划方法，其特征在于，包括如下步骤：

S1.对移动机械臂，建立二次规划统一协调的冗余度解析方案，所述解析方案的最小性能指标为移动平台和机械臂的联合速度向量的二次函数，受约束于移动平台和机械臂的统一描述方程、联合速度极限和联合角度极限；

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

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

所述步骤S1的二次规划统一协调的冗余度解析方案具体为：最小化受约束于q^-≤q≤q⁺、其中为欲优化的运动性能指标，表示移动平台和机械臂的联合速度向量，上标^T表示矩阵和向量的转置，W为非零系数矩阵，p为系数向量，且W和p由欲优化的目标决定，等式约束对应移动平台和机械臂的统一描述方程，K表示机械臂末端执行器速度向量和移动机械臂联合速度向量的连接矩阵，表示移动机械臂末端执行器的速度向量，q表示移动平台和机械臂的联合角度向量，q^-≤q≤q⁺和分别表示联合角度范围和联合速度范围，q^±表示联合角度上下极限，表示联合速度上下极限。

2.根据权利要求1所述的二次规划统一协调的移动机械臂运动描述与规划方法，其特征在于：所述等式约束为移动平台和机械臂的统一运动学描述方程，根据给定的移动机械臂，通过对其结构分析，求得具体的连接矩阵K。

3.根据权利要求1所述的二次规划统一协调的移动机械臂运动描述与规划方法，其特征在于：所述步骤S1的二次规划统一协调的冗余度解析方案转化为一个标准二次规划：最小化x^TWx/2+p^Tx，受约束于Kx＝d，其中 和分别表示联合速度的双端不等式约束的两个端点，和的第i个元素分别定义为 正的常数k用来调节和保证联合速度足够大的可行域；通过二次规划求解器求解所述标准二次规划，从而得到二次规划统一协调的移动机械臂运动描述与规划方法的最优解。