CN113043286B - 一种多自由度机械臂实时避障路径规划系统及方法 - Google Patents

Abstract

本发明公开了一种多自由度机械臂实时避障路径规划系统及方法。该系统包括人机交互模块、数据采集模块、A/D转换模块、中央处理模块、D/A转换模块、控制器。中央处理模块是以机械臂各关节转角变化量之和的最小值为目标函数进行优化计算的。以机械臂各关节转角变化量之和的最小值为目标函数优化后得到的结果相比于机械臂个关节角运动的最短距离作为目标函数得到的结果更易于机械臂运动控制。该方法能够在不影响其末端执行器需要执行的操作的前提下，规划出一条实现对静态、动态障碍物避障的最优路径，且实时性高。这对机械臂在实际工作环境中工作成效具有重要意义。

Description

一种多自由度机械臂实时避障路径规划系统及方法

技术领域

本发明属于机械臂路径规划领域，涉及一种多自由度机械臂实时避障路径规划系统及方法。

背景技术

从世界上第一台工业机器人诞生至今,机器人技术已经被广泛应用到人们的工作和生活中，促进了国家生产力的提高和整个社会的进步，也促进了人们生活的改善。许多危险繁重的工业作业，例如组装、焊接、喷漆和高温锻造等，都能由工业机器人替代工人完成。除了在制造业有着广泛的应用，工业机器人也在医疗救援、物流仓储、娱乐服务、农业采摘甚至在太空探索等领域发挥着重要的角色。除了在制造业有着广泛的应用，工业机器人也在医疗救援、物流仓储、娱乐服务、农业采摘甚至在太空探索等领域发挥着重要的角色。随着机械臂的广泛应用和工作场景复杂化程度越来越高，避障路径规划研究变得日益重要。在实际工作过程中，机械臂的工作环境非常复杂，外部可能存在动态的障碍物。因此，如何规划机械臂的运动路径，有效避开动态障碍物，对机械臂的工作成效具有重要意义。

发明内容

本发明的目的是针对机械臂的作业环境中存在动态障碍物的情况(其中动态障碍物是在固定的范围内且缓慢运动的障碍物)，提出了一种多自由度机械臂实时避障路径规划系统。

该系统包括人机交互模块、数据采集模块、A/D转换模块、中央处理模块、D/A转换模块、控制器。

人机交互模块用于设定机械臂相关参数，安全距离d_safe，目标状态机械臂各关节转角等参数，并传送到中央处理模块；其中机械臂相关参数包括连杆长度、连杆扭转角、连杆偏距、关节转角。

数据采集模块用于对机械臂运行数据进行实时采集；机械臂运行数据包括机械臂各关节的实时转角以及机械臂与障碍物之间的实时距离d，d是通过机械臂雷达传感器测量所得。

中央处理模块根据数据采集模块和人机交互模块传送的数据，获取最优目标函数值和最优变量值，并将其输送到D/A转换模块；

A/D转换模块用于将接收到的模拟量转换为相应的数字量，并将其输送到中央处理模块；

D/A转换模块接收到的数字量转换成相应的模拟量，并将其输送到控制器；

控制器用于控制机械臂各关节的驱动电机。

该方法实现了机械臂在作业环境中实时避障，提高了一定的安全性。

本发明的另一个目的是提供一种多自由度机械臂实时避障路径规划方法，主要步骤如下：

步骤A1、人机交互模块将设定的机械臂相关参数，目标状态机械臂各关节转角以及安全距离d_safe等参数传送到中央处理模块；其中机械臂相关参数包括连杆长度、连杆扭转角、连杆偏距、关节转角。

步骤A2、数据采集模块对机械臂运行数据进行实时采集，并通过A/D转换模块转换成数字量将其输送到中央处理模块。机械臂运行数据包括机械臂各关节的实时转角以及机械臂与障碍物之间的实时距离d，d是通过机械臂雷达传感器测量所得。

步骤A3、中央处理模块以当前状态的机械臂各关节转角作为初始状态，调用优化算法、机理模型计算获取机械臂避障的最优路径以及最优目标函数值；

步骤A4、中央处理模块将得到的机械臂避障的最优路径作为设定值，通过D/A转换模块发送给控制器；

步骤A5、控制器以最优路径为设定目标，发送控制指令驱动机械臂上各个关节角的电机，使各个关节向目标转角运动。

步骤A6、在机械臂各关节运动的过程中，中央处理模块判断数据采集模块采集的实时距离d与d_safe的大小关系。如果d≥d_safe，则继续运动直至到达目标状态，反之则转至步骤A3。

作为优选，所述中央处理模块存储有机械臂实时避障的路径规划优化模型，包括目标函数、机械臂运动学模型、约束条件；

机械臂实时避障的路径规划优化问题形成如下：

步骤B1、目标函数为：

其中t＝1,2,...,h，h为规划产生的时间点，b＝1,2,...,N，N为机械臂的关节个数，

为t时间点机械臂第b个关节的转角，

为t+1时间点机械臂第b个关节的转角。

步骤B2、机械臂正运动学模型

为每个连杆设定一个相对坐标系，通过连杆长度、连杆扭转角、连杆偏距、关节转角四个参数表示出坐标系{b-1}与坐标系{b}的齐次变换矩阵

其中a_b-1，α_b-1分别为第b-1个连杆的连杆长度、连杆扭转角；d_b，θ_b分别为第b个连杆的连杆偏距、关节转角。

由相邻坐标系的齐次变换矩阵，通过矩阵乘法可以求出每个连杆坐标系相对于基座坐标系的齐次变换矩阵：

步骤B3、约束条件

1)初始状态约束

Θ₀＝Θ_int (4)

其中

表示0时间点机械臂N个关节角的转角，

表示初始状态时机械臂N个关节角的转角，T表示转置。

2)目标状态约束

Θ_h＝Θ_tar (5)

其中

表示h时间点机械臂N个关节角的转角，

表示目标状态时机械臂N个关节角的转角。

3)关节角约束

Θ_min≤Θ_t≤Θ_max，t＝1,2,...,h (6)

其中Θ_min＝[θ_1min,θ_2min,...,θ_Nmin]^T表示机械臂N个关节角的运动下限，Θ_max＝[θ_1max,θ_2max,...,θ_Nmax]^T表示机械臂N个关节角的运动上限，

表示t时间点机械臂N个关节角的转角。

4)无碰撞约束

为了简化机械臂与障碍物之间的距离计算，我们可以采用简单是球体对机械臂进行近似，障碍物的简化与机械臂同理。R表示机械臂用球体近似后的球心，O表示障碍物上的点。因此，机械臂与障碍物之间的距离可以表示为：

其中

为机械臂第b个连杆上的第i个球心，b＝1,2,...,N，N为机械臂的连杆数，其与机械臂的关节个数相等，i＝1,2,...,M，M为机械臂的连杆上所需近似球体的个数，M∈[1,50]；O_j为障碍物上第j个球心，j＝1,2,...,P，P为障碍物上所需近似球体的个数，P∈[1,100]。

因此，无碰撞约束可表示为：

其中

为t时间点机械臂第b个连杆上的第i个近似球心与障碍物上第j个近似球心之间的距离。

通过优化算法计算式(1)-(8)组成的优化问题，具体步骤如下：

步骤C1、将式(1)-(8)组成的优化问题转化成如下式(9)的非线性优化问题：

其中_x为_n维变量，f(x)和c(x)分别表示连续可微的目标函数和_m维约束方程，x^L和x^U分别表示变量的上下界约束。

步骤C2、采用迭代计算方法求解上式(9)所表示的优化问题，在_x第k次迭代(k为大于等于零的整数)的迭代点x_k处QP子问题表示为以下形式：

式中d_k为搜索方向，g_k和

分别表示在x_k处目标函数的导数和约束方程的雅克比矩阵，c_k表示在x_k处c(x_k)的值，W_k为拉格朗日函数的Hessian阵。其中拉格朗日函数为：

L(x,λ,v,π)＝f(x)+λ^Tc(x)+v^T(x-x^U)-π^T(x-x^L) (11)

式中λ、v和π分别表示与等式约束、上边界约束和下边界约束相关的拉格朗日乘子，λ^T、v^T和π^T分别表示相应拉格朗日乘子的转置。

步骤C3、在迭代过程中将搜索空间分解为两个子空间_Y和_Z，将式(10)表示的QP子问题转化为低维QP子问题。其中Z∈R^n×(n-m)，由雅克比矩阵的零空间向量组成，Y∈R^n×m，由雅克比矩阵的值空间向量组成。在x_k处子空间_Y和_Z的值表示为Y_k和Z_k，Z_k满足下式：

搜索方向d_k可表示为零值空间方向的移动量：

d_k＝Y_kp_y+Z_kp_z (13)

其中p_y和p_z表示值空间和零空间移动的矢量矩阵，且p_y∈R^m，p_z∈R^n-m。n、m为维度，都为正整数，n＞m。

将式(12)和式(13)带入到QP子问题(10)中的等式约束，可得：

因此根据式(14)p_y被唯一确定：

从而搜索方向可表示为：

将式(16)中的搜索方向dk代入到以上QP子问题中，并去掉与变量p_z无关的常数项，则QP子问题表示为式(17)以p_z∈R^n-m为变量的以下QP子问题形式：

其中w_k为(n-m)×1矩阵

B_k为(n-m)×(n-m)矩阵

p_z为(n-m)维变量。

步骤C4、采用积极集方法求解式(17)获得p_z，然后根据式(13)获得搜索方向矢量的值d_k，其中式(17)表示的QP子问题的积极集求解方法为成熟技术。

步骤C5、令x_k+1＝x_k+αd_k，这样就得到了下一个迭代点。其中α∈(0,1]，通过一维搜索方法求取，该求取方法为成熟技术。

步骤C6、求取搜索方向二范数norm(d_k,2)、拉格朗日函数梯度与搜索方向乘积的值和一阶优化条件值；如果一阶优化条件值小于设定误差ε₁，或者norm(d_k,2)的值和拉格朗日函数梯度与搜索方向乘积的值同时小于设定误差ε₁，则停止计算，得到最优目标函数值和最优变量值；否则令x_k＝x_k+1，转步骤C2，继续计算。

与现有的技术相比，本发明具有如下优点：

1)本发明的中央处理模块是以机械臂各关节转角变化量之和的最小值为目标函数进行优化计算的。以机械臂各关节转角变化量之和的最小值为目标函数优化后得到的结果相比于机械臂个关节角运动的最短距离作为目标函数得到的结果更易于机械臂运动控制。

2)本发明是基于在线规划的机械臂实时避障系统，该系统能够在不影响其末端执行器需要执行的操作前提下，规划出一条实现对静态、动态障碍物避障的最优路径，且实时性高。这对机械臂在实际工作环境中工作成效具有重要意义。

附图说明

图1为系统整体结构示意图；

图2为机械臂正运动学建模参数示意图；

图3为机械臂避障模型简化示意图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步的分析。

一种多自由度机械臂实时避障路径规划系统，该方法主要步骤如下：

步骤A1、人机交互模块将设定的机械臂相关参数，目标状态机械臂各关节转角以及安全距离d_safe等参数传送到中央处理模块；其中机械臂相关参数包括连杆长度、连杆扭转角、连杆偏距、关节转角。

步骤A2、数据采集模块对机械臂运行数据进行实时采集，并通过A/D转换模块转换成数字量将其输送到中央处理模块。机械臂运行数据包括机械臂各关节的实时转角以及机械臂与障碍物之间的实时距离d，d是通过机械臂雷达传感器测量所得。

步骤A3、中央处理模块以当前状态的机械臂各关节转角作为初始状态，调用优化算法、机理模型计算获取机械臂避障的最优路径以及最优目标函数值；

步骤A4、中央处理模块将得到的机械臂避障的最优路径作为设定值，通过D/A转换模块发送给控制器；

步骤A5、控制器以最优路径为设定目标，发送控制指令驱动机械臂上各个关节角的电机，使各个关节向目标转角运动。

步骤A6、在机械臂各关节运动的过程中，中央处理模块判断数据采集模块采集的实时距离d与d_safe的大小关系。如果d≥d_safe，则继续运动直至到达目标状态，反之则转至步骤A3。

作为优选，所述中央处理模块存储有机械臂实时避障的路径规划优化模型，包括目标函数、机械臂运动学模型、约束条件；

机械臂实时避障的路径规划优化问题形成如下：

步骤B1、目标函数为：

其中t＝1,2,...,h，h为规划产生的时间点，b＝1,2,...,N，N为机械臂的关节个数，

为t时间点机械臂第b个关节的转角，

为t+1时间点机械臂第b个关节的转角。

步骤B2、机械臂正运动学模型

为每个连杆设定一个相对坐标系，通过连杆长度、连杆扭转角、连杆偏距、关节转角四个参数表示出坐标系{b-1}与坐标系{b}的齐次变换矩阵

其中a_b-1，α_b-1分别为第b-1个连杆的连杆长度、连杆扭转角；d_b，θ_b分别为第b个连杆的连杆偏距、关节转角。

由相邻坐标系的齐次变换矩阵，通过矩阵乘法可以求出每个连杆坐标系相对于基座坐标系的齐次变换矩阵：

步骤B3、约束条件

5)初始状态约束

Θ₀＝Θ_int (4)

其中

表示0时间点机械臂N个关节角的转角，

表示初始状态时机械臂N个关节角的转角，T表示转置。

6)目标状态约束

Θ_h＝Θ_tar (5)

其中

表示h时间点机械臂N个关节角的转角，

表示目标状态时机械臂N个关节角的转角。

7)关节角约束

Θ_min≤Θ_t≤Θ_max，t＝1,2,...,h (6)

其中Θ_min＝[θ_1min,θ_2min,...,θ_Nmin]^T表示机械臂N个关节角的运动下限，Θ_max＝[θ_1max,θ_2max,...,θ_Nmax]^T表示机械臂N个关节角的运动上限，

表示t时间点机械臂N个关节角的转角。

8)无碰撞约束

为了简化机械臂与障碍物之间的距离计算，我们可以采用简单是球体对机械臂进行近似，障碍物的简化与机械臂同理。R表示机械臂用球体近似后的球心，O表示障碍物上的点。因此，机械臂与障碍物之间的距离可以表示为：

其中

为机械臂第b个连杆上的第i个球心，b＝1,2,...,N，N为机械臂的连杆数，其与机械臂的关节个数相等，i＝1,2,...,M，M为机械臂的连杆上所需近似球体的个数，M∈[1,50]；O_j为障碍物上第j个球心，j＝1,2,...,P，P为障碍物上所需近似球体的个数，P∈[1,100]。

因此，无碰撞约束可表示为：

其中

为t时间点机械臂第b个连杆上的第i个近似球心与障碍物上第j个近似球心之间的距离。

通过优化算法计算式(1)-(8)组成的优化问题，具体步骤如下：

步骤C1、将式(1)-(8)组成的优化问题转化成如下式(9)的非线性优化问题：

其中x为n维变量，f(x)和c(x)分别表示连续可微的目标函数和m维约束方程，x^L和x^U分别表示变量的上下界约束。

步骤C2、采用迭代计算方法求解上式(9)所表示的优化问题，在x第k次迭代(k为大于等于零的整数)的迭代点x_k处QP子问题表示为以下形式：

式中d_k为搜索方向，g_k和

分别表示在x_k处目标函数的导数和约束方程的雅克比矩阵，c_k表示在x_k处c(x_k)的值，W_k为拉格朗日函数的Hessian阵。其中拉格朗日函数为：

L(x,λ,v,π)＝f(x)+λ^Tc(x)+v^T(x-x^U)-π^T(x-x^L) (11)

式中λ、v和π分别表示与等式约束、上边界约束和下边界约束相关的拉格朗日乘子，λ^T、v^T和π^T分别表示相应拉格朗日乘子的转置。

步骤C3、在迭代过程中将搜索空间分解为两个子空间Y和Z，将式(10)表示的QP子问题转化为低维QP子问题。其中Z∈R^n×(n-m)，由雅克比矩阵的零空间向量组成，Y∈R^n×m，由雅克比矩阵的值空间向量组成。在x_k处子空间_Y和_Z的值表示为Y_k和Z_k，Z_k满足下式：

搜索方向d_k可表示为零值空间方向的移动量：

d_k＝Y_kp_y+Z_kp_z (13)

其中p_y和p_z表示值空间和零空间移动的矢量矩阵，且p_y∈R^m，p_z∈R^n-m。n、m为维度，都为正整数，n＞m。

将式(12)和式(13)带入到QP子问题(10)中的等式约束，可得：

因此根据式(14)p_y被唯一确定：

从而搜索方向可表示为：

将式(16)中的搜索方向d_k代入到以上QP子问题中，并去掉与变量p_z无关的常数项，则QP子问题表示为式(17)以p_z∈R^n-m为变量的以下QP子问题形式：

其中w_k为(n-m)×1矩阵

B_k为(n-m)×(n-m)矩阵

p_z为(n-m)维变量。

步骤C4、采用积极集方法求解式(17)获得p_z，然后根据式(13)获得搜索方向矢量的值d_k，其中式(17)表示的QP子问题的积极集求解方法为成熟技术。

步骤C5、令x_k+1＝x_k+αd_k，这样就得到了下一个迭代点。其中α∈(0,1]，通过一维搜索方法求取，该求取方法为成熟技术。

步骤C6、求取搜索方向二范数norm(d_k,2)、拉格朗日函数梯度与搜索方向乘积的值和一阶优化条件值；如果一阶优化条件值小于设定误差ε₁，或者norm(d_k,2)的值和拉格朗日函数梯度与搜索方向乘积的值同时小于设定误差ε₁，则停止计算，得到最优目标函数值和最优变量值；否则令x_k＝x_k+1，转步骤C2，继续计算。

如图2所示，机械臂正运动学建模参数示意图，具体过程为：

为了确定各连杆之间的相对运动和位姿关系，在每一个连杆上设定一个坐标系。而连杆坐标系的设定不是随意的，需要遵循一定的原则。首先，

轴与关节轴b轴线重合，

轴与关节轴b-1轴线重合。根据右手定则确定

轴和

轴的正方向。确定

轴和

轴的方向后，需要确定连杆坐标系的原点。找

轴与

轴的公法线，用给相互正交的直线将关节轴线b和b-1相连，坐标系{b-1}的原点放在公法线与关节轴b-1的交点处。

轴选在对应的公法线方向上，

轴则根据

来唯一确定。按照以上原则设定连杆坐标系{b-1}的

和坐标系{b}的

第b-1个连杆长度a_b-1表示沿

轴，从

移动到

的距离；连杆扭转角α_b-1表示绕

轴，从

旋转到

的角度；连杆偏距d_b表示沿

轴，从

移动到

的距离；关节转角θ_b表示绕

轴，从

旋转到

的角度。

如图3所示，机械臂避障模型简化示意图，具体过程为：

为了简化机械臂与障碍物之间的距离计算，采用简单的球体对机械臂进行近似。在机械臂第b个连杆的中心线上选取一组关键点

M为机械臂的连杆上所需近似球体的个数，M∈[1,50]，并定义一个半径

使以点

为球心，

为半径的一系列球体组成的集合

能够完全包围机械臂的本体。

以上内容是结合具体的优选实施方式对本发明所做的进一步详细说明，不能认定本发明的具体实施只限于这些说明。对于本发明所属技术领域的技术人员来说，在不脱离发明构思的前提下还可以做出一定程度的简单推演或者替换，都应当视为属于本发明的保护范围。

Claims (2)

1.一种多自由度机械臂实时避障路径规划方法，其特征在于包括以下步骤：

步骤A1、人机交互模块将设定的机械臂相关参数，目标状态机械臂各关节转角以及安全距离d_safe传送到中央处理模块；其中机械臂相关参数包括连杆长度、连杆扭转角、连杆偏距、关节转角；

步骤A2、数据采集模块对机械臂运行数据进行实时采集，并通过A/D转换模块转换成数字量将其输送到中央处理模块；机械臂运行数据包括机械臂各关节的实时转角以及机械臂与障碍物之间的实时距离d；

步骤A3、中央处理模块以当前状态的机械臂各关节转角作为初始状态，调用优化算法、机理模型计算获取机械臂避障的最优路径以及最优目标函数值；

步骤A4、中央处理模块将得到的机械臂避障的最优路径作为设定值，通过D/A转换模块发送给控制器；

步骤A5、控制器以最优路径为设定目标，发送控制指令驱动机械臂上各个关节角的电机，使各个关节向目标转角运动；

步骤A6、在机械臂各关节运动的过程中，中央处理模块判断数据采集模块采集的实时距离d与d_safe的大小关系；如果d≥d_safe，则继续运动直至到达目标状态，反之则转至步骤A3；

所述中央处理模块存储有机械臂实时避障的路径规划优化模型，其优化问题包括目标函数、机械臂运动学模型、约束条件；

步骤B1、目标函数为：

其中t＝1,2,...,h，h为规划产生的时间点，b＝1,2,...,N，N为机械臂的关节个数，

为t时间点机械臂第b个关节的转角，

为t+1时间点机械臂第b个关节的转角；

步骤B2、机械臂正运动学模型

为每个连杆设定一个相对坐标系，通过连杆长度、连杆扭转角、连杆偏距、关节转角四个参数表示出坐标系{b-1}与坐标系{b}的齐次变换矩阵

其中a_b-1，α_b-1分别为第b-1个连杆的连杆长度、连杆扭转角；d_b，θ_b分别为第b个连杆的连杆偏距、关节转角；

由相邻坐标系的齐次变换矩阵，通过矩阵乘法可以求出每个连杆坐标系相对于基座坐标系的齐次变换矩阵：

步骤B3、约束条件

1)初始状态约束

Θ₀＝Θ_int (4)

其中

表示0时间点机械臂N个关节角的转角，

表示初始状态时机械臂N个关节角的转角，T表示转置；

2)目标状态约束

Θ_h＝Θ_tar (5)

其中

表示h时间点机械臂N个关节角的转角，

表示目标状态时机械臂N个关节角的转角；

3)关节角约束

Θ_min≤Θ_t≤Θ_max，t＝1,2,...,h (6)

其中Θ_min＝[θ_1min,θ_2min,...,θ_Nmin]^T表示机械臂N个关节角的运动下限，Θ_max＝[θ_1max,θ_2max,...,θ_Nmax]^T表示机械臂N个关节角的运动上限，

表示t时间点机械臂N个关节角的转角；

4)无碰撞约束

机械臂与障碍物之间的距离可以表示为：

其中

为机械臂第b个连杆上的第i个球心，b＝1,2,...,N，N为机械臂的连杆数，其与机械臂的关节个数相等，i＝1,2,...,M，M为机械臂的连杆上所需近似球体的个数；O_j为障碍物上第j个球心，j＝1,2,...,P，P为障碍物上所需近似球体的个数；

因此，无碰撞约束可表示为：

其中

为t时间点机械臂第b个连杆上的第i个近似球心与障碍物上第j个近似球心之间的距离；

通过优化算法计算式(1)-(8)组成的优化问题，具体步骤如下：

步骤C1、将式(1)-(8)组成的优化问题转化成如下式(9)的非线性优化问题：

其中x为n维变量，f(x)和c(x)分别表示连续可微的目标函数和m维约束方程，x^L和x^U分别表示变量的上下界约束；

步骤C2、采用迭代计算方法求解上式(9)所表示的优化问题，在x第k次迭代的迭代点x_k处QP子问题表示为以下形式：

式中d_k为搜索方向，g_k和

分别表示在x_k处目标函数的导数和约束方程的雅克比矩阵，c_k表示在x_k处c(x_k)的值，W_k为拉格朗日函数的Hessian阵；

其中拉格朗日函数为：

L(x,λ,v,π)＝f(x)+λ^Tc(x)+v^T(x-x^U)-π^T(x-x^L) (11)

式中λ、v和π分别表示与等式约束、上边界约束和下边界约束相关的拉格朗日乘子，λ^T、v^T和π^T分别表示相应拉格朗日乘子的转置；

步骤C3、在迭代过程中将搜索空间分解为两个子空间Y和Z，将式(10)表示的QP子问题转化为低维QP子问题；其中Z∈R^n×(n-m)，由雅克比矩阵的零空间向量组成，Y∈R^n×m，由雅克比矩阵的值空间向量组成；

在x_k处子空间Y和Z的值表示为Y_k和Z_k，Z_k满足下式：

搜索方向dk可表示为零值空间方向的移动量：

d_k＝Y_kp_y+Z_kp_z (13)

其中p_y和p_z表示值空间和零空间移动的矢量矩阵，且p_y∈R^m，p_z∈R^n-m；n、m为维度，n＞m；

将式(12)和式(13)带入到QP子问题(10)中的等式约束，可得：

因此根据式(14)p_y被唯一确定：

从而搜索方向可表示为：

将式(16)中的搜索方向dk代入到以上QP子问题中，并去掉与变量p_z无关的常数项，则QP子问题表示为式(17)以p_z∈R^n-m为变量的以下QP子问题形式：

其中w_k为(n-m)×1矩阵

Bk为(n-m)×(n-m)矩阵

p_z为(n-m)维变量；

步骤C4、求解式(17)获得p_z，然后根据式(13)获得搜索方向矢量的值d_k；

步骤C5、更新x_k+1＝x_k+αd_k，α∈(0,1]；

步骤C6、求取搜索方向二范数norm(d_k,2)、拉格朗日函数梯度与搜索方向乘积的值和一阶优化条件值；如果一阶优化条件值小于设定误差ε₁，或者norm(d_k,2)的值和拉格朗日函数梯度与搜索方向乘积的值同时小于设定误差ε₁，则得到最优目标函数值和最优变量值；否则令x_k＝x_k+1，转步骤C2。

2.一种利用如权利要求1所述多自由度机械臂实时避障路径规划方法的系统，其特征在于包括人机交互模块、数据采集模块、A/D转换模块、中央处理模块、D/A转换模块、控制器；

人机交互模块用于设定机械臂相关参数，安全距离d_safe，目标状态机械臂各关节转角，并传送到中央处理模块；其中机械臂相关参数包括连杆长度、连杆扭转角、连杆偏距、关节转角；

数据采集模块用于对机械臂运行数据进行实时采集；机械臂运行数据包括机械臂各关节的实时转角以及机械臂与障碍物之间的实时距离d；

中央处理模块根据数据采集模块和人机交互模块传送的数据，获取最优目标函数值和最优变量值，并将其输送到D/A转换模块；

A/D转换模块用于将接收到的模拟量转换为相应的数字量，并将其输送到中央处理模块；

D/A转换模块接收到的数字量转换成相应的模拟量，并将其输送到控制器；

控制器用于控制机械臂各关节的驱动电机。

Images