[发明专利]一种面向冗余移动机械臂容错型重复运动规划方法

权利要求书

1.一种面向冗余移动机械臂容错型重复运动规划方法，包括以下步骤：

1)确定冗余移动机械臂末端执行器期望目标轨迹r_wd^*(t)和期望回拢的运动关节角度θ^*(0)；

其特征在于，所述方法还包括以下步骤：

2)设计基于有限值终态网络的终态吸引优化指标，形成移动机械臂重复运动规划方案，其中移动机械臂实际运动时的初始关节角任意指定，只需要保证关节角的运动幅度在关节角物理限制范围之内，不要求各运动关节角处于期望轨迹上；给定冗余移动机械臂实际运动时的初始关节角度θ(0),以θ(0)为运动起始点，形成的重复运动规划方案描述为具有有限时间收敛优化指标的二次规划：

其中K∈R^m×(n+2)表示整个移动机械臂系统的雅克比矩阵，r_wd∈Rⁿ为移动机械臂末端执行器运动的期望轨迹；表示末端执行器运动时的速度向量，r_w为末端执行器实际运动的轨迹，κ＞0表示位置的参数增益，用来调节末端执行器运动到期望路径的速率；I是单位矩阵，C＝[B；Acosφ]∈R^m×2，φ为移动平台的航向角，分别为左、右轮子的转动角度，r为轮子半径，b为轮子到点P₀的距离，d为点P₀到点P_c的距离；

p_c＝[x_c,y_c]^T，x_c、y_c分别为点p_c在水平面上的坐标，θ＝[θ₁,θ₂,θ₃,θ₄,θ₅,θ₆]^T，θ₁,θ₂,θ₃,θ₄,θ₅,θ₆分别为各关节旋转角度，β_E1＞0，β_E2＞0，β_E3＞0，β_E1、β_E2、β_E3都为可调节常量，p_c(0)、φ(0)、θ(0)分别为p_c、φ、θ的初始状态；为了避免机械臂的移动过程中受到损坏，在移动机械臂运动过程中需要考虑各个关节角转动范围的限制；

q⁺＝[∞,∞,0.5236,0.1745,6.2832,6.2832,6.2832,6.2832]^T,q^-＝-q⁺，

3)构建有限值激活函数的终态网络模型，其动态特性由下述方程描述

其中，关节角位移偏差E(t)＝θ(t)-θ(0)，此动态方程所表达的系统有限时间收敛于零，0＜δ＜1，β_E＞0；

在指标函数达到最小值时，冗余移动机械臂的各个关节角回拢到期望的目标轨迹上；

为求解步骤2)中的重复运动规划方案，建立拉格朗日函数

式中，W＝D^TD∈R^(n+2)×(n+2)，h＝D^Tz∈Rⁿ⁺²，λ(t)为拉格朗日乘子向量，λ^T(t)是λ(t)向量的转置，通过拉格朗日函数对各个变量求导，并令其为零，得下述矩阵方程

E＝Mu-p (3)

最后，记时变矩阵方程误差E＝Mu-p，为求解矩阵方程(3)，依据终态神经网络动态方程(2)构建神经网络模型(4)，得到各关节角自运动轨迹；

4)将步骤3)中求解得到的结果用于控制各关节电机，驱动机械臂执行任务。