CN104760041A - 一种基于突加度的障碍物躲避运动规划方法 - Google Patents

Abstract

本发明公开一种基于突加度的障碍物躲避运动规划方法，包括：1）设计基于突加度的障碍物躲避运动性能指标，所设计的运动性能指标受约束于基于突加度的雅可比矩阵等式、基于突加度的障碍物躲避不等式、关节角度极限、关节速度极限、关节加速度极限和关节突加度极限，生成二次型优化冗余度解析方案；2）将步骤1）中所生成的二次型优化冗余度解析方案转化为二次规划问题；3）将步骤2）中的二次规划问题运用二次规划求解器进行求解；4）将步骤3）的求解结果传递给下位机控制器驱动机械臂运动。本发明通过设计一种基于突加度的障碍物躲避运动性能指标，控制机械臂在突加度层上躲避障碍物，同时也使得机械臂完成给定的末端任务。

Description

一种基于突加度的障碍物躲避运动规划方法

技术领域

本发明涉及冗余度机械臂运动规划及控制领域，具体涉及一种基于突加度(加速度的导数，Jerk)的障碍物躲避运动规划方法。

背景技术

冗余度机械臂是一种所具有的自由度数量大于执行任务所需最小自由度数量的机械臂，广泛地应用于各种国民经济生产活动中。冗余度解析是操作冗余度机械臂中的一个重要的问题，即是依据不同的优化性能指标和二级任务，通过冗余度机械臂的末端位置和姿态来确定机械臂各个关节最优角度的问题。冗余度机械臂的二级任务包括障碍物躲避、关节极限角躲避和重复运动，通常被描述成等式约束、不等式约束或者双端约束。障碍物躲避是冗余度解析问题中的一个重要的问题。目前有效的障碍物躲避运动规划方法都是在速度层或者加速度层上进行解析的。然而，这不能满足某些采用突加度控制冗余度机械臂的要求，而且不能考虑到突加度极限，从而可能导致机械臂在运动过程中超出突加度极限。

发明内容

本发明所要解决的技术问题是提供一种基于突加度的障碍物躲避运动规划方法，该方法具有计算量小的特点。

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

一种基于突加度的障碍物躲避运动规划方法，包括如下步骤：

1)设计基于突加度的障碍物躲避运动性能指标，生成二次型优化冗余度解析方案；其中基于突加度的障碍物躲避运动性能指标受约束于突加度的雅可比矩阵等式、基于突加度的障碍物躲避不等式、关节角度极限、关节速度极限、关节加速度极限和关节突加度极限；

2)将步骤1)中所生成的基于突加度的障碍物躲避二次型优化冗余度解析方案转化为二次规划问题；

3)将步骤2)中的二次规划问题运用二次规划求解器进行求解；

4)将步骤3)的求解结果传递给下位机控制器驱动机械臂运动。

基于突加度的障碍物躲避运动性能指标设计为：最小化受约束于基于突加度的雅可比矩阵等式基于突加度的障碍物躲避不等式关节角度极限θ^-≤θ≤θ⁺、关节速度极限关节加速度极限关节突加度极限其中，表示优化性能指标，表示机械臂关节突加度，上标T表示矩阵和向量的转置，W为单位矩阵；等式约束表示机械臂末端运动轨迹，J_e表示机械臂末端执行器的雅可比矩阵，表示机械臂末端执行器突加度向量，是J_e的时间导数，是的时间导数，θ表示机械臂的关节角度，表示机械臂的关节速度，表示机械臂的关节加速度；表示基于突加度的障碍物躲避不等式约束，J_z、和分别定义如下：

其中，(x_c,y_c,z_c)和(x_o,y_o,z_o)分别是判据点C和障碍物点O相对于机械臂基座的坐标，J_c(θ)表示判据点C处的雅可比矩阵，是J_c(θ)的时间导数，是的时间导数，向量与矩阵乘法操作◇定义如下：

s◇V＝[s₁V₁,s₂V₂,…s_lV_l]^T

其中，列向量s＝[s₁,s₁,…,s_l]^T和行向量V_i表示矩阵V的第i行，i＝1,2,…,l；θ^±表示关节角上下限，表示关节速度上下限，表示关节加速度上下限表示关节突加度上下限。

上述的障碍物躲避二次型优化冗余度解析方案转化为二次规划问题，设计其性能指标为最小化x^TWx/2，受约束于Ax＝b，Cx≤d，ζ^-≤x≤ζ⁺，其中，W为单位矩阵，A＝J_e，C＝J_z，ζ^-和ζ⁺分别表示合成双端约束的上下极限，它们的第j个元素分别定义为：

其中，j表示关节序号，j＝1,2,...,n，n为关节数目，和ε>0为关节转换裕度，关节极限转换参数k_p>0，k_v>0和k_a>0用来调节和保证关节突加度够大的可行域。

上述的二次规划问题运用二次规划求解器进行求解。

将上述的二次规划求解器求解结果转化为电机驱动所需要的控制信号，从而驱动各关节电机使机械臂完成障碍物躲避运动。

与现有技术相比，本发明有如下优点：现有有效的障碍物躲避运动规划方法都是在速度层或者加速度层上进行解析的，这不能满足某些采用突加度控制的冗余度机械臂的要求，而且不能考虑到突加度极限，从而可能导致机械臂在运动过程中超出突加度极限。本发明是一种计算量小的基于突加度的障碍物躲避运动规划方法。

附图说明

图1为本发明的流程图。

图2为实现本发明的机械臂模型图。

具体实施方式

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

图1所示的一种基于突加度的障碍物躲避运动规划方法，首先提出基于突加度的障碍物躲避运动性能指标与约束，生成二次型优化冗余度解析方案；然后将二次型优化冗余度解析方案转化为二次规划问题；接着运用二次规划求解器求解；最后下位机控制器根据求解结果控制机械臂的运动。

图2所示实现本发明的机械臂为一个七自由度的机械臂。该机械臂由七个连杆组成，通过关节1、关节2、关节3、关节4、关节5、关节6和关节7组成。

本发明基于突加度的障碍物躲避运动规划方法的二次型优化冗余度解析方案设计为：

最小化： ${\stackrel{...}{\mathrm{\θ}}}^{T}W\stackrel{...}{\mathrm{\θ}}/2---\left(1\right)$

约束条件： $J_e\stackrel{...}{\mathrm{\θ}}=\stackrel{...}{r_{\mathrm{\α}}}---\left(2\right)$

$J_z\stackrel{...}{\mathrm{\θ}}+2{\stackrel{.}{J}}_z\stackrel{..}{\mathrm{\θ}}+{\stackrel{..}{J}}_z\stackrel{.}{\mathrm{\θ}}\≤0---\left(3\right)$

θ^-≤θ≤θ⁺   (4)

${\stackrel{.}{\mathrm{\θ}}}^{-}\≤\stackrel{.}{\mathrm{\θ}}\≤{\stackrel{.}{\mathrm{\θ}}}^{+}---\left(5\right)$

${\stackrel{..}{\mathrm{\θ}}}^{-}\≤\stackrel{..}{\mathrm{\θ}}\≤{\stackrel{..}{\mathrm{\θ}}}^{+}---\left(6\right)$

${\stackrel{...}{\mathrm{\θ}}}^{-}\≤\stackrel{...}{\mathrm{\θ}}\≤{\stackrel{...}{\mathrm{\θ}}}^{+}---\left(7\right)$

其中，表示优化性能指标，表示机械臂关节突加度，上标T表示矩阵和向量的转置，W为单位矩阵；等式约束表示机械臂末端运动轨迹，J_e表示机械臂末端执行器的雅可比矩阵，表示机械臂末端执行器突加度向量，是J_e的时间导数，是的时间导数，θ表示机械臂的关节角度，表示机械臂的关节速度，表示机械臂的关节加速度；表示基于突加度的障碍物躲避不等式约束，J_z、和分别定义如下：

其中，(x_c,y_c,z_c)和(x_o,y_o,z_o)分别是判据点C和障碍物点O相对于机械臂基座的坐标，J_c(θ)表示判据点C处的雅可比矩阵，是J_c(θ)的时间导数，是的时间导数，向量与矩阵乘法操作◇定义如下：

s◇V＝[s₁V₁,s₂V₂,…s_lV_l]^T

其中，列向量s＝[s₁,s₁,…,s_l]^T和行向量V_i表示矩阵V的第i行，i＝1,2,…,l；θ^±表示关节角度上下限，表示关节速度上下限，表示关节加速度上下限，表示关节突加度上下限。

由于上述的二次型优化冗余度解析方案是在突加度层上求解，因此需将机械臂的关节角度约束(4)、关节速度约束(5)、关节加速度约束(6)和关节突加度约束(7)合并，从而可以得到基于突加度的双端不等式约束：其中，ζ^-和ζ⁺分别表示合成双端约束的上下极限，它们的第j个元素分别定义为：

其中，j表示关节序号，j＝1,2,...,n，n为关节数目，和δ>0为关节转换裕度，关节极限转换参数k_p>0，k_v>0和k_a>0用来调节和保证关节突加度够大的可行域。

进而，使用x表示机械臂的关节突加度上述带物理约束的基于突加度的障碍物躲避运动规划方法(1)-(7)便可描述为如下的二次规划问题：

最小化：x^TWx/2   (8)

约束条件：Ax＝b   (9)

Cx≤d   (10)

ζ^-≤x≤ζ⁺   (11)

其中，W为单位矩阵，A＝J_e，C＝J_z， $d=-2{\stackrel{.}{J}}_z\stackrel{..}{\mathrm{\θ}}-{\stackrel{..}{J}}_z\stackrel{.}{\mathrm{\θ}}.$

另外，上述的二次型规划问题(8)-(11)等价于如下的分段线性投影方程：

P_Ω(y-(My+q))-y＝0   (12)

其中，P_Ω(·)表示分段线性投影算子。分段线性投影方程(12)中的原对偶决策变向量y，增广系数矩阵M和向量q分别定义如下：

$y=\left[\begin{array}{c}x\\ u\\ v\end{array}\right],M=\left[\begin{array}{ccc}W& -A^T& C^T\\ A& 0& 0\\ -C& 0& 0\end{array}\right],q=\left[\begin{array}{c}0\\ -b\\ d\end{array}\right]$

其中，对偶决策变量u和v分别对应于等式约束(9)和不等式约束(10)。对于上述的分段线性投影方程(12)和二次型规划问题(8)-(11)，进而可以采用如下的二次型规划数值算法(即，二次型规划求解器)来求解：

e(y^k)＝y^k-P_Ω(y^k-(My^k+q))

φ(y^k)＝M^Te(y^k)+My^k+q

其中，迭代次数k＝0,1,2,…。给定初始值y⁰，通过该二次型规划数值算法的不断迭代，便可得到分段线性投影方程(12)的解，得到二次型规划问题(8)-(11)的最优解，也即是二次型优化冗余度解析方案(1)-(7)的最优解，从而得到基于突加度的障碍物躲避运动规划方法的最优解。

最后，通过二次型规划求解器得到该二次型规划问题的解后，将求解结果传递给下位机控制器驱动机械臂的运动，控制机械臂在突加度层上躲避障碍物，同时也使得机械臂完成给定的末端任务。

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

Claims (4)

1.一种基于突加度的障碍物躲避运动规划方法，其特征在于，包括如下步骤：

1)设计基于突加度的障碍物躲避运动性能指标，生成二次型优化冗余度解析方案；其中基于突加度的障碍物躲避运动性能指标受约束于基于突加度的雅可比矩阵等式、基于突加度的障碍物躲避不等式、关节角度极限、关节速度极限、关节加速度极限和关节突加度极限，

即最小化为受约束于基于突加度的雅可比矩阵等式基于突加度的障碍物躲避不等式关节角度极限θ^-≤θ≤θ⁺、关节速度极限关节加速度极限和关节突加度极限 ${\stackrel{\·\·\·}{\mathrm{\θ}}}^{-}\≤\stackrel{\·\·\·}{\mathrm{\θ}}\≤{\stackrel{\·\·\·}{\mathrm{\θ}}}^{+};$

其中，表示优化性能指标，表示机械臂关节突加度，上标T表示矩阵和向量的转置，W为单位矩阵；等式约束表示机械臂末端运动轨迹，J_e表示机械臂末端执行器的雅可比矩阵， 表示机械臂末端执行器突加度向量，是J_e的时间导数，是的时间导数，θ表示机械臂的关节角度，表示机械臂的关节速度，表示机械臂的关节加速度；表示基于突加度的障碍物躲避不等式约束，J_z、和分别定义如下：

其中， ${\stackrel{\→}{\stackrel{\·\·\·}{r}}}_c=[x_c-x_o,y_c-y_o,z_c-z_o],$ (x_c,y_c,z_c)和(x_o,y_o,z_o)分别是判据点C和障碍物点O相对于机械臂基座的坐标，J_c(θ)表示判据点C处的雅可比矩阵，是J_c(θ)的时间导数，是的时间导数，向量与矩阵乘法操作定义如下：

其中，列向量s＝[s₁,s₁,…,s_l]^T和行向量V_i表示矩阵V的第i行，i＝1,2,…,l；θ^±表示关节角度上下限，表示关节速度上下限，表示关节加速度上下限，表示关节突加度上下限；

2)将步骤1)中所生成的基于突加度的障碍物躲避二次型优化冗余度解析方案转化为二次规划问题；

3)将步骤2)中的二次规划问题运用二次规划求解器进行求解；

4)将步骤3)的求解结果传递给下位机控制器驱动机械臂运动。

2.根据权利要求1所述的基于突加度的障碍物躲避运动规划方法，其特征在于，所述步骤2)的障碍物躲避二次型优化冗余度解析方案转化为二次规划问题，设计其性能指标为最小化x^TWx/2，受约束于Ax＝b，Cx≤d，ζ^-≤x≤ζ⁺，其中， $x=\stackrel{\·\·\·}{\mathrm{\θ}},$ W为单位矩阵，A＝J_e， $b={\stackrel{\·\·\·}{r}}_{\mathrm{\α}},$ C＝J_z， $d=-2{\stackrel{\·}{J}}_z\stackrel{\·\·}{\mathrm{\θ}}-{\stackrel{\·\·}{J}}_z\stackrel{\·}{\mathrm{\θ}},$ ζ^-和ζ⁺分别表示合成双端约束的上下极限，它们的第j个元素分别定义为：

其中，j表示关节序号，j＝1,2,...,n，n为关节数目，和δ>0为关节转换裕度，关节极限转换参数k_p>0，k_v>0和k_a>0用来调节和保证关节突加度够大的可行域。

3.根据权利要求2所述的基于突加度的障碍物躲避运动规划方法，其特征在于所述步骤3)运用二次规划求解器进行求解。

4.根据权利要求3所述的基于突加度的障碍物躲避运动规划方法，其特征在于所述步骤4)将二次规划求解器求解结果转化为电机驱动所需要的控制信号，从而驱动各关节电机使机械臂完成障碍物躲避运动。