[发明专利]一种应用于冗余度机械臂的障碍物躲避解决方法

权利要求书

1.一种应用于冗余度机械臂的障碍物躲避解决方法，其特征在于，所述方法包括以下步骤：

S1、通过建立机械臂的D-H矩阵获得机械臂的正运动学模型，并对其求导后建立速度层的目标轨迹等式约束指标；

S2、建立基于矢量的障碍物躲避不等式约束指标；

S3、将步骤S1建立的速度层的目标轨迹等式约束指标和步骤S2建立的基于矢量的障碍物躲避不等式约束指标写为统一形式的二次规划问题；

S4、将步骤S3中统一形式的二次规划问题转化为线性变分不等式；

S5、使用原对偶神经网络求解器求解步骤S4中的线性变分不等式；

S6、将步骤S5中原对偶神经网络求解器求解出来的机械臂关节角控制变量输出到机械臂以实现控制冗余度机械臂障碍物躲避；

所述步骤S2的具体过程为：

S2.1、首先寻找到障碍物上距离机械臂最近的一点，即障碍物点O，以及机械臂上距离障碍物点O最近的一点，即脆弱点C；

S2.2、通过以下算法原理建立基于矢量的障碍物躲避不等式约束指标：

其中，J'_O定义为表示从障碍物点O指向脆弱点C的矢量，J_C表示脆弱点C的雅克比矩阵，J'_O∈R^1×n，表示机械臂关节角的角速度以及移动平台的驱动轮角速度，ν'＝s(d)·max(J'_Oθ|d＝d₂,0)，θ表示机械臂关节角的移动角度以及移动平台的驱动轮移动角度，函数max(·,·)表示两个数中取最大值，d表示障碍物点O与脆弱点C的距离，d₂表示设定的开始执行障碍物躲避算法时障碍物点O与脆弱点C的距离，光滑函数s(d)定义如下：

其中，d₁表示设定的障碍物点O与脆弱点C的最小距离，d₂表示设定的开始执行障碍物躲避算法时障碍物点O与脆弱点C的距离；

步骤S3中，所述统一形式的二次规划问题为：

其中，表示机械臂关节角的角速度以及移动平台的驱动轮角速度，J_E表示机械臂末端执行器的雅克比矩阵，J'_O定义为表示从障碍物点O指向脆弱点C的矢量，J_C表示脆弱点C的雅克比矩阵，表示速度层的机械臂末端执行器的目标轨迹；

步骤S1中建立的速度层的目标轨迹等式约束指标为：

其中，J_E表示机械臂末端执行器的雅克比矩阵，表示机械臂关节角的角速度以及移动平台的驱动轮角速度，表示速度层的机械臂末端执行器的目标轨迹。

2.根据权利要求1所述的一种应用于冗余度机械臂的障碍物躲避解决方法，其特征在于，步骤S4的具体过程为：设定原对偶矢量u^*，满足以下条件：

(u-u^*)^T(Mu^*+q)≥0,

其中是原对偶决定变量矢量，矢量g表示等式的原对偶决定变量矢量，Ω为原对偶决定变量矢量的取值范围，为一凸集，其中的各符号定义如下：

Ω＝{u|u^-≤u≤u⁺}

J＝[J_E；J'_O],and

其中E为单位矩阵，u⁺、u^-分别为u的上下数值极限；上述分段线性变分不等式能够转化为以下线性变分不等式：

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

其中，函数P_Ω表示分段线性投影算子，投影到Ω。

3.根据权利要求2所述的一种应用于冗余度机械臂的障碍物躲避解决方法，其特征在于，步骤S5中，所述原对偶神经网络求解器为：

其中，β表示原对偶神经网络的收敛率，且β＞0，对此处的进行积分，即可得到u，从而得到