CN110076770A - 一种用于冗余机械臂的自运动方法 - Google Patents

Abstract

本发明公开了一种用于冗余机械臂的自运动方法，其特征在于，具体按照以下步骤实施：步骤1、确定冗余机械臂逆运动学方程；步骤2、对自运动方程两边同时求导；步骤3、将所述逆运动学方程设计为时变凸二次规划问题；步骤4、在所述时变凸二次规划问题中分别引入自运动指标和末端位置反馈；步骤5、引入拉格朗日函数，求解时变凸二次规划问题受等式约束的时变凸二次规划方程，并将其转化为时变矩阵方程，并得到最优解；步骤6、对步骤5得到的速度层上的最优解进行积分，即可得到各个关节角的最优解；本发明提供了一种计算量小，用于速度控制的冗余机械臂自运动方案。

Description

一种用于冗余机械臂的自运动方法

技术领域

本发明属于机械臂自运动方法技术领域，涉及一种用于冗余机械臂的自运动方法。

背景技术

当一个机械臂的自由度大于完成末端执行器主任务所需的自由度时，这个机械臂就称为冗余机械臂。冗余机械臂由于其灵活性，被广泛运用于各个领域，特别是一些高危领域，如化工，冶炼等。冗余机械臂的逆运动学问题是已知末端执行器的姿态，求解机械臂各个关节的关节角。由于机械臂是冗余的，所以逆运动学问题存在无穷个可行解。当一个冗余机械臂进行一项新的末端执行器任务时，通常需要从固定的关节构型出发，例如在重复运动中，机械臂完成一次重复运动时需要回到初始的关节状态，如果没有回到初始的状态，运动的精度可能会受到影响，严重时甚至毁坏机械臂。但是冗余机械臂存在关节漂移问题，所以需要额外的运动来调整机械臂的关节构型，这种运动即称为自运动，即是将保持末端不动的情况下，使机械臂到达一个期望或者更优的状态，这种自运动通常用于躲避机械臂关节物理极限，躲避障碍物等。

发明内容

本发明的目的是提供一种用于冗余机械臂的自运动方法，提供了一种计算量小，用于速度控制的冗余机械臂自运动方案。

本发明所采用的技术方案是，一种用于冗余机械臂的自运动方法，具体按照以下步骤实施：

步骤1、确定冗余机械臂逆运动学方程；

步骤2、对自运动方程两边同时求导；

步骤3、将所述逆运动学方程设计为时变凸二次规划问题；

步骤4、在所述时变凸二次规划问题中分别引入自运动指标和末端位置反馈；

步骤5、引入拉格朗日函数，求解时变凸二次规划问题受等式约束的时变凸二次规划方程，并将其转化为时变矩阵方程，并得到最优解；

步骤6、对步骤5得到的速度层上的最优解进行积分，即可得到各个关节角的最优解；

步骤7、将求解得到的最优角速度x^*传给下位机控制器驱动机械臂。

本发明的特征还在于，

步骤1中逆运动学方程表示如下：

f(θ(t))＝r(t) (1)

式(1)中，θ(t)＝[θ₁(t)，θ₂(t)…θ_n(t)]^T是关节角度列向量；r(t)＝[x(t)，y(t)，z(t)]^T是期望的末端轨迹；f(·)是一个非线性映射。

步骤2中自运动方程两边同时求导后表示如下：

其中，J(θ(t))∈R^n×m为速度雅克比矩阵，n为机械臂自由度，m表示末端执行器空间维度；和表示关节角速度向量和末端执行器速度向量。

步骤3中逆运动学方程设计为时变凸二次规划问题后表示如下：

式(3)中，T表示矩阵的转置，W表示单位矩阵I,c表示性能指标。

步骤4中自运动指标表示为：

c＝γ(θ(t)-θ_e) (4)

式(4)中，γ表示关节漂移响应系数，θ(t)表示机械臂的关节角度，

θ_e是期望达到的角关节状态向量；

末端位置反馈表示为：k(r(t)-f(θ)) (5)

式(5)中，k为位置反馈系数；r(t)为初始关节状态q0所对应的末端位置向量，为一个常数列向量，f(θ)表示机械臂运动过程中实际的末端位置向量；

引入自运动指标和末端位置反馈后，公式(3)可以表示为：

式(6)中，f(θ)表示机械臂运动过程中实际的末端位置向量，

步骤5中时变矩阵方程表示为：

用x替换J替换J(θ(t))，

将(7)式简化为：L(t)＝x^TWx/2+c^Tx+λ(J·x-b) (8)

式(8)中,λ为拉格朗日乘子；

对公式(8)分别对x和λ求偏导，得：

令公式(9)等于0，并将其简化为如下矩阵：

Qy＝u (10)

式(10)中，

使用一种变参递归神经网络对公式(10)进行求解，

将矩阵方程改写为：

ε(t)＝Qy-u (11)

根据神经动力学的方法，设计神经动力学公式为：

将其分别带入矩阵方程(11)，得到变参递归神经网络，表示为：

对公式(12)进行求解得到最优解y^*，其前n项即最优角速度x^*。

本发明的有益效果是：

本发明一种用于冗余机械臂的自运动方法，将速度层逆运动学方程设计为受等式约束的与二次规划问题；引入末端位置反馈，与关节状态反馈，将二次规划问题转化为在自运动条件约束下的求解；利用一个变参递归神经网络求解器对上述标准二次规划进行；将求解得到的结果传递给机器人控制器；驱动机器人本体进行自运动。

附图说明

图1是本发明一种用于冗余机械臂的自运动方法的流程图；

图2是本发明一种用于冗余机械臂的自运动方法仿真实验机械臂的关节状态图；

图3是本发明一种用于冗余机械臂的自运动方法仿真实验机械臂运动的过程；

图4是本发明一种用于冗余机械臂的自运动方法仿真实验机械臂运动的过程机械臂各个关节角度变化的曲线；

图5是本发明一种用于冗余机械臂的自运动方法仿真实验各个关节速度变化的曲线；

图6是本发明一种用于冗余机械臂的自运动方法仿真实验表示的误差。

具体实施方式

下面结合附图和具体实施方式对本发明进行详细说明。

本发明一种用于冗余机械臂的自运动方法，流程如如图1所示，具体按照以下步骤实施：

步骤1、确定冗余机械臂逆运动学方程，逆运动学方程表示如下：

f(θ(t))＝r(i) (1)

其中θ(t)＝[θ₁(t)，θ₂(t)…θ_n(t)]^T，是关节角度列向量；r(t)＝[x(t)，y(t)，z(t)]^T是期望的末端轨迹；f(·)是一个非线性映射；

步骤2、对自运动方程两边同时求导，自运动方程两边同时求导后表示如下：

其中，J(θ(t))∈R^n×m为速度雅克比矩阵，n为机械臂自由度，m表示末端执行器空间维度；和表示关节角速度向量和末端执行器速度向量；

步骤3、将所述逆运动学方程设计为时变凸二次规划问题，逆运动学方程设计为时变凸二次规划问题后表示如下：

式(3)中，T表示矩阵的转置，W表示单位矩阵I,c表示自运动指标；

步骤4、在所述时变凸二次规划问题中分别引入自运动指标和末端位置反馈；

自运动指标表示为：

c＝γ(θ(i)-θ_e) (4)

式(4)中，γ表示关节漂移响应系数，θ(t)表示机械臂的关节角度，θ_e是期望达到的角关节状态向量；

末端位置反馈表示为：k(r(t)-f(θ)) (5)

式(5)中，k为位置反馈系数；r(t)为初始关节状态q0所对应的末端位置向量，为一个常数列向量，f(θ)表示机械臂运动过程中实际的末端位置向量；

引入自运动指标和末端位置反馈后，公式(3)可以表示为：

式(6)中，f(θ)表示机械臂运动过程中实际的末端位置向量；

步骤5、引入拉格朗日函数，求解时变凸二次规划问题受等式约束的时变凸二次规划方程，并将其转化为时变矩阵方程，并得到最优解；

步骤6、对步骤5得到的速度层上的最优解进行积分，即可得到各个关节角的最优解；

时变矩阵方程表示为：

用x替换J替换J(θ(t))，

将(7)式简化为：L(t)＝x^TWx/2+c^Tx+λ(J·x-b) (8)

式(8)中,λ为拉格朗日乘子；

对公式(8)分别对x和λ求偏导，得：

将公式(9)简化为如下矩阵：Qy＝u (10)

式(10)中，

使用一种变参递归神经网络对公式(10)进行求解，

将矩阵方程改写为：ε(t)＝Qy-u，根据神经动力学的方法，设计神经动力学公式为：将其分别带入矩阵方程(10)，得到变参递归神经网络，表示为：

对公式(11)进行求解得到最优解y^*，其前n项即最优角速度x^*；

步骤7、将求解得到的最优角速度x^*传给下位机控制器驱动机械臂。

实施例

图2中曲线1表示机械臂初始的关节状态，曲线2表示期望的关节状态；图3表示机械臂运动的过程，不同线型表示不同的关节；图4表示机械臂各个关节角度变化的曲线；图5表示各个关节速度变化的曲线，图6表示仿真的误差。

由图2和图3，可以看出机械臂完成了由初始状态到期望状态的自运动过程，且运动过程中机械臂末端位置未发生变化，实现了机械臂的自运动，验证了所提方案的有效性。

通过上述方式，本发明一种用于冗余机械臂的自运动方法，将速度层逆运动学方程设计为受等式约束的与二次规划问题；引入末端位置反馈，与关节状态反馈，将二次规划问题转化为在自运动条件约束下的求解；利用一个变参递归神经网络求解器对上述标准二次规划进行；将求解得到的结果传递给机器人控制器；驱动机器人本体进行自运动，本发明提供了一种计算量小，用于速度控制的冗余机械臂自运动方案。

Claims (6)

1.一种用于冗余机械臂的自运动方法，其特征在于，具体按照以下步骤实施：

步骤1、确定冗余机械臂逆运动学方程；

步骤2、对自运动方程两边同时求导；

步骤3、将所述逆运动学方程设计为时变凸二次规划问题；

步骤4、在所述时变凸二次规划问题中分别引入自运动指标和末端位置反馈；

步骤5、引入拉格朗日函数，求解时变凸二次规划问题受等式约束的时变凸二次规划方程，并将其转化为时变矩阵方程，并得到最优解；

步骤6、对步骤5得到的速度层上的最优解进行积分，即可得到各个关节角的最优解；

步骤7、将求解得到的最优角速度x^*传给下位机控制器驱动机械臂。

2.根据权利要求1所述一种用于冗余机械臂的自运动方法，其特征在于，步骤1中逆运动学方程表示如下：

f(θ(t))＝r(t) (1)

其中θ(t)＝[θ₁(t)，θ₂(t)…θ_n(t)]^T，是关节角度列向量；r(t)＝[x(t)，y(t)，z(t)]^T是期望的末端轨迹；f(·)是一个非线性映射。

3.根据权利要求1所述一种用于冗余机械臂的自运动方法，其特征在于，步骤2中自运动方程两边同时求导后表示如下：

其中，J(θ(t))∈R^n×m为速度雅克比矩阵，n为机械臂自由度，m表示末端执行器空间维度；和表示关节角速度向量和末端执行器速度向量。

4.根据权利要求1所述一种用于冗余机械臂的自运动方法，其特征在于，步骤3中逆运动学方程设计为时变凸二次规划问题后表示如下：

式(3)中，T表示矩阵的转置，W表示单位矩阵I,c表示自运动指标。

5.根据权利要求3所述一种用于冗余机械臂的自运动方法，其特征在于，步骤4中自运动指标表示为：

c＝γ(θ(t)-θ_e) (4)

式(4)中，γ表示关节漂移响应系数，θ(t)表示机械臂的关节角度，θ_e是期望达到的角关节状态向量；

末端位置反馈表示为：k(r(t)-f(θ)) (5)

式(5)中，k为位置反馈系数；r(t)为初始关节状态q0所对应的末端位置向量，为一个常数列向量，f(θ)表示机械臂运动过程中实际的末端位置向量；

引入自运动指标和末端位置反馈后，公式(3)可以表示为：

式(6)中，f(θ)表示机械臂运动过程中实际的末端位置向量。

6.根据权利要求1所述一种用于冗余机械臂的自运动方法，其特征在于，步骤5中时变矩阵方程表示为：

用x替换J替换J(θ(t))，

将(7)式简化为：L(t)＝x^TWx/2+c^Tx+λ(J·x-b) (8)

式(8)中,λ为拉格朗日乘子；

对公式(8)分别对x和λ求偏导，得：

将公式(9)简化为如下矩阵：Qy＝u (10)

式(10)中，

使用一种变参递归神经网络对公式(10)进行求解，

将矩阵方程改写为：ε(t)＝Qy-u，根据神经动力学的方法，设计神经动力学公式为：将其分别带入矩阵方程(10)，得到变参递归神经网络，表示为：

对公式(11)进行求解得到最优解y^*，其前n项即最优角速度x^*。