CN114559435B - 基于球体包络且性能最优目标下的机械臂轨迹规划方法 - Google Patents
基于球体包络且性能最优目标下的机械臂轨迹规划方法 Download PDFInfo
- Publication number
- CN114559435B CN114559435B CN202210292083.0A CN202210292083A CN114559435B CN 114559435 B CN114559435 B CN 114559435B CN 202210292083 A CN202210292083 A CN 202210292083A CN 114559435 B CN114559435 B CN 114559435B
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- robot
- joint
- time
- waist
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 31
- 238000005265 energy consumption Methods 0.000 claims abstract description 14
- 230000008569 process Effects 0.000 claims abstract description 9
- 238000005457 optimization Methods 0.000 claims description 36
- 238000012545 processing Methods 0.000 claims description 32
- 210000002310 elbow joint Anatomy 0.000 claims description 29
- 210000000323 shoulder joint Anatomy 0.000 claims description 26
- 210000001624 hip Anatomy 0.000 claims description 20
- 230000003993 interaction Effects 0.000 claims description 19
- 238000006243 chemical reaction Methods 0.000 claims description 18
- 210000001503 joint Anatomy 0.000 claims description 16
- 238000004364 calculation method Methods 0.000 claims description 15
- 239000011159 matrix material Substances 0.000 claims description 9
- 230000036962 time dependent Effects 0.000 claims description 7
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 6
- 230000001133 acceleration Effects 0.000 claims description 3
- 230000004069 differentiation Effects 0.000 claims description 3
- 238000009826 distribution Methods 0.000 claims description 3
- 230000017105 transposition Effects 0.000 claims 1
- 230000003068 static effect Effects 0.000 abstract description 10
- 238000004519 manufacturing process Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 3
- 230000004888 barrier function Effects 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000003245 working effect Effects 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P80/00—Climate change mitigation technologies for sector-wide applications
- Y02P80/10—Efficient use of energy, e.g. using compressed air or pressurized fluid as energy carrier
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
- Numerical Control (AREA)
Abstract
本发明公开基于球体包络且性能最优目标下的机械臂轨迹规划方法。针对机械臂在作业环境中碰到静态障碍物的情况，提出以机械臂时间与能耗综合最优为目标的轨迹规划，在规划过程中不仅考虑了避障，还考虑了机械臂的动力学约束，从而规划的轨迹能够在避开静态障碍物的同时还能最大限度地缩短机器人的工作时间，提高工作效率，降低能耗。该方法实现了机械臂在作业环境中高性能的静态避障，提高效率的同时也保证了安全性。在避开静态障碍物的同时还能最大限度地缩短机器人的工作时间，提高工作效率。
Description
技术领域
本发明属于基于人工智能的智能制造领域，涉及一种基于球体包络且性能最优目标下的机械臂轨迹规划方法。
背景技术
制造业作为国家经济发展的关键产业与人们的生活息息相关。工业机器人作为一种典型的数字化智能装备，凭借其高精度、可重复性、高效、灵活等特点，在制造业中具有无可替代的地位。随着制造业的自动化与智能化的水平逐渐提高，工业机器人将会面对更加严苛的应用环境，所以对工业机器人的作业效率、能量消耗和运动平稳性等各方面性能的要求变得更加严格，然而在实际工作过程中，机械臂的工作环境非常复杂，外部可能存在各种的障碍物，因此，如何规划出能有效避开静态障碍物的同时还具有高性能的轨迹对机械臂的工作成效具有重要意义。
发明人早期发表了CN113043286A一种多自由度机械臂实时避障路径规划方法，提出的技术方案是以机械臂各关节转角变化量之和的最小值为目标函数，将避障作为约束，从而规划出一条实现对静态、动态障碍物避障的最短路径。其侧重的是实现含有避障功能的最短路径的规划。经发明人后续研究发现，机械臂轨迹规划的过程中，随着工业机器人的广泛运用，人们希望机器人的工作效率得以提高的同时还能降低其能耗，从而使得效益最大化。所以提出了以机械臂时间与能耗综合最优为目标的轨迹规划，在规划过程中不仅考虑了避障，还考虑了机械臂的动力学约束，从而规划的轨迹能够在避开静态障碍物的同时还能最大限度地缩短机器人的工作时间，提高工作效率，降低能耗。且目前关于这方面的研究还并未有人提出。
发明内容
本发明的目的是针对机械臂在作业环境中碰到静态障碍物的情况，提出的一种基于球体包络性能最优的工业机械臂轨迹规划方法。
本发明基于球体包络性能最优的工业机械臂轨迹规划方法，基于以下系统包括人机交互模块、数据采集模块、A/D转换模块、中央处理模块、D/A转换模块、控制器。
人机交互模块用于设定机械臂相关参数、目标状态Manutec R3机器人腰和肩以及肘关节轴上的关节角度qtar、机器人完成作业的终端时间tf、时间网格数N、控制变量的初始值u0、安全距离dsafe、设定时间T和非线性规划求解的精度ε，并传送到中央处理模块，同时接收中央处理模块计算的优化结果；机械臂相关参数包括连杆长度、连杆扭转角、连杆偏距和关节转角等参数，所述机器人完成作业的终端时间tf凭借工程经验设定；
数据采集模块用于采集初始状态的Manutec R3机器人腰和肩以及肘关节轴上的关节角度qinitial、机械臂实时运行的数据和机械臂与障碍物之间的距离等参数，其中机械臂与障碍物之间的距离是通过机械臂雷达传感器测量所获得的；机械臂运行数据包括机械臂各关节的实时转角。
A/D转换模块用于将数据采集模块传输的模拟量数据转换为相应的数字量，并将其输送到中央处理模块；
中央处理模块是根据A/D转换模块和人机交互模块传送的数据，来获取最优目标函数值和最优变量值，并将其输送到D/A转换模块，同时反馈给人机交互模块优化结果；其存储有机械臂避障的性能最优轨迹规划模型，包括目标函数和约束条件。
D/A转换模块用于将中央处理模块传输的数字量数据转换成相应的模拟量控制指令，并将其输送到控制器；
控制器根据D/A转换模块传输的控制指令来控制机械臂各关节相应的驱动电机，通过驱动电机实现各关节转动，使各关节转动到目标角度。
本发明一种基于球体包络性能最优的工业机械臂轨迹规划方法主要包括步骤如下：
步骤A1、通过人机交互模块设定机械臂相关参数，目标状态Manutec R3机器人腰和肩以及肘关节轴上的关节角度qtar、机器人完成作业的终端时间tf、时间网格数N、控制变量的初始值u0、安全距离dsafe、最大优化时间T和非线性规划求解的精度ε，并传送到中央处理模块；机械臂相关参数包括连杆长度、连杆扭转角、连杆偏距和关节转角；
步骤A2、数据采集模块采集初始状态的Manutec R3机器人腰和肩以及肘等关节轴上的关节角度，机械臂实时运行的数据以及机械臂与障碍物之间的距离，并通过A/D转换模块发送给中央处理模块。其中机械臂与障碍物之间的距离是通过机械臂雷达传感器测量所获得的；机械臂运行数据包括机械臂各关节的实时转角。
步骤A3、中央处理模块以当前Manutec R3机器人的状态作为初始状态，调用优化算法、述机械臂避障的性能最优轨迹规划模型计算获取机械臂避障的最优路径以及最优目标函数值；
步骤A4、中央处理模块将得到的机械臂避障的最优路径作为设定值，通过D/A转换模块发送给控制器，同时反馈给人机交互模块；
步骤A5、控制器以最优路径为设定目标，发送控制指令驱动机械臂上各关节相应的驱动电机，使各个关节转动到目标角度；
步骤A6、在机械臂各关节运动的过程中，中央处理模块判断数据采集模块采集的机械臂与障碍物之间的距离d与dsafe的大小关系，dsafe为机械臂与障碍物之间的安全距离；如果d≥dsafe，则继续运动直至到达目标状态，反之则转至步骤A3；
所述机械臂避障的性能最优轨迹规划模型，包括目标函数和约束条件：
步骤B1、根据时间与能耗综合最优的轨迹规划目标确立目标函数J为：
其中，β为常数且β∈[0,1]，表示时间和能耗综合优化的权重系数；t表示时间；dt表示时间变量t的微分；tf为终端时间；u1(t)，u2(t)，u3(t)分别为Manutec R3机器人腰、肩、肘等关节轴上的与时间相关的扭矩电压；分别是Manutec R3机器人腰、肩、肘等关节轴上的与时间相关的关节角速度，单位为rad/s；b＝1,2,3为机械臂的连杆数。
步骤B2、约束条件为：
1)动力学约束
其中，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))是引力造成的力矩。
2)初始状态约束
其中，q0表示初始状态Manutec R3机器人腰、肩、肘等关节轴上的关节角度；qinitial初始状态Manutec R3机器人腰、肩、肘等关节轴上的关节角度；表示初始状态Manutec R3机器人腰、肩、肘等关节轴上的关节角的角速度。
3)目标状态约束
其中，qf表示终端时刻Manutec R3机器人腰、肩、肘等关节轴上的关节角度；qtar表示目标状态Manutec R3机器人腰、肩、肘等关节轴上的关节角度；表示终端时间ManutecR3机器人腰、肩、肘等关节轴上的关节角的角速度。
4)关节角约束
qmin≤q(t)≤qmax,t＝1,2,...,tf (5)
其中，qmin表示Manutec R3机器人腰、肩、肘等关节轴上关节角度的运动下限，qmax表示Manutec R3机器人腰、肩、肘等关节轴上关节角度的运动上限。
5)关节角速度约束
其中，表示Manutec R3机器人腰、肩、肘等关节轴上关节角速度的下限，/>表示Manutec R3机器人腰、肩、肘等关节轴上关节角速度的上限。
6)电压约束
|u(t)|≤umax (7)
其中，umax为Manutec R3机器人腰、肩、肘等关节轴上的扭矩电压的最大值；
7)无碰撞约束
为了简化机械臂与障碍物之间的距离计算，采用多个球体对机械臂进行包络，障碍物也用球体进行包络。R表示机械臂用球体包络后的球心，O表示障碍物用球体包络后的球心。因此，机械臂与障碍物之间的距离表示为：
其中，为机械臂第b个连杆上的第i个包络球的球心；i＝1…P，其中P为机械臂的连杆上包络球体的个数；Oj为障碍物上第j个包络球的球心；j＝1…Q，其中Q为障碍物上包络球体的个数。
因此，无碰撞约束表示为：
其中，dsafe为机械臂与障碍物之间的安全距离。
所述的中央处理模块调用优化算法计算上述的机械臂避障的性能最优轨迹规划问题(1)-(9)，具体步骤如下：
步骤C1、中央处理模块接收人机交互模块输入的目标状态Manutec R3机器人腰关节和肩关节以及肘关节的关节角度qtar、机器人完成作业的终端时间tf、时间网格数N、控制变量的初始值u0、安全距离dsafe和非线性规划求解的精度ε，数据采集模块获得的初始状态Manutec R3机器人腰关节、肩关节、肘关节的关节角度以及机械臂与障碍物之间的距离。
步骤C2、以机器人的基座为参考物，建立相对坐标系。根据从数据采集模块获得机械臂与障碍物之间的距离确立障碍物的包络球球心坐标
步骤C3、根据终端时间、时间网格数，采用均匀离散化的方式，计算出起始时间网格分布。
步骤C4、通过非线性规划算法求解该最优轨迹规划问题，得到当前时间网格下的最优的控制参数和目标函数值。
所述的非线性规划算法具体步骤如下：
步骤D1、将式(1)-(9)组成的优化问题转化成如下式(10)的非线性优化问题：
其中，x为n维实数变量，f(x)和c(x)分别表示连续可微的目标函数和m维约束方程，xL和xU分别表示变量的上下界约束。
步骤D2、根据划分的离散网格，通过ODE求解微分方程初值问题，获取离散后各网格点变量值，以此作为非线性规划问题的初值；
步骤D3、采用迭代计算方法求解上式(10)所表示的优化问题，在x第k次迭代(k为大于等于零的整数)的迭代xk处..QP子问题表示为以下形式：
其中，pk为搜索方向，gk和分别表示在xk处目标函数的导数和约束方程的雅克比矩阵，ck表示在xk处c(xk)的值，Wk为拉格朗日函数的Hessian阵。其中拉格朗日函数为：
L(x,λ,v,π)＝f(x)+λTc(x)+vT(x-xU)-πT(x-xL) (12)
其中，λ，v和π分别表示与等式约束、上边界约束和下边界约束相关的拉格朗日乘子，λT，vT和πT分别表示相应拉格朗日乘子的转置。
步骤D4、更新下一次迭代下的变量迭代点：
xk+1＝xk+αpk (13)
其中α∈(0,1]，通过一维搜索方法求取。
步骤D5、根据最优性条件和给定的设定误差ε进行收敛性判断。如果达到最优条件，则得到xk+1和对应的目标函数值，否则，判断计算时间是否大于最大优化时间T，如果大于最大优化时间T，则终止计算，并返回求解失败信息给人机交互模块。如果小于最大优化时间T，则令k＝k+1,xk＝xk+1,转步骤D3。
本发明的有益效果是：
本发明提出考虑时间与能耗综合优化的工业机械臂轨迹规划目标函数，将其运动学方程与避障作为约束进行优化计算，规划的轨迹能够在避开静态障碍物的同时还能最大限度地缩短机器人的工作时间，提高工作效率，降低能耗。该方法实现了机械臂在作业环境中高性能的静态避障，提高效率和减小生产成本的同时也保证了安全性。
附图说明
图1为系统整体结构示意图；
图2为机械臂避障模型简化示意图。
具体实施方式
下面结合附图和具体实施例对本发明作进一步的分析。
如图1所示，系统整体结构示意图，具体过程为：
步骤A1、通过人机交互模块设定机械臂相关参数，目标状态Manutec R3机器人腰和肩以及肘关节轴上的关节角度qtar、机器人完成作业的终端时间tf、时间网格数N、控制变量的初始值u0、安全距离dsafe、最大优化时间T和非线性规划求解的精度ε，并传送到中央处理模块；机械臂相关参数包括连杆长度、连杆扭转角、连杆偏距和关节转角；
步骤A2、数据采集模块采集初始状态的Manutec R3机器人腰和肩以及肘等关节轴上的关节角度，机械臂实时运行的数据以及机械臂与障碍物之间的距离，并通过A/D转换模块发送给中央处理模块。其中机械臂与障碍物之间的距离是通过机械臂雷达传感器测量所获得的；机械臂运行数据包括机械臂各关节的实时转角。
步骤A3、中央处理模块以当前Manutec R3机器人的状态作为初始状态，调用优化算法、述机械臂避障的性能最优轨迹规划模型计算获取机械臂避障的最优路径以及最优目标函数值；
步骤A4、中央处理模块将得到的机械臂避障的最优路径作为设定值，通过D/A转换模块发送给控制器，同时反馈给人机交互模块；
步骤A5、控制器以最优路径为设定目标，发送控制指令驱动机械臂上各关节相应的驱动电机，使各个关节转动到目标角度；
步骤A6、在机械臂各关节运动的过程中，中央处理模块判断数据采集模块采集的机械臂与障碍物之间的距离d与dsafe的大小关系，dsafe为机械臂与障碍物之间的安全距离；如果d≥dsafe，则继续运动直至到达目标状态，反之则转至步骤A3；
所述机械臂避障的性能最优轨迹规划模型，包括目标函数和约束条件：
步骤B1、根据时间与能耗综合最优的轨迹规划目标确立目标函数J为：
其中，β为常数且β∈[0,1]，表示时间和能耗综合优化的权重系数；t表示时间；dt表示时间变量t的微分；tf为终端时间；u1(t)，u2(t)，u3(t)分别为Manutec R3机器人腰、肩、肘等关节轴上的与时间相关的扭矩电压；分别是Manutec R3机器人腰、肩、肘等关节轴上的与时间相关的关节角速度，单位为rad/s；b＝1,2,3为机械臂的连杆数。
步骤B2、约束条件为：
1)动力学约束
其中，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))是引力造成的力矩。。
2)初始状态约束
其中，q0表示初始状态Manutec R3机器人腰、肩、肘等关节轴上的关节角度；qinitial初始状态Manutec R3机器人腰、肩、肘等关节轴上的关节角度；表示初始状态Manutec R3机器人腰、肩、肘等关节轴上的关节角的角速度。
3)目标状态约束
其中，qf表示终端时刻Manutec R3机器人腰、肩、肘等关节轴上的关节角度；qtar表示目标状态Manutec R3机器人腰、肩、肘等关节轴上的关节角度；表示终端时间ManutecR3机器人腰、肩、肘等关节轴上的关节角的角速度。
4)关节角约束
qmin≤q(t)≤qmax,t＝1,2,...,tf (5)
其中，qmin表示Manutec R3机器人腰、肩、肘等关节轴上关节角度的运动下限，qmax表示Manutec R3机器人腰、肩、肘等关节轴上关节角度的运动上限。
5)关节角速度约束
其中，表示Manutec R3机器人腰、肩、肘等关节轴上关节角速度的下限，/>表示Manutec R3机器人腰、肩、肘等关节轴上关节角速度的上限。
6)电压约束
|u(t)|≤umax (7)
其中，umax为Manutec R3机器人腰、肩、肘等关节轴上的扭矩电压的最大值；
7)无碰撞约束
为了简化机械臂与障碍物之间的距离计算，我们采用多个球体对机械臂进行包络，障碍物也用球体进行包络。R表示机械臂用球体包络后的球心，O表示障碍物用球体包络后的球心。因此，机械臂与障碍物之间的距离可以表示为：
其中，为机械臂第b个连杆上的第i个包络球的球心；i＝1…P，其中P为机械臂的连杆上包络球体的个数；Oj为障碍物上第j个包络球的球心；j＝1…Q，其中Q为障碍物上包络球体的个数。
因此，无碰撞约束可表示为：
其中，dsafe为机械臂与障碍物之间的安全距离。
所述的中央处理模块调用优化算法计算上述的机械臂避障的性能最优轨迹规划问题(1)-(9)，具体步骤如下：
步骤C1、中央处理模块接收人机交互模块输入的目标状态Manutec R3机器人腰关节和肩关节以及肘关节的关节角度qtar、机器人完成作业的终端时间tf、时间网格数N、控制变量的初始值u0、安全距离dsafe和非线性规划求解的精度ε，数据采集模块获得的初始状态Manutec R3机器人腰关节、肩关节、肘关节的关节角度以及机械臂与障碍物之间的距离。
步骤C2、以机器人的基座为参考物，建立相对坐标系。根据从数据采集模块获得机械臂与障碍物之间的距离确立障碍物的包络球球心坐标
步骤C3、根据终端时间、时间网格数，采用均匀离散化的方式，计算出起始时间网格分布。
步骤C4、通过非线性规划算法求解该最优轨迹规划问题，得到当前时间网格下的最优的控制参数和目标函数值。
所述的非线性规划算法具体步骤如下：
步骤D1、将式(1)-(9)组成的优化问题转化成如下式(10)的非线性优化问题：
其中，x为n维实数变量，f(x)和c(x)分别表示连续可微的目标函数和m维约束方程，xL和xU分别表示变量的上下界约束。
步骤D2、根据划分的离散网格，通过ODE求解微分方程初值问题，获取离散后各网格点变量值，以此作为非线性规划问题的初值；
步骤D3、采用迭代计算方法求解上式(10)所表示的优化问题，在在x第k次迭代(k为大于等于零的整数)的迭代xk处QP子问题表示为以下形式：
其中，pk为搜索方向，gk和分别表示在xk处目标函数的导数和约束方程的雅克比矩阵，ck表示在xk处c(xk)的值，Wk为拉格朗日函数的Hessian阵。其中拉格朗日函数为：
L(x,λ,v,π)＝f(x)+λTc(x)+vT(x-xU)-πT(x-xL) (12)
其中，λ，v和π分别表示与等式约束、上边界约束和下边界约束相关的拉格朗日乘子，λT，vT和πT分别表示相应拉格朗日乘子的转置。
步骤D4、更新下一次迭代下的变量迭代点：
xk+1＝xk+αpk (13)
其中α∈(0,1]，通过一维搜索方法求取。
步骤D5、根据最优性条件和给定的设定误差ε进行收敛性判断。如果达到最优条件，则得到xk+1和对应的目标函数值，否则，判断计算时间是否大于最大优化时间T，如果大于最大优化时间T，则终止计算，并返回求解失败信息给人机交互模块。如果小于最大优化时间T，则令k＝k+1,xk＝xk+1,转步骤D3。
如图2所示，机械臂避障模型简化示意图，具体过程为：
为了简化机械臂与障碍物之间的距离计算，采用简单的球体对机械臂进行近似。在机械臂第b个连杆的中心线上选取一组关键点M为机械臂的连杆上所需近似包围球的个数，M∈[1,50]，并定义一个半径/>使以点/>为球心，/>为半径的一系列球体组成的集合/>能够完全包围机械臂的本体。
以上内容是结合具体的优选实施方式对本发明所做的进一步详细说明，不能认定本发明的具体实施只限于这些说明。对于本发明所属技术领域的技术人员来说，在不脱离发明构思的前提下还可以做出一定程度的简单推演或者替换，都应当视为属于本发明的保护范围。
Claims (4)
1.基于球体包络性能最优的工业机械臂轨迹规划方法，包括如下步骤：
步骤A1、通过人机交互模块设定机械臂相关参数，目标状态ManutecR3机器人腰和肩以及肘关节轴上的关节角度qtar、机器人完成作业的终端时间tf、时间网格数N、控制变量的初始值u0、机械臂与障碍物之间的安全距离dsafe、最大优化时间T和非线性规划求解的精度ε，并传送到中央处理模块；机械臂相关参数包括连杆长度、连杆扭转角、连杆偏距和关节转角；
步骤A2、数据采集模块采集初始状态的ManutecR3机器人腰和肩以及肘关节轴上的关节角度，机械臂实时运行数据以及机械臂与障碍物之间的距离，并通过A/D转换模块发送给中央处理模块；其中机械臂实时运行数据包括机械臂各关节的实时转角；
步骤A3、中央处理模块以当前ManutecR3机器人的状态作为初始状态，调用优化算法、机械臂避障的性能最优轨迹规划模型计算获取机械臂避障的最优路径以及最优目标函数值；
步骤A4、中央处理模块将得到的机械臂避障的最优路径作为设定值，通过D/A转换模块发送给控制器，同时反馈给人机交互模块；
步骤A5、控制器以最优路径为设定目标，发送控制指令驱动机械臂上各关节相应的驱动电机，使各个关节转动到目标角度；
步骤A6、在机械臂各关节运动的过程中，中央处理模块判断数据采集模块采集的机械臂与障碍物之间的距离d与dsafe的大小关系；如果d≥dsafe，则继续运动直至到达目标状态，反之则转至步骤A3；
其特征在于：
所述机械臂避障的性能最优轨迹规划模型，包括目标函数和约束条件：
步骤B1、根据时间与能耗综合最优的轨迹规划目标确立目标函数J为：
其中，β为常数且β∈[0,1]，表示时间和能耗综合优化的权重系数；t表示时间；dt表示时间变量t的微分；tf为终端时间；u1(t)，u2(t)，u3(t)分别为Manutec R3机器人腰、肩、肘关节轴上与时间相关的扭矩电压；分别是Manutec R3机器人腰、肩、肘关节轴上与时间相关的关节角速度，单位为rad/s；b＝1,2,3为机械臂的连杆数；
步骤B2、约束条件为：
1)动力学约束
其中，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/s2；u(t)＝[u1(t),u2(t),u3(t)]T；D为3×3对角矩阵；/>是科里奥力和离心力引起的力矩；χg(q(t))是引力造成的力矩；
2)初始状态约束
其中，q0表示初始状态Manutec R3机器人腰、肩、肘关节轴上的关节角度；qinitial表示Manutec R3机器人腰、肩、肘关节轴上的关节角度设定值；表示初始状态Manutec R3机器人腰、肩、肘关节轴上的关节角的角速度；
3)目标状态约束
其中，qf表示终端时刻Manutec R3机器人腰、肩、肘关节轴上的关节角度；qtar表示目标状态Manutec R3机器人腰、肩、肘关节轴上的关节角度；表示终端时间Manutec R3机器人腰、肩、肘关节轴上的关节角的角速度；
4)关节角约束
qmin≤q(t)≤qmax,t＝1,2,...,tf (5)
其中，qmin表示Manutec R3机器人腰、肩、肘关节轴上关节角度的运动下限，qmax表示Manutec R3机器人腰、肩、肘关节轴上关节角度的运动上限；
5)关节角速度约束
其中，表示Manutec R3机器人腰、肩、肘关节轴上关节角速度的下限，/>表示Manutec R3机器人腰、肩、肘关节轴上关节角速度的上限；
6)电压约束
|u(t)|≤umax (7)
其中，umax为Manutec R3机器人腰、肩、肘关节轴上的扭矩电压的最大值；
7)无碰撞约束
为了简化机械臂与障碍物之间的距离计算，采用多个球体对机械臂进行包络，障碍物也用球体进行包络；R表示机械臂用球体包络后的球心，O表示障碍物用球体包络后的球心；因此，机械臂与障碍物之间的距离表示为：
其中，为机械臂第b个连杆上的第i个包络球的球心；i＝1…P，其中P为机械臂的连杆上包络球体的个数；Oj为障碍物上第j个包络球的球心；j＝1…Q，其中Q为障碍物上包络球体的个数；
故无碰撞约束表示为：
2.如权利要求1所述的基于球体包络性能最优的工业机械臂轨迹规划方法，其特征在于中央处理模块调用优化算法计算式(1)-(9)组成的机械臂避障的性能最优轨迹规划问题，具体步骤如下：
步骤C1、中央处理模块接收人机交互模块输入的目标状态Manutec R3机器人腰关节和肩关节以及肘关节的关节角度qtar、机器人完成作业的终端时间tf、时间网格数N、控制变量的初始值u0、机械臂与障碍物之间的安全距离dsafe和非线性规划求解的精度ε，数据采集模块获得的初始状态Manutec R3机器人腰关节、肩关节、肘关节的关节角度以及机械臂与障碍物之间的距离；
步骤C2、以机器人的基座为参考物，建立相对坐标系；根据从数据采集模块获得机械臂与障碍物之间的距离确立障碍物的包络球球心坐标
步骤C3、根据终端时间、时间网格数，采用均匀离散化的方式，计算出起始时间网格分布；
步骤C4、通过非线性规划算法求解该最优轨迹规划问题，得到当前时间网格下的最优的控制参数和目标函数值。
3.如权利要求2所述的基于球体包络性能最优的工业机械臂轨迹规划方法，其特征在于步骤C4中所述的非线性规划算法具体步骤如下：
步骤D1、将式(1)-(9)组成的优化问题转化成如下式(10)的非线性优化问题：
其中，x为n维实数变量，f(x)和c(x)分别表示连续可微的目标函数和m维约束方程，xL和xU分别表示变量的上下界约束；
步骤D2、根据划分的离散网格，通过ODE求解微分方程初值问题，获取离散后各网格点变量值，以此作为非线性优化问题的初值；
步骤D3、采用迭代计算方法求解上式(10)所表示的非线性优化问题，在x第k次迭代的迭代xk处QP子问题表示为以下形式：
其中，pk为搜索方向，gk和分别表示在xk处目标函数的导数和约束方程的雅克比矩阵，ck表示在xk处c(xk)的值，Wk为拉格朗日函数的Hessian阵，k为大于等于零的整数；其中拉格朗日函数为：
L(x,λ,v,π)＝f(x)+λTc(x)+vT(x-xU)-πT(x-xL) (12)
其中，λ，v和π分别表示与等式约束、上边界约束和下边界约束相关的拉格朗日乘子，λT，vT和πT分别表示相应拉格朗日乘子的转置；
步骤D4、更新下一次迭代下的变量迭代点：
xk+1＝xk+αpk (13)
其中α∈(0,1]，通过一维搜索方法求取；
步骤D5、根据最优条件和给定的设定误差进行收敛性判断；如果达到最优条件，则得到xk+1和对应的目标函数值，否则，判断计算时间是否大于最大优化时间T，如果大于最大优化时间T，则终止计算，并返回求解失败信息给人机交互模块；如果小于最大优化时间T，则令k＝k+1,xk＝xk+1,转步骤D3。
4.实现权利要求1-3任一项所述基于球体包络性能最优的工业机械臂轨迹规划方法的系统，其特征在于：包括人机交互模块、数据采集模块、A/D转换模块、中央处理模块、D/A转换模块、控制器；
所述人机交互模块用于设定机械臂相关参数、目标状态Manutec R3机器人腰和肩以及肘关节轴上的关节角度qtar、机器人完成作业的终端时间tf、时间网格数N、控制变量的初始值u0、安全距离dsafe、设定时间T和非线性规划求解的精度ε，并传送到中央处理模块，同时接收中央处理模块计算的优化结果；机械臂相关参数包括连杆长度、连杆扭转角、连杆偏距和关节转角；
所述数据采集模块用于采集初始状态的Manutec R3机器人腰和肩以及肘关节轴上的关节角度q0、机械臂实时运行的数据和机械臂与障碍物之间的距离；机械臂运行数据包括机械臂各关节的实时转角；
所述A/D转换模块用于将数据采集模块传输的模拟量数据转换为相应的数字量，并将其输送到中央处理模块；
所述中央处理模块是根据A/D转换模块和人机交互模块传送的数据，来获取最优目标函数值和最优变量值，并将其输送到D/A转换模块，同时反馈给人机交互模块优化结果；中央处理模块存储有机械臂避障的性能最优轨迹规划模型，包括目标函数和约束条件；
所述D/A转换模块用于将中央处理模块传输的数字量数据转换成相应的模拟量控制指令，并将其输送到控制器；
所述控制器根据D/A转换模块传输的控制指令来控制机械臂各关节相应的驱动电机，通过驱动电机实现各关节转动，使各关节转动到目标角度。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210292083.0A CN114559435B (zh) | 2022-03-23 | 2022-03-23 | 基于球体包络且性能最优目标下的机械臂轨迹规划方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210292083.0A CN114559435B (zh) | 2022-03-23 | 2022-03-23 | 基于球体包络且性能最优目标下的机械臂轨迹规划方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114559435A CN114559435A (zh) | 2022-05-31 |
| CN114559435B true CN114559435B (zh) | 2023-08-29 |
Family
ID=81720261
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210292083.0A Active CN114559435B (zh) | 2022-03-23 | 2022-03-23 | 基于球体包络且性能最优目标下的机械臂轨迹规划方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114559435B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117207202B (zh) * | 2023-11-09 | 2024-04-02 | 国网山东省电力公司东营供电公司 | 带电作业机器人防碰撞约束控制方法、系统、终端及介质 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108274465A (zh) * | 2018-01-10 | 2018-07-13 | 上海理工大学 | 基于优化a*的人工势场机械臂三维避障路径规划方法 |
| CN108621157A (zh) * | 2018-04-27 | 2018-10-09 | 上海师范大学 | 基于模型约束的机械臂能量最优轨迹规划控制方法及装置 |
| CN110696000A (zh) * | 2019-11-21 | 2020-01-17 | 河北工业大学 | 一种机械臂试探感知的避障方法 |
| WO2021065196A1 (en) * | 2019-10-03 | 2021-04-08 | Mitsubishi Electric Corporation | Method and system for trajectory optimization for nonlinear robotic systems with geometric constraints |
| CN113547506A (zh) * | 2021-06-28 | 2021-10-26 | 北京精密机电控制设备研究所 | 一种基于六维力传感器的双臂机器人精密装配方法 |
| CN114211492A (zh) * | 2021-12-22 | 2022-03-22 | 武汉鼎元同立科技有限公司 | 一种基于模型的多自由度机械臂的最优轨迹规划方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US11724387B2 (en) * | 2020-04-03 | 2023-08-15 | Fanuc Corporation | Fast robot motion optimization with distance field |
-
2022
- 2022-03-23 CN CN202210292083.0A patent/CN114559435B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108274465A (zh) * | 2018-01-10 | 2018-07-13 | 上海理工大学 | 基于优化a*的人工势场机械臂三维避障路径规划方法 |
| CN108621157A (zh) * | 2018-04-27 | 2018-10-09 | 上海师范大学 | 基于模型约束的机械臂能量最优轨迹规划控制方法及装置 |
| WO2021065196A1 (en) * | 2019-10-03 | 2021-04-08 | Mitsubishi Electric Corporation | Method and system for trajectory optimization for nonlinear robotic systems with geometric constraints |
| CN110696000A (zh) * | 2019-11-21 | 2020-01-17 | 河北工业大学 | 一种机械臂试探感知的避障方法 |
| CN113547506A (zh) * | 2021-06-28 | 2021-10-26 | 北京精密机电控制设备研究所 | 一种基于六维力传感器的双臂机器人精密装配方法 |
| CN114211492A (zh) * | 2021-12-22 | 2022-03-22 | 武汉鼎元同立科技有限公司 | 一种基于模型的多自由度机械臂的最优轨迹规划方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114559435A (zh) | 2022-05-31 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Lee | Robot arm kinematics, dynamics, and control | |
| Chen et al. | An integrated two-level self-calibration method for a cable-driven humanoid arm | |
| CN113043286B (zh) | 一种多自由度机械臂实时避障路径规划系统及方法 | |
| CN114559435B (zh) | 基于球体包络且性能最优目标下的机械臂轨迹规划方法 | |
| CN105354433B (zh) | 一种空间机械臂参数对运动可靠性影响比重的确定方法 | |
| CN113146617A (zh) | 一种柔性机器人动力学建模及轨迹跟随控制方法及装置 | |
| Mu et al. | Dynamic feedforward control of spatial cable-driven hyper-redundant manipulators for on-orbit servicing | |
| CN114942593A (zh) | 一种基于干扰观测器补偿的机械臂自适应滑模控制方法 | |
| Jing et al. | A recursive dynamic modeling and control for dual-arm manipulator with elastic joints | |
| CN110253573A (zh) | 一种基于串并联估计模型的柔性机械臂系统模糊控制方法 | |
| WO2023165174A1 (zh) | 构建机器人的控制器的方法、机器人的运动控制方法、装置以及机器人 | |
| Jiang | Workspace trajectory control of flexible robot manipulators using neural network and visual sensor feedback | |
| Cong et al. | Time-jerk synthetic optimal trajectory planning of robot based on fuzzy genetic algorithm | |
| CN108227493A (zh) | 一种机器人轨迹跟踪方法 | |
| Pajak et al. | Planning of a point to point collision-free trajectory for mobile manipulators | |
| CN112428262A (zh) | 基于超椭球映射解析算法并联冗余柔索机构伺服控制方法 | |
| Carter et al. | Motion control of non-fixed base robotic manipulators | |
| Gao et al. | Research of peg-in-hole assembly for a redundant dual-arm robot based on neural networks | |
| Park et al. | Reliable computation of minimum‐time motions for manipulators moving in obstacle fields using a successive search for minimum‐overload trajectories | |
| Xu et al. | Optimization design of a novel 4-DOF parallel manipulator | |
| TWI808852B (zh) | 六軸機器手臂深度學習穩定控制方法 | |
| Ghafil et al. | Optimum dynamic analysis of a robot arm using flower pollination algorithm | |
| Huang et al. | Particle swarm optimization algorithm for optimal configurations of an omnidirectional mobile service robot | |
| Meng et al. | Trajectory optimization using time-separating strategy with improved PSO on mechanical arms | |
| Yang | Research on cruise control of hydrostatic tractor based on CAN bus |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |