CN105598984A - 一种冗余度机械臂加速度层运动规划的初始化方法 - Google Patents

Abstract

本发明中的冗余度机械臂加速度层运动规划的初始化方法通过先将冗余度机械臂调节到其极限位置，而后利用该极限位置作为起始值，并设定机械臂任务执行位置作为终止值，建立基于二次型优化描述的加速度层位置调整方案，而后利用数值算法对加速度层位置调整方案求解，最后通过该求解结果驱动机械臂使其由极限位置转动到任务执行位置，该冗余度机械臂加速度层运动规划的初始化方法具有操作方便、工作量小、作业规范的优点。

Description

一种冗余度机械臂加速度层运动规划的初始化方法

技术领域

本发明涉及冗余度机械臂加速度层运动规划领域，具体涉及一种冗余度机械臂加速度层运动规划的初始化方法。

背景技术

冗余度机械臂是一种末端能动的机械装置，其自由度多于执行末端任务所需最少自由度；目前已广泛应用于装备制造、产品加工、机器作业等国民经济生产活动中。冗余度机械臂在工作空间中不同的位置执行任务时，当结束一个任务后，往往需要从当前位置移动到下一个任务的执行位置。一般而言，冗余度机械臂从当前位置移动到下一个任务的执行位置，都是通过慢慢转动机械臂各个关节并不断调整来实现。然而，这种做法是相当繁琐且存在着不足的：每次改变任务操作位置时都有可能需要多次度量各个关节角度以使机械臂准确到达其执行任务的位置；并且任务执行位置测量或调整的不准确也将影响到机械臂正常/成功完成所要求的任务。

发明内容

本发明的目的在于克服一般技术的不足，提供一种操作方便、工作量少、作业规范且是在加速度层上实现的冗余度机械臂运动规划初始化方法。

本发明提供的技术方案如下：一种冗余度机械臂加速度层运动规划的初始化方法，包括以下步骤：

1)启动机械臂控制器，将冗余度机械臂调节到其极限位置，具体操作是依次驱动机械臂各个关节的电机使其最终达到各自的极限位置；

2)将步骤1)的极限位置作为起始值，并设定机械臂任务执行位置作为终止值，建立基于二次型优化描述的加速度层位置调整方案，所述的优化方案受约束于关节角度极限、关节速度极限和关节加速度极限；对于拥有n自由度的机械臂而言，加速度层位置调整方案设计为：

最小化

其中，设计参数γ>0∈R，上标^T表示矩阵或向量的转置；θ∈Rⁿ表示关节角度，表示关节速度，表示关节加速度；θ_d∈Rⁿ为机械臂任务执行初始位置；加速度层位置调整方案受约束于：θ^-≤θ≤θ⁺，

其中，θ^±∈Rⁿ、和分别表示关节角度极限、关节速度极限和关节加速度极限；

3)将步骤2)的加速度层位置调整方案利用数值算法进行求解；

4)下位机控制器根据步骤3)的求解结果，驱动机械臂使其由极限位置转动到任务执行位置。

在本发明的较佳实施例中，步骤2)中，将机械臂的关节极限转化为在加速度层上描述的双端约束，具体操作是利用约束转换方法将θ^-≤θ≤θ⁺，和转化描述为一个基于关节加速度的双端约束，即，其中η^-∈Rⁿ和η⁺∈Rⁿ的第i个元素分别定义为和i＝1,2,…n，关节极限转换参数k₁>0∈R和k₂>0∈R，关节极限转换裕量

在本发明的较佳实施例中，步骤3)中，通过数值算法对加速度层位置调整方案进行求解，具体操作是将所述加速度层位置调整方案进一步变换为分段线性投影方程，从而构造相应的数值算法进行求解。

本发明中的冗余度机械臂加速度层运动规划的初始化方法通过先将冗余度机械臂调节到其极限位置，而后利用该极限位置作为起始值，并设定机械臂任务执行位置作为终止值，建立基于二次型优化描述的加速度层位置调整方案，而后利用数值算法对加速度层位置调整方案求解，最后通过该求解结果驱动机械臂使其由极限位置转动到任务执行位置，该冗余度机械臂加速度层运动规划的初始化方法具有操作方便、工作量小、作业规范的优点。

具体实施方式

一种冗余度机械臂加速度层运动规划的初始化方法主要由冗余度机械臂调节到其极限位置、加速度层位置调整方案、数值算法求解器、下位机控制器、冗余度机械臂转动到任务执行位置组成。

首先启动机械臂控制器，将冗余度机械臂调节到其极限位置；再将该极限位置作为起始值，并设定机械臂任务执行位置作为终止值，从而建立基于二次型优化描述的加速度层位置调整方案；然后构造相应的数值算法来求解该加速度层位置调整方案；最后把求解结果用于驱动机械臂使其由极限位置转动到任务执行位置。

基于最小化机械臂各个关节的当前位置与任务执行位置之间偏移量的思想，n自由度冗余度机械臂的加速度层位置调整方案可设计为：

最小化： ${\stackrel{\·\·}{\mathrm{\θ}}}^T\stackrel{\·\·}{\mathrm{\θ}}/2+{(2\mathrm{\γ}\stackrel{\·}{\mathrm{\θ}}+{\mathrm{\γ}}^2(\mathrm{\θ}-{\mathrm{\θ}}_d))}^T\stackrel{\·\·}{\mathrm{\θ}}---\left(1\right)$

约束条件：θ^-≤θ≤θ⁺，(2)

${\stackrel{\·}{\mathrm{\θ}}}^{-}\≤\stackrel{\·}{\mathrm{\θ}}\≤{\stackrel{\·}{\mathrm{\θ}}}^{+},---\left(3\right)$

${\stackrel{\·\·}{\mathrm{\θ}}}^{-}\≤\stackrel{\·\·}{\mathrm{\θ}}\≤{\stackrel{\·\·}{\mathrm{\θ}}}^{+},---\left(4\right)$

其中，设计参数γ>0∈R，上标^T表示矩阵或向量的转置；θ∈Rⁿ表示关节角度，表示关节速度，表示关节加速度；θ_d∈Rⁿ为机械臂任务执行初始位置；θ^±∈Rⁿ、和表示关节角度极限、关节速度极限和关节加速度极限。

利用约束转换方法将(2)-(4)转化描述为一个基于关节加速度的双端约束，即，其中η^-∈Rⁿ和η⁺∈Rⁿ的第i个元素分别定义为和i＝1,2,…n，关节极限转换参数k₁>0∈R和k₂>0∈R，关节极限转换裕量用x∈Rⁿ表示机械臂的关节加速度上述的冗余度机械臂加速度层位置调整方案(1)-(4)便可描述为如下的二次型优化问题：

最小化：x^TQx/2+p^Tx(5)

约束条件：η^-≤x≤η⁺，(6)

其中，Q＝I∈R^n×n，I为单位矩阵。

并且，求解上述的二次型优化问题(5)-(6)等价于求解如下的分段线性投影方程：

P_Ω(x-(Qx+p))-x＝0∈Rⁿ，(7)

其中，P_Ω(·)表示分段线性投影算子。对于分段线性投影方程(7)和二次型优化问题(5)-(6)，可采用如下的数值算法来求解：

e(x^k)＝x^k-P_Ω(x^k-(Qx^k+p))，

x^k+1＝x^k-ρ(x^k)φ(x^k)，

φ(x^k)＝(Q^T+I)e(x^k)，

$\mathrm{\ρ}\left(x^k\right)=\left|\right|e\left(x^k\right)|{|}_2^2/|\left|\mathrm{\φ}\left(x^k\right)\right|{|}_2^2,$

其中，迭代次数k＝0,1,2,…，||·||₂表示向量的二范数。给定初始值x⁰∈Rⁿ，通过该算法的不断迭代，便可得到分段线性投影方程(7)的解，从而得到二次型优化问题(5)-(6)的最优解，也即加速度层位置调整方案(1)-(4)的最优解。

通过数值算法得到该二次型优化问题的解后，再将求解结果传递给下位机控制器来驱动机械臂使其由极限位置转动到任务执行位置，从而完成冗余度机械臂加速度层运动规划的初始化工作。

综上所述，该冗余度机械臂加速度层运动规划的初始化方法通过先将冗余度机械臂调节到其极限位置1，而后利用该极限位置作为起始值，并设定机械臂任务执行位置作为终止值，建立基于二次型优化描述的加速度层位置调整方案2，而后利用数值算法求解器3对加速度层位置调整方案求解，最后通过该求解结果驱动机械臂使其由极限位置转动到任务执行位置，该冗余度机械臂加速度层运动规划的初始化方法具有操作方便、工作量小、作业规范的优点。

上述仅为本发明的一个具体实施例，但本发明的设计构思并不局限于此，凡利用此构思对本发明进行非实质性的改动，均应属于侵犯本发明保护范围的行为。

Claims (3)

1.一种冗余度机械臂加速度层运动规划的初始化方法，其特征在于：包括以下步骤：

1)启动机械臂控制器，将冗余度机械臂调节到其极限位置，具体操作是依次驱动机械臂各个关节的电机使其最终达到各自的极限位置；

2)将步骤1)的极限位置作为起始值，并设定机械臂任务执行位置作为终止值，建立基于二次型优化描述的加速度层位置调整方案，所述的优化方案受约束于关节角度极限、关节速度极限和关节加速度极限；对于拥有n自由度的机械臂而言，加速度层位置调整方案设计为：

最小化 ${\stackrel{\·\·}{\mathrm{\θ}}}^T\stackrel{\·\·}{\mathrm{\θ}}/2+{(2\mathrm{\γ}\stackrel{\·}{\mathrm{\θ}}+{\mathrm{\γ}}^2(\mathrm{\θ}-{\mathrm{\θ}}_d\left)\right)}^T\stackrel{\·\·}{\mathrm{\θ}},$

其中，设计参数γ>0∈R，上标^T表示矩阵或向量的转置；θ∈Rⁿ表示关节角度，表示关节速度，表示关节加速度；θ_d∈Rⁿ为机械臂任务执行初始位置；

加速度层位置调整方案受约束于：

${\mathrm{\θ}}^{-}\≤\mathrm{\θ}\≤{\mathrm{\θ}}^{+},{\stackrel{\·}{\mathrm{\θ}}}^{-}\≤\stackrel{\·}{\mathrm{\θ}}\≤{\stackrel{\·}{\mathrm{\θ}}}^{+},{\stackrel{\·\·}{\mathrm{\θ}}}^{-}\≤\stackrel{\·\·}{\mathrm{\θ}}\≤{\stackrel{\·\·}{\mathrm{\θ}}}^{+};$

其中，θ^±∈Rⁿ、和分别表示关节角度极限、关节速度极限和关节加速度极限；

3)将步骤2)的加速度层位置调整方案利用数值算法进行求解；

4)下位机控制器根据步骤3)的求解结果，驱动机械臂使其由极限位置转动到任务执行位置。

2.根据权利要求1所述的冗余度机械臂加速度层运动规划的初始化方法，其特征在于：步骤2)中，将机械臂的关节极限转化为在加速度层上描述的双端约束，具体操作是利用约束转换方法将θ^-≤θ≤θ⁺，和转化描述为一个基于关节加速度的双端约束，即，其中η^-∈Rⁿ和η⁺∈Rⁿ的第i个元素分别定义为和i＝1,2,…n，关节极限转换参数k₁>0∈R和k₂>0∈R，关节极限转换裕量

3.根据权利要求1所述的冗余度机械臂加速度层运动规划的初始化方法，其特征在于：步骤3)中，通过数值算法对加速度层位置调整方案进行求解，具体操作是将所述加速度层位置调整方案进一步变换为分段线性投影方程，从而构造相应的数值算法进行求解。