基于球体包络且性能最优目标下的机械臂轨迹规划方法

1.本发明属于基于人工智能的智能制造领域，涉及一种基于球体包络且性能最优目标下的机械臂轨迹规划方法。


背景技术：

2.制造业作为国家经济发展的关键产业与人们的生活息息相关。近年来，由于人口老龄化和劳动力成本的不断增加，并且机器人的制造成本不断下降的原因，导致了利用亚洲廉价劳动力进行加工生产的商业模式正在逐渐消失，与此同时，机器人及智能装备产业的发展也受到了越来越广泛的关注。工业机器人作为一种典型的数字化智能装备，凭借其高精度、可重复性、高效、灵活等特点，在制造业中具有无可替代的地位。随着制造业的自动化与智能化的水平逐渐提高，工业机器人将会面对更加严苛的应用环境，所以对工业机器人的作业效率、能量消耗和运动平稳性等各方面性能的要求变得更加严格，然而在实际工作过程中，机械臂的工作环境非常复杂，外部可能存在各种的障碍物，因此，如何规划出能有效避开静态障碍物的同时还具有高性能的轨迹对机械臂的工作成效具有重要意义。
3.发明人早期发表了cn113043286a一种多自由度机械臂实时避障路径规划方法，提出的技术方案是以机械臂各关节转角变化量之和的最小值为目标函数，将避障作为约束，从而规划出一条实现对静态、动态障碍物避障的最短路径。其侧重的是实现含有避障功能的最短路径的规划。经发明人后续研究发现，机械臂轨迹规划的过程中，随着工业机器人的广泛运用，人们希望机器人的工作效率得以提高的同时还能降低其能耗，从而使得效益最大化。所以提出了以机械臂时间与能耗综合最优为目标的轨迹规划，在规划过程中不仅考虑了避障，还考虑了机械臂的动力学约束，从而规划的轨迹能够在避开静态障碍物的同时还能最大限度地缩短机器人的工作时间，提高工作效率，降低能耗。且目前关于这方面的研究还并未有人提出。


技术实现要素：

4.本发明的目的是针对机械臂在作业环境中碰到静态障碍物的情况，提出的一种基于球体包络性能最优的工业机械臂轨迹规划方法。
5.本发明基于球体包络性能最优的工业机械臂轨迹规划方法，基于以下系统包括人机交互模块、数据采集模块、a/d转换模块、中央处理模块、d/a转换模块、控制器。
6.人机交互模块用于设定机械臂相关参数、目标状态manutec r3机器人腰和肩以及肘关节轴上的关节角度q
tar
、机器人完成作业的终端时间tf、时间网格数n、控制变量的初始值u0、安全距离d
safe
、设定时间t和非线性规划求解的精度ε，并传送到中央处理模块，同时接收中央处理模块计算的优化结果；机械臂相关参数包括连杆长度、连杆扭转角、连杆偏距和关节转角等参数，所述机器人完成作业的终端时间tf凭借工程经验设定；
7.数据采集模块用于采集初始状态的manutec r3机器人腰和肩以及肘关节轴上的关节角度q
initial
、机械臂实时运行的数据和机械臂与障碍物之间的距离等参数，其中机械
臂与障碍物之间的距离是通过机械臂雷达传感器测量所获得的；机械臂运行数据包括机械臂各关节的实时转角。
8.a/d转换模块用于将数据采集模块传输的模拟量数据转换为相应的数字量，并将其输送到中央处理模块；
9.中央处理模块是根据a/d转换模块和人机交互模块传送的数据，来获取最优目标函数值和最优变量值，并将其输送到d/a转换模块，同时反馈给人机交互模块优化结果；其存储有机械臂避障的性能最优轨迹规划模型，包括目标函数和约束条件。
10.d/a转换模块用于将中央处理模块传输的数字量数据转换成相应的模拟量控制指令，并将其输送到控制器；
11.控制器根据d/a转换模块传输的控制指令来控制机械臂各关节相应的驱动电机，通过驱动电机实现各关节转动，使各关节转动到目标角度。
12.本发明一种基于球体包络性能最优的工业机械臂轨迹规划方法主要包括步骤如下：
13.步骤a1、通过人机交互模块设定机械臂相关参数，目标状态manutec r3机器人腰和肩以及肘关节轴上的关节角度q
tar
、机器人完成作业的终端时间tf、时间网格数n、控制变量的初始值u0、安全距离d
safe
、最大优化时间t和非线性规划求解的精度ε，并传送到中央处理模块；机械臂相关参数包括连杆长度、连杆扭转角、连杆偏距和关节转角；
14.步骤a2、数据采集模块采集初始状态的manutec r3机器人腰和肩以及肘等关节轴上的关节角度，机械臂实时运行的数据以及机械臂与障碍物之间的距离，并通过a/d转换模块发送给中央处理模块。其中机械臂与障碍物之间的距离是通过机械臂雷达传感器测量所获得的；机械臂运行数据包括机械臂各关节的实时转角。
15.步骤a3、中央处理模块以当前manutec r3机器人的状态作为初始状态，调用优化算法、述机械臂避障的性能最优轨迹规划模型计算获取机械臂避障的最优路径以及最优目标函数值；
16.步骤a4、中央处理模块将得到的机械臂避障的最优路径作为设定值，通过d/a转换模块发送给控制器，同时反馈给人机交互模块；
17.步骤a5、控制器以最优路径为设定目标，发送控制指令驱动机械臂上各关节相应的驱动电机，使各个关节转动到目标角度；
18.步骤a6、在机械臂各关节运动的过程中，中央处理模块判断数据采集模块采集的机械臂与障碍物之间的距离d与d
safe
的大小关系，d
safe
为机械臂与障碍物之间的安全距离；如果d≥d
safe
，则继续运动直至到达目标状态，反之则转至步骤a3；
19.所述机械臂避障的性能最优轨迹规划模型，包括目标函数和约束条件：
20.步骤b1、根据时间与能耗综合最优的轨迹规划目标确立目标函数j为：
[0021][0022]
其中，β为常数且β∈[0,1]，表示时间和能耗综合优化的权重系数；t表示时间；dt表示时间变量t的微分；tf为终端时间；u1(t)，u2(t)，u3(t)分别为manutec r3机器人腰、肩、肘等关节轴上的与时间相关的扭矩电压；分别是manutec r3机器人腰、肩、肘等关节轴上的与时间相关的关节角速度，单位为rad/s；b＝1,2,3为机械臂的连杆数。
[0023]
步骤b2、约束条件为：
[0024]
1)动力学约束
[0025][0026]
其中，m(q(t))是包含转动惯量的3
×
3正定对称质量矩阵；q(t)＝[q1(t),q2(t),q3(t)]
t
，其中q1(t)，q2(t)，q3(t)分别是manutec r3机器人腰、肩、肘等关节轴上的与时间相关的关节角度，单位为rad；其中分别是manutec r3机器人腰、肩、肘等关节轴上的与时间相关的关节角速度，单位为rad/s；其中分别是manutec r3机器人腰、肩、肘等关节轴上的与时间相关的关节角加速度，单位为rad/s2；u(t)＝[u1(t),u2(t),u3(t)]
t
，其中u1(t)，u2(t)，u3(t)分别为manutec r3机器人腰、肩、肘等关节轴上的与时间相关的扭矩电压；d为3
×
3对角矩阵；是科里奥利和离心力引起的力矩；χg(q(t))是引力造成的力矩。
[0027]
2)初始状态约束
[0028][0029]
其中，q0表示初始状态manutec r3机器人腰、肩、肘等关节轴上的关节角度；q
initial
初始状态manutec r3机器人腰、肩、肘等关节轴上的关节角度；表示初始状态manutec r3机器人腰、肩、肘等关节轴上的关节角的角速度。
[0030]
3)目标状态约束
[0031][0032]
其中，qf表示终端时刻manutec r3机器人腰、肩、肘等关节轴上的关节角度；q
tar
表示目标状态manutec r3机器人腰、肩、肘等关节轴上的关节角度；表示终端时间manutec r3机器人腰、肩、肘等关节轴上的关节角的角速度。
[0033]
4)关节角约束
[0034]qmin
≤q(t)≤q
max
,t＝1,2,...,tfꢀꢀꢀ
(5)
[0035]
其中，q
min
表示manutec r3机器人腰、肩、肘等关节轴上关节角度的运动下限，q
max
表示manutec r3机器人腰、肩、肘等关节轴上关节角度的运动上限。
[0036]
5)关节角速度约束
[0037][0038]
其中，表示manutec r3机器人腰、肩、肘等关节轴上关节角速度的下限，表示manutec r3机器人腰、肩、肘等关节轴上关节角速度的上限。
[0039]
6)电压约束
[0040]
|u(t)|≤u
max
ꢀꢀꢀ
(7)
[0041]
其中，u
max
为manutec r3机器人腰、肩、肘等关节轴上的扭矩电压的最大值；
[0042]
7)无碰撞约束
[0043]
为了简化机械臂与障碍物之间的距离计算，采用多个球体对机械臂进行包络，障
碍物也用球体进行包络。r表示机械臂用球体包络后的球心，o表示障碍物用球体包络后的球心。因此，机械臂与障碍物之间的距离表示为：
[0044][0045]
其中，为机械臂第b个连杆上的第i个包络球的球心；i＝1
…
p，其中p为机械臂的连杆上包络球体的个数；oj为障碍物上第j个包络球的球心；j＝1
…
q，其中q为障碍物上包络球体的个数。
[0046]
因此，无碰撞约束表示为：
[0047][0048]
其中，d
safe
为机械臂与障碍物之间的安全距离。
[0049]
所述的中央处理模块调用优化算法计算上述的机械臂避障的性能最优轨迹规划问题(1)-(9)，具体步骤如下：
[0050]
步骤c1、中央处理模块接收人机交互模块输入的目标状态manutec r3机器人腰关节和肩关节以及肘关节的关节角度q
tar
、机器人完成作业的终端时间tf、时间网格数n、控制变量的初始值u0、安全距离d
safe
和非线性规划求解的精度ε，数据采集模块获得的初始状态manutec r3机器人腰关节、肩关节、肘关节的关节角度以及机械臂与障碍物之间的距离。
[0051]
步骤c2、以机器人的基座为参考物，建立相对坐标系。根据从数据采集模块获得机械臂与障碍物之间的距离确立障碍物的包络球球心坐标
[0052]
步骤c3、根据终端时间、时间网格数，采用均匀离散化的方式，计算出起始时间网格分布。
[0053]
步骤c4、通过非线性规划算法求解该最优轨迹规划问题，得到当前时间网格下的最优的控制参数和目标函数值。
[0054]
所述的非线性规划算法具体步骤如下：
[0055]
步骤d1、将式(1)-(9)组成的优化问题转化成如下式(10)的非线性优化问题：
[0056][0057]
其中，x为n维实数变量，f(x)和c(x)分别表示连续可微的目标函数和m维约束方程，x
l
和xu分别表示变量的上下界约束。
[0058]
步骤d2、根据划分的离散网格，通过ode求解微分方程初值问题，获取离散后各网格点变量值，以此作为非线性规划问题的初值；
[0059]
步骤d3、采用迭代计算方法求解上式(10)所表示的优化问题，在x第k次迭代(k为大于等于零的整数)的迭代xk处..qp子问题表示为以下形式：
[0060]
[0061]
其中，pk为搜索方向，gk和分别表示在xk处目标函数的导数和约束方程的雅克比矩阵，ck表示在xk处c(xk)的值，wk为拉格朗日函数的hessian阵。其中拉格朗日函数为：
[0062]
l(x,λ,v,π)＝f(x) λ
t
c(x) v
t
(x-xu)-π
t
(x-x
l
)
ꢀꢀꢀ
(12)
[0063]
其中，λ，v和π分别表示与等式约束、上边界约束和下边界约束相关的拉格朗日乘子，λ
t
，v
t
和π
t
分别表示相应拉格朗日乘子的转置。
[0064]
步骤d4、更新下一次迭代下的变量迭代点：
[0065]
x
k 1
＝xk αpkꢀꢀꢀ
(13)
[0066]
其中α∈(0,1]，通过一维搜索方法求取。
[0067]
步骤d5、根据最优性条件和给定的设定误差ε进行收敛性判断。如果达到最优条件，则得到x
k 1
和对应的目标函数值，否则，判断计算时间是否大于最大优化时间t，如果大于最大优化时间t，则终止计算，并返回求解失败信息给人机交互模块。如果小于最大优化时间t，则令k＝k 1,xk＝x
k 1
,转步骤d3。
[0068]
本发明的有益效果是：
[0069]
本发明提出考虑时间与能耗综合优化的工业机械臂轨迹规划目标函数，将其运动学方程与避障作为约束进行优化计算，规划的轨迹能够在避开静态障碍物的同时还能最大限度地缩短机器人的工作时间，提高工作效率，降低能耗。该方法实现了机械臂在作业环境中高性能的静态避障，提高效率和减小生产成本的同时也保证了安全性。
附图说明
[0070]
图1为系统整体结构示意图；
[0071]
图2为机械臂避障模型简化示意图。
具体实施方式
[0072]
下面结合附图和具体实施例对本发明作进一步的分析。
[0073]
如图1所示，系统整体结构示意图，具体过程为：
[0074]
步骤a1、通过人机交互模块设定机械臂相关参数，目标状态manutec r3机器人腰和肩以及肘关节轴上的关节角度q
tar
、机器人完成作业的终端时间tf、时间网格数n、控制变量的初始值u0、安全距离d
safe
、最大优化时间t和非线性规划求解的精度ε，并传送到中央处理模块；机械臂相关参数包括连杆长度、连杆扭转角、连杆偏距和关节转角；
[0075]
步骤a2、数据采集模块采集初始状态的manutec r3机器人腰和肩以及肘等关节轴上的关节角度，机械臂实时运行的数据以及机械臂与障碍物之间的距离，并通过a/d转换模块发送给中央处理模块。其中机械臂与障碍物之间的距离是通过机械臂雷达传感器测量所获得的；机械臂运行数据包括机械臂各关节的实时转角。
[0076]
步骤a3、中央处理模块以当前manutec r3机器人的状态作为初始状态，调用优化算法、述机械臂避障的性能最优轨迹规划模型计算获取机械臂避障的最优路径以及最优目标函数值；
[0077]
步骤a4、中央处理模块将得到的机械臂避障的最优路径作为设定值，通过d/a转换模块发送给控制器，同时反馈给人机交互模块；
[0078]
步骤a5、控制器以最优路径为设定目标，发送控制指令驱动机械臂上各关节相应
的驱动电机，使各个关节转动到目标角度；
[0079]
步骤a6、在机械臂各关节运动的过程中，中央处理模块判断数据采集模块采集的机械臂与障碍物之间的距离d与d
safe
的大小关系，d
safe
为机械臂与障碍物之间的安全距离；如果d≥d
safe
，则继续运动直至到达目标状态，反之则转至步骤a3；
[0080]
所述机械臂避障的性能最优轨迹规划模型，包括目标函数和约束条件：
[0081]
步骤b1、根据时间与能耗综合最优的轨迹规划目标确立目标函数j为：
[0082][0083]
其中，β为常数且β∈[0,1]，表示时间和能耗综合优化的权重系数；t表示时间；dt表示时间变量t的微分；tf为终端时间；u1(t)，u2(t)，u3(t)分别为manutec r3机器人腰、肩、肘等关节轴上的与时间相关的扭矩电压；分别是manutec r3机器人腰、肩、肘等关节轴上的与时间相关的关节角速度，单位为rad/s；b＝1,2,3为机械臂的连杆数。
[0084]
步骤b2、约束条件为：
[0085]
1)动力学约束
[0086][0087]
其中，m(q(t))是包含转动惯量的3
×
3正定对称质量矩阵；q(t)＝[q1(t),q2(t),q3(t)]
t
，其中q1(t)，q2(t)，q3(t)分别是manutec r3机器人腰、肩、肘等关节轴上的与时间相关的关节角度，单位为rad；其中分别是manutec r3机器人腰、肩、肘等关节轴上的与时间相关的关节角速度，单位为rad/s；其中分别是manutec r3机器人腰、肩、肘等关节轴上的与时间相关的关节角加速度，单位为rad/s2；u(t)＝[u1(t),u2(t),u3(t)]
t
，其中u1(t)，u2(t)，u3(t)分别为manutec r3机器人腰、肩、肘等关节轴上的与时间相关的扭矩电压；d为3
×
3对角矩阵；是科里奥利和离心力引起的力矩；χg(q(t))是引力造成的力矩。。
[0088]
2)初始状态约束
[0089][0090]
其中，q0表示初始状态manutec r3机器人腰、肩、肘等关节轴上的关节角度；q
initial
初始状态manutec r3机器人腰、肩、肘等关节轴上的关节角度；表示初始状态manutec r3机器人腰、肩、肘等关节轴上的关节角的角速度。
[0091][0092]
3)目标状态约束
[0093][0094]
其中，qf表示终端时刻manutec r3机器人腰、肩、肘等关节轴上的关节角度；q
tar
表示目标状态manutec r3机器人腰、肩、肘等关节轴上的关节角度；表示终端时间manutec r3机器人腰、肩、肘等关节轴上的关节角的角速度。
[0095]
4)关节角约束
[0096]qmin
≤q(t)≤q
max
,t＝1,2,...,tfꢀꢀꢀ
(5)
[0097]
其中，q
min
表示manutec r3机器人腰、肩、肘等关节轴上关节角度的运动下限，q
max
表示manutec r3机器人腰、肩、肘等关节轴上关节角度的运动上限。
[0098]
5)关节角速度约束
[0099][0100]
其中，表示manutec r3机器人腰、肩、肘等关节轴上关节角速度的下限，表示manutec r3机器人腰、肩、肘等关节轴上关节角速度的上限。
[0101]
6)电压约束
[0102]
|u(t)|≤u
max
ꢀꢀꢀ
(7)
[0103]
其中，u
max
为manutec r3机器人腰、肩、肘等关节轴上的扭矩电压的最大值；
[0104]
7)无碰撞约束
[0105]
为了简化机械臂与障碍物之间的距离计算，我们采用多个球体对机械臂进行包络，障碍物也用球体进行包络。r表示机械臂用球体包络后的球心，o表示障碍物用球体包络后的球心。因此，机械臂与障碍物之间的距离可以表示为：
[0106][0107]
其中，为机械臂第b个连杆上的第i个包络球的球心；i＝1
…
p，其中p为机械臂的连杆上包络球体的个数；oj为障碍物上第j个包络球的球心；j＝1
…
q，其中q为障碍物上包络球体的个数。
[0108]
因此，无碰撞约束可表示为：
[0109][0110]
其中，d
safe
为机械臂与障碍物之间的安全距离。
[0111]
所述的中央处理模块调用优化算法计算上述的机械臂避障的性能最优轨迹规划问题(1)-(9)，具体步骤如下：
[0112]
步骤c1、中央处理模块接收人机交互模块输入的目标状态manutec r3机器人腰关节和肩关节以及肘关节的关节角度q
tar
、机器人完成作业的终端时间tf、时间网格数n、控制变量的初始值u0、安全距离d
safe
和非线性规划求解的精度ε，数据采集模块获得的初始状态manutec r3机器人腰关节、肩关节、肘关节的关节角度以及机械臂与障碍物之间的距离。
[0113]
步骤c2、以机器人的基座为参考物，建立相对坐标系。根据从数据采集模块获得机械臂与障碍物之间的距离确立障碍物的包络球球心坐标
[0114]
步骤c3、根据终端时间、时间网格数，采用均匀离散化的方式，计算出起始时间网格分布。
[0115]
步骤c4、通过非线性规划算法求解该最优轨迹规划问题，得到当前时间网格下的最优的控制参数和目标函数值。
[0116]
所述的非线性规划算法具体步骤如下：
[0117]
步骤d1、将式(1)-(9)组成的优化问题转化成如下式(10)的非线性优化问题：
[0118][0119]
其中，x为n维实数变量，f(x)和c(x)分别表示连续可微的目标函数和m维约束方程，x
l
和xu分别表示变量的上下界约束。
[0120]
步骤d2、根据划分的离散网格，通过ode求解微分方程初值问题，获取离散后各网格点变量值，以此作为非线性规划问题的初值；
[0121]
步骤d3、采用迭代计算方法求解上式(10)所表示的优化问题，在在x第k次迭代(k为大于等于零的整数)的迭代xk处qp子问题表示为以下形式：
[0122][0123]
其中，pk为搜索方向，gk和分别表示在xk处目标函数的导数和约束方程的雅克比矩阵，ck表示在xk处c(xk)的值，wk为拉格朗日函数的hessian阵。其中拉格朗日函数为：
[0124]
l(x,λ,v,π)＝f(x) λ
t
c(x) v
t
(x-xu)-π
t
(x-x
l
)
ꢀꢀꢀ
(12)
[0125]
其中，λ，v和π分别表示与等式约束、上边界约束和下边界约束相关的拉格朗日乘子，λ
t
，v
t
和π
t
分别表示相应拉格朗日乘子的转置。
[0126]
步骤d4、更新下一次迭代下的变量迭代点：
[0127]
x
k 1
＝xk αpkꢀꢀꢀ
(13)
[0128]
其中α∈(0,1]，通过一维搜索方法求取。
[0129]
步骤d5、根据最优性条件和给定的设定误差ε进行收敛性判断。如果达到最优条件，则得到x
k 1
和对应的目标函数值，否则，判断计算时间是否大于最大优化时间t，如果大于最大优化时间t，则终止计算，并返回求解失败信息给人机交互模块。如果小于最大优化时间t，则令k＝k 1,xk＝x
k 1
,转步骤d3。
[0130]
如图2所示，机械臂避障模型简化示意图，具体过程为：
[0131]
为了简化机械臂与障碍物之间的距离计算，采用简单的球体对机械臂进行近似。在机械臂第b个连杆的中心线上选取一组关键点i＝1,2,...,m，m为机械臂的连杆上所需近似包围球的个数，m∈[1,50]，并定义一个半径使以点为球心，为半径的一系列球体组成的集合能够完全包围机械臂的本体。
[0132]
以上内容是结合具体的优选实施方式对本发明所做的进一步详细说明，不能认定本发明的具体实施只限于这些说明。对于本发明所属技术领域的技术人员来说，在不脱离发明构思的前提下还可以做出一定程度的简单推演或者替换，都应当视为属于本发明的保护范围。