CN113580146A - 一种融合动态***与模型预测控制的机械臂实时避障方法 - Google Patents

一种融合动态***与模型预测控制的机械臂实时避障方法 Download PDF

Info

Publication number
CN113580146A
CN113580146A CN202111008177.2A CN202111008177A CN113580146A CN 113580146 A CN113580146 A CN 113580146A CN 202111008177 A CN202111008177 A CN 202111008177A CN 113580146 A CN113580146 A CN 113580146A
Authority
CN
China
Prior art keywords
mechanical arm
obstacle
time
joint
dynamic system
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
Application number
CN202111008177.2A
Other languages
English (en)
Other versions
CN113580146B (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.)
Zhejiang University of Technology ZJUT
Original Assignee
Zhejiang University of Technology ZJUT
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 Zhejiang University of Technology ZJUT filed Critical Zhejiang University of Technology ZJUT
Priority to CN202111008177.2A priority Critical patent/CN113580146B/zh
Publication of CN113580146A publication Critical patent/CN113580146A/zh
Application granted granted Critical
Publication of CN113580146B publication Critical patent/CN113580146B/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
    • 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

Landscapes

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

Abstract

本发明公开了一种融合动态***与模型预测控制的机械臂实时避障方法，该方法结合动态***和模型预测控制，能够处理动态环境中的实时避障问题。该方法可根据移动障碍物的位置信息实时规划出避障轨迹，并通过使用模型预测控制方法进行机械臂控制，考虑多项线性或非线性约束条件。本发明提供一种融合动态***和模型预测控制的机械臂实时避障方法，提高了机械臂末端跟踪的精度，有效地提升了机械臂实时避障的能力。

Description

一种融合动态***与模型预测控制的机械臂实时避障方法
技术领域
本发明属于机械臂实时避障技术领域，尤其是一种融合动态***与模型预测控制的机械臂实时避障方法。
背景技术
近年来，由于人力成本逐渐增长等原因，机器人产业高速发展，整体规模持续增长，机器人代替劳动者从事繁重枯燥的工作，提升生产效率，改善生产环境。随着人机协作场景的广泛应用及生产需求的复杂化，机器人对动态环境的适应能力也变得更加重要。基于动态***的机器人避障方法作为机器人学的一个研究重点，已经在人机协作、工业生产以及医疗康复等领域发挥了重要的作用。
然而，基于动态***的普通机器人避障方法无法保证高维机械臂各连杆的防碰性，其生成的避障轨迹仅仅针对点状的移动机器人，若将该方法迁移到多自由度的机械臂上，必然会涉及到高维空间下的避障问题。模型预测控制是一种有效的优化方法，可同时对多个指定目标进行优化，并且能够满足多项线性或非线性约束，能够确保机械臂末端跟踪所规划出的避障轨迹的同时保持各连杆与移动障碍物保持一定的动态距离，防止发生碰撞。在目前以动态***来对机械臂进行实时避障的方法中，还未能有效解决以采用模型预测控制的多约束满足性质来提升高维机械臂实时避障能力的问题。
发明内容
在基于动态***的机器人实时避障中，为了解决由于机械臂多自由度而产生的高维避障问题，本发明提供一种融合动态***与模型预测控制的机械臂实时避障方法，利用动态***方法进行实时避障轨迹的规划，将机械臂各连杆与障碍物间的动态距离作为控制的约束条件，在机械臂末端跟踪规划轨迹的同时保持动态距离约束的满足，有效地提高了机械臂实时避障的有效性。
本发明解决其技术问题所采用的技术方案是：
一种融合动态***与模型预测控制的机械臂实时避障方法，其特征在于：包括如下步骤：
步骤1)建立一个原始的动态***DS模型
Figure BDA0003235232170000021
作为机械臂末端在无障碍物环境下期望的运动轨迹方程，并给出相应的末端位置序列；所述的DS模型
Figure BDA0003235232170000022
表示用户指定的一阶常微分方程，代表机械臂末端的运动规律，其中ξ为三维空间的位置信息，
Figure BDA0003235232170000023
为三维空间的速度信息；
步骤2)通过视觉传感器测得k时刻机械臂工作空间内最近球形障碍物的半径ro和质心位置
Figure BDA0003235232170000024
并计算其势函数
Figure BDA0003235232170000025
Figure BDA0003235232170000026
并进行坐标系转换
Figure BDA0003235232170000027
(所述坐标系转换是指从相机坐标系下转换到障碍物坐标系下)，得到其梯度矩阵
Figure BDA0003235232170000028
(所述的梯度矩阵为势函数对ξ进行求导)，对原始DS进行调制，得到
Figure BDA0003235232170000029
即机械臂末端在障碍物环境下的避障轨迹方程，并得到相应的末端位置序列
Figure BDA00032352321700000210
其中k为离散时间序列，I表示单位矩阵；
步骤3)根据机械臂的运动学模型，计算机械臂各关节与障碍物的安全域指标中的最小值ESFk以及机械臂的末端误差ek(所述的末端误差为期望的轨迹点位置与实际位置之差)计算公式如下：
Figure BDA0003235232170000031
Figure BDA00032352321700000313
其中，d为机械臂关节个数，i＝1,2,…,d表示机械臂关节的标号，
Figure BDA0003235232170000032
为k时刻i关节的三维空间位置，
Figure BDA0003235232170000033
为k时刻i关节的三维空间速度，
Figure BDA0003235232170000034
为k时刻障碍物质心的三维空间速度，由前后时刻质心的位移量除以时间间隔得到，
Figure BDA0003235232170000035
为k时刻调制DS所规划出的机械臂末端下一个预到达路径点的位置信息，
Figure BDA0003235232170000036
为k时刻机械臂末端的实际位置信息；
步骤4)设置关节域阈值
Figure BDA0003235232170000037
将该约束
Figure BDA0003235232170000038
和其他约束条件以及末端误差ek代入MPC求解器进行求解，使末端误差最小化的同时保持关节域约束，得到当前预测域NΔt上关节速度控制量的最优解
Figure BDA0003235232170000039
计算关节速度最优控制量
Figure BDA00032352321700000310
公式如下：
Figure BDA00032352321700000311
Figure BDA00032352321700000312
其中，ek+i|k为末端误差基于k时刻在未来第k+i时刻的情况，ESFk+i|k为机械臂各关节的最小安全域基于k时刻在未来第k+i的情况；
步骤5)通过关节控制器将所得到的关节速度用于机械臂的控制，并按照设定的控制率等待到下一时刻；
重复执行步骤2)-5)，对各时刻下的机械臂进行避障规划与控制，直到机械臂的末端到达目标位置。
进一步的，所述步骤2)中，所述障碍物的质心位置为在视觉传感器的相机坐标系下x，y，z坐标；障碍物质心的位置信息
Figure BDA0003235232170000041
Figure BDA0003235232170000042
分别为k时刻障碍物质心的x，y，z坐标信息。
进一步的，所述步骤2)中，所述3D视觉传感器提供障碍物3D位置信息输出的RGB-D传感器。
进一步的，所述步骤4)中，所述的关节域阈值
Figure BDA0003235232170000043
为用户指定，其取值表示机械臂上点的和移动障碍物之间的动态距离。
进一步的，所述步骤4)中，所述其他约束条件包括关节角度极限的约束、关节速度变化率的约束以及视觉传感器的遮挡约束。
进一步的，所述步骤4)中，所述的预测域NΔt中，N为预测步长，Δt为预测模型的采样周期，其取值是依据实际控制需要所确定的。
本发明的有益效果主要表现在：提出了一种融合动态***和模型预测控制的机械臂实时避障方法，该方法可根据移动障碍物的位置信息实时规划出避障轨迹，并通过使用模型预测控制方法，考虑多项线性或非线性约束条件以及多个目标函数，提高了机械臂末端跟踪的精度，有效地提升了机械臂实时避障的能力。
附图说明
图1是机械臂实时避障规划控制流程图。
具体实施方式
下面结合说明书附图对本发明做进一步描述。
参照图1，一种融合动态***和模型预测控制的机械臂实时避障方法，所述方法包括以下步骤：
步骤1)建立一个原始的DS模型
Figure BDA00032352321700000511
作为机械臂末端在无障碍物环境下期望的运动轨迹方程，并给出相应的末端位置序列；
步骤2)通过视觉传感器测得k时刻机械臂工作空间内最近球形障碍物(最近球形障碍物的意思为在机械臂工作空间中距离机械臂距离最近的障碍物，为方便考虑，将障碍物均视为球形状，通过障碍物的质心及大小及和机械臂各连杆的距离来确定障碍物和机械臂的最近距离，依次来确定最近障碍物)的半径ro和质心位置
Figure BDA0003235232170000051
并计算其势函数
Figure BDA0003235232170000052
并进行坐标系转换
Figure BDA0003235232170000053
得到其梯度矩阵
Figure BDA0003235232170000054
对原始DS进行调制，得到
Figure BDA0003235232170000055
Figure BDA0003235232170000056
即机械臂末端在障碍物环境下的避障轨迹方程，并得到相应的末端位置序列
Figure BDA0003235232170000057
步骤3)根据机械臂的运动学模型，计算机械臂各关节与障碍物的安全域指标中的最小值ESFk以及机械臂的末端误差ek；
步骤4)设置关节域阈值
Figure BDA0003235232170000058
将该约束
Figure BDA0003235232170000059
和其他约束条件以及末端误差ek代入MPC求解器进行求解，使末端误差最小化的同时保持关节域约束，得到当前预测域NΔt上关节速度控制量的最优解
Figure BDA00032352321700000510
步骤5)通过关节控制器将所得到的关节速度用于机械臂的控制，并按照设定的控制率等待到下一时刻。
重复执行步骤2)-5)，对各时刻下的机械臂进行避障规划与控制，直到机械臂的末端到达目标位置。
机械臂实时避障规划控制流程图如图1所示。建立机械臂末端运动方程的原始DS模型，将障碍物的势函数用于DS模型的调制，势函数和经坐标系转换后的梯度矩阵及调制过程分别建立如下：
Figure BDA0003235232170000061
Figure BDA0003235232170000062
Figure BDA0003235232170000063
其中，k为离散时间序列，障碍物质心的位置信息
Figure BDA0003235232170000064
Figure BDA0003235232170000065
分别为k时刻障碍物质心的x，y，z坐标信息。
对机械臂的实时控制采用了模型预测控制方法。以k时刻为例，首先，根据障碍物信息对原始DS调制后所得到的末端避障的位置序列
Figure BDA0003235232170000066
然后，通过机械臂的运动学模型，计算当前时刻下机械臂所有关节与障碍物的最短安全域ESFk以及机械臂的末端误差ek；其次，进行MPC的优化求解，将安全域约束和其他诸如关节极限等约束作为优化的限定条件，将末端误差ek作为优化目标，得到当前预测域上关节速度控制量的最优解
Figure BDA0003235232170000067
最后将关节速度最优控制量通过控制器进行机械臂的关节控制；最短距离及其末端误差的计算公式如下：
Figure BDA0003235232170000068
Figure BDA0003235232170000069
其中，d为机械臂关节个数，i＝1,2,…,d表示机械臂关节的标号，
Figure BDA00032352321700000610
为k时刻i关节的三维空间位置，
Figure BDA0003235232170000071
为k时刻i关节的三维空间速度，
Figure BDA0003235232170000072
为k时刻障碍物质心的三维空间速度，由前后时刻质心的位移量除以时间间隔得到，
Figure BDA0003235232170000073
为k时刻调制DS所规划出的机械臂末端下一个预到达路径点的位置信息，
Figure BDA0003235232170000074
为k时刻机械臂末端的实际位置信息。
优化末端误差并满足各项约束条件，计算关节速度最优控制量
Figure BDA0003235232170000075
Figure BDA0003235232170000076
Figure BDA0003235232170000077
其中，ek+i|k为末端误差基于k时刻在未来第k+i时刻的情况，ESFk+i|k为机械臂各关节的最小安全域基于k时刻在未来第k+i的情况。
重复执行公式1)-7)对机械臂的关节速度进行实时控制，最终实现机械臂末端到达期望位置的目标。

Claims (6)

1.一种融合动态***与模型预测控制的机械臂实时避障方法，其特征在于：包括如下步骤：
步骤1)建立一个原始的动态***DS模型
Figure FDA0003235232160000011
作为机械臂末端在无障碍物环境下期望的运动轨迹方程，并给出相应的末端位置序列；其中ξ为三维空间的位置信息，
Figure FDA0003235232160000012
为三维空间的速度信息；
步骤2)通过视觉传感器测得k时刻机械臂工作空间内最近球形障碍物的半径ro和质心位置
Figure FDA0003235232160000013
并计算其势函数
Figure FDA0003235232160000014
Figure FDA0003235232160000015
并进行坐标系转换
Figure FDA0003235232160000016
得到其梯度矩阵
Figure FDA0003235232160000017
对原始DS进行调制，得到
Figure FDA0003235232160000018
Figure FDA0003235232160000019
即机械臂末端在障碍物环境下的避障轨迹方程，并得到相应的末端位置序列
Figure FDA00032352321600000110
其中k为离散时间序列，I表示单位矩阵；
步骤3)根据机械臂的运动学模型，计算机械臂各关节与障碍物的安全域指标中的最小值ESFk以及机械臂的末端误差ek，计算公式如下：
Figure FDA00032352321600000111
Figure FDA00032352321600000112
其中，d为机械臂关节个数，i＝1,2,…,d表示机械臂关节的标号，
Figure FDA00032352321600000113
为k时刻i关节的三维空间位置，
Figure FDA00032352321600000114
为k时刻i关节的三维空间速度，
Figure FDA00032352321600000115
为k时刻障碍物质心的三维空间速度，由前后时刻质心的位移量除以时间间隔得到，
Figure FDA00032352321600000116
为k时刻调制DS所规划出的机械臂末端下一个预到达路径点的位置信息，
Figure FDA0003235232160000021
为k时刻机械臂末端的实际位置信息；
步骤4)设置关节域阈值
Figure FDA0003235232160000022
将该约束
Figure FDA0003235232160000023
和其他约束条件以及末端误差ek代入MPC求解器进行求解，使末端误差最小化的同时保持关节域约束，得到当前预测域NΔt上关节速度控制量的最优解
Figure FDA0003235232160000024
计算关节速度最优控制量
Figure FDA0003235232160000025
公式如下：
Figure FDA0003235232160000026
Figure FDA0003235232160000027
其中，ek+i|k为末端误差基于k时刻在未来第k+i时刻的情况，ESFk+i|k为机械臂各关节的最小安全域基于k时刻在未来第k+i的情况；
步骤5)通过关节控制器将所得到的关节速度用于机械臂的控制，并按照设定的控制率等待到下一时刻；
重复执行步骤2)-5)，对各时刻下的机械臂进行避障规划与控制，直到机械臂的末端到达目标位置。
2.如权利要求1所述的一种融合动态***与模型预测控制的机械臂实时避障方法，其特征在于：所述步骤2)中，所述障碍物的质心位置为在视觉传感器的相机坐标系下x，y，z坐标；障碍物质心的位置信息
Figure FDA0003235232160000028
Figure FDA0003235232160000029
分别为k时刻障碍物质心的x，y，z坐标信息。
3.如权利要求1所述的一种融合动态***与模型预测控制的机械臂实时避障方法，其特征在于：所述步骤2)中，所述视觉传感器是提供障碍物3D位置信息输出的RGB-D传感器。
4.如权利要求1所述的一种融合动态***与模型预测控制的机械臂实时避障方法，其特征在于：所述步骤4)中，所述的关节域阈值
Figure FDA0003235232160000031
为用户指定，其取值表示机械臂上点的和移动障碍物之间的动态距离。
5.如权利要求1所述的一种融合动态***与模型预测控制的机械臂实时避障方法，其特征在于：所述步骤4)中，所述其他约束条件包括关节角度极限的约束、关节速度变化率的约束以及视觉传感器的遮挡约束。
6.如权利要求1所述的一种融合动态***与模型预测控制的机械臂实时避障方法，其特征在于：所述步骤4)中，所述的预测域NΔt中，N为预测步长，Δt为预测模型的采样周期，其取值是依据实际控制需要所确定的。
CN202111008177.2A 2021-08-29 2021-08-29 一种融合动态***与模型预测控制的机械臂实时避障方法 Active CN113580146B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111008177.2A CN113580146B (zh) 2021-08-29 2021-08-29 一种融合动态***与模型预测控制的机械臂实时避障方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111008177.2A CN113580146B (zh) 2021-08-29 2021-08-29 一种融合动态***与模型预测控制的机械臂实时避障方法

Publications (2)

Publication Number Publication Date
CN113580146A true CN113580146A (zh) 2021-11-02
CN113580146B CN113580146B (zh) 2022-08-05

Family

ID=78240418

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111008177.2A Active CN113580146B (zh) 2021-08-29 2021-08-29 一种融合动态***与模型预测控制的机械臂实时避障方法

Country Status (1)

Country Link
CN (1) CN113580146B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116572258A (zh) * 2023-07-14 2023-08-11 中铁十四局集团大盾构工程有限公司 一种焊接机器人动态避障控制方法及计算机可读存储介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105479490A (zh) * 2015-12-24 2016-04-13 华中科技大学 一种双机器人实时动态避障装置及其避障方法
CN105955268A (zh) * 2016-05-12 2016-09-21 哈尔滨工程大学 一种考虑局部避碰的uuv动目标滑模跟踪控制方法
US20190118384A1 (en) * 2016-04-29 2019-04-25 Softbank Robotics Europe A mobile robot with enhanced balanced motion and behavior capabilities
CN110532665A (zh) * 2019-08-26 2019-12-03 哈尔滨工程大学 一种固定航线任务下的移动对象动态轨迹预测方法
CN111168675A (zh) * 2020-01-08 2020-05-19 北京航空航天大学 一种家用服务机器人的机械臂动态避障运动规划方法
WO2021065196A1 (en) * 2019-10-03 2021-04-08 Mitsubishi Electric Corporation Method and system for trajectory optimization for nonlinear robotic systems with geometric constraints

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105479490A (zh) * 2015-12-24 2016-04-13 华中科技大学 一种双机器人实时动态避障装置及其避障方法
US20190118384A1 (en) * 2016-04-29 2019-04-25 Softbank Robotics Europe A mobile robot with enhanced balanced motion and behavior capabilities
CN105955268A (zh) * 2016-05-12 2016-09-21 哈尔滨工程大学 一种考虑局部避碰的uuv动目标滑模跟踪控制方法
CN110532665A (zh) * 2019-08-26 2019-12-03 哈尔滨工程大学 一种固定航线任务下的移动对象动态轨迹预测方法
WO2021065196A1 (en) * 2019-10-03 2021-04-08 Mitsubishi Electric Corporation Method and system for trajectory optimization for nonlinear robotic systems with geometric constraints
CN111168675A (zh) * 2020-01-08 2020-05-19 北京航空航天大学 一种家用服务机器人的机械臂动态避障运动规划方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116572258A (zh) * 2023-07-14 2023-08-11 中铁十四局集团大盾构工程有限公司 一种焊接机器人动态避障控制方法及计算机可读存储介质
CN116572258B (zh) * 2023-07-14 2023-09-19 中铁十四局集团大盾构工程有限公司 一种焊接机器人动态避障控制方法及计算机可读存储介质

Also Published As

Publication number Publication date
CN113580146B (zh) 2022-08-05

Similar Documents

Publication Publication Date Title
Xu et al. Motion planning of manipulators for simultaneous obstacle avoidance and target tracking: An RNN approach with guaranteed performance
CN110977992B (zh) 一种面向机械臂的无运动学模型轨迹跟踪方法及一种机械臂***
CN111975771A (zh) 一种基于偏差重定义神经网络的机械臂运动规划方法
CN113829343B (zh) 基于环境感知的实时多任务多人人机交互***
Li et al. A hybrid visual servo control method for simultaneously controlling a nonholonomic mobile and a manipulator
CN113580146B (zh) 一种融合动态***与模型预测控制的机械臂实时避障方法
CN112605996A (zh) 一种面向冗余机械臂的无模型碰撞避免控制方法
CN104298244A (zh) 一种工业机器人三维实时高精度定位装置及方法
Navarro-Alarcon et al. Visually servoed deformation control by robot manipulators
CN111515928B (zh) 机械臂运动控制***
CN114800517B (zh) 一种多自由度液压机械臂实时控制***及方法
CN114131617B (zh) 一种工业机器人的智能柔顺控制方法和装置
CN114986498A (zh) 一种移动操作臂协同控制方法
KR101981641B1 (ko) 다중 이동 로봇의 대형 제어 방법 및 시스템
CN112650217A (zh) 一种基于评估函数的机器人轨迹跟踪策略动态优化方法
Bae et al. A dynamic visual servoing of robot manipulator with eye-in-hand camera
CN114967441A (zh) 一种网络化非完整约束多机器人分群一致追踪控制方法、微控单元及控制***
CN109794939B (zh) 一种焊接机器人运动规划并行束方法
CN114274145A (zh) 一种腹腔镜手术中多机械臂的实时避障方法
CN109895096B (zh) 一种焊接机器人运动模型稳定割平面方法
CN113510699A (zh) 一种基于改进蚁群优化算法的机械臂运动轨迹规划方法
Chen et al. Robot Zero-Moment Control Algorithm Based on Parameter Identification of Low-Speed Dynamic Balance
Yang et al. An obstacle avoidance and trajectory tracking algorithm for redundant manipulator end
Kang et al. Real-time human motion estimation for human robot collaboration
CN118219260A (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