CN112975938B - 一种基于零空间的机械臂速度层轨迹规划方法 - Google Patents

一种基于零空间的机械臂速度层轨迹规划方法 Download PDF

Info

Publication number
CN112975938B
CN112975938B CN201911274963.XA CN201911274963A CN112975938B CN 112975938 B CN112975938 B CN 112975938B CN 201911274963 A CN201911274963 A CN 201911274963A CN 112975938 B CN112975938 B CN 112975938B
Authority
CN
China
Prior art keywords
mechanical arm
obstacle
task
obstacle avoidance
barrier
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
CN201911274963.XA
Other languages
English (en)
Other versions
CN112975938A (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.)
Shenyang Institute of Automation of CAS
Original Assignee
Shenyang Institute of Automation of CAS
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 Shenyang Institute of Automation of CAS filed Critical Shenyang Institute of Automation of CAS
Priority to CN201911274963.XA priority Critical patent/CN112975938B/zh
Publication of CN112975938A publication Critical patent/CN112975938A/zh
Application granted granted Critical
Publication of CN112975938B publication Critical patent/CN112975938B/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/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
    • B25J9/1666—Avoiding collision or forbidden zones

Landscapes

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

Abstract

本发明涉及一种基于零空间的协作型机械臂速度层轨迹规划方法。设计了吸引速度函数和排斥速度函数，避免了势场法在特殊场景下存在的几种问题；根据机械臂和障碍物移动方向与夹角判断障碍物运动趋势，并通过在原本避障方向的基础上叠加一个额外的方向向量实现动态避障轨迹规划；利用机械臂零空间的特性，将末端作业任务和避障任务划分优先级处理，通过将低优先级任务映射到高优先级任务的零空间中，机械臂可以在满足高优先级任务的条件下完成低优先级的任务。本发明基于机械臂的零空间特性，能够实现在动态非结构化环境下，机械臂在保证自身和环境安全的条件下同时完成机械臂末端的作业任务和本体的避障任务。

Description

一种基于零空间的机械臂速度层轨迹规划方法
技术领域
本发明涉及智能机器人技术领域，尤其涉及一种机械臂在动态非结构化环境下的速度层轨迹规划方法。
背景技术
协作型机器人具有人机融合、安全易用、灵敏精准及灵活通用等特征，不仅适应工业领域中小批量、多品种、用户定制的柔性制造需求，在服务、医疗和电力等领域也有潜在的应用前景，已经成为引领下一代工业机器人发展的重要方向。在动态非结构化的任务环境中，如何使其能够根据环境变化实时动态调整运动轨迹，实现自主避障并到达目标位置，这是协作型机器人应用研究中尚待解决的一个关键问题。
当机械臂感知到工作空间中的障碍物并且障碍物有可能干扰到机械臂的作业任务时，机械臂应该及时地调整自己的运动轨迹，从而避免发生碰撞。机械臂自身可以看做是由一系列连杆串联而成的运动链，因此在轨迹规划过程中除了需要考虑末端执行器的避障外，还需要考虑本体连杆的避障策略。
发明内容
本发明的目的是提供一种基于零空间的机械臂速度层轨迹规划方法。
本发明为实现上述目的所采用的技术方案是：
一种基于零空间的机械臂速度层轨迹规划方法，包括以下步骤：
1)根据机械臂末端执行器的坐标判断机械臂是否达到目标点，若达到目标点，结束任务，否则，进行步骤2)；
2)根据机械臂与障碍物的最近距离，判断机械臂是否进入障碍物范围，若进入障碍物范围，进入步骤3)，否则，进入步骤7)；
3)根据机械臂移动的单位方向向量与障碍物移动的单位方向向量所成的夹角，判断障碍物的运动趋势，若障碍物接近机械臂，进行步骤4)，若障碍物远离机械臂，进入步骤5)；
4)求解机械臂的排斥速度矢量，并修正机械臂的避障方向，进入步骤6)；
5)求解机械臂的排斥速度矢量，进入步骤6)；
6)根据优先级策略计算吸引速度矢量和机械臂各关节控制量，进入步骤9)；
7)求解机械臂的吸引速度矢量，进入步骤8)；
8)计算机械臂各关节控制量，进入步骤9)；
9)上位机根据机械臂各关节控制量，控制机械臂运动，进入步骤1)。
步骤3)为：
机械臂移动的单位方向向量为
Figure BDA0002315318740000021
障碍物的最近点到机械臂的单位方向向量为
Figure BDA0002315318740000022
障碍物移动的单位方向向量为
Figure BDA0002315318740000023
当
Figure BDA0002315318740000024
时，障碍物朝向远离机械臂的方向移动，不需要调整避障方向，此时
Figure BDA0002315318740000025
当
Figure BDA0002315318740000026
时，障碍物朝接近机械臂的方向移动。
所述排斥速度表达式为：
Figure BDA0002315318740000027
其中，
Figure BDA0002315318740000028
为排斥速度矢量，
Figure BDA0002315318740000029
为机械臂的避障方向，d0为机械臂本体与障碍物最近距离的大小，dm为障碍物作用范围的半径大小，γ为人为设定的函数参数。
所述修正机械臂的避障方向为在原避障方向的基础上，叠加一个额外的方向向量，具体为：
机械臂移动的单位方向向量为
Figure BDA00023153187400000210
障碍物的最近点到机械臂的单位方向向量为
Figure BDA00023153187400000211
障碍物移动的单位方向向量为
Figure BDA00023153187400000212
调整后的避障方向向量为在
Figure BDA00023153187400000213
的基础上叠加方向向量
Figure BDA00023153187400000214
其中
Figure BDA00023153187400000215
Figure BDA00023153187400000216
为方向垂直于
Figure BDA00023153187400000217
和
Figure BDA00023153187400000218
所构成的平面的方向向量，即
Figure BDA00023153187400000219
Figure BDA00023153187400000220
为方向垂直于
Figure BDA00023153187400000221
和
Figure BDA00023153187400000222
所构成的平面的方向向量，并且与
Figure BDA00023153187400000223
的夹角小于90°。
所述优先级策略为：将机械臂的避障任务设置为高优先级，机械臂末端执行器作业任务即机械臂的行为任务设置为低优先级，同时满足机械臂末端执行器作业任务以及机械臂本体避障任务的表达式为：
Figure BDA0002315318740000031
其中，
Figure BDA0002315318740000032
为机械臂避障任务所需的控制量，
Figure BDA0002315318740000033
为低优先级任务即目标任务减去由避障任务所造成的运动耦合后的控制量，
Figure BDA0002315318740000034
的秩对应避障任务所对应的零空间自由度的大小，
Figure BDA0002315318740000035
的秩则对应末端目标任务所需的自由度大小，
Figure BDA0002315318740000036
为机械臂各关节的控制量，
Figure BDA0002315318740000037
为避障点处雅克比矩阵的伪逆，
Figure BDA0002315318740000038
为排斥速度矢量，I为单位矩阵，J0为避障点处的雅克比矩阵，Je为作业任务所对应的雅克比矩阵，
Figure BDA0002315318740000039
为吸引速度矢量，αh为平滑因子，其大小取决于机械臂与障碍物之间的最小距离，表达式为：
Figure BDA00023153187400000310
其中γ和β为可调的参数，x为机械臂与障碍物之间的最小距离。
所述吸引速度表达式为：
Figure BDA00023153187400000311
其中，
Figure BDA00023153187400000312
为吸引速度矢量，da为
Figure BDA00023153187400000313
的大小增大到常数的阈值，d为机械臂末端与目标点的距离。
本发明具有以下有益效果及优点：
1.本发明能够同时满足机械臂末端和本体连杆的避障，确机械臂在保证自身和环境安全的条件下完成作业任务。
2.本发明对局部环境下的反应式轨迹规划算法进行了改进，能够有效应对动态障碍物在运动过程中的复杂性和不可预测性。
3.运算速度快。本发明将控制量叠加在速度层，避免了机械臂动力学模型在计算过程中带来的参数获取以及计算复杂等问题。
附图说明
图1为本发明实施的方法流程图；
图2为本发明的避障方向示意图；
图3为机械臂的避障示意图。
具体实施方式
下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。
本发明涉及智能机器人技术领域，尤其涉及一种机械臂在动态非结构化环境下的速度层轨迹规划方法。在本实例中，如图1所示，包括以下步骤：
1)根据机械臂末端执行器的坐标判断机械臂是否达到目标点，若达到目标点，结束任务，否则，进行步骤2)；
2)根据机械臂与障碍物的最近距离，判断机械臂是否进入障碍物范围，若进入障碍物范围，进入步骤3)，否则，进入步骤7)；
3)根据机械臂移动的单位方向向量与障碍物移动的单位方向向量所成的夹角，判断障碍物的运动趋势，若障碍物接近机械臂，进行步骤4)，若障碍物远离机械臂，进入步骤5)；
4)求解机械臂的排斥速度矢量，并修正机械臂的避障方向，进入步骤6)；
5)求解机械臂的排斥速度矢量，进入步骤6)；
6)根据优先级策略计算吸引速度矢量和机械臂各关节控制量，进入步骤9)；
7)求解机械臂的吸引速度矢量，进入步骤8)；
8)计算机械臂各关节控制量，进入步骤9)；
9)上位机根据机械臂各关节控制量，控制机械臂运动，进入步骤1)。
考虑到机械臂离目标点较远可能产生较大的吸引速度的问题，设计了吸引速度函数，其表达式如下。采用饱和函数的思想，随着机械臂末端与目标点的距离增大，吸引速度的大小逐渐趋于常数。与一般的饱和函数不同，吸引速度表达式在函数拐点处做了修正，使机械臂在该点处的速度更为平滑。
Figure BDA0002315318740000051
考虑到目标点处有障碍物存在可能导致目标点不可达的问题，设计了排斥速度函数，其中dm为障碍物作用范围的大小。随着机械臂本体与障碍物的距离减小，其受到的排斥速度大小也趋于稳定。因此当目标点附近存在障碍物时，其排斥速度不会随着距离的缩短而迅速增大，减缓了排斥速度场的作用。当机械臂末端与目标点的距离小于机械臂末端与障碍物的距离时，可以通过减小排斥速度大小或者移除排斥作用等策略来趋使机械臂到达目标点。随着机械臂本体与障碍物的距离增大，排斥速度大小也逐渐趋于零。
Figure BDA0002315318740000052
如图2所示，所述动态轨迹规划方法通过在原本避障方向的基础上叠加一个额外的方向向量实现：
假设机械臂移动的单位方向向量为
Figure BDA0002315318740000053
障碍物的最近点到机械臂本体的单位方向向量为
Figure BDA0002315318740000054
障碍物移动的单位方向向量为
Figure BDA0002315318740000055
当
Figure BDA0002315318740000056
时，障碍物朝向远离机械臂的方向移动，因此不需要调整避障方向，此时
Figure BDA0002315318740000057
当
Figure BDA0002315318740000058
时，障碍物朝向靠近机械臂的方向移动。如果不调整机械臂的避障方向，若障碍物的移动速度大于机械臂的移动速度，则有可能发生碰撞。调整后的避障方向向量为在
Figure BDA0002315318740000059
的基础上叠加方向向量
Figure BDA00023153187400000510
其中
Figure BDA00023153187400000511
Figure BDA00023153187400000512
的方向垂直于
Figure BDA00023153187400000513
和
Figure BDA00023153187400000514
所构成的平面，即
Figure BDA00023153187400000515
Figure BDA00023153187400000516
的方向垂直于
Figure BDA00023153187400000517
和
Figure BDA00023153187400000518
所构成的平面，并且与
Figure BDA00023153187400000519
的夹角小于90°。
机械臂对障碍物的避障示意图如图3所示。其中dm为障碍物的作用范围，v0为障碍物对机械臂本体的排斥速度，
Figure BDA00023153187400000520
为末端执行器期望轨迹所对应的速度量。同时满足机械臂末端执行器作业任务以及机械臂本体避障任务的表达式，如下所示。
Figure BDA0002315318740000061
其中，
Figure BDA0002315318740000062
为机械臂各关节的控制量，
Figure BDA0002315318740000063
为避障点处雅克比矩阵的伪逆，
Figure BDA0002315318740000064
为排斥速度矢量，I为单位矩阵，J0为避障点处的雅克比矩阵，Je为作业任务所对应的雅克比矩阵，
Figure BDA0002315318740000065
为吸引速度矢量。
Figure BDA0002315318740000066
其中，γ和β为可调的参数，x为机械臂与障碍物之间的最小距离。

Claims (3)

1.一种基于零空间的机械臂速度层轨迹规划方法，其特征在于,包括以下步骤：
1)根据机械臂末端执行器的坐标判断机械臂是否达到目标点，若达到目标点，结束任务，否则，进行步骤2)；
2)根据机械臂与障碍物的最近距离，判断机械臂是否进入障碍物范围，若进入障碍物范围，进入步骤3)，否则，进入步骤7)；
3)根据机械臂移动的单位方向向量与障碍物移动的单位方向向量所成的夹角，判断障碍物的运动趋势，若障碍物接近机械臂，进行步骤4)，若障碍物远离机械臂，进入步骤5)；
4)求解机械臂的排斥速度矢量，并修正机械臂的避障方向，进入步骤6)；
5)求解机械臂的排斥速度矢量，进入步骤6)；
6)根据优先级策略计算吸引速度矢量和机械臂各关节控制量，进入步骤9)；
7)求解机械臂的吸引速度矢量，进入步骤8)；
8)计算机械臂各关节控制量，进入步骤9)；
9)上位机根据机械臂各关节控制量，控制机械臂运动，进入步骤1)；
所述优先级策略为：将机械臂的避障任务设置为高优先级，机械臂末端执行器作业任务即机械臂的行为任务设置为低优先级，同时满足机械臂末端执行器作业任务以及机械臂本体避障任务的表达式为：
Figure FDA0003510788480000011
其中，
Figure FDA0003510788480000012
为机械臂避障任务所需的控制量，
Figure FDA0003510788480000013
为低优先级任务即目标任务减去由避障任务所造成的运动耦合后的控制量，
Figure FDA0003510788480000014
的秩对应避障任务所对应的零空间自由度的大小，
Figure FDA0003510788480000015
的秩则对应末端目标任务所需的自由度大小，
Figure FDA0003510788480000016
为机械臂各关节的控制量，
Figure FDA0003510788480000017
为避障点处雅克比矩阵的伪逆，
Figure FDA0003510788480000018
为排斥速度矢量，I为单位矩阵，J0为避障点处的雅克比矩阵，Je为作业任务所对应的雅克比矩阵，
Figure FDA0003510788480000019
为吸引速度矢量，αh为平滑因子，其大小取决于机械臂与障碍物之间的最小距离，表达式为：
Figure FDA0003510788480000021
其中γ和β为可调的参数，x为机械臂与障碍物之间的最小距离；
所述排斥速度表达式为：
Figure FDA0003510788480000022
其中，
Figure FDA0003510788480000023
为排斥速度矢量，
Figure FDA0003510788480000024
为机械臂的避障方向，d0为机械臂本体与障碍物最近距离的大小，dm为障碍物作用范围的半径大小，γ为人为设定的函数参数；所述吸引速度表达式为：
Figure FDA0003510788480000025
其中，
Figure FDA0003510788480000026
为吸引速度矢量，da为
Figure FDA0003510788480000027
的大小增大到常数的阈值，d为机械臂末端与目标点的距离。
2.根据权利要求1所述的一种基于零空间的机械臂速度层轨迹规划方法，其特征在于,步骤3)为：
机械臂移动的单位方向向量为
Figure FDA0003510788480000028
障碍物的最近点到机械臂的单位方向向量为
Figure FDA0003510788480000029
障碍物移动的单位方向向量为
Figure FDA00035107884800000210
当
Figure FDA00035107884800000211
时，障碍物朝向远离机械臂的方向移动，不需要调整避障方向，此时
Figure FDA00035107884800000212
当
Figure FDA00035107884800000213
时，障碍物朝接近机械臂的方向移动。
3.根据权利要求1所述的一种基于零空间的机械臂速度层轨迹规划方法，其特征在于,所述修正机械臂的避障方向为在原避障方向的基础上，叠加一个额外的方向向量，具体为：
机械臂移动的单位方向向量为
Figure FDA00035107884800000214
障碍物的最近点到机械臂的单位方向向量为
Figure FDA00035107884800000215
障碍物移动的单位方向向量为
Figure FDA00035107884800000216
调整后的避障方向向量为在
Figure FDA00035107884800000217
的基础上叠加方向向量
Figure FDA0003510788480000031
其中
Figure FDA0003510788480000032
Figure FDA0003510788480000033
为方向垂直于
Figure FDA0003510788480000034
和
Figure FDA0003510788480000035
所构成的平面的方向向量，即
Figure FDA0003510788480000036
Figure FDA0003510788480000037
为方向垂直于
Figure FDA0003510788480000038
和
Figure FDA0003510788480000039
所构成的平面的方向向量，并且与
Figure FDA00035107884800000310
的夹角小于90°。
CN201911274963.XA 2019-12-12 2019-12-12 一种基于零空间的机械臂速度层轨迹规划方法 Active CN112975938B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911274963.XA CN112975938B (zh) 2019-12-12 2019-12-12 一种基于零空间的机械臂速度层轨迹规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911274963.XA CN112975938B (zh) 2019-12-12 2019-12-12 一种基于零空间的机械臂速度层轨迹规划方法

Publications (2)

Publication Number Publication Date
CN112975938A CN112975938A (zh) 2021-06-18
CN112975938B true CN112975938B (zh) 2022-04-05

Family

ID=76331594

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911274963.XA Active CN112975938B (zh) 2019-12-12 2019-12-12 一种基于零空间的机械臂速度层轨迹规划方法

Country Status (1)

Country Link
CN (1) CN112975938B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113580130B (zh) * 2021-07-20 2022-08-30 佛山智能装备技术研究院 六轴机械臂避障控制方法、系统及计算机可读存储介质
CN113858196A (zh) * 2021-09-26 2021-12-31 中国舰船研究设计中心 一种考虑机器人避碰轨迹的机器人拆卸序列规划方法
CN113733106B (zh) * 2021-11-05 2022-04-15 深圳市优必选科技股份有限公司 力控仿人机器人的全身控制方法、装置、设备及介质
CN114083537B (zh) * 2021-11-30 2023-07-07 深圳市优必选科技股份有限公司 机械臂夹持控制方法、装置、机器人和可读存储介质

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101075352A (zh) * 2007-06-29 2007-11-21 中国科学院计算技术研究所 一种用于动态实体的分层避障方法
CN108326849A (zh) * 2018-01-04 2018-07-27 浙江大学 一种基于改进人工势场法的多自由度机械臂动态避障路径规划方法
CN108555911A (zh) * 2018-04-22 2018-09-21 北京工业大学 基于虚拟推力的遥操作机械臂三维避障方法
CN108873915A (zh) * 2018-10-12 2018-11-23 湖南万为智能机器人技术有限公司 动态避障方法及其全向安防机器人

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101075352A (zh) * 2007-06-29 2007-11-21 中国科学院计算技术研究所 一种用于动态实体的分层避障方法
CN108326849A (zh) * 2018-01-04 2018-07-27 浙江大学 一种基于改进人工势场法的多自由度机械臂动态避障路径规划方法
CN108555911A (zh) * 2018-04-22 2018-09-21 北京工业大学 基于虚拟推力的遥操作机械臂三维避障方法
CN108873915A (zh) * 2018-10-12 2018-11-23 湖南万为智能机器人技术有限公司 动态避障方法及其全向安防机器人

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Research on Dynamic Trajectory Planning of Collaborative Robots Base on RRT-RV Algorithm;Guodong Liu等;《PROCEEDINGS OF 2018 IEEE 4TH INFORMATION TECHNOLOGY AND MECHATRONICS ENGINEERING CONFERENCE (ITOEC 2018)》;20181231;1020-1023 *
一种协作型机器人运动性能分析与仿真;胡明伟等;《智能系统学报》;20170228;75-81 *

Also Published As

Publication number Publication date
CN112975938A (zh) 2021-06-18

Similar Documents

Publication Publication Date Title
CN112975938B (zh) 一种基于零空间的机械臂速度层轨迹规划方法
CN108237534B (zh) 一种连续型机械臂的空间避障轨迹规划方法
JP4903173B2 (ja) 衝突回避機能を有するロボット
CN110421556B (zh) 一种冗余双臂服务机器人实时避碰的轨迹规划方法及平稳运行方法
CN111230873B (zh) 一种基于示教学习的协作搬运控制系统及方法
CN109901397B (zh) 一种使用粒子群优化算法的机械臂逆运动学方法
Zhang et al. Path planning of six-DOF serial robots based on improved artificial potential field method
CN113601512B (zh) 一种机械臂奇异点的通用规避方法与系统
CN114571469B (zh) 一种机械臂零空间实时避障控制方法及系统
CN111309002A (zh) 一种基于矢量的轮式移动机器人避障方法及系统
Walker et al. Robot-human handovers based on trust
Shah et al. Modelling and optimization of robotic manipulator mechanism for computed tomography guided medical procedure
Wang et al. A Bio-inspired trajectory planning method for robotic manipulators based on improved bacteria foraging optimization algorithm and tau theory
Li et al. Human-robot collaborative manipulation with the suppression of human-caused disturbance
CN114063621B (zh) 一种轮式机器人编队跟踪与避障控制方法
CN112077841B (zh) 一种提升机器人手臂操纵精度的多关节联动方法及系统
Vergara et al. Generating reactive approach motions towards allowable manifolds using generalized trajectories from demonstrations
Man et al. Intelligent Motion Control Method Based on Directional Drive for 3-DOF Robotic Arm
CN116766181B (zh) 一种基于全驱系统理论的机械臂主动柔顺控制方法
Zheng et al. Real-time whole-body obstacle avoidance for 7-DOF redundant manipulators
Ding et al. Task-Oriented Adaptive Position/Force Control for Robotic Systems Under Hybrid Constraints
Li et al. Real-time flow optimization of hydraulic manipulator with one degree of redundancy considering joint limit constraint
Tu et al. Whole-body control for velocity-controlled mobile collaborative robots using coupling dynamic movement primitives
Soucy et al. Flexible fuzzy logic control for collision-free manipulator operation
Li et al. Research on Obstacle Avoidance of a 7-DOF Manipulator

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