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

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

【技术实现步骤摘要】
一种多自由度机械臂实时避障路径规划系统及方法


[0001]本专利技术属于机械臂路径规划领域，涉及一种多自由度机械臂实时避障路径规划系统及方法。

技术介绍

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

技术实现思路

[0003]本专利技术的目的是针对机械臂的作业环境中存在动态障碍物的情况(其中动态障碍物是在固定的范围内且缓慢运动的障碍物)，提出了一种多自由度机械臂实时避障路径规划系统。
[0004]该系统包括人机交互模块、数据采集模块、A/D转换模块、中央处理模块、D/A转换模块、控制器。
[0005]人机交互模块用于设定机械臂相关参数，安全距离d
safe
，目标状态机械臂各关节转角等参数，并传送到中央处理模块；其中机械臂相关参数包括连杆长度、连杆扭转角、连杆偏距、关节转角。
[0006]数据采集模块用于对机械臂运行数据进行实时采集；机械臂运行数据包括机械臂各关节的实时转角以及机械臂与障碍物之间的实时距离d，d是通过机械臂雷达传感器测量所得。
[0007]中央处理模块根据数据采集模块和人机交互模块传送的数据，获取最优目标函数值和最优变量值，并将其输送到D/A转换模块；
[0008]A/D转换模块用于将接收到的模拟量转换为相应的数字量，并将其输送到中央处理模块；
[0009]D/A转换模块接收到的数字量转换成相应的模拟量，并将其输送到控制器；
[0010]控制器用于控制机械臂各关节的驱动电机。
[0011]该方法实现了机械臂在作业环境中实时避障，提高了一定的安全性。
[0012]本专利技术的另一个目的是提供一种多自由度机械臂实时避障路径规划方法，主要步骤如下：
[0013]步骤A1、人机交互模块将设定的机械臂相关参数，目标状态机械臂各关节转角以及安全距离d
safe
等参数传送到中央处理模块；其中机械臂相关参数包括连杆长度、连杆扭转角、连杆偏距、关节转角。
[0014]步骤A2、数据采集模块对机械臂运行数据进行实时采集，并通过A/D转换模块转换成数字量将其输送到中央处理模块。机械臂运行数据包括机械臂各关节的实时转角以及机械臂与障碍物之间的实时距离d，d是通过机械臂雷达传感器测量所得。
[0015]步骤A3、中央处理模块以当前状态的机械臂各关节转角作为初始状态，调用优化算法、机理模型计算获取机械臂避障的最优路径以及最优目标函数值；
[0016]步骤A4、中央处理模块将得到的机械臂避障的最优路径作为设定值，通过D/A转换模块发送给控制器；
[0017]步骤A5、控制器以最优路径为设定目标，发送控制指令驱动机械臂上各个关节角的电机，使各个关节向目标转角运动。
[0018]步骤A6、在机械臂各关节运动的过程中，中央处理模块判断数据采集模块采集的实时距离d与d
safe
的大小关系。如果d≥d
safe
，则继续运动直至到达目标状态，反之则转至步骤A3。
[0019]作为优选，所述中央处理模块存储有机械臂实时避障的路径规划优化模型，包括目标函数、机械臂运动学模型、约束条件；
[0020]机械臂实时避障的路径规划优化问题形成如下：
[0021]步骤B1、目标函数为：
[0022][0023]其中t＝1,2,...,h，h为规划产生的时间点，b＝1,2,...,N，N为机械臂的关节个数，为t时间点机械臂第b个关节的转角，为t+1时间点机械臂第b个关节的转角。
[0024]步骤B2、机械臂正运动学模型
[0025]为每个连杆设定一个相对坐标系，通过连杆长度、连杆扭转角、连杆偏距、关节转角四个参数表示出坐标系{b
‑
1}与坐标系{b}的齐次变换矩阵
[0026][0027]其中a
b
‑1，α
b
‑1分别为第b
‑
1个连杆的连杆长度、连杆扭转角；d
b
，θ
b
分别为第b个连杆的连杆偏距、关节转角。
[0028]由相邻坐标系的齐次变换矩阵，通过矩阵乘法可以求出每个连杆坐标系相对于基座坐标系的齐次变换矩阵：
[0029][0030]步骤B3、约束条件
[0031]1)初始状态约束
[0032]Θ0＝Θ
int
ꢀꢀ
(4)
[0033]其中表示0时间点机械臂N个关节角的转角，表示初始状态时机械臂N个关节角的转角，T表示转置。
[0034]2)目标状态约束
[0035]Θ
h
＝Θ
tar
ꢀꢀ
(5)
[0036]其中表示h时间点机械臂N个关节角的转角，表示目标状态时机械臂N个关节角的转角。
[0037]3)关节角约束
[0038]Θ
min
≤Θ
t
≤Θ
max
，t＝1,2,...,h
ꢀꢀ
(6)
[0039]其中Θ
min
＝[θ
1min
,θ
2min
,...,θ
Nmin
]T
表示机械臂N个关节角的运动下限，Θ
max
＝[θ
1max
,θ
2max
,...,θ
Nmax
]T
表示机械臂N个关节角的运动上限，表示t时间点机械臂N个关节角的转角。
[0040]4)无碰撞约束
[0041]为了简化机械臂与障碍物之间的距离计算，我们可以采用简单是球体对机械臂进行近似，障碍物的简化与机械臂同理。R表示机械臂用球体近似后的球心，O表示障碍物上的点。因此，机械臂与障碍物之间的距离可以表示为：
[0042][0043]其中为机械臂第b个连杆上的第i个球心，b＝1,2,...,N，N为机械臂的连杆数，其与机械臂的关节个数相等，i＝1,2,...,M，M为机械臂的连杆上所需近似球体的个数，M∈[1,50]；O
j
为障碍物上第j个球心，j＝1,2,...,P，P为障碍物上本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种多自由度机械臂实时避障路径规划方法，其特征在于包括以下步骤：步骤A1、人机交互模块将设定的机械臂相关参数，目标状态机械臂各关节转角以及安全距离d
safe
等参数传送到中央处理模块；其中机械臂相关参数包括连杆长度、连杆扭转角、连杆偏距、关节转角；步骤A2、数据采集模块对机械臂运行数据进行实时采集，并通过A/D转换模块转换成数字量将其输送到中央处理模块；机械臂运行数据包括机械臂各关节的实时转角以及机械臂与障碍物之间的实时距离d；步骤A3、中央处理模块以当前状态的机械臂各关节转角作为初始状态，调用优化算法、机理模型计算获取机械臂避障的最优路径以及最优目标函数值；步骤A4、中央处理模块将得到的机械臂避障的最优路径作为设定值，通过D/A转换模块发送给控制器；步骤A5、控制器以最优路径为设定目标，发送控制指令驱动机械臂上各个关节角的电机，使各个关节向目标转角运动；步骤A6、在机械臂各关节运动的过程中，中央处理模块判断数据采集模块采集的实时距离d与d
safe
的大小关系；如果d≥d
safe
，则继续运动直至到达目标状态，反之则转至步骤A3。2.如权利要求1所述的一种多自由度机械臂实时避障路径规划方法，其特征在于所述中央处理模块存储有机械臂实时避障的路径规划优化模型，其优化问题包括目标函数、机械臂运动学模型、约束条件；步骤B1、目标函数为：其中t＝1,2,...,h，h为规划产生的时间点，b＝1,2,...,N，N为机械臂的关节个数，为t时间点机械臂第b个关节的转角，为t+1时间点机械臂第b个关节的转角；步骤B2、机械臂正运动学模型为每个连杆设定一个相对坐标系，通过连杆长度、连杆扭转角、连杆偏距、关节转角四个参数表示出坐标系{b
‑
1}与坐标系{b}的齐次变换矩阵1}与坐标系{b}的齐次变换矩阵其中a
b
‑1，α
b
‑1分别为第b
‑
1个连杆的连杆长度、连杆扭转角；d
b
，θ
b
分别为第b个连杆的连杆偏距、关节转角；由相邻坐标系的齐次变换矩阵，通过矩阵乘法可以求出每个连杆坐标系相对于基座坐标系的齐次变换矩阵：步骤B3、约束条件1)初始状态约束
Θ0＝Θ
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个近似球心之间的距离。3.如权利要求2所述的一种多自由度机械臂实时避障路径规划方法，其特征在于通过优化算法计算式(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

【专利技术属性】
技术研发人员：林雅媚，江爱朋，王浩坤，吴文浩，赵伟杰，
申请(专利权)人：杭州电子科技大学，
类型：发明
国别省市：