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

技术领域

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

背景技术

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

发明内容

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

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

人机交互模块用于设定机械臂相关参数，安全距离d

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

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

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

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

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

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

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

步骤A1、人机交互模块将设定的机械臂相关参数，目标状态机械臂各关节转角以及安全距离d

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

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

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

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

步骤A6、在机械臂各关节运动的过程中，中央处理模块判断数据采集模块采集的实时距离d与d

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

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

步骤B1、目标函数为：

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

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

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

其中a

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

步骤B3、约束条件

1)初始状态约束

Θ

其中

2)目标状态约束

Θ

其中

3)关节角约束

Θ

其中Θ

4)无碰撞约束

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

其中

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

其中

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

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

其中

步骤C2、采用迭代计算方法求解上式(9)所表示的优化问题，在

式中d

L(x,λ,v,π)＝f(x)+λ

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

步骤C3、在迭代过程中将搜索空间分解为两个子空间

搜索方向d

d

其中p

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

因此根据式(14)p

从而搜索方向可表示为：

将式(16)中的搜索方向dk代入到以上QP子问题中，并去掉与变量p

其中w

步骤C4、采用积极集方法求解式(17)获得p

步骤C5、令x

步骤C6、求取搜索方向二范数norm(d

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

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

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

附图说明

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

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

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

具体实施方式

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

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

步骤A1、人机交互模块将设定的机械臂相关参数，目标状态机械臂各关节转角以及安全距离d

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

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

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

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

步骤A6、在机械臂各关节运动的过程中，中央处理模块判断数据采集模块采集的实时距离d与d

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

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

步骤B1、目标函数为：

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

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

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

其中a

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

步骤B3、约束条件

5)初始状态约束

Θ

其中

6)目标状态约束

Θ

其中

7)关节角约束

Θ

其中Θ

8)无碰撞约束

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

其中

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

其中

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

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

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

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

式中d

L(x,λ,v,π)＝f(x)+λ

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

步骤C3、在迭代过程中将搜索空间分解为两个子空间Y和Z，将式(10)表示的QP子问题转化为低维QP子问题。其中Z∈R

搜索方向d

d

其中p

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

因此根据式(14)p

从而搜索方向可表示为：

将式(16)中的搜索方向d

其中w

步骤C4、采用积极集方法求解式(17)获得p

步骤C5、令x

步骤C6、求取搜索方向二范数norm(d

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

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

第b-1个连杆长度a

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

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

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