CN113276121B - 一种基于二次规划的冗余度机械臂移动障碍物躲避方法 - Google Patents

一种基于二次规划的冗余度机械臂移动障碍物躲避方法 Download PDF

Info

Publication number
CN113276121B
CN113276121B CN202110596692.0A CN202110596692A CN113276121B CN 113276121 B CN113276121 B CN 113276121B CN 202110596692 A CN202110596692 A CN 202110596692A CN 113276121 B CN113276121 B CN 113276121B
Authority
CN
China
Prior art keywords
mechanical arm
moving
obstacle
redundant manipulator
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.)
Active
Application number
CN202110596692.0A
Other languages
English (en)
Other versions
CN113276121A (zh
Inventor
张智军
赵黄婷
陈思远
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
South China University of Technology SCUT
Original Assignee
South China University of Technology SCUT
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by South China University of Technology SCUT filed Critical South China University of Technology SCUT
Priority to CN202110596692.0A priority Critical patent/CN113276121B/zh
Publication of CN113276121A publication Critical patent/CN113276121A/zh
Application granted granted Critical
Publication of CN113276121B publication Critical patent/CN113276121B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/1674—Programme controls characterised by safety, monitoring, diagnostic
    • B25J9/1676—Avoiding collision or forbidden zones

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)

Abstract

本发明公开了一种基于二次规划的冗余度机械臂移动障碍物躲避方法，首先获取并计算移动障碍物和冗余度机械臂脆弱点的位置和速度信息，据此建立增大移动障碍物与冗余度机械臂脆弱点距离的不等式约束、驱使冗余度机械臂脆弱点趋向障碍物分速度反方向的不等式约束，然后合并两个不等式约束得到一个统一的移动障碍物躲避不等式约束，再将不等式约束并入到二次规划问题中，并用递归神经网络求解器求解该二次规划问题，求解结果传递给移动平台和机械臂控制器，从而驱动移动平台与机械臂在完成末端任务的同时躲避移动障碍物。本发明能够使机械臂在完成末端任务的同时，有方向地躲避移动障碍物，大大提高了躲避移动障碍物的效率和机械臂的安全性。

Description

一种基于二次规划的冗余度机械臂移动障碍物躲避方法
技术领域
本发明涉及冗余度机械臂运动规划技术领域，特别是一种基于二次规划的冗余度机械臂移动障碍物躲避方法。
背景技术
冗余度机械臂由于拥有多个自由度，具有极强的灵活度，能够在执行主要末端执行器任务的同时完成各种次要子任务。随着机械臂的应用日益多样化，机械臂的工作环境也变得越发复杂，移动障碍物的存在是其中一个不可忽视的问题。在这种动态的工作环境中，机器人在执行末端执行器的跟踪任务时，需要避免与移动障碍物碰撞。
传统的移动障碍物避障方法主要有人工势场法、遗传算法、快速扩展随机树、蚁群优化和概率路线图方法。人工势场法主要表现为目标点对机械臂末端的引力和障碍物对机械臂的斥力，由于机械手和目标点之间的距离与引力成正比，如果机械手离目标点太远，引力就会过大，从而导致碰撞。而且，如果目标点附近有障碍物，机械手可能无法到达目标点。遗传算法稳定性较差，不能解决高维非线性约束问题。快速扩展随机树、蚁群优化和概率路线图方法通常需要大量计算，难以实现实时控制。此外还有一些新的避障算法，但是这些方法都没有考虑到障碍物的速度信息，因此仍存在障碍物与机械臂碰撞的风险。
发明内容
本发明的第一目的在于解决现有技术的不足，提出一种基于二次规划的冗余度机械臂移动障碍物躲避方法，该方法同时考虑了位置信息和速度信息，计算量小，能高效实时地躲避移动障碍物。
本发明的第二目的在于提出一种冗余度机械臂系统。
本发明的第三目的在于提出一种计算机可读存储介质。
本发明的第四目的在于提出一种计算设备。
本发明的第一目的通过下述技术方案实现：一种基于二次规划的冗余度机械臂移动障碍物躲避方法，包括如下步骤：
1)获取移动障碍物和冗余度机械臂的运动信息，进而计算移动障碍物和冗余度机械臂脆弱点的位置和速度信息；
2)基于步骤1)计算的位置和速度信息，建立增大移动障碍物与冗余度机械臂脆弱点距离的不等式约束，并进一步建立驱使冗余度机械臂脆弱点趋向障碍物分速度反方向的不等式约束；
3)将步骤2)的两个不等式约束进行合并，进而得到一个统一的移动障碍物躲避不等式约束；
4)将步骤3)的不等式约束并入到以最小化速度范数为优化目标，以极限约束和机械臂末端姿态保持约束为约束条件的二次规划问题中；
5)将步骤4)的二次规划问题用递归神经网络求解器进行求解；
6)将步骤5)的求解结果传递给移动平台和机械臂控制器，以驱动移动平台与机械臂在完成末端任务的同时躲避移动障碍物。
优选的，步骤1)中，移动障碍物和冗余度机械臂脆弱点的位置和速度信息包括：移动障碍点与机械臂脆弱点之间的距离d，移动障碍点到机械臂脆弱点的矢量
Figure BDA0003091397780000021
由移动障碍点的速度矢量
Figure BDA0003091397780000022
分解的垂直于
Figure BDA0003091397780000023
的分矢量
Figure BDA0003091397780000024
其中，移动障碍点是移动障碍物上距离冗余度机械臂最近的点，机械臂脆弱点为冗余度机械臂或冗余度机械臂延长线上距离移动障碍物最近的点；移动障碍点的速度矢量
Figure BDA0003091397780000025
Figure BDA0003091397780000026
表示障碍点在时间点t的速度；
Figure BDA0003091397780000031
表示预测的障碍点在时间(t+dt)的速度；d为时间段；
Figure BDA0003091397780000032
表示计算的障碍点在时间t的加速度；
Figure BDA0003091397780000033
表示障碍点在时间(t-dt)的速度，R3表示
Figure BDA0003091397780000034
是一个可在三维空间内任意变化的矢量速度；α是预先设定的一个权重参数。
更进一步的，步骤2)中，增大移动障碍物与冗余度机械臂脆弱点距离的不等式约束为：
Figure BDA0003091397780000035
驱使冗余度机械臂脆弱点趋向障碍物分速度反方向的不等式约束为：
Figure BDA0003091397780000036
其中，
Figure BDA0003091397780000037
表示关节角速度；
Figure BDA0003091397780000038
是对
Figure BDA0003091397780000039
进行单位化处理；JC是机械臂脆弱点的雅可比矩阵；
Figure BDA00030913977800000310
Figure BDA00030913977800000311
是对
Figure BDA00030913977800000312
进行单位化处理；d2是机械臂的外安全阈值；
为避免角速度的不连续致使机械臂损伤，设计先减慢机械臂脆弱点的速度，然后再强制其往反方向躲避移动障碍物，用函数S(d)表示如下：
Figure BDA00030913977800000313
式中，d1是机械臂的内安全阈值，取决于机械臂连杆的半径；d0是介于d1、d2之间的安全阈值，当机械臂脆弱点与障碍物之间的距离大于d0时，S(d)随着距离的缩短而逐渐减小直到-1值，这是机械臂关节角速度的缓冲期；当机械臂脆弱点与障碍物之间的距离小于d0时，强制增大移动障碍物与机械臂脆弱点之间的距离，驱使机械臂脆弱点趋向障碍物分速度反方向；k是一个可调参数，调参数时应满足d1≤d0≤d2。
更进一步的，步骤3)中，统一的移动障碍物躲避不等式约束为：
Figure BDA0003091397780000041
其中，
JN＝λJO+(1-λ)JV，
Figure BDA0003091397780000042
Figure BDA0003091397780000043
式中，λ是一个权重参数，
Figure BDA0003091397780000044
β是一个收敛系数，
Figure BDA0003091397780000045
是
Figure BDA0003091397780000046
的模，代表移动障碍物的速度大小。
更进一步的，步骤4)中，将不等式约束并入到二次规划问题中，即：
Figure BDA0003091397780000047
Figure BDA0003091397780000048
Figure BDA0003091397780000049
Figure BDA00030913977800000410
Figure BDA00030913977800000411
其中，
Figure BDA00030913977800000412
是基于机械臂前向运动学方程的等式约束，用于对机械臂末端的运动轨迹进行限定，JE是机械臂末端轨迹的雅可比矩阵，
Figure BDA00030913977800000413
是目标轨迹rd关于机械臂关节角度的导数；
Figure BDA00030913977800000414
是用于保持机械臂末端姿态的等式约束，Jm是姿态的方向向量的雅可比矩阵；
Figure BDA00030913977800000415
表示机械臂关节角度和角速度的物理极限，
Figure BDA00030913977800000416
分别表示机械臂关节在两个不同方向的最大物理极限。
优选的，递归神经网络求解器采用线性变分不等式原对偶神经网络求解器，求解结果为机械臂的关节角速度，可用于引导脆弱点远离移动障碍物。
本发明的第二目的通过下述技术方案实现：一种冗余度机械臂系统，所述冗余度机械臂系统通过本发明第一目的所述的基于二次规划的冗余度机械臂移动障碍物躲避方法来实现在完成末端任务的同时躲避移动障碍物。
本发明的第三目的通过下述技术方案实现：一种计算机可读存储介质，存储有程序，所述程序被处理器执行时，实现本发明第一目的所述的基于二次规划的冗余度机械臂移动障碍物躲避方法。
本发明的第四目的通过下述技术方案实现：一种计算设备，包括处理器以及用于存储处理器可执行程序的存储器，所述处理器执行存储器存储的程序时，实现本发明第一目的所述的基于二次规划的冗余度机械臂移动障碍物躲避方法。
本发明相对于现有技术具有如下的优点及效果：
本发明方法充分利用了移动障碍物和冗余度机械臂的位置和速度信息，在机械臂完成末端任务的同时，能根据障碍物的速度信息驱动机械臂沿着一定方向躲避，有效提高了机械臂进入危险区域时避开移动障碍物的效率，大大降低了障碍物与机械臂碰撞的可能性。此外，本发明方法还考虑了末端执行器的姿态保持和关节的物理极限，使机械臂避障更加准确，机械臂更加安全。
附图说明
图1为本发明基于二次规划的冗余度机械臂移动障碍物躲避方法的流程图。
图2为移动障碍点和冗余度机械臂脆弱点的示意图。
图3为机械臂的危险区域以及内、外安全阈值的示意图。
图4为不等式约束
Figure BDA0003091397780000051
的几何示意图。
图5为不等式约束
Figure BDA0003091397780000052
的几何示意图。
图6为在进行三角波运动的移动障碍物的干扰下，未使用避障保护和使用本发明方法避障的机械臂在完成路径跟踪任务时的效果对比图。
图7为在进行李萨如曲线运动的移动障碍物的干扰下，未使用避障保护和使用本发明方法避障的机械臂在完成路径跟踪任务时的效果对比图。
具体实施方式
下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。
实施例1
如图1所示，本实施例提供了一种基于二次规划的冗余度机械臂移动障碍物躲避方法，包括如下步骤：
1)获取移动障碍物和冗余度机械臂的运动信息，进而计算移动障碍物和冗余度机械臂脆弱点的位置和速度信息。
这里，移动障碍物和冗余度机械臂脆弱点的位置和速度信息包括：移动障碍点与机械臂脆弱点之间的距离d，移动障碍点到机械臂脆弱点的矢量
Figure BDA0003091397780000061
由移动障碍点的速度矢量
Figure BDA0003091397780000062
分解而来的垂直于
Figure BDA0003091397780000063
的分矢量
Figure BDA0003091397780000064
其中，如图2所示，移动障碍点是移动障碍物上距离冗余度机械臂最近的点，机械臂脆弱点为冗余度机械臂或冗余度机械臂延长线上距离移动障碍物最近的点。移动障碍物和冗余度机械臂之间的距离可利用摄像头识别、3D激光雷达测距等方法来检测障碍物的位移。障碍物的移动速度可通过障碍物的位移和时间差计算得出。
移动障碍点的速度矢量
Figure BDA0003091397780000065
Figure BDA0003091397780000066
表示障碍点在时间点t的速度；
Figure BDA0003091397780000067
表示预测的障碍点在时间(t+dt)的速度；dt为一短暂的时间段，取决于位移检测器的精度；
Figure BDA0003091397780000068
表示计算的障碍点在时间t的加速度；
Figure BDA0003091397780000069
表示障碍点在时间(t-dt)的速度，R3是一个三维实数集，表示
Figure BDA0003091397780000071
是一个可在三维空间内任意变化的矢量速度；α是一个权重参数，需要根据环境提前设置。
2)基于计算的位置和速度信息，建立增大移动障碍物与冗余度机械臂脆弱点距离的不等式约束(记为不等式约束1)，并进一步建立驱使冗余度机械臂脆弱点趋向障碍物分速度反方向的不等式约束(记为不等式约束2)。
其中，不等式约束1具体如下：
Figure BDA0003091397780000072
不等式约束2具体如下：
Figure BDA0003091397780000073
其中，
Figure BDA0003091397780000074
表示关节角速度；
Figure BDA0003091397780000075
Figure BDA0003091397780000076
是对
Figure BDA0003091397780000077
进行单位化处理；JC是机械臂脆弱点的雅可比矩阵；
Figure BDA0003091397780000078
Figure BDA0003091397780000079
是对
Figure BDA00030913977800000710
进行单位化处理。
为避免角速度的不连续也即角速度突变而致使机械臂损伤，设计先减慢机械臂脆弱点的速度，然后再强制其往反方向躲避移动障碍物，用函数S(d)表示如下：
Figure BDA00030913977800000711
式中，d0是介于d1、d2之间的安全阈值，当机械臂脆弱点与障碍物之间的距离大于d0时，S(d)随着距离的缩短而逐渐减小直到-1值，这是机械臂关节角速度的缓冲期；当机械臂脆弱点与障碍物之间的距离小于d0时，强制增大移动障碍物与机械臂脆弱点之间的距离，驱使机械臂脆弱点趋向障碍物分速度反方向。
参见图3，d2是机械臂的外安全阈值，可人为设置d2的大小，设置d2主要是考虑到机械臂需要一定的空间来响应和避开障碍物；而d1是机械臂的内安全阈值(图3中的深灰色区域)，其取决于机械臂连杆的半径。当障碍点和机械臂脆弱点之间的距离小于d2时，意味着障碍点进入危险区域(图3中的浅灰色区域)，此时移动避障约束开始发挥作用，并将逃逸速度信息赋给机械臂脆弱点使其远离移动障碍物。k是一个可调参数，调参数时应满足d1≤d0≤d2。
不等式约束1的几何示意图可参见图4，图4中考虑了障碍点和脆弱点的位置信息，是不等式约束1的直观表现，因为
Figure BDA0003091397780000081
当
Figure BDA0003091397780000082
时，
Figure BDA0003091397780000083
也就是说向量
Figure BDA0003091397780000084
和向量
Figure BDA0003091397780000085
形成一个锐角或直角，这意味着脆弱点在为施加不等式约束前就是在以远离障碍物的方向运动，因此只需要让其进一步远离障碍物即可。而当
Figure BDA0003091397780000086
时，向量
Figure BDA0003091397780000087
和向量
Figure BDA0003091397780000088
形成一个钝角，即脆弱点此时在靠近障碍物，因此需要让其先放慢速度然后朝着反方向运动以远离障碍物，这一点通过函数S(d)实现。
不等式约束2的几何示意图可参见图5，图5中考虑了障碍点和脆弱点的速度信息，是不等式约束2的直观表现，它可以有效避免当障碍物的速度大于机械臂的极限速度时两者的碰撞。当
Figure BDA0003091397780000089
时，障碍点的分速度
Figure BDA00030913977800000810
与脆弱点的速度形成锐角，如果脆弱点继续以这个速度运动，障碍点早晚会赶上脆弱点并相碰撞，因此需要让脆弱点速度方向逐渐趋向于
Figure BDA00030913977800000811
的反方向。当
Figure BDA00030913977800000812
时，只需要让脆弱点的速度继续沿着这个方向进一步增大即可有效防止与障碍物的碰撞。
3)将上述两个不等式约束进行合并，进而得到一个同时考虑了位置和速度信息，统一的移动障碍物躲避不等式约束(记为不等式约束3)，表示如下：
Figure BDA00030913977800000813
其中，
JN＝λJO+(1-λ)JV，
Figure BDA0003091397780000091
Figure BDA0003091397780000092
式中，JN、g仅是为简化表达式的符号。λ是一个权重参数，可表述为
Figure BDA0003091397780000093
β是一个收敛系数，e是自然常数，
Figure BDA0003091397780000094
是
Figure BDA0003091397780000095
的模，代表移动障碍物的速度大小。λ的值随着障碍点与脆弱点之间的距离减小而增大，从而使不等式约束3侧重于增大障碍点与脆弱点之间的距离；λ的值随着障碍点的速度大小增大而减小，从而使不等式约束3侧重于改变脆弱点的速度方向。
4)将不等式约束3并入到以最小化速度范数为优化目标，以极限约束和机械臂末端姿态保持约束为约束条件的二次规划问题中，即：
Figure BDA0003091397780000096
Figure BDA0003091397780000097
Figure BDA0003091397780000098
Figure BDA0003091397780000099
Figure BDA00030913977800000910
其中，
Figure BDA00030913977800000911
是基于机械臂前向运动学方程的等式约束，用于对机械臂末端的运动轨迹进行限定，JE是机械臂末端轨迹的雅可比矩阵，
Figure BDA00030913977800000912
是目标轨迹rd关于机械臂关节角度的导数；
Figure BDA00030913977800000913
是用于保持机械臂末端姿态的等式约束；Jm是姿态的方向向量的雅可比矩阵；
Figure BDA00030913977800000914
表示机械臂关节角度和角速度的物理极限，
Figure BDA00030913977800000915
分别表示机械臂关节在两个不同方向的最大物理极限。
5)将上述二次规划问题用递归神经网络求解器进行求解。这里的递归神经网络求解器可采用线性变分不等式原对偶神经网络求解器，求解结果为机械臂的关节角速度，该关节角速度可使机械臂脆弱点躲避移动障碍物，远离危险区域。
6)将求解结果传递给移动平台和机械臂控制器，以驱动移动平台与机械臂在完成末端任务的同时躲避移动障碍物。
为验证本实施例方法在实现冗余度机械臂躲避复杂障碍物上的有效性，下面结合两个具体的实例操作加以说明。
在冗余度机械臂的运动方案实施过程中，首先，设置机械臂初始位置关节角度为θ(0)＝[262.6,260,86,228,104,138]T度。由于机械臂末端需要在移动障碍物的干扰下跟踪所需要的路径，故设置机械臂末端跟踪半径为0.12米的圆路径，内、外部安全阈值d1和d2分别设置为0.05米和0.10米，设置d1为3/25米(即k＝2.6)，权重参数α设为0.5。
在第一个实例操作中，移动障碍物进行平面三角波运动，收敛系数β设为140。在机械臂的基底坐标系中，障碍物的运动轨迹为(0.2st(0.16t)-0.08,0.0018t-0.42,0.3)米，其中，st(t)是一个周期为2π的周期函数，且
Figure BDA0003091397780000101
最终，未使用避障保护时机械臂与移动障碍物的运动轨迹如图6中的(a)图所示，使用本实施例方法时机械臂与移动障碍物的运动轨迹如图6中的(b)图所示，未使用避障保护时机械臂与障碍物之间的随时间变化的最小距离曲线图如图6中的(c)图所示，使用本实施例方法时机械臂与障碍物之间的随时间变化的最小距离曲线图如图6中的(d)图所示。
在第二个实施例中，移动障碍物进行李萨如曲线运动，收敛系数β设为60。在机械臂的基底坐标系中，障碍物的运动轨迹为(-0.12+0.13sin(0.04πt)，-0.48+0.13sin(0.08πt),0.295)米。
最终，未使用避障保护时机械臂与移动障碍物的运动轨迹如图7中的(a)图所示，使用本实施例方法时机械臂与移动障碍物的运动轨迹如图7中的(b)图所示，未使用避障保护时机械臂与障碍物之间的随时间变化的最小距离曲线图如图7中的(c)图所示，使用本实施例方法时机械臂与障碍物之间的随时间变化的最小距离曲线图如图7中的(d)图所示。
当最小距离d≤d1时，认为机械臂与障碍物会在运动过程中发生碰撞。从图6和图7可以看出，未使用避障保护时机械臂将与移动障碍物会碰撞两次，而在使用本实施例方法时，机械臂成功地避开了障碍物。此外，当障碍物在进入危险区域时(参见图6和图7中d和d1两条线的交点处)，机械臂的速度方向则会在避障约束的影响下发生变化。这种变化是根据障碍物的速度进行的，有效地防止了障碍物再次进入危险区域。
实施例2
本实施例提供了一种冗余度机械臂系统，其包括机械臂控制器、移动平台、冗余度机械臂，其中，冗余度机械臂安装在移动平台上并由移动平台带动其移动，冗余度机械臂连接机械臂控制器并由机械臂控制器控制其关节旋转角度。该冗余度机械臂系统可实现实施例1中的基于二次规划的冗余度机械臂移动障碍物躲避方法，方法具体如下：
1)获取移动障碍物和冗余度机械臂的运动信息，进而计算移动障碍物和冗余度机械臂脆弱点的位置和速度信息；
2)基于步骤1)计算的位置和速度信息，建立增大移动障碍物与冗余度机械臂脆弱点距离的不等式约束，并进一步建立驱使冗余度机械臂脆弱点趋向障碍物分速度反方向的不等式约束；
3)将步骤2)的两个不等式约束进行合并，进而得到一个统一的移动障碍物躲避不等式约束；
4)将步骤3)的不等式约束并入到以最小化速度范数为优化目标，以极限约束和机械臂末端姿态保持约束为约束条件的二次规划问题中；
5)将步骤4)的二次规划问题用递归神经网络求解器进行求解；
6)将步骤5)的求解结果传递给移动平台和机械臂控制器，从而驱动移动平台与机械臂在完成末端任务的同时躲避移动障碍物。
实施例3
本实施例提供了一种计算机可读存储介质，存储有程序，所述程序被处理器执行时，实现实施例1中的基于二次规划的冗余度机械臂移动障碍物躲避方法，具体如下：
1)获取移动障碍物和冗余度机械臂的运动信息，进而计算移动障碍物和冗余度机械臂脆弱点的位置和速度信息；
2)基于步骤1)计算的位置和速度信息，建立增大移动障碍物与冗余度机械臂脆弱点距离的不等式约束，并进一步建立驱使冗余度机械臂脆弱点趋向障碍物分速度反方向的不等式约束；
3)将步骤2)的两个不等式约束进行合并，进而得到一个统一的移动障碍物躲避不等式约束；
4)将步骤3)的不等式约束并入到以最小化速度范数为优化目标，以极限约束和机械臂末端姿态保持约束为约束条件的二次规划问题中；
5)将步骤4)的二次规划问题用递归神经网络求解器进行求解；
6)将步骤5)的求解结果传递给移动平台和机械臂控制器，以驱动移动平台与机械臂在完成末端任务的同时躲避移动障碍物。
本实施例中的计算机可读存储介质可以是磁盘、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、U盘、移动硬盘等介质。
实施例4
本实施例提供了一种计算设备，包括处理器以及用于存储处理器可执行程序的存储器，所述处理器执行存储器存储的程序时，实现实施例1中的基于二次规划的冗余度机械臂移动障碍物躲避方法，具体如下：
1)获取移动障碍物和冗余度机械臂的运动信息，进而计算移动障碍物和冗余度机械臂脆弱点的位置和速度信息；
2)基于步骤1)计算的位置和速度信息，建立增大移动障碍物与冗余度机械臂脆弱点距离的不等式约束，并进一步建立驱使冗余度机械臂脆弱点趋向障碍物分速度反方向的不等式约束；
3)将步骤2)的两个不等式约束进行合并，进而得到一个统一的移动障碍物躲避不等式约束；
4)将步骤3)的不等式约束并入到以最小化速度范数为优化目标，以极限约束和机械臂末端姿态保持约束为约束条件的二次规划问题中；
5)将步骤4)的二次规划问题用递归神经网络求解器进行求解；
6)将步骤5)的求解结果传递给移动平台和机械臂控制器，以驱动移动平台与机械臂在完成末端任务的同时躲避移动障碍物。
本实施例中所述的计算设备可以是台式电脑、笔记本电脑、智能手机、PDA手持终端、平板电脑或其他具有处理器功能的终端设备。
以上仅为本发明创造的较佳实施例而已，并不用以限制本发明创造，凡在本发明创造的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明创造的保护范围之内。

Claims (6)

1.一种基于二次规划的冗余度机械臂移动障碍物躲避方法，其特征在于，包括如下步骤：
1)获取移动障碍物和冗余度机械臂的运动信息，进而计算移动障碍物和冗余度机械臂脆弱点的位置和速度信息；
2)基于步骤1)计算的位置和速度信息，建立增大移动障碍物与冗余度机械臂脆弱点距离的不等式约束，并进一步建立驱使冗余度机械臂脆弱点趋向障碍物分速度反方向的不等式约束；
3)将步骤2)的两个不等式约束进行合并，进而得到一个统一的移动障碍物躲避不等式约束；
4)将步骤3)的不等式约束并入到以最小化速度范数为优化目标，以极限约束和机械臂末端姿态保持约束为约束条件的二次规划问题中；
5)将步骤4)的二次规划问题用递归神经网络求解器进行求解；
6)将步骤5)的求解结果传递给移动平台和机械臂控制器，以驱动移动平台与机械臂在完成末端任务的同时躲避移动障碍物；
步骤1)中，移动障碍物和冗余度机械臂脆弱点的位置和速度信息包括：移动障碍点与机械臂脆弱点之间的距离d，移动障碍点到机械臂脆弱点的矢量
Figure FDA0003635772440000011
由移动障碍点的速度矢量
Figure FDA0003635772440000012
分解的垂直于
Figure FDA0003635772440000013
的分矢量
Figure FDA0003635772440000014
其中，移动障碍点是移动障碍物上距离冗余度机械臂最近的点，机械臂脆弱点为冗余度机械臂或冗余度机械臂延长线上距离移动障碍物最近的点；移动障碍点的速度矢量
Figure FDA0003635772440000015
表示障碍点在时间点t的速度；
Figure FDA0003635772440000016
表示预测的障碍点在时间(t+dt)的速度；dt为时间段；
Figure FDA0003635772440000017
Figure FDA0003635772440000018
表示计算的障碍点在时间t的加速度；
Figure FDA0003635772440000019
表示障碍点在时间(t-dt)的速度，R3表示
Figure FDA00036357724400000110
是一个可在三维空间内任意变化的矢量速度；α是预先设定的一个权重参数；
步骤2)中，增大移动障碍物与冗余度机械臂脆弱点距离的不等式约束为：
Figure FDA0003635772440000021
驱使冗余度机械臂脆弱点趋向障碍物分速度反方向的不等式约束为：
Figure FDA0003635772440000022
其中，
Figure FDA0003635772440000023
表示关节角速度；
Figure FDA0003635772440000024
是对
Figure FDA0003635772440000025
进行单位化处理；JC是机械臂脆弱点的雅可比矩阵；
Figure FDA0003635772440000026
是对
Figure FDA0003635772440000027
进行单位化处理；d2是机械臂的外安全阈值；
为避免角速度的不连续致使机械臂损伤，设计先减慢机械臂脆弱点的速度，然后再强制其往反方向躲避移动障碍物，用函数S(d)表示如下：
Figure FDA0003635772440000028
式中，d1是机械臂的内安全阈值，取决于机械臂连杆的半径；d0是介于d1、d2之间的安全阈值，当机械臂脆弱点与障碍物之间的距离大于d0时，S(d)随着距离的缩短而逐渐减小直到-1值，这是机械臂关节角速度的缓冲期；当机械臂脆弱点与障碍物之间的距离小于d0时，强制增大移动障碍物与机械臂脆弱点之间的距离，驱使机械臂脆弱点趋向障碍物分速度反方向；k是一个可调参数，调参数时应满足d1≤d0≤d2；
步骤3)中，统一的移动障碍物躲避不等式约束为：
Figure FDA0003635772440000029
其中，
JN＝λJO+(1-λ)JV，
Figure FDA0003635772440000031
Figure FDA0003635772440000032
式中，λ是一个权重参数，
Figure FDA0003635772440000033
β是一个收敛系数，
Figure FDA0003635772440000034
是
Figure FDA0003635772440000035
的模，代表移动障碍物的速度大小。
2.根据权利要求1所述的冗余度机械臂移动障碍物躲避方法，其特征在于，步骤4)中，将不等式约束并入到二次规划问题中，即：
Figure FDA0003635772440000036
Figure FDA0003635772440000037
Figure FDA0003635772440000038
Figure FDA0003635772440000039
Figure FDA00036357724400000310
其中，
Figure FDA00036357724400000311
是基于机械臂前向运动学方程的等式约束，用于对机械臂末端的运动轨迹进行限定，JE是机械臂末端轨迹的雅可比矩阵，
Figure FDA00036357724400000312
是目标轨迹rd关于机械臂关节角度的导数；
Figure FDA00036357724400000313
是用于保持机械臂末端姿态的等式约束，Jm是姿态的方向向量的雅可比矩阵；
Figure FDA00036357724400000314
表示机械臂关节角度和角速度的物理极限，
Figure FDA00036357724400000315
分别表示机械臂关节在两个不同方向的最大物理极限。
3.根据权利要求1所述的冗余度机械臂移动障碍物躲避方法，其特征在于，递归神经网络求解器采用线性变分不等式原对偶神经网络求解器，求解结果为机械臂的关节角速度，用于引导脆弱点远离移动障碍物。
4.一种冗余度机械臂系统，其特征在于，所述冗余度机械臂系统通过权利要求1至3中任一项所述的基于二次规划的冗余度机械臂移动障碍物躲避方法来实现在完成末端任务的同时躲避移动障碍物。
5.一种计算机可读存储介质，存储有程序，其特征在于，所述程序被处理器执行时，实现权利要求1至3中任一项所述的基于二次规划的冗余度机械臂移动障碍物躲避方法。
6.一种计算设备，包括处理器以及用于存储处理器可执行程序的存储器，其特征在于，所述处理器执行存储器存储的程序时，实现权利要求1至3中任一项所述的基于二次规划的冗余度机械臂移动障碍物躲避方法。
CN202110596692.0A 2021-05-31 2021-05-31 一种基于二次规划的冗余度机械臂移动障碍物躲避方法 Active CN113276121B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110596692.0A CN113276121B (zh) 2021-05-31 2021-05-31 一种基于二次规划的冗余度机械臂移动障碍物躲避方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110596692.0A CN113276121B (zh) 2021-05-31 2021-05-31 一种基于二次规划的冗余度机械臂移动障碍物躲避方法

Publications (2)

Publication Number Publication Date
CN113276121A CN113276121A (zh) 2021-08-20
CN113276121B true CN113276121B (zh) 2022-08-09

Family

ID=77282535

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110596692.0A Active CN113276121B (zh) 2021-05-31 2021-05-31 一种基于二次规划的冗余度机械臂移动障碍物躲避方法

Country Status (1)

Country Link
CN (1) CN113276121B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113618739B (zh) * 2021-08-24 2022-07-29 深圳市优必选科技股份有限公司 一种机器人动态避障方法、装置和机器人
CN113843793B (zh) * 2021-09-21 2023-07-21 兰州大学 一种具有避障功能的移动冗余机械臂模型预测控制方法
CN114347036B (zh) * 2022-02-14 2023-10-17 北京理工大学 优化机械臂关节中心距离和关节运动范围的方法
CN116638515A (zh) * 2023-05-31 2023-08-25 重庆日联科技有限公司 一种用于铅房内机器人x光检测系统的防撞保护方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104392081A (zh) * 2014-06-30 2015-03-04 管小清 一种冗余机械臂运动障碍物避障算法
CN104760041A (zh) * 2015-03-19 2015-07-08 中山大学 一种基于突加度的障碍物躲避运动规划方法
WO2018107851A1 (zh) * 2016-12-16 2018-06-21 广州视源电子科技股份有限公司 冗余机械臂的控制方法及装置
CN111309002A (zh) * 2019-11-26 2020-06-19 华南理工大学 一种基于矢量的轮式移动机器人避障方法及系统

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106945041B (zh) * 2017-03-27 2019-08-20 华南理工大学 一种冗余度机械臂重复运动规划方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104392081A (zh) * 2014-06-30 2015-03-04 管小清 一种冗余机械臂运动障碍物避障算法
CN104760041A (zh) * 2015-03-19 2015-07-08 中山大学 一种基于突加度的障碍物躲避运动规划方法
WO2018107851A1 (zh) * 2016-12-16 2018-06-21 广州视源电子科技股份有限公司 冗余机械臂的控制方法及装置
CN111309002A (zh) * 2019-11-26 2020-06-19 华南理工大学 一种基于矢量的轮式移动机器人避障方法及系统

Also Published As

Publication number Publication date
CN113276121A (zh) 2021-08-20

Similar Documents

Publication Publication Date Title
CN113276121B (zh) 一种基于二次规划的冗余度机械臂移动障碍物躲避方法
US8160745B2 (en) Robots with occlusion avoidance functionality
Ragaglia et al. Trajectory generation algorithm for safe human-robot collaboration based on multiple depth sensor measurements
US8311731B2 (en) Robots with collision avoidance functionality
CN112809682B (zh) 机械臂避障路径规划方法、系统及存储介质
Masehian et al. Sensor-based motion planning of wheeled mobile robots in unknown dynamic environments
CN108772835B (zh) 一种障碍物与物理极限躲避方法
Safeea et al. Minimum distance calculation for safe human robot interaction
Elmokadem et al. A method for autonomous collision-free navigation of a quadrotor UAV in unknown tunnel-like environments
Xu et al. The mobile robot path planning with motion constraints based on Bug algorithm
Ding et al. Collision avoidance with proximity servoing for redundant serial robot manipulators
WO2020093253A1 (zh) 机器人的运动控制方法、控制系统和存储装置
CN114460965B (zh) 一种基于改进人工势场法的无人机三维避障方法
Damas et al. Avoiding moving obstacles: the forbidden velocity map
Gyenes et al. Motion planning for mobile robots using the safety velocity obstacles method
Lim et al. Designing path of collision avoidance for mobile manipulator in worker safety monitoring system using reinforcement learning
Kivelä et al. Redundant robotic manipulator path planning for real-time obstacle and self-collision avoidance
US20220009101A1 (en) Control device, control method, and non-transitory recording medium
Le Boudec et al. Modeling and adaptive control of redundant robots
Zhao et al. Bi-criteria acceleration level obstacle avoidance of redundant manipulator
CN114378809A (zh) 软体机器人操纵器的无奇点运动学参数化
CN113263496A (zh) 一种六自由度机械臂优化路径的方法和计算机设备
Gu et al. Dynamic obstacle avoidance of mobile robot based on adaptive velocity obstacle
Elmokadem Real-time safe navigation strategy for mobile robots in 3D: a deforming path approach
CN113146637A (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