CN107966907B - 一种应用于冗余度机械臂的障碍物躲避解决方法 - Google Patents
一种应用于冗余度机械臂的障碍物躲避解决方法 Download PDFInfo
- Publication number
- CN107966907B CN107966907B CN201711240667.9A CN201711240667A CN107966907B CN 107966907 B CN107966907 B CN 107966907B CN 201711240667 A CN201711240667 A CN 201711240667A CN 107966907 B CN107966907 B CN 107966907B
- Authority
- CN
- China
- Prior art keywords
- obstacle
- point
- obstacle avoidance
- representing
- vector
- 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
- 238000013528 artificial neural network Methods 0.000 claims abstract description 14
- 239000011159 matrix material Substances 0.000 claims abstract description 14
- 238000009795 derivation Methods 0.000 claims abstract description 5
- 238000004422 calculation algorithm Methods 0.000 claims description 28
- 238000000034 method Methods 0.000 claims description 27
- 239000012636 effector Substances 0.000 claims description 7
- 230000008569 process Effects 0.000 claims description 3
- 238000009499 grossing Methods 0.000 claims description 2
- 230000007246 mechanism Effects 0.000 claims description 2
- 238000005094 computer simulation Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 6
- 238000004364 calculation method Methods 0.000 description 4
- 230000008859 change Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
Landscapes
- Engineering & Computer Science (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Numerical Control (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种应用于冗余度机械臂的障碍物躲避解决方法,包括以下步骤:S1、通过建立机械臂的D‑H矩阵获得机械臂的正运动学模型,并对其求导后建立速度层的目标轨迹等式约束指标;S2、建立基于矢量的障碍物躲避不等式约束指标;S3、将步骤S1建立的速度层的目标轨迹等式约束指标和步骤S2建立的基于矢量的障碍物躲避不等式约束指标写为统一形式的二次规划问题;S4、将步骤S3中统一形式的二次规划问题转化为线性变分不等式;S5、使用原对偶神经网络求解器求解步骤S4中的线性变分不等式;S6、将步骤S5中原对偶神经网络求解器求解出来的机械臂关节角控制变量输出到机械臂以实现控制冗余度机械臂障碍物躲避。
Description
技术领域
本发明涉及冗余度机械臂领域,具体涉及一种应用于冗余度机械臂的障碍物躲避解决方法。
背景技术
冗余度机械臂是指机械臂的自由度数多余完成任务所必须的自由度数,由于具有更多的自由度,冗余度机械臂在完成末端执行器的主要任务时,还可以完成诸如躲避障碍物、关机极限位置、机械臂奇异状态等额外任务。近年来,冗余度机械臂在生活和工业上有着越来越多的应用。冗余度机械臂有着冗余的自由度,因此能在完成主任务(如轨迹跟踪)的同时能完成障碍物躲避的子任务,对于机械臂来说,在执行主任务时,躲避障碍物十分有必要。这是因为在机械臂一般的应用场景中几乎都存在着障碍物,如果在机械的工作环境中有障碍物,而机械臂在执行过程中无法避开障碍物时,与障碍物发生的碰撞将导致机械臂的损坏或障碍物的损坏。因此障碍物躲避算法十分有意义。传统的障碍物躲避算法有基于人工势场的障碍物躲避算法。但是,基于人工势场的障碍物躲避算法更适用于移动平台而不是冗余度机械臂。另外,目前有的障碍物躲避算法采用了基于障碍物-机械臂距离计算并设置避障距离的避障算法。这种算法是基于二次规划的,但是存在不能有效地避开障碍物、计算精度低、计算时间长的问题。
发明内容
本发明的目的是针对现有技术的不足,提供了一种应用于冗余度机械臂的障碍物躲避解决方法,所述方法通过设计基于矢量的障碍物躲避算法,实现了冗余度机械臂的障碍物躲避可行空间与理想的障碍物躲避空间的一致,不仅适用于固定的冗余度机械臂,同时适用于移动的冗余度机械臂。
本发明的目的可以通过如下技术方案实现:
一种应用于冗余度机械臂的障碍物躲避解决方法,所述方法包括以下步骤:
S1、通过建立机械臂的D-H矩阵获得机械臂的正运动学模型,并对其求导后建立速度层的目标轨迹等式约束指标;
S2、建立基于矢量的障碍物躲避不等式约束指标;
S3、将步骤S1建立的速度层的目标轨迹等式约束指标和步骤S2建立的基于矢量的障碍物躲避不等式约束指标写为统一形式的二次规划问题;
S4、将步骤S3中统一形式的二次规划问题转化为线性变分不等式;
S5、使用原对偶神经网络求解器求解步骤S4中的线性变分不等式;
S6、将步骤S5中原对偶神经网络求解器求解出来的机械臂关节角控制变量输出到机械臂以实现控制冗余度机械臂障碍物躲避。
所述应用于冗余度机械臂的障碍物躲避解决方法通过对每一个机械臂连杆计算其障碍物点O与脆弱点C的距离来实现多连杆机械臂的障碍物躲避。
所述应用于冗余度机械臂的障碍物躲避解决方法不仅适用于单点避障,同时可以对多点依次计算O-C距离实现多点避障;或是将各种形态的障碍物抽象为多个点,用上述方法实现多点避障。
所述应用于冗余度机械臂的障碍物躲避解决方法不仅适用于冗余度机械臂,同时适用于移动冗余度机械臂、多连杆机构以及任何能以D-H建模法建模的具有自由度的机械来实现障碍物躲避,只需将其D-H模型带入该方法中。
本发明与现有技术相比,具有如下优点和有益效果:
1、本发明采用了原对偶神经网络求解器,相比于传统的伪逆矩阵算法计算速度更快,精度更高。
2、本发明采用了基于矢量的障碍物躲避算法,相比于传统的基于符号函数的障碍物躲避算法,在避障成功率、避障后机械臂的执行精度、关节角控制量求解时间三个方面有着巨大的提升。
附图说明
图1为本发明实施例应用于冗余度机械臂的障碍物躲避解决方法的流程图。
图2为理想的障碍物躲避算法中脆弱点C的可行空间示意图。
图3为本发明实施例应用于冗余度机械臂的障碍物躲避解决方法的原理图。
图4为本发明实施例应用到的固定冗余度机械臂示意图。
图5为本发明实施例应用到的移动冗余度机械臂示意图。
图6为未采用本发明实施例障碍物躲避解决方法的移动冗余机械臂的计算机仿真结果图。
图7为未采用本发明实施例障碍物躲避解决方法的O-C点距离的计算机仿真结果图。
图8为采用了本发明实施例障碍物躲避解决方法的移动冗余机械臂的计算机仿真结果图。
图9为采用了本发明实施例障碍物躲避解决方法的O-C点距离的计算机仿真结果图。
具体实施方式
下面结合实施例及附图对本发明作进一步详细的描述,但本发明的实施方式不限于此。
实施例:
本实施例提供了一种应用于冗余度机械臂的障碍物躲避解决方法,所述方法的流程图如图1所示,包括以下步骤:
S1、通过建立机械臂的D-H矩阵获得机械臂的正运动学模型,并对其求导后建立速度层的目标轨迹等式约束指标;
具体地,给图4所示的冗余度机械臂建立D-H模型,给移动平台建立D-H模型,将冗余度机械臂与移动平台的D-H模型结合在一起,建立起如图5所示的移动冗余度机械臂D-H模型,并对其求导后建立的速度层的目标轨迹等式约束指标为:
S2、建立基于矢量的障碍物躲避不等式约束指标;
首先我们说明理想的躲避障碍物可行空间:理想的脆弱点可行空间如图2所示。图中,X-Y-Z坐标系为机械臂的工作空间,O点为工作空间中一障碍物,C点为机械臂上的脆弱点(为使示意图简洁,机械臂省略不画出,注意,机械臂上可能有多个脆弱点,对每个脆弱点运用该算法,以实现避障功能)设此时OC点间距离进入距离d2(设定的开始执行障碍物躲避算法时障碍物点O与脆弱点C的距离)。面P为以O点为圆心、OC为半径的球位于点C的切面。理想的可行空间为面P的在O点另一侧的空间。即机械臂上的脆弱点点C向面P的外侧或在面P上运动就等价于OC点间的距离不会变大,亦即机械臂不会与障碍物点发生碰撞。所以,面P的外侧或在面P上这一空间是脆弱点可行空间的全集。
为充分利用脆弱点C的躲避障碍物可行空间,本实施例提出了基于矢量的碍物躲避指标。具体过程为:
S2.1、首先寻找到障碍物上距离机械臂最近的一点,即障碍物点O,以及机械臂上距离障碍物点O最近的一点,即脆弱点C;
S2.2、通过以下算法原理建立基于矢量的障碍物躲避不等式约束指标:
其中,J'O定义为 表示从障碍物点O指向脆弱点C的矢量,JC表示脆弱点C的雅克比矩阵,J'O∈R1×n,表示机械臂关节角的角速度以及移动平台的驱动轮角速度,ν'为障碍物躲避的参考矢量,其定义为:ν'=s(d)·max(J'Oθ|d=d2,0),θ表示机械臂关节角的移动角度以及移动平台的驱动轮移动角度,函数max(·,·)表示两个数中取最大值,d表示障碍物点O与脆弱点C的距离,d2表示设定的开始执行障碍物躲避算法时障碍物点O与脆弱点C的距离,光滑函数s(d)定义如下:
其中,d1表示设定的障碍物点O与脆弱点C的最小距离,d2表示设定的开始执行障碍物躲避算法时障碍物点O与脆弱点C的距离。当障碍物躲避算法采用了光滑函数s(d)时,一旦机械臂与障碍物间的距离进入缓冲区[d1,d2],那么将会给机械臂一个逐渐限制运动的约束,这样一来就能避免机械臂进入距离d2时发生急停的现象。
其中,这里矢量为机械臂上脆弱点C的速度矢量,若令即为其物理意义为矢量和矢量之间的夹角小于等于90°,参照图3可知,当脆弱点C满足时就满足矢量和矢量之间的夹角小于等于90°,那么也就满足C点向面P的外侧或在面P上运动,即C点在算法中的可行空间为C点避障的可行的全集。如此一来,该算法就实现了理想的障碍物躲避。
S3、将步骤S1建立的速度层的目标轨迹等式约束指标和步骤S2建立的基于矢量的障碍物躲避不等式约束指标写为统一形式的二次规划问题;
所述统一形式的二次规划问题为:
S4、将步骤S3中统一形式的二次规划问题转化为线性变分不等式;
为了解决二次规划问题,设定原对偶矢量u*,满足以下条件:
其中E为单位矩阵,u+、u-分别为u的上下数值极限,由机械臂的物理参数以及障碍物躲避算法决定,其它变量与前文的定义一致。
分段线性变分不等式可以转化为以下分段线性投影方程:
PΩ(u-(Mu+q))-u=0
其中,函数PΩ为分段线性投影算子,投影到Ω,另外矩阵M、矢量u与前文定义一致。
S5、使用原对偶神经网络求解器求解步骤S4中的线性变分不等式;
所述原对偶神经网络求解器为:
S6、将步骤S5中原对偶神经网络求解器求解出来的机械臂关节角控制变量输出到机械臂以实现控制冗余度机械臂障碍物躲避。
最终对该基于矢量的障碍物躲避算法的计算机仿真结果如图8、图9所示,作为对比,图6、图7为未采用障碍物躲避算法的计算机仿真结果,在图8中,应用了基于矢量的障碍物躲避算法的冗余移动机械臂成功地躲避开了障碍物点,图9显示O-C点即机械臂障碍物点间距离始终大于预设的距离d1。而图6中未采用该算法的冗余移动机械臂与障碍物点发生碰撞,图7显示在第2秒与第3秒之间O-C点距离小于d1,发生碰撞。
以上所述,仅为本发明专利较佳的实施例,但本发明专利的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明专利所公开的范围内,根据本发明专利的技术方案及其发明专利构思加以等同替换或改变,都属于本发明专利的保护范围。
Claims (5)
1.一种应用于冗余度机械臂的障碍物躲避解决方法,其特征在于,所述方法包括以下步骤:
S1、通过建立机械臂的D-H矩阵获得机械臂的正运动学模型,并对其求导后建立速度层的目标轨迹等式约束指标;
S2、建立基于矢量的障碍物躲避不等式约束指标;
S3、将步骤S1建立的速度层的目标轨迹等式约束指标和步骤S2建立的基于矢量的障碍物躲避不等式约束指标写为统一形式的二次规划问题;
S4、将步骤S3中统一形式的二次规划问题转化为线性变分不等式;
S5、使用原对偶神经网络求解器求解步骤S4中的线性变分不等式;
S6、将步骤S5中原对偶神经网络求解器求解出来的机械臂关节角控制变量输出到机械臂以实现控制冗余度机械臂障碍物躲避;
所述步骤S2的具体过程为:
S2.1、首先寻找到障碍物上距离机械臂最近的一点,即障碍物点O,以及机械臂上距离障碍物点O最近的一点,即脆弱点C;
S2.2、通过以下算法原理建立基于矢量的障碍物躲避不等式约束指标:
其中,J'O定义为 表示从障碍物点O指向脆弱点C的矢量,JC表示脆弱点C的雅克比矩阵,J'O∈R1×n,表示机械臂关节角的角速度以及移动平台的驱动轮角速度,ν'=s(d)·max(J'Oθ|d=d2,0),θ表示机械臂关节角的移动角度以及移动平台的驱动轮移动角度,函数max(·,·)表示两个数中取最大值,d表示障碍物点O与脆弱点C的距离,d2表示设定的开始执行障碍物躲避算法时障碍物点O与脆弱点C的距离,光滑函数s(d)定义如下:
其中,d1表示设定的障碍物点O与脆弱点C的最小距离,d2表示设定的开始执行障碍物躲避算法时障碍物点O与脆弱点C的距离;
步骤S3中,所述统一形式的二次规划问题为:
其中,表示机械臂关节角的角速度以及移动平台的驱动轮角速度,JE表示机械臂末端执行器的雅克比矩阵,J'O定义为 表示从障碍物点O指向脆弱点C的矢量,JC表示脆弱点C的雅克比矩阵,表示速度层的机械臂末端执行器的目标轨迹;
步骤S1中建立的速度层的目标轨迹等式约束指标为:
4.根据权利要求1所述的一种应用于冗余度机械臂的障碍物躲避解决方法,其特征在于:所述应用于冗余度机械臂的障碍物躲避解决方法不仅适用于单点避障,同时能够对多点依次计算障碍物点O与脆弱点C的距离实现多点避障,或是将各种形态的障碍物抽象为多个点,用上述方法实现多点避障。
5.根据权利要求1所述的一种应用于冗余度机械臂的障碍物躲避解决方法,其特征在于:所述应用于冗余度机械臂的障碍物躲避解决方法不仅适用于冗余度机械臂,同时适用于移动冗余度机械臂、多连杆机构以及任何能以D-H建模法建模的具有自由度的机械来实现障碍物躲避,只需将其D-H模型带入该方法中。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711240667.9A CN107966907B (zh) | 2017-11-30 | 2017-11-30 | 一种应用于冗余度机械臂的障碍物躲避解决方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711240667.9A CN107966907B (zh) | 2017-11-30 | 2017-11-30 | 一种应用于冗余度机械臂的障碍物躲避解决方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN107966907A CN107966907A (zh) | 2018-04-27 |
CN107966907B true CN107966907B (zh) | 2020-09-22 |
Family
ID=61999245
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201711240667.9A Active CN107966907B (zh) | 2017-11-30 | 2017-11-30 | 一种应用于冗余度机械臂的障碍物躲避解决方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN107966907B (zh) |
Families Citing this family (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108714894A (zh) * | 2018-05-03 | 2018-10-30 | 华南理工大学 | 一种求解双冗余机械臂互相碰撞的动力学方法 |
CN108772835B (zh) * | 2018-05-03 | 2020-09-22 | 华南理工大学 | 一种障碍物与物理极限躲避方法 |
CN108972548B (zh) * | 2018-06-29 | 2020-09-22 | 华南理工大学 | 一种移动平台-机械臂系统建模方法 |
CN109015657B (zh) * | 2018-09-07 | 2021-12-10 | 浙江科技学院 | 一种面向冗余机械臂重复运动规划的终态网络优化方法 |
CN109782601B (zh) * | 2019-01-31 | 2021-09-14 | 浙江大学 | 一种协调机械臂自适应神经网络同步鲁棒控制器设计方法 |
CN110682286B (zh) * | 2019-05-28 | 2020-07-28 | 广东省智能制造研究所 | 一种协作机器人实时避障方法 |
CN111309002A (zh) * | 2019-11-26 | 2020-06-19 | 华南理工大学 | 一种基于矢量的轮式移动机器人避障方法及系统 |
CN114030652B (zh) * | 2021-09-22 | 2023-09-12 | 北京电子工程总体研究所 | 一种避障路径规划方法和系统 |
CN114932549B (zh) * | 2022-05-15 | 2024-05-31 | 西北工业大学 | 空间冗余机械臂的运动规划方法与装置 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101804627A (zh) * | 2010-04-02 | 2010-08-18 | 中山大学 | 一种冗余度机械臂运动规划方法 |
CN101890718A (zh) * | 2010-06-01 | 2010-11-24 | 中山大学 | 一种冗余度机械臂运动规划的初始化方法 |
CN104029203A (zh) * | 2014-06-18 | 2014-09-10 | 大连大学 | 实现空间机械臂避障的路径规划方法 |
CN104392081A (zh) * | 2014-06-30 | 2015-03-04 | 管小清 | 一种冗余机械臂运动障碍物避障算法 |
CN107378952A (zh) * | 2017-08-16 | 2017-11-24 | 华南理工大学 | 一种冗余度机械臂末端执行器姿态保持的解决方法 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH06332538A (ja) * | 1993-05-19 | 1994-12-02 | Fanuc Ltd | 柔軟なサーボ制御方法 |
CN106842907B (zh) * | 2017-02-16 | 2020-03-27 | 香港理工大学深圳研究院 | 一种多冗余度机械臂系统的协同控制方法与装置 |
-
2017
- 2017-11-30 CN CN201711240667.9A patent/CN107966907B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101804627A (zh) * | 2010-04-02 | 2010-08-18 | 中山大学 | 一种冗余度机械臂运动规划方法 |
CN101890718A (zh) * | 2010-06-01 | 2010-11-24 | 中山大学 | 一种冗余度机械臂运动规划的初始化方法 |
CN104029203A (zh) * | 2014-06-18 | 2014-09-10 | 大连大学 | 实现空间机械臂避障的路径规划方法 |
CN104392081A (zh) * | 2014-06-30 | 2015-03-04 | 管小清 | 一种冗余机械臂运动障碍物避障算法 |
CN107378952A (zh) * | 2017-08-16 | 2017-11-24 | 华南理工大学 | 一种冗余度机械臂末端执行器姿态保持的解决方法 |
Non-Patent Citations (1)
Title |
---|
《一种采用雅克比转置技术的带反馈的障碍物躲避方案》;郭东生 等;《中国科技论文》;20170131;第220-225页 * |
Also Published As
Publication number | Publication date |
---|---|
CN107966907A (zh) | 2018-04-27 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107966907B (zh) | 一种应用于冗余度机械臂的障碍物躲避解决方法 | |
CN107490965B (zh) | 一种空间自由漂浮机械臂的多约束轨迹规划方法 | |
CN111562797B (zh) | 确保收敛的无人机飞行时间最优实时轨迹优化方法 | |
CN112757306B (zh) | 一种机械臂逆解多解选择和时间最优轨迹规划算法 | |
CN102646148B (zh) | 仿人机器人机械臂避碰运动轨迹规划方法 | |
CN111761582B (zh) | 一种基于随机采样的移动机械臂避障规划方法 | |
CN107378952B (zh) | 一种冗余度机械臂末端执行器姿态保持的解决方法 | |
CN111309002A (zh) | 一种基于矢量的轮式移动机器人避障方法及系统 | |
CN104760041A (zh) | 一种基于突加度的障碍物躲避运动规划方法 | |
CN108908347B (zh) | 一种面向冗余移动机械臂容错型重复运动规划方法 | |
CN105159096A (zh) | 一种基于粒子群算法的冗余度空间机械臂关节力矩优化方法 | |
CN108714894A (zh) | 一种求解双冗余机械臂互相碰撞的动力学方法 | |
CN107160401B (zh) | 一种解决冗余度机械臂关节角偏移问题的方法 | |
CN108772835B (zh) | 一种障碍物与物理极限躲避方法 | |
CN107538493A (zh) | 一种避免线缆干涉约束的工业机器人轨迹规划方法 | |
CN106844951B (zh) | 基于分段几何法求解超冗余机器人逆运动学的方法及系统 | |
CN108098777B (zh) | 一种冗余度机械臂力矩层重复运动控制方法 | |
CN110561419B (zh) | 臂型线约束柔性机器人轨迹规划方法及装置 | |
CN114942593B (zh) | 一种基于干扰观测器补偿的机械臂自适应滑模控制方法 | |
CN111684380A (zh) | 机器人的运动控制方法、控制系统和存储装置 | |
CN108972548B (zh) | 一种移动平台-机械臂系统建模方法 | |
CN115026816B (zh) | 一种基于虚拟力的机械臂末端避障方法 | |
CN112223290A (zh) | 一种关节空间连续点轨迹过渡方法及装置 | |
Mu et al. | A hybrid obstacle-avoidance method of spatial hyper-redundant manipulators for servicing in confined space | |
CN110026986B (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 |