CN110561440A - 一种冗余度机械臂加速度层多目标规划方法 - Google Patents

Abstract

一种冗余度机械臂加速度层多目标规划方法，分别设计加速度层的优化性能指标和不等式判据；结合机械臂末端的规划任务以及机械臂关节的物理极限，建立加速度层多目标规划方案，方案受约束于加速度层雅可比矩阵等式、机械臂动力学方程、避障不等式、关节角度极限、关节速度极限、关节加速度极限和关节力矩极限；将上述的多目标规划方案转化为一个标准的二次型优化问题，并采用数值算法来对其进行求解；下位机控制器根据算法的求解结果，驱动机械臂各个关节使其完成给定的末端规划任务。本发明设计的加速度层多目标规划方案，能够使得机械臂在关节受限并具有障碍物的情况下仍可完成给定的末端规划任务，且达到重复运动的目的。

Description

一种冗余度机械臂加速度层多目标规划方法

技术领域

本发明涉及冗余度机械臂的运动规划及控制领域，特别是指一种冗余度机械臂加速度层多目标规划方法。

背景技术

冗余度机械臂在工业自动化生产活动(如焊接、组装和喷漆等)中扮演着越来越重要的角色。冗余度机械臂应用研究中的两个热点是重复运动规划和障碍物躲避：这关系到机械臂能否在复杂工业环境中成功完成重复性的末端规划任务。

目前已有多种重复运动规划方案和避障规划方案被提出并有效应用于机械臂。然而，大部分的规划方案都只能实现单一目标(即重复运动或障碍物躲避)，仅有为数不多的规划方案是可以同时实现重复运动和障碍物躲避。对于这些可实现多个目标的规划方案，其都是在速度层上进行设计和在忽略机械臂动力学的情况下进行研究的。显然，它们无法处理机械臂的关节加速度极限和关节力矩极限，也无法直接应用于与加速度或力矩控制相关的机械臂，因而方案的适用范围受到极大的限制。更重要的是，由于缺乏对机械臂动力学的考虑，这些方案在工业应用实践中往往容易失效，从而导致机械臂无法完成给定的末端规划任务。

发明内容

本发明的主要目的在于克服现有技术中的上述缺陷，提出一种冗余度机械臂加速度层多目标规划方法，能够使得机械臂在关节受限并具有障碍物的情况下仍可完成给定的末端规划任务，且达到重复运动的目的。

本发明采用如下技术方案：

一种冗余度机械臂加速度层多目标规划方法，其特征在于,包括如下步骤:

1)设计加速度层的优化性能指标和避障不等式；

2)结合机械臂末端的规划任务、机械臂关节的物理极限及加速度层的优化性能指标建立加速度层多目标规划方案，其受约束于加速度层雅可比矩阵等式、机械臂动力学方程、避障不等式、关节角度极限、关节速度极限、关节加速度极限和关节力矩极限；

3)将加速度层多目标规划方案转化为一个标准的二次型优化问题，并采用数值算法来对其进行求解；

4)下位机控制器根据求解结果，驱动机械臂各个关节动作完成给定的末端规划任务。

优选的，所述加速度层的优化性能指标为：

其中||·||₂表示向量的二范数，表示机械臂关节加速度，表示机械臂关节速度，θ表示机械臂关节角度，θ(0)表示机械臂关节的初始状态；设计参数λ＞0用于调节性能指标以使机械臂达到重复运动规划的目的。

优选的，所述避障不等式为：

其中，系数矩阵M和N分别定义为M＝-[x_C-x_O,y_C-y_O,z_C-z_O]^TJ_C和上标^T表示矩阵或向量的转置，(x_C,y_C,z_C)表示机械臂判据点的空间坐标，(x_O,y_O,z_O)表示环境障碍物点的空间坐标，J_C表示机械臂判据点的雅克比矩阵，表示J_C的时间导数。

优选的，所述加速度层多目标规划方案设计为：

最小化

受约束于

其中等式约束对应于机械臂末端在加速度层的运动规划任务，α和β表示设计参数且其数值满足α＞0和β＞0,J表示机械臂的雅可比矩阵，表示J的时间导数，e表示机械臂末端的位置误差且定义为e＝f(θ)-r_d，f(·)表示一个非线性映射函数，r_d表示机械臂末端运动的期望轨迹，ε表示机械臂末端的速度误差且定义为表示r_d的时间导数，表示的时间导数；等式约束表示机械臂的动力学方程，H表示机械臂惯性矩阵，c表示离心力变量，g表示重力变量，τ表示机械臂关节力矩；不等式约束用于实现机械臂对环境障碍物的躲避；和τ^±分别表示关节角度极限、关节速度极限、关节加速度极限和关节力矩极限。

优选的，将加速度层多目标规划方案转化为一个标准的二次型优化问题，其性能指标为x^Tx/2+p^Tx，约束条件为Cx＝d，Ax≤b，η^-≤x≤η⁺，其中， η^±表示联立关节角度θ、关节速度和关节加速度后的上下极限，并且， k₁＞0和k₂＞0表示关节极限转换参数，δ＞0表示关节极限转换裕量。

优选的，采用数值算法来对二次型优化问题进行求解，具体为：将二次型优化问题的求解转化等效为一个分段线性投影方程的求解，从而采用相应的数值算法来计算得到分段线性投影方程的解以及二次型优化问题的最优化解。

由上述对本发明的描述可知，与现有技术相比，本发明具有如下有益效果：

本发明能有效克服现有方法的不足，提供了一种能够使得冗余度机械臂在关节受限并具有障碍物的情况下仍可完成给定的末端规划任务，且达到重复运动的目的的加速度层多目标规划方法，对机械臂在复杂工业环境中的应用实践具有重要意义和价值。

附图说明

图1为本发明的流程图。

以下结合附图和具体实施例对本发明作进一步详述。

具体实施方式

以下通过具体实施方式对本发明作进一步的描述。

图1所示的一种冗余度机械臂加速度层多目标规划方法主要由设计可实现重复运动和障碍物躲避的性能指标和不等式判据、建立加速度层的多目标规划方案、转为标准的二次型优化问题、数值算法求解器、下位机控制器、冗余度机械臂这六个部分组成。具体的，包括如下步骤：

1)首先，为实现机械臂重复运动和障碍物躲避的目的，分别设计加速度层的优化性能指标和避障不等式。

可实现重复运动的加速度层性能指标设计为：

其中||·||₂表示向量的二范数，表示机械臂关节加速度，表示机械臂关节速度，θ表示机械臂关节角度，θ(0)表示机械臂关节的初始状态；设计参数λ＞0用于调节性能指标以使机械臂达到重复运动的目的。

可实现障碍物躲避的不等式判据设计为

其中系数矩阵M和N分别定义为M＝-[x_C-x_O,y_C-y_O,z_C-z_O]^TJ_C和上标^T表示矩阵或向量的转置，(x_C,y_C,z_C)表示机械臂判据点的空间坐标，(x_O,y_O,z_O)表示环境障碍物点的空间坐标，J_C表示机械臂判据点的雅克比矩阵，表示J_C的时间导数。

2)然后，结合机械臂末端的规划任务以及机械臂关节的物理极限，建立可同时实现多种规划目标的方案，即加速度层多目标规划方案。

基于上述性能指标(1)和不等式判据(2)，结合机械臂末端的规划任务以及机械臂关节的物理极限，便可建立如下的加速度层多目标规划方案：

最小化

约束条件：

θ^-≤θ≤θ⁺ (7)

τ^-≤τ≤τ⁺ (10)

其中等式约束对应于机械臂末端在加速度层的运动规划任务，J表示机械臂的雅可比矩阵，表示J的时间导数，e表示机械臂末端的位置误差且定义为e＝f(θ)-r_d，f(·)表示一个非线性映射函数，r_d表示机械臂末端运动的期望轨迹，ε表示机械臂末端的速度误差且定义为表示r_d的时间导数，表示的时间导数；等式约束表示机械臂的动力学方程，H表示机械臂惯性矩阵，c表示离心力变量，g表示重力变量，τ表示机械臂关节力矩；不等式约束用于实现机械臂对环境障碍物的躲避；θ^±、和τ^±分别表示关节角度极限、关节速度极限、关节加速度极限和关节力矩极限。

3)进而，将该规划方案转化为一个标准的二次型优化问题，并采用数值算法来进行求解，具体如下：

对于上述加速度层多目标规划方案(3)-(10)，其可转化为如下标准的二次型优化问题：

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

约束条件：Cx＝d (12)

Ax≤b (13)

η^-≤x≤η⁺ (14)

其中，A＝[M^T -H^T H^T]^T，η^±表示联立关节角度θ、关节速度和关节加速度后的上下极限，并且，k₁＞0和k₂＞0表示关节极限转换参数，δ＞0表示关节极限转换裕量。

上述二次型优化问题(11)-(14)的求解可转化等效为如下分段线性投影方程的求解：

u-P_Ω(u-(Mu+q))＝0 (15)

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

其中，I表示单位矩阵，对偶变量w和v分别对应于等式约束(12)和不等式约束(13)。为了求解上述分段线性投影方程(15)以及相应的二次型优化问题(11)-(14)，可采用如下的数值算法：

σ(u^k)＝u^k-P_Ω(u^k-(Mu^k+q))

其中上标k表示迭代次数且k＝0,1,2,…。给定一个初始值u⁰，通过该数值算法的不断迭代计算，便可获得分段线性投影方程(15)的理论解，从而得到二次型优化问题(11)-(14)的最优解，也即前文所述的加速度层多目标规划方案(3)-(10)的最优解。

4)最后，将求解结果用于驱动机械臂的各个关节以使机械臂在关节受限并具有障碍物的情况下仍可完成给定的末端规划任务，且达到重复运动的目的，具体如下：

采用上述数值算法求解器计算得到加速度层多目标规划方案(3)-(10)的最优解之后，再将相应的求解结果传递给下位机控制器来驱动机械臂各个关节的运动，从而使得机械臂在关节受限并具有障碍物的情况下仍可完成给定的末端规划任务，且达到重复运动的目的。

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

Claims (6)

1.一种冗余度机械臂加速度层多目标规划方法，其特征在于,包括如下步骤:

1)设计加速度层的优化性能指标和避障不等式；

2)结合机械臂末端的规划任务、机械臂关节的物理极限及加速度层的优化性能指标建立加速度层多目标规划方案，其受约束于加速度层雅可比矩阵等式、机械臂动力学方程、避障不等式、关节角度极限、关节速度极限、关节加速度极限和关节力矩极限；

3)将加速度层多目标规划方案转化为一个标准的二次型优化问题，并采用数值算法来对其进行求解；

4)下位机控制器根据求解结果，驱动机械臂各个关节动作完成给定的末端规划任务。

2.如权利要求1所述的一种冗余度机械臂加速度层多目标规划方法，其特征在于,所述加速度层的优化性能指标为：

其中||·||₂表示向量的二范数，表示机械臂关节加速度，表示机械臂关节速度，θ表示机械臂关节角度，θ(0)表示机械臂关节的初始状态；设计参数λ＞0用于调节性能指标以使机械臂达到重复运动规划的目的。

3.如权利要求2所述的一种冗余度机械臂加速度层多目标规划方法，其特征在于,所述避障不等式为：

其中，系数矩阵M和N分别定义为M＝-[x_C-x_O,y_C-y_O,z_C-z_O]^TJ_C和上标^T表示矩阵或向量的转置，(x_C,y_C,z_C)表示机械臂判据点的空间坐标，(x_O,y_O,z_O)表示环境障碍物点的空间坐标，J_C表示机械臂判据点的雅克比矩阵，表示J_C的时间导数。

4.如权利要求3所述的一种冗余度机械臂加速度层多目标规划方法，其特征在于,所述加速度层多目标规划方案设计为：

最小化

受约束于θ^-≤θ≤θ⁺，τ^-≤τ≤τ⁺；

其中等式约束对应于机械臂末端在加速度层的运动规划任务，α和β表示设计参数且其数值满足α＞0和β＞0,J表示机械臂的雅可比矩阵，表示J的时间导数，e表示机械臂末端的位置误差且定义为e＝f(θ)-r_d，f(·)表示一个非线性映射函数，r_d表示机械臂末端运动的期望轨迹，ε表示机械臂末端的速度误差且定义为 表示r_d的时间导数，表示的时间导数；等式约束表示机械臂的动力学方程，H表示机械臂惯性矩阵，c表示离心力变量，g表示重力变量，τ表示机械臂关节力矩；不等式约束用于实现机械臂对环境障碍物的躲避；θ^±、和τ^±分别表示关节角度极限、关节速度极限、关节加速度极限和关节力矩极限。

5.如权利要求4所述的一种冗余度机械臂加速度层多目标规划方法，其特征在于,将加速度层多目标规划方案转化为一个标准的二次型优化问题，其性能指标为x^Tx/2+p^Tx，约束条件为Cx＝d，Ax≤b，η^-≤x≤η⁺，其中， C＝J，A＝[M^T-H^T H^T]^T，η^±表示联立关节角度θ、关节速度和关节加速度后的上下极限，并且， k₁＞0和k₂＞0表示关节极限转换参数，δ＞0表示关节极限转换裕量。

6.如权利要求1或2或3或4或5所述的一种冗余度机械臂加速度层多目标规划方法，其特征在于,采用数值算法来对二次型优化问题进行求解，具体为：将二次型优化问题的求解转化等效为一个分段线性投影方程的求解，从而采用相应的数值算法来计算得到分段线性投影方程的解以及二次型优化问题的最优化解。