CN113787525A - 一种基于关节性能限制的机械臂运动时间优化方法 - Google Patents
一种基于关节性能限制的机械臂运动时间优化方法 Download PDFInfo
- Publication number
- CN113787525A CN113787525A CN202111366701.3A CN202111366701A CN113787525A CN 113787525 A CN113787525 A CN 113787525A CN 202111366701 A CN202111366701 A CN 202111366701A CN 113787525 A CN113787525 A CN 113787525A
- Authority
- CN
- China
- Prior art keywords
- value
- track
- mechanical arm
- speed
- point
- 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.)
- Granted
Links
- 230000033001 locomotion Effects 0.000 title claims abstract description 50
- 238000000034 method Methods 0.000 title claims abstract description 27
- 238000005457 optimization Methods 0.000 title claims abstract description 15
- 230000001133 acceleration Effects 0.000 claims abstract description 87
- 239000012636 effector Substances 0.000 claims abstract description 51
- 230000010354 integration Effects 0.000 claims abstract description 29
- 238000013507 mapping Methods 0.000 claims abstract description 14
- 238000012545 processing Methods 0.000 claims abstract description 13
- 239000011159 matrix material Substances 0.000 claims description 45
- 238000009499 grossing Methods 0.000 claims description 35
- 238000003491 array Methods 0.000 claims description 10
- 230000008878 coupling Effects 0.000 claims description 9
- 238000010168 coupling process Methods 0.000 claims description 9
- 238000005859 coupling reaction Methods 0.000 claims description 9
- 230000007935 neutral effect Effects 0.000 claims description 9
- 230000008569 process Effects 0.000 claims description 7
- 230000008859 change Effects 0.000 claims description 6
- 230000003247 decreasing effect Effects 0.000 claims description 4
- 101100149325 Escherichia coli (strain K12) setC gene Proteins 0.000 claims description 3
- 238000009795 derivation Methods 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 238000010606 normalization Methods 0.000 claims description 3
- 238000012163 sequencing technique Methods 0.000 claims description 3
- 239000013598 vector Substances 0.000 claims description 3
- 238000004422 calculation algorithm Methods 0.000 abstract description 3
- 238000004364 calculation method Methods 0.000 description 8
- 238000010586 diagram Methods 0.000 description 3
- 238000012795 verification Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000012549 training Methods 0.000 description 2
- 238000013528 artificial neural network Methods 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 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
- 238000004088 simulation Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
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/1612—Programme controls characterised by the hand, wrist, grip control
-
- 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/1628—Programme controls characterised by the control loop
- B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
-
- 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/1628—Programme controls characterised by the control loop
- B25J9/1651—Programme controls characterised by the control loop acceleration, rate control
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Health & Medical Sciences (AREA)
- General Health & Medical Sciences (AREA)
- Orthopedic Medicine & Surgery (AREA)
- Numerical Control (AREA)
- Manipulator (AREA)
Abstract
Description
技术领域
本发明涉及一种机械臂控制技术领域,具体涉及一种基于关节性能限制的机械臂运动时间优化方法。
背景技术
机械臂的应用极大的提高了生产效率,随着智能制造政策的落实,机器人的应用范围也越来越广泛。在设计机械臂时,关节电机的性能往往是依赖于工程师的工作经验,对固定的运动轨迹进行仿真计算从而确定关节电机的选型。对于机械臂用户来说,所规划的运动轨迹是复杂且多变的,机械臂在沿着用户设定的轨迹运行时,其末端执行器往往以较低的速度沿轨迹运动,这虽然保证了机械臂能够完成所规划的轨迹,但是却没有发挥关节电机性能的最大效率。现有技术中,为提高机械臂运动轨迹的速度,优化运动时间,大多采用神经网络等智能算法,但这需要大量的数据集进行训练,不仅消耗时间而且同样需要工程经验进行调参。
发明内容
本发明目的在于克服现有技术的不足,提供一种基于关节性能限制的机械臂运动时间优化方法,该方法的算法简单,基于机械臂的关节电机性能,有效地提高机械臂运动时的速度,从而实现对运动时间的优化。
本发明的目的通过以下技术方案实现:
一种基于关节性能限制的机械臂运动时间优化方法,包括以下步骤:
(1)机械臂的各个关节电机之间存在耦合关系,可通过Lagrange方程对此进行描述,其中Lagrange方程为:
式中,M(θ)为6×6的关节空间惯量矩阵,为6×6的科氏力和向心力的耦合矩阵,G(θ)为6×1的重力矢量矩阵,θ为6×1的机械臂关节空间的旋转角度矩阵,τ为6×1的机械臂的关节力矩矩阵;为对θ求导,即机械臂关节的速度矩阵;为对θ求两次导,即机械臂关节的加速度矩阵;
利用Christoffel符号将上述方程可以改写为以下动力学运动方程:
(2)对机械臂关节电机的力矩进行限制,具体为:
式中,i为机械臂的第i个关节,τ i 为机械臂的第i个关节电机某时刻的力矩,τ i 为τ矩阵中的其中一个数值;τ i min为第i个关节电机的最小力矩,τ i max为第i个关节电机的最大力矩;
(3)对轨迹点位进行加速度求解;先获取在该轨迹点位上的每个机械臂关节能够提供的末端执行器加速度范围,并在该轨迹点位上的每个机械臂关节能够提供的末端执行器加速度范围的交集中取最大值作为该轨迹点位的加速度限制值,记为sdd;其中,对约束不等式(5)中的符号进行变换:
从而将约束不等式转化为:
其中,根据运算规则,其中的a(s)、b(s)和c(s)均为12×1的矩阵;
根据约束不等式(6),轨迹点位上的机械臂每个关节能够提供的末端执行器加速度上限值和末端执行器加速度下限值的获取如下:
当a i (s)>0时,机械臂关节能提供的末端执行器加速度上限值为:
当a i (s)<0时,机械臂关节能提供的末端执行器加速度下限值为:
当a i (s)=0时,选取该轨迹点位前后相邻的两个轨迹点位中的较小的末端执行器加速度限制值作为该轨迹点位能够提供的末端执行器加速度限制值;
在公式(7)和公式(8)中,i为机械臂上的第i个关节;a i (s)为12×1矩阵a(s)中与第i个关节所对应的数值,b i (s)为12×1矩阵b(s)中与第i个关节所对应的数值,c i (s)为12×1矩阵c(s)中与第i个关节所对应的数值;
(4)将机械臂关节电机的速度限制映射为沿轨迹运动的末端执行器速度限制,具体地:
式中,t为时间,为机械臂关节角度的变化量,dt为积分时间间隔,ds为轨迹长度的变化量,为机械臂末端执行器的速度限制,为机械臂关节电机的速度限制,上述公式(9)的后部分具体为由机械臂关节电机的速度限制转换为机械臂末端执行器的速度限制;
(5)对各类特征轨迹点位的加速度限制值进行前后数值积分处理,获取基于关节电机力矩限制对应的平面曲线;其中各类特征轨迹点位的加速度限制值通过步骤(3)获取,所述各类特征轨迹点位包括起始点、终止点以及间断点;具体地,积分公式如下:
式中,为轨迹点位向前的轨迹长度值,为轨迹点位向前的速度值,为当前轨迹点位处的轨迹长度值,为当前轨迹点位处的机械臂末端执行器的速度值,为轨迹点位向后的轨迹长度值,为轨迹点位向后的速度值,dt为积分时间间隔,sdd为步骤(3)中求得的该轨迹点位的加速度限制值;轨迹点位的前向为轨迹长度从1往0的方向,轨迹点位的后向为轨迹长度从0往1的方向,由于机械臂的运动轨迹已经确定,因此采用归一方法可得出轨迹长度s的范围为0-1;
(6)将步骤(4)中获得的基于关节电机速度限制对应的平面曲线与步骤(5)中获得的基于关节电机力矩限制对应的平面曲线进行结合;将基于关节电机力矩限制对应的平面曲线中高于基于关节电机速度限制对应的平面曲线的部分舍去,同时在同一点位上取较小的速度作为曲线速度值,从而得到同时满足关节电机速度限制和关节电机力矩限制且连续的轨迹长度-速度限制曲线;
(7)将步骤(6)中所得到的轨迹长度-速度限制曲线作为依据条件,机械臂末端执行器进行指定运动轨迹的运行,完成机械臂运动时间的优化。
本发明的一个优选方案,在步骤(5)中,对间断点进行前后数值积分时,逐步提高间断点处的速度积分起始值的选取,以获取该间断点在满足轨迹长度条件下最高的加速度值进行积分;在逐步提高间断点处的速度积分起始值的选取时,根据预设的固定增长比例,不断逼近间断点处的最小值,且通过设定阈值终止速度积分起始值的选取。
本发明的一个优选方案,采用多次B样条曲线对轨迹长度-速度限制曲线的速度规划进行平滑处理,从而获得平滑的轨迹长度-速度限制曲线。
优选地,将u记为轨迹长度-速度限制曲线上离散点到轨迹起始点的距离与轨迹长度的比值,并令为单调不减的数组序列,n为所述数组序列U中的元素数量,可自定义选取;其中的u 0,u 1,…u n 表示轨迹长度-速度限制曲线上不同离散点的u值,即下述公式(14)中u的具体数值;同时,具体采用三次B样条曲线对轨迹长度-速度限制曲线进行平滑处理,因此三次B样条曲线方程可以表达:
进一步地,p(u)值可通过u值得出,因此上述公式(14)中的控制点C j 为唯一变量;此时,将数组序列中的所有u值代入到公式(14)以及标准的德布尔-考克斯递推公式,即可求得控制点C j 序列,从而得出平滑的轨迹长度-速度限制曲线。
优选地,对平滑处理后的轨迹长度-速度限制曲线的速度进行验证;在平滑处理后的轨迹长度-速度限制曲线上选取若干个待验证的离散点,并将每个待验证的离散点的平滑速度值V′与该待验证的离散点在平滑处理前的轨迹长度-速度限制曲线对应的速度值V进行比较,且根据比较结果进行曲线调整;
当V′≤V时,不做处理;
当V′>V时,则将所有控制点C j 的纵向坐标下降指定比例,再次比较该离散点的V′值与V值,直至选取的所有待验证的离散点均满足V′≤V。
优选地,完成平滑处理后的轨迹长度-速度限制曲线的平滑速度验证后,对平滑处理后的轨迹长度-速度限制曲线的平滑加速度进行验证;在平滑处理后的轨迹长度-速度限制曲线上选取若干个待验证的离散点,对同一个待验证的离散点分别计算平滑加速度值a以及平滑加速度限制值sdd′,并对同一个待验证的离散点的平滑加速度值a与平滑加速度限制值sdd′进行比较和调整处理,其中的平滑加速度限制值sdd′通过步骤(3)即可求得,其中的平滑加速度值a通过以下公式求得:
若a ≤ sdd′,不做处理;
若a > sdd′,则将所有控制点C j 的纵向坐标下降指定比例,重新计算和比较该离散点的a值与sdd′值,直至选取的所有待验证的离散点均满足a ≤ sdd′。
本发明与现有技术相比具有以下有益效果:
1、在机械臂的运动轨迹已经确定的前提下,本发明通过对机械臂的运动速度进行限制,通过轨迹长度-速度限制曲线,实现对机械臂进行运动轨迹时的指导,充分发挥关节电机的性能,让机械臂快速高效地完成轨迹运动,缩短运动时间,从而达到机械臂运动时间优化的目的。
2、本发明从机械臂的关节电机的力矩限制和速度限制两方面条件因素求解出机械臂预定轨迹的时间优化规划,使得机械臂的运动时间优化准确度高,且能充分发挥关节电机的性能,让机械臂以尽可能大的速度沿预定轨迹进行运动。
3、本发明采用数值算法的方式实现对机械臂的运动时间优化,具有计算实时效果,且计算量小,无需大量的数据训练或繁杂的计算过程,具有实时性和良好的可靠性,能适应任何预定运动轨迹。
附图说明
图1为本发明的基于关节性能限制的机械臂运动时间优化方法的流程框图。
图5为轨迹长度-速度限制曲线(粗实线L4)示意图。
图6为平滑处理后的轨迹长度-速度限制曲线(双点划线L5)示意图。
具体实施方式
下面结合实施例和附图对本发明作进一步描述,但本发明的实施方式不仅限于此。
参见图1-图6,本实施例公开一种基于关节性能限制的机械臂运动时间优化方法,包括以下步骤:
(1)机械臂的各个关节电机之间存在耦合关系,可通过Lagrange方程对此进行描述,其中Lagrange方程为:
式中,M(θ)为6×6的关节空间惯量矩阵,为6×6的科氏力和向心力的耦合矩阵,G(θ)为6×1的重力矢量矩阵,θ为6×1的机械臂关节空间的旋转角度矩阵,τ为6×1的机械臂的关节力矩矩阵;为对θ求导,即机械臂关节的速度矩阵;为对θ求两次导,即机械臂关节的加速度矩阵;
利用Christoffel符号将上述方程可以改写为以下动力学运动方程:
机械臂运动轨迹的时间最优问题实际上就是求解机械臂在满足动力学方程的条件下速度的最大值问题,把此问题转化到平面上将能更加直观的感受所求解的目标。而Lagrange方程是在关节空间对机械臂的运动状态进行描述,因此通过构造映射的方式,将Lagrange方程映射到平面,从而得出:
(2)对机械臂关节电机的力矩进行限制,具体为:
式中,i为机械臂的第i个关节,τ i 为机械臂的第i个关节电机某时刻的力矩,τ i 为τ矩阵中的其中一个数值;τ i min为第i个关节电机的最小力矩,τ i max为第i个关节电机的最大力矩;
(3)对轨迹点位进行加速度求解;先获取在该轨迹点位上的每个机械臂关节能够提供的末端执行器加速度范围,并在该轨迹点位上的每个机械臂关节能够提供的末端执行器加速度范围的交集中取最大值作为该轨迹点位的加速度限制值,记为sdd;其中,对约束不等式(5)中的符号进行变换:
从而将约束不等式转化为:
其中,根据运算规则,其中的a(s)、b(s)和c(s)均为12×1的矩阵;
根据约束不等式(6),轨迹点位上的机械臂每个关节能够提供的末端执行器加速度上限值和末端执行器加速度下限值的通过以下方程公式获取。由于本实施例中采用的机械臂为六轴机械臂,而经过前面的计算后,对于每个机械臂关节,会存在小于不等式(公式(7))和大于不等式(公式(8))两个结果,因此总的来说,每个轨迹点位将会包括12个不等式。
当a i (s)>0时,机械臂关节能提供的末端执行器加速度上限值为:
当a i (s)<0时,机械臂关节能提供的末端执行器加速度下限值为:
当a i (s)=0时,选取该轨迹点位前后相邻的两个轨迹点位中的较小的末端执行器加速度限制值作为该轨迹点位能够提供的末端执行器加速度限制值。具体地,例如此时该机械臂关节i所对应的s为0.1,取多条积分虚线中s=0.1附近能提供的末端执行器加速度最小值作为该机械臂关节能提供的末端执行器加速度上限值(这里可理解为:如果ai(s)=0,那么这个轨迹点位的计算先放着不管,当计算完其他所有不为零的离散点之后,先通过积分形成多条虚线,再从多条虚线中取得这个轨迹点位的相邻点sdd最小值作为该轨迹点位的sdd,从而补全该轨迹点位的虚线)。
在上述公式(7)和公式(8)中,i为机械臂上的第i个关节;a i (s)为12×1矩阵a(s)中与第i个关节所对应的数值,b i (s)为12×1矩阵b(s)中与第i个关节所对应的数值,c i (s)为12×1矩阵c(s)中与第i个关节所对应的数值。
式中,t为时间,为机械臂关节角度的变化量,dt为积分时间间隔,ds为轨迹长度的变化量,为机械臂末端执行器的速度限制,为机械臂关节电机的速度限制,上述公式(9)的后部分具体为由机械臂关节电机的速度限制转换为机械臂末端执行器的速度限制;
通过上述公式(9)的映射,得出沿指定运动轨迹的所有离散点上基于关节电机速度限制对应的平面曲线。如图2的细实线L1所示,该平面曲线为连续的曲线,但更一般的情况是如图3的细实线L2所示,该平面曲线存在不连续点P1、P2,甚至更多的不连续点。
(5)对各类特征轨迹点位的加速度限制值进行前后数值积分处理,获取基于关节电机力矩限制对应的平面曲线,同样包含了多条不连续的曲线,如图4的虚线L3所示,而图4中的细实线L2为基于关节电机速度限制对应的平面曲线。其中各类特征轨迹点位的加速度限制值通过步骤(3)获取,所述各类特征轨迹点位包括起始点、终止点以及间断点;具体地,积分公式如下:
式中,为轨迹点位向前的轨迹长度值,为轨迹点位向前的速度值,为当前轨迹点位处的轨迹长度值,为当前轨迹点位处的机械臂末端执行器的速度值,为轨迹点位向后的轨迹长度值,为轨迹点位向后的速度值,dt为积分时间间隔,sdd为步骤(3)中求得的该轨迹点位的加速度限制值;轨迹点位的前向为轨迹长度从1往0的方向,轨迹点位的后向为轨迹长度从0往1的方向,由于机械臂的运动轨迹已经确定,因此采用归一方法可得出轨迹长度s的范围为0-1。
另外,在对间断点进行前后数值积分时,逐步提高间断点处的速度积分起始值的选取,以获取该间断点在满足轨迹长度条件下最高的加速度值进行积分;在逐步提高间断点处的速度积分起始值的选取时,根据预设的固定增长比例,不断逼近间断点处的最小值,且通过设定阈值终止速度积分起始值的选取。具体地,可在对间断点进行前后数值积分时,先选取不连续点处的较小值的三分之一作为速度积分起始值,经过数值积分后,若满足轨迹长度0-1的范围,则继续提高速度积分起始值的选取,将其选取为不连续点处的较小值的三分之二,并重新数值积分,直至在满足轨迹长度0-1的范围前提下,逐渐逼近不连续点处的较小值;速度积分起始值的选取,可通过设置与不连续点处的较小值的误差阈值进行终止。这样,有利于让间断点在基于关节电机力矩限制对应的(s,s ̇)平面上尽可能地高,从而使得在后续处理中该点的速度限制值越高,以提高机械臂运动时的速度,运动时间得到进一步的优化。
(6)将步骤(4)中获得的基于关节电机速度限制对应的平面曲线(如图3和图4的细实线L2所示)与步骤(5)中获得的基于关节电机力矩限制对应的平面曲线(如图4的虚线L3所示)进行结合。在结合时,将基于关节电机力矩限制对应的平面曲线中高于基于关节电机速度限制对应的平面曲线的部分舍去,同时在同一点位上取较小的速度作为曲线速度值,从而得到同时满足关节电机速度限制和关节电机力矩限制且连续的轨迹长度-速度限制曲线,如图5中的粗实线L4所示,而图5中的细实线L2为基于关节电机速度限制对应的平面曲线,虚线为基于关节电机力矩限制对应的平面曲线中舍去的部分。
(7)采用多次B样条曲线对轨迹长度-速度限制曲线的速度规划进行平滑处理,从而获得平滑的轨迹长度-速度限制曲线,如图6中的双点划线L5所示。
首先,将u记为轨迹长度-速度限制曲线上离散点到轨迹起始点的距离与轨迹长度的比值,并令为单调不减的数组序列,n为所述数组序列U中的元素数量,可自定义选取;其中的u 0,u 1,…u n 表示轨迹长度-速度限制曲线上不同离散点的u值,即下述公式(14)中u的具体数值;同时,具体采用三次B样条曲线对轨迹长度-速度限制曲线进行平滑处理,因此三次B样条曲线方程可以表达:
在上述处理过程中,p(u)值可通过u值得出,因此上述公式(14)中的控制点C j 为唯一变量;此时,将数组序列中的所有u值代入到公式(14)以及标准的德布尔-考克斯递推公式,即可求得控制点C j 序列,从而得出平滑的轨迹长度-速度限制曲线。
其中,本实施例中的标准的德布尔-考克斯递推公式如下:
式中,j为该基函数在数组序列U上的位置,k=3;
同时,由于三次B样条曲线的特性,需要额外增加两个控制点,本实施例中具体增加的两个控制点的方程组分别为轨迹起始点、终止点处的速度为0的两个方程。
例如,以轨迹长度为100cm作为参考,则可选取50个离散点的比例来确定控制点C j ,由于三次B样条曲线的特性,则另外增加两个控制点,因此一共有52个控制点。上述50个离散点在轨迹长度100cm上均匀选取,即u分别为1/50、2/50、3/50、…、50/50,同时根据这些u值即可相应地获得p(u)值;这样,就得出50条关于获取控制点的方程,即50条上述方程公式(14),随后经过解方程即可得出50个控制点。在得出控制点后,根据B样条曲线的特性,即可获得平滑处理的轨迹长度-速度限制曲线。
(8)对平滑处理后的轨迹长度-速度限制曲线的速度进行验证。在平滑处理后的轨迹长度-速度限制曲线上选取若干个待验证的离散点,并将每个待验证的离散点的平滑速度值V′与该待验证的离散点在平滑处理前的轨迹长度-速度限制曲线对应的速度值V进行比较,且根据比较结果进行曲线调整;
当V′≤V时,不做处理;
当V′>V时,则将所有控制点C j 的纵向坐标下降指定比例,再次比较该离散点的V′值与V值,直至选取的所有待验证的离散点均满足V′≤V。例如,对于轨迹长度为100cm的参考值,可等距选取100个离散点,并依次计算该100个离散点在平滑后的曲线上的平滑速度值,且与平滑后的曲线上的速度值进行比较,根据比较结果进行平滑后的曲线的调整。调整时,可先对所有控制点C j 的纵向坐标下降10%,再次比较计算后,若仍不满足条件,则继续对所有控制点C j 的纵向坐标再下降10%,直至所有离散点满足条件位置。
(9)对平滑处理后的轨迹长度-速度限制曲线的平滑加速度进行验证,从而获得经过平滑和验证调整的轨迹长度-速度限制曲线。在平滑处理后的轨迹长度-速度限制曲线上选取若干个待验证的离散点,对同一个待验证的离散点分别计算平滑加速度值a以及平滑加速度限制值sdd′,并对同一个待验证的离散点的平滑加速度值a与平滑加速度限制值sdd′进行比较和调整处理,其中的平滑加速度限制值sdd′通过步骤(3)即可求得,其中的平滑加速度值a通过以下公式求得:
式中,为待验证的离散点的平滑速度值,为相邻点的平滑速度值,dm为待验证的离散点与相邻点之间的距离;其中的相邻点可在待验证的离散点的左侧或右侧选取,此处目的是将待验证的离散点的平滑加速度值a求出即可,对相邻点的选取不作限定;
若a ≤ sdd′,不做处理;
若a > sdd′,则将所有控制点C j 的纵向坐标下降指定比例,重新计算和比较该离散点的a值与sdd′值,直至选取的所有待验证的离散点均满足a ≤ sdd′。
(10)将步骤(9)的经过平滑和验证调整的轨迹长度-速度限制曲线作为依据条件,机械臂末端执行器进行指定运动轨迹的运行,完成机械臂运动时间的优化。
本实施例中,在得出轨迹长度-速度限制曲线后,由于曲线并非是平滑的,因此进一步采用B样条曲线作平滑处理,使得机械臂进行指定运动轨迹时的速度更加平滑,从而有利于提高机械臂运动时的稳定性。与此同时,对平滑后的曲线进行验证,以确保平滑后的曲线仍然在机械臂关节电机的速度和力矩限制的范围内,从而提高优化后的曲线可靠性。当然,对于本实施例的轨迹长度-速度限制曲线,具体的B样条曲线平滑处理可参见现有技术,也可采用其他方式进行平滑处理。
上述为本发明较佳的实施方式,但本发明的实施方式并不受上述内容的限制,其他的任何未背离本发明的精神实质与原理下所做的改变、修饰、替代、组合、简化,均应为等效的置换方式,都包含在本发明的保护范围之内。
Claims (6)
1.一种基于关节性能限制的机械臂运动时间优化方法,其特征在于,包括以下步骤:
(1)机械臂的各个关节电机之间存在耦合关系,可通过Lagrange方程对此进行描述,其中Lagrange方程为:
式中,M(θ)为6×6的关节空间惯量矩阵,为6×6的科氏力和向心力的耦合矩阵,G (θ)为6×1的重力矢量矩阵,θ为6×1的机械臂关节空间的旋转角度矩阵,τ为6×1的机械臂的关节力矩矩阵;为对θ求导,即机械臂关节的速度矩阵;为对θ求两次导,即机械臂关节的加速度矩阵;
利用Christoffel符号将上述方程可以改写为以下动力学运动方程:
(2)对机械臂关节电机的力矩进行限制,具体为:
式中,i为机械臂的第i个关节,τ i 为机械臂的第i个关节电机某时刻的力矩,τ i 为τ矩阵中的其中一个数值;τ i min 为第i个关节电机的最小力矩,τ i max 为第i个关节电机的最大力矩;
(3)对轨迹点位进行加速度求解;先获取在该轨迹点位上的每个机械臂关节能够提供的末端执行器加速度范围,并在该轨迹点位上的每个机械臂关节能够提供的末端执行器加速度范围的交集中取最大值作为该轨迹点位的加速度限制值,记为sdd;其中,对约束不等式(5)中的符号进行变换:
从而将约束不等式转化为:
其中,根据运算规则,其中的a(s)、b(s)和c(s)均为12×1的矩阵;
根据约束不等式(6),轨迹点位上的机械臂每个关节能够提供的末端执行器加速度上限值和末端执行器加速度下限值的获取如下:
当a i (s)>0时,机械臂关节能提供的末端执行器加速度上限值为:
当a i (s)<0时,机械臂关节能提供的末端执行器加速度下限值为:
当a i (s)=0时,选取该轨迹点位前后相邻的两个轨迹点位中的较小的末端执行器加速度限制值作为该轨迹点位能够提供的末端执行器加速度限制值;
在公式(7)和公式(8)中,i为机械臂上的第i个关节;a i (s)为12×1矩阵a(s)中与第i个关节所对应的数值,b i (s)为12×1矩阵b(s)中与第i个关节所对应的数值,c i (s)为12×1矩阵c(s)中与第i个关节所对应的数值;
(4)将机械臂关节电机的速度限制映射为沿轨迹运动的末端执行器速度限制,具体地:
式中,t为时间,为机械臂关节角度的变化量,dt为积分时间间隔,ds为轨迹长度的变化量,为机械臂末端执行器的速度限制,为机械臂关节电机的速度限制,上述公式(9)的后部分具体为由机械臂关节电机的速度限制转换为机械臂末端执行器的速度限制;
(5)对各类特征轨迹点位的加速度限制值进行前后数值积分处理,获取基于关节电机力矩限制对应的平面曲线;其中各类特征轨迹点位的加速度限制值通过步骤(3)获取,所述各类特征轨迹点位包括起始点、终止点以及间断点;具体地,积分公式如下:
式中,为轨迹点位向前的轨迹长度值,为轨迹点位向前的速度值,为当前轨迹点位处的轨迹长度值,当前轨迹点位处的机械臂末端执行器的速度值,为轨迹点位向后的轨迹长度值,为轨迹点位向后的速度值,dt为积分时间间隔,sdd为步骤(3)中求得的该轨迹点位的加速度限制值;轨迹点位的前向为轨迹长度从1往0的方向,轨迹点位的后向为轨迹长度从0往1的方向,由于机械臂的运动轨迹已经确定,因此采用归一方法可得出轨迹长度s的范围为0-1;
(6)将步骤(4)中获得的基于关节电机速度限制对应的平面曲线与步骤(5)中获得的基于关节电机力矩限制对应的平面曲线进行结合;将基于关节电机力矩限制对应的平面曲线中高于基于关节电机速度限制对应的平面曲线的部分舍去,同时在同一点位上取较小的速度作为曲线速度值,从而得到同时满足关节电机速度限制和关节电机力矩限制且连续的轨迹长度-速度限制曲线;
(7)将步骤(6)中所得到的轨迹长度-速度限制曲线作为依据条件,机械臂末端执行器进行指定运动轨迹的运行,完成机械臂运动时间的优化。
2.根据权利要求1所述的基于关节性能限制的机械臂运动时间优化方法,其特征在于,在步骤(5)中,对间断点进行前后数值积分时,逐步提高间断点处的速度积分起始值的选取,以获取该间断点在满足轨迹长度条件下最高的加速度值进行积分;在逐步提高间断点处的速度积分起始值的选取时,根据预设的固定增长比例,不断逼近间断点处的最小值,且通过设定阈值终止速度积分起始值的选取。
3.根据权利要求1或2所述的基于关节性能限制的机械臂运动时间优化方法,其特征在于,采用多次B样条曲线对轨迹长度-速度限制曲线的速度规划进行平滑处理,从而获得平滑的轨迹长度-速度限制曲线。
4.根据权利要求3所述的基于关节性能限制的机械臂运动时间优化方法,其特征在于,将u记为轨迹长度-速度限制曲线上离散点到轨迹起始点的距离与轨迹长度的比值,并令为单调不减的数组序列,n为所述数组序列U中的元素数量,可自定义选取;其中的u 0,u 1,… u n 表示轨迹长度-速度限制曲线上不同离散点的u值,即下述公式(14)中u的具体数值;同时,具体采用三次B样条曲线对轨迹长度-速度限制曲线进行平滑处理,因此三次B样条曲线方程可以表达:
进一步地,p(u)值可通过u值得出,因此上述公式(14)中的控制点C j 为唯一变量;此时,将数组序列中的所有u值代入到公式(14)以及标准的德布尔-考克斯递推公式,即可求得控制点C j 序列,从而得出平滑的轨迹长度-速度限制曲线。
5.根据权利要求4所述的基于关节性能限制的机械臂运动时间优化方法,其特征在于,对平滑处理后的轨迹长度-速度限制曲线的速度进行验证;在平滑处理后的轨迹长度-速度限制曲线上选取若干个待验证的离散点,并将每个待验证的离散点的平滑速度值V′与该待验证的离散点在平滑处理前的轨迹长度-速度限制曲线对应的速度值V进行比较,且根据比较结果进行曲线调整;
当V′≤V时,不做处理;
当V′>V时,则将所有控制点C j 的纵向坐标下降指定比例,再次比较该离散点的V′值与V值,直至选取的所有待验证的离散点均满足V′≤V。
6.根据权利要求5所述的基于关节性能限制的机械臂运动时间优化方法,其特征在于,完成平滑处理后的轨迹长度-速度限制曲线的平滑速度值验证后,对平滑处理后的轨迹长度-速度限制曲线的平滑加速度值进行验证;在平滑处理后的轨迹长度-速度限制曲线上选取若干个待验证的离散点,对同一个待验证的离散点分别计算平滑加速度值a以及平滑加速度限制值sdd′,并对同一个待验证的离散点的平滑加速度值a与平滑加速度限制值sdd′进行比较和调整处理,其中的平滑加速度限制值sdd′通过步骤(3)即可求得,其中的平滑加速度值a通过以下公式求得:
若a ≤ sdd′,不做处理;
若a > sdd′,则将所有控制点C j 的纵向坐标下降指定比例,重新计算和比较该离散点的a值与sdd′值,直至选取的所有待验证的离散点均满足a ≤ sdd′。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111366701.3A CN113787525B (zh) | 2021-11-18 | 2021-11-18 | 一种基于关节性能限制的机械臂运动时间优化方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111366701.3A CN113787525B (zh) | 2021-11-18 | 2021-11-18 | 一种基于关节性能限制的机械臂运动时间优化方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113787525A true CN113787525A (zh) | 2021-12-14 |
CN113787525B CN113787525B (zh) | 2022-02-01 |
Family
ID=78955372
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111366701.3A Active CN113787525B (zh) | 2021-11-18 | 2021-11-18 | 一种基于关节性能限制的机械臂运动时间优化方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113787525B (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117564430A (zh) * | 2024-01-15 | 2024-02-20 | 中国核动力研究设计院 | 曲面工件扩散焊接的加压组件、设备及焊接方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP2384863A2 (en) * | 2010-01-21 | 2011-11-09 | Institutul de Mecanica Solidelor al Academiei Romane | Method and device for dynamic control of a walking robot |
CN109623810A (zh) * | 2018-11-26 | 2019-04-16 | 南京航空航天大学 | 一种机器人平滑的时间最优轨迹规划的方法 |
CN110209048A (zh) * | 2019-05-20 | 2019-09-06 | 华南理工大学 | 基于动力学模型的机器人时间最优轨迹规划方法、设备 |
CN110421569A (zh) * | 2019-08-29 | 2019-11-08 | 南京邮电大学 | 一种基于有限时间扰动观测器的反演滑模机械臂控制器设计方法 |
CN113119111A (zh) * | 2021-03-18 | 2021-07-16 | 深圳市优必选科技股份有限公司 | 机械臂及其轨迹规划方法和装置 |
CN113290555A (zh) * | 2021-05-08 | 2021-08-24 | 浙江大学 | 一种工业机器人时间最优控制轨迹的优化方法 |
-
2021
- 2021-11-18 CN CN202111366701.3A patent/CN113787525B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP2384863A2 (en) * | 2010-01-21 | 2011-11-09 | Institutul de Mecanica Solidelor al Academiei Romane | Method and device for dynamic control of a walking robot |
CN109623810A (zh) * | 2018-11-26 | 2019-04-16 | 南京航空航天大学 | 一种机器人平滑的时间最优轨迹规划的方法 |
CN110209048A (zh) * | 2019-05-20 | 2019-09-06 | 华南理工大学 | 基于动力学模型的机器人时间最优轨迹规划方法、设备 |
CN110421569A (zh) * | 2019-08-29 | 2019-11-08 | 南京邮电大学 | 一种基于有限时间扰动观测器的反演滑模机械臂控制器设计方法 |
CN113119111A (zh) * | 2021-03-18 | 2021-07-16 | 深圳市优必选科技股份有限公司 | 机械臂及其轨迹规划方法和装置 |
CN113290555A (zh) * | 2021-05-08 | 2021-08-24 | 浙江大学 | 一种工业机器人时间最优控制轨迹的优化方法 |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117564430A (zh) * | 2024-01-15 | 2024-02-20 | 中国核动力研究设计院 | 曲面工件扩散焊接的加压组件、设备及焊接方法 |
CN117564430B (zh) * | 2024-01-15 | 2024-04-02 | 中国核动力研究设计院 | 曲面工件扩散焊接的加压组件、设备及焊接方法 |
Also Published As
Publication number | Publication date |
---|---|
CN113787525B (zh) | 2022-02-01 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106647282B (zh) | 一种考虑末端运动误差的六自由度机器人轨迹规划方法 | |
CN109343345B (zh) | 基于qpso算法的机械臂多项式插值轨迹规划方法 | |
CN106671982B (zh) | 基于多智能体的无人驾驶电动汽车自动超车***及方法 | |
CN112975992B (zh) | 一种误差可控的机器人轨迹同步优化方法 | |
Leottau et al. | Decentralized reinforcement learning of robot behaviors | |
CN109782779B (zh) | 基于种群超启发式算法的洋流环境下auv路径规划方法 | |
CN109765887B (zh) | 一种自动驾驶控制方法 | |
CN109240091B (zh) | 一种基于强化学习的水下机器人控制方法及其进行跟踪的控制方法 | |
CN110928290B (zh) | 五次曲线路径规划方法及车道中心线维护方法 | |
CN105676636B (zh) | 一种基于nsga-ii算法的冗余度空间机械臂多目标优化方法 | |
Rubies-Royo et al. | A classification-based approach for approximate reachability | |
CN114952868B (zh) | 7自由度srs型机械臂控制方法及装置、弹琴机器人 | |
CN113787525B (zh) | 一种基于关节性能限制的机械臂运动时间优化方法 | |
CN109822550B (zh) | 一种复杂曲面机器人高效高精度示教方法 | |
CN108427282A (zh) | 一种基于示教学习的机器人逆运动学求解方法 | |
CN111283683B (zh) | 一种机器人视觉特征规划轨迹的伺服跟踪加速收敛方法 | |
CN114779785A (zh) | 一种基于pso参数整定的移动机器人平滑轨迹规划方法 | |
CN116772880B (zh) | 一种基于无人机视觉的无人车路径规划方法 | |
CN115685988A (zh) | 一种自由曲面喷涂路径自动生成方法与装置 | |
CN118201742A (zh) | 使用图神经网络的多机器人协调 | |
CN109634294B (zh) | 一种基于机动能力辨识的抗干扰四元数姿态机动路径规划方法 | |
CN113848725A (zh) | 一种基于自适应遗传算法的机械臂时间最优轨迹规划优化方法 | |
CN111515954A (zh) | 一种机械臂高质量运动路径生成方法 | |
Hansen et al. | Waypoint planning with Dubins curves using genetic algorithms | |
CN108776432A (zh) | 基于网络的机场跑道检测机器人预测控制方法 |
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 |