CN110977989B - 一种协作机器人的运动规划与实时避障方法及*** - Google Patents
一种协作机器人的运动规划与实时避障方法及*** Download PDFInfo
- Publication number
- CN110977989B CN110977989B CN201911382289.7A CN201911382289A CN110977989B CN 110977989 B CN110977989 B CN 110977989B CN 201911382289 A CN201911382289 A CN 201911382289A CN 110977989 B CN110977989 B CN 110977989B
- Authority
- CN
- China
- Prior art keywords
- robot
- obstacle
- representing
- state variable
- time
- 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
- 238000000034 method Methods 0.000 title claims abstract description 46
- 238000005457 optimization Methods 0.000 claims abstract description 43
- 239000011159 matrix material Substances 0.000 claims description 52
- 239000013641 positive control Substances 0.000 claims description 29
- 230000008569 process Effects 0.000 claims description 16
- 230000001133 acceleration Effects 0.000 claims description 10
- 238000004364 calculation method Methods 0.000 claims description 10
- 230000008859 change Effects 0.000 claims description 4
- 238000010276 construction Methods 0.000 claims description 3
- 230000006870 function Effects 0.000 description 38
- 230000004888 barrier function Effects 0.000 description 6
- 238000013459 approach Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000002955 isolation Methods 0.000 description 1
- 230000003287 optical effect Effects 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/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
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种协作机器人的运动规划与实时避障方法及***,其中,所述方法包括:一种协作机器人的运动规划与实时避障方法,其特征在于,所述方法包括:初始化后获得机器人的当前状态信息;获得机器人与障碍物之间的所形成的位置关系信息;确立机器人的等式约束;构建用于躲避障碍物的实时变化的优化函数;构建机器人的不等式约束;求解计算机器人的状态变量与控制量的更新率;获得所述机器人的更新状态变量和控制量,并发送至所述机器人;判断当前时间是否大于预设任务时间,若是,则结束,若否,返回获得机器人的当前状态信息步骤。在本发明实施例中,能够提高机器人的控制效率和控制精度高,且能够保证机器人不超过其物理约束。
Description
技术领域
本发明涉及机器人控制技术领域,尤其涉及一种协作机器人的运动规划与实时避障方法及***。
背景技术
协作机器人是一种为了实现人机共融而兴起的新一代工业机器人。与传统工业机器人相比,协作机器人能够与人类工作在相同的工作空间中,无需物理隔离,从而具有更好的人机友好性。但是,人机共用的工作环境给机器人的控制带来了困难,尤其是在规划层面:如何根据机器人的给定任务与空间的占用情况,自动生成机器人的运动指令。目前针对协作机器人的运动规划面临以下困难:1)对人机共存的工作场景来说,在线的运动规划更有实用价值;2)机器人需要满足自身物理约束的限制;3)机器人需要实现对障碍物的实时躲避。
发明内容
本发明的目的在于克服现有技术的不足,本发明提供了一种协作机器人的运动规划与实时避障方法及***,能够提高机器人的控制效率和控制精度高,且能够保证机器人不超过其物理约束。
为了解决上述技术问题,本发明实施例提供了一种协作机器人的运动规划与实时避障方法,所述方法包括:
对机器人进行初始化,在初始化后获得所述机器人的当前状态信息;
获得所述机器人与障碍物之间的所形成的位置关系信息,所述位置关系信息包括所述机器人与障碍物之间的最近距离、所述机器人最靠近障碍物的位置、障碍物指向所述机器人最近障碍物位置的向量和所述位置对应的雅克比矩阵;
基于所述机器人的期望运动轨迹确立所述机器人的等式约束;
基于所述最近距离构建用于躲避障碍物的实时变化的优化函数;
基于所述机器人的物理约束以及所述优化函数构建所述机器人的不等式约束;
基于所述等式约束和不等式约束利用在线求解计算机器人的状态变量与控制量的更新率;
根据所述更新率获得所述机器人的更新状态变量和控制量,并发送至所述机器人;
判断当前时间是否大于预设任务时间,若是,则结束,若否,返回获得所述机器人的当前状态信息步骤。
可选的,所述当前状态信息包括所述机器人的当前关节角度、当前关节角速度、当前末端位置、当前速度及当前状态变量。
可选的,所述基于所述机器人的期望运动轨迹确立所述机器人的等式约束,包括:
基于所述机器人当前位置与期望位置的差值与期望轨迹的速度定义所述机器人运动规划的等式约束,所述等式约束如下:
其中,e表示所述机器人当前位置与期望位置的差值,e=xd-x,xd表示期望位置,x表示所述机器人当前位置;J表示所述机器人的雅克比矩阵;表示所述机器人的期望运动速度;k,ρ表示正的控制参数,其中0<ρ<1。
可选的,所述基于所述最近距离构建用于躲避障碍物的实时变化的优化函数,包括:
定义所述机器人与障碍物之间的安全距离,根据所述机器人与障碍物之间的最近距离与所述安全距离之间的大小关系确立一个实时变化的优化函数。
可选的,所述实时变化的优化函数的函数公式如下:
其中,d表示所述机器人与障碍物之间的最近距离,D表示所述机器人与障碍物之间的安全距离;Ji表示所述位置对应的雅克比矩阵;AT表示障碍物指向所述机器人最近障碍物位置的向量转置;表示所述机器人关节角速度。
可选的,所述基于所述机器人的物理约束以及所述优化函数构建所述机器人的不等式约束,包括:
将所述机器人的物理约束量在加速度层进行归并,并根据所述归并结果设计所述机器人最靠近障碍物的位置的不等式约束;
其中,所述将所述机器人的物理约束量在加速度层进行归并过程如下:
所述不等式约束如下:
其中,α表示正控制参数;θ表示所述机器人的关节角度;θmin表示所述机器人的关节角度的下界;θmax表示所述机器人的关节角度的上界;表示所述机器人关节角速度的下界;表示所述机器人关节角速度的上界;表示所述机器人关节角速度;AT表示障碍物指向所述机器人最近障碍物位置的向量转置;Ji表示所述位置对应的雅克比矩阵。
可选的,所述基于所述等式约束和不等式约束利用在线求解计算机器人的状态变量与控制量的更新率公式如下:
其中,∈表示正控制参数;表示所述机器人的关节角角速度;e表示所述机器人当前位置与期望位置的差值,e=xd-x,xd表示期望位置,x所述机器人当前位置;J表示所述机器人的雅克比矩阵;JT表示所述机器人的雅克比矩阵的转置;表示所述机器人的期望速度;PΩ(·)表示一个饱和函数;表示所述位置对应的雅克比矩阵的转置;Ji表示所述位置对应的雅克比矩阵;AT表示障碍物指向所述机器人最近障碍物位置的向量转置;A表示障碍物指向所述机器人最近障碍物位置的向量;k表示正的控制参数; d表示所述机器人与障碍物之间的最近距离,D表示所述机器人与障碍物之间的安全距离;表示所述机器人关节角速度;ρ表示正的控制参数,其中0<ρ<1;λ1,λ2分别表示第一状态变量和第二状态变量;分别表示第一状态变量和第二状态变量的一阶导数。
可选的,所述根据所述更新率获得所述机器人的更新状态变量和控制量,包括:
根据所述更新率计算所述机器人下一刻的控制量及状态变量,获得所述机器人的更新状态变量和控制量。
可选的,所述根据所述更新率计算所述机器人下一刻的控制量及状态变量的计算公式如下:
另外,本发明实施例还提供了一种协作机器人的运动规划与实时避障***,所述***包括:
状态信息获得模块:用于对机器人进行初始化,在初始化后获得所述机器人的当前状态信息;
位置关系信息获得模块:用于获得所述机器人与障碍物之间的所形成的位置关系信息,所述位置关系信息包括所述机器人与障碍物之间的最近距离、所述机器人最靠近障碍物的位置、障碍物指向所述机器人最近障碍物位置的向量和所述位置对应的雅克比矩阵;
等式约束构建模块:基于所述机器人的期望运动轨迹确立所述机器人的等式约束;
优化函数构建模块:用于基于所述最近距离构建用于躲避障碍物的实时变化的优化函数;
不等式约束构建模块:用于基于所述机器人的物理约束以及所述优化函数构建所述机器人的不等式约束;
更新率计算模块:用于基于所述等式约束和不等式约束利用在线求解计算机器人的状态变量与控制量的更新率;
更新控制模块:用于根据所述更新率获得所述机器人的更新状态变量和控制量,并发送至所述机器人;
判断模块:用于判断当前时间是否大于预设任务时间,若是,则结束,若否,返回获得所述机器人的当前状态信息步骤。
在本发明实施例中,实施本发明中的方法能够提高机器人的控制效率和控制精度;能够保证机器人不超过其物理约束;只需要求取机器人与障碍物之间的最近点,具有求解效率高、计算量小的优点;同时可变性能优化指标的设计尽在机器人靠近障碍物时起作用,在不必要时可以简化规划算法。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见的,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其它的附图。
图1是本发明实施例中的协作机器人的运动规划与实时避障方法的流程示意图;
图2是本发明实施例中的协作机器人的运动规划与实时避障***的结构组成示意图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例,都属于本发明保护的范围。
实施例
请参阅图1,图1是本发明实施例中的协作机器人的运动规划与实时避障方法的流程示意图。
如图1所示,一种协作机器人的运动规划与实时避障方法,所述方法包括:
S11:对机器人进行初始化,在初始化后获得所述机器人的当前状态信息;
在本发明具体实施过程中,所述当前状态信息包括所述机器人的当前关节角度、当前关节角速度、当前末端位置、当前速度及当前状态变量。
在初始化之后,机器人就获得机器人的当前状态信息;该当前状态信息包括所述机器人的当前关节角度、当前关节角速度、当前末端位置、当前速度及当前状态变量。
S12:获得所述机器人与障碍物之间的所形成的位置关系信息,所述位置关系信息包括所述机器人与障碍物之间的最近距离、所述机器人最靠近障碍物的位置、障碍物指向所述机器人最近障碍物位置的向量和所述位置对应的雅克比矩阵;
在本发明具体实施过程中,位置关系信息包括所述机器人与障碍物之间的最近距离、所述机器人最靠近障碍物的位置、障碍物指向所述机器人最近障碍物位置的向量和所述位置对应的雅克比矩阵;既有,通过激光雷达或者红外传感器等设备获得机器人与障碍物之间的最近距离d,再通过机器人的基座标系获得机器人最靠近障碍物的位置xi,以及获得障碍物指向机器人最靠近障碍物位置的向量A,并确定机器人最靠近障碍物的位置xi所对应的雅克比矩阵Ji。
S13:基于所述机器人的期望运动轨迹确立所述机器人的等式约束;
在本发明具体实施过程中,所述基于所述机器人的期望运动轨迹确立所述机器人的等式约束,包括:
基于所述机器人当前位置与期望位置的差值与期望轨迹的速度定义所述机器人运动规划的等式约束,所述等式约束如下:
其中,e表示所述机器人当前位置与期望位置的差值,e=xd-x,xd表示期望位置,x表示所述机器人当前位置;J表示所述机器人的雅克比矩阵;表示所述机器人的期望运动速度;k,ρ表示正的控制参数,其中0<ρ<1。
具体的,根据机器人当前位置与期望位置的差值e=xd-x与期望轨迹的速度定义机器人运动规划的等式约束,等式约束如下:
S14:基于所述最近距离构建用于躲避障碍物的实时变化的优化函数;
在本发明具体实施过程中,所述基于所述最近距离构建用于躲避障碍物的实时变化的优化函数,包括:定义所述机器人与障碍物之间的安全距离,根据所述机器人与障碍物之间的最近距离与所述安全距离之间的大小关系确立一个实时变化的优化函数。
所述实时变化的优化函数的函数公式如下:
其中,d表示所述机器人与障碍物之间的最近距离,D表示所述机器人与障碍物之间的安全距离;Ji表示所述位置对应的雅克比矩阵;AT表示障碍物指向所述机器人最近障碍物位置的向量转置;表示所述机器人关节角速度。
具体的,定义该机器人与障碍物之间的安全距离,并根据该机器人与障碍物之间的最近距离与该安全距离之间的大小关系来判断确定一个实时变化的优化函数。
该实时变化的优化函数的函数公式所定义性能优化指标如下:
其中,d表示所述机器人与障碍物之间的最近距离,D表示所述机器人与障碍物之间的安全距离;Ji表示所述位置对应的雅克比矩阵;AT表示障碍物指向所述机器人最近障碍物位置的向量转置;表示所述机器人关节角速度。
其意义为:当最小距离大于安全距离时,无需进行处理,即若k=0,优化指数-1/2kd2的导数恒为0;当最小距离小于安全距离时,此时使最靠近障碍物的位置xi得移动方向与障碍物指向机器人最靠近障碍物点的向量尽可能一致,实现避障目的。
S15:基于所述机器人的物理约束以及所述优化函数构建所述机器人的不等式约束;
在本发明具体实施过程中,所述基于所述机器人的物理约束以及所述优化函数构建所述机器人的不等式约束,包括:
将所述机器人的物理约束量在加速度层进行归并,并根据所述归并结果设计所述机器人最靠近障碍物的位置的不等式约束;
其中,所述将所述机器人的物理约束量在加速度层进行归并过程如下:
所述不等式约束如下:
其中,α表示正控制参数;θ表示所述机器人的关节角度;θmin表示所述机器人的关节角度的下界;θmax表示所述机器人的关节角度的上界;表示所述机器人关节角速度的下界;表示所述机器人关节角速度的上界;表示所述机器人关节角速度;AT表示障碍物指向所述机器人最近障碍物位置的向量转置;Ji表示所述位置对应的雅克比矩阵。
因此,该不等式约束如下:
其中,α表示正控制参数;θ表示所述机器人的关节角度;θmin表示所述机器人的关节角度的下界;θmax表示所述机器人的关节角度的上界;表示所述机器人关节角速度的下界;表示所述机器人关节角速度的上界;表示所述机器人关节角速度;AT表示障碍物指向所述机器人最近障碍物位置的向量转置;Ji表示所述位置对应的雅克比矩阵。
S16:基于所述等式约束和不等式约束利用在线求解计算机器人的状态变量与控制量的更新率;
在本发明具体实施过程中,所述基于所述等式约束和不等式约束利用在线求解计算机器人的状态变量与控制量的更新率公式如下:
其中,∈表示正控制参数;表示所述机器人的关节角角速度;e表示所述机器人当前位置与期望位置的差值,e=xd-x,xd表示期望位置,x所述机器人当前位置;J表示所述机器人的雅克比矩阵;JT表示所述机器人的雅克比矩阵的转置;表示所述机器人的期望速度;PΩ(·)表示一个饱和函数;表示所述位置对应的雅克比矩阵的转置;Ji表示所述位置对应的雅克比矩阵;AT表示障碍物指向所述机器人最近障碍物位置的向量转置;A表示障碍物指向所述机器人最近障碍物位置的向量;k表示正的控制参数; d表示所述机器人与障碍物之间的最近距离,D表示所述机器人与障碍物之间的安全距离;表示所述机器人关节角速度;ρ表示正的控制参数,其中0<ρ<1;λ1,λ2分别表示第一状态变量和第二状态变量;分别表示第一状态变量和第二状态变量的一阶导数。
具体的,根据等式约束和不等式约束利用在线求解计算机器人的状态变量与控制量的更新率公式如下:
其中,∈表示正控制参数;表示所述机器人的关节角角速度;e表示所述机器人当前位置与期望位置的差值,e=xd-x,xd表示期望位置,x所述机器人当前位置;J表示所述机器人的雅克比矩阵;JT表示所述机器人的雅克比矩阵的转置;表示所述机器人的期望速度;PΩ(·)表示一个饱和函数;表示所述位置对应的雅克比矩阵的转置;Ji表示所述位置对应的雅克比矩阵;AT表示障碍物指向所述机器人最近障碍物位置的向量转置;A表示障碍物指向所述机器人最近障碍物位置的向量;k表示正的控制参数; d表示所述机器人与障碍物之间的最近距离,D表示所述机器人与障碍物之间的安全距离;表示所述机器人关节角速度;ρ表示正的控制参数,其中0<ρ<1;λ1,λ2分别表示第一状态变量和第二状态变量;分别表示第一状态变量和第二状态变量的一阶导数。
S17:根据所述更新率获得所述机器人的更新状态变量和控制量,并发送至所述机器人;
在本发明具体实施过程中,所述根据所述更新率获得所述机器人的更新状态变量和控制量,包括:根据所述更新率计算所述机器人下一刻的控制量及状态变量,获得所述机器人的更新状态变量和控制量。
进一步的,所述根据所述更新率计算所述机器人下一刻的控制量及状态变量的计算公式如下:
进一步的,计算下一时刻的机器人的控制量以及状态变量:
S18:判断当前时间是否大于预设任务时间;
S19:用于判断当前时间大于预设任务时间时,结束对所述机器人的控制,若否,返回获得所述机器人的当前状态信息步骤。
在本发明实施例中,实施本发明中的方法能够提高机器人的控制效率和控制精度;能够保证机器人不超过其物理约束;只需要求取机器人与障碍物之间的最近点,具有求解效率高、计算量小的优点;同时可变性能优化指标的设计尽在机器人靠近障碍物时起作用,在不必要时可以简化规划算法。
实施例
请参阅图2,图2是本发明实施例中的协作机器人的运动规划与实时避障***的结构组成示意图。
如图2所示,一种协作机器人的运动规划与实时避障***,所述***包括:
状态信息获得模块21:用于对机器人进行初始化,在初始化后获得所述机器人的当前状态信息;
在本发明具体实施过程中,所述当前状态信息包括所述机器人的当前关节角度、当前关节角速度、当前末端位置、当前速度及当前状态变量。
在初始化之后,机器人就获得机器人的当前状态信息;该当前状态信息包括所述机器人的当前关节角度、当前关节角速度、当前末端位置、当前速度及当前状态变量。
位置关系信息获得模块22:用于获得所述机器人与障碍物之间的所形成的位置关系信息,所述位置关系信息包括所述机器人与障碍物之间的最近距离、所述机器人最靠近障碍物的位置、障碍物指向所述机器人最近障碍物位置的向量和所述位置对应的雅克比矩阵;
在本发明具体实施过程中,位置关系信息包括所述机器人与障碍物之间的最近距离、所述机器人最靠近障碍物的位置、障碍物指向所述机器人最近障碍物位置的向量和所述位置对应的雅克比矩阵;既有,通过激光雷达或者红外传感器等设备获得机器人与障碍物之间的最近距离d,再通过机器人的基座标系获得机器人最靠近障碍物的位置xi,以及获得障碍物指向机器人最靠近障碍物位置的向量A,并确定机器人最靠近障碍物的位置xi所对应的雅克比矩阵Ji。
等式约束构建模块23:基于所述机器人的期望运动轨迹确立所述机器人的等式约束;
在本发明具体实施过程中,所述基于所述机器人的期望运动轨迹确立所述机器人的等式约束,包括:
基于所述机器人当前位置与期望位置的差值与期望轨迹的速度定义所述机器人运动规划的等式约束,所述等式约束如下:
其中,e表示所述机器人当前位置与期望位置的差值,e=xd-x,xd表示期望位置,x表示所述机器人当前位置;J表示所述机器人的雅克比矩阵;表示所述机器人的期望运动速度;k,ρ表示正的控制参数,其中0<ρ<1。
具体的,根据机器人当前位置与期望位置的差值e=xd-x与期望轨迹的速度定义机器人运动规划的等式约束,等式约束如下:
优化函数构建模块24:用于基于所述最近距离构建用于躲避障碍物的实时变化的优化函数;
在本发明具体实施过程中,所述基于所述最近距离构建用于躲避障碍物的实时变化的优化函数,包括:定义所述机器人与障碍物之间的安全距离,根据所述机器人与障碍物之间的最近距离与所述安全距离之间的大小关系确立一个实时变化的优化函数。
所述实时变化的优化函数的函数公式如下:
其中,d表示所述机器人与障碍物之间的最近距离,D表示所述机器人与障碍物之间的安全距离;Ji表示所述位置对应的雅克比矩阵;AT表示障碍物指向所述机器人最近障碍物位置的向量转置;表示所述机器人关节角速度。
具体的,定义该机器人与障碍物之间的安全距离,并根据该机器人与障碍物之间的最近距离与该安全距离之间的大小关系来判断确定一个实时变化的优化函数。
该实时变化的优化函数的函数公式所定义性能优化指标如下:
其中,d表示所述机器人与障碍物之间的最近距离,D表示所述机器人与障碍物之间的安全距离;Ji表示所述位置对应的雅克比矩阵;AT表示障碍物指向所述机器人最近障碍物位置的向量转置;表示所述机器人关节角速度。
其意义为:当最小距离大于安全距离时,无需进行处理,即若k=0,优化指数-1/2kd2的导数恒为0;当最小距离小于安全距离时,此时使最靠近障碍物的位置xi得移动方向与障碍物指向机器人最靠近障碍物点的向量尽可能一致,实现避障目的。
不等式约束构建模块25:用于基于所述机器人的物理约束以及所述优化函数构建所述机器人的不等式约束;
在本发明具体实施过程中,所述基于所述机器人的物理约束以及所述优化函数构建所述机器人的不等式约束,包括:
将所述机器人的物理约束量在加速度层进行归并,并根据所述归并结果设计所述机器人最靠近障碍物的位置的不等式约束;
其中,所述将所述机器人的物理约束量在加速度层进行归并过程如下:
所述不等式约束如下:
其中,α表示正控制参数;θ表示所述机器人的关节角度;θmin表示所述机器人的关节角度的下界;θmax表示所述机器人的关节角度的上界;表示所述机器人关节角速度的下界;表示所述机器人关节角速度的上界;表示所述机器人关节角速度;AT表示障碍物指向所述机器人最近障碍物位置的向量转置;Ji表示所述位置对应的雅克比矩阵。
因此,该不等式约束如下:
其中,α表示正控制参数;θ表示所述机器人的关节角度;θmin表示所述机器人的关节角度的下界;θmax表示所述机器人的关节角度的上界;表示所述机器人关节角速度的下界;表示所述机器人关节角速度的上界;表示所述机器人关节角速度;AT表示障碍物指向所述机器人最近障碍物位置的向量转置;Ji表示所述位置对应的雅克比矩阵。
更新率计算模块26:用于基于所述等式约束和不等式约束利用在线求解计算机器人的状态变量与控制量的更新率;
在本发明具体实施过程中,所述基于所述等式约束和不等式约束利用在线求解计算机器人的状态变量与控制量的更新率公式如下:
其中,∈表示正控制参数;表示所述机器人的关节角角速度;e表示所述机器人当前位置与期望位置的差值,e=xd-x,xd表示期望位置,x所述机器人当前位置;J表示所述机器人的雅克比矩阵;JT表示所述机器人的雅克比矩阵的转置;表示所述机器人的期望速度;PΩ(·)表示一个饱和函数;表示所述位置对应的雅克比矩阵的转置;Ji表示所述位置对应的雅克比矩阵;AT表示障碍物指向所述机器人最近障碍物位置的向量转置;A表示障碍物指向所述机器人最近障碍物位置的向量;k表示正的控制参数; d表示所述机器人与障碍物之间的最近距离,D表示所述机器人与障碍物之间的安全距离;表示所述机器人关节角速度;ρ表示正的控制参数,其中0<ρ<1;λ1,λ2分别表示第一状态变量和第二状态变量;分别表示第一状态变量和第二状态变量的一阶导数。
具体的,根据等式约束和不等式约束利用在线求解计算机器人的状态变量与控制量的更新率公式如下:
其中,∈表示正控制参数;表示所述机器人的关节角角速度;e表示所述机器人当前位置与期望位置的差值,e=xd-x,xd表示期望位置,x所述机器人当前位置;J表示所述机器人的雅克比矩阵;JT表示所述机器人的雅克比矩阵的转置;表示所述机器人的期望速度;PΩ(·)表示一个饱和函数;表示所述位置对应的雅克比矩阵的转置;Ji表示所述位置对应的雅克比矩阵;AT表示障碍物指向所述机器人最近障碍物位置的向量转置;A表示障碍物指向所述机器人最近障碍物位置的向量;k表示正的控制参数; d表示所述机器人与障碍物之间的最近距离,D表示所述机器人与障碍物之间的安全距离;表示所述机器人关节角速度;ρ表示正的控制参数,其中0<ρ<1;λ1,λ2分别表示第一状态变量和第二状态变量;分别表示第一状态变量和第二状态变量的一阶导数。
更新控制模块27:用于根据所述更新率获得所述机器人的更新状态变量和控制量,并发送至所述机器人;
在本发明具体实施过程中,所述根据所述更新率获得所述机器人的更新状态变量和控制量,包括:根据所述更新率计算所述机器人下一刻的控制量及状态变量,获得所述机器人的更新状态变量和控制量。
进一步的,所述根据所述更新率计算所述机器人下一刻的控制量及状态变量的计算公式如下:
进一步的,计算下一时刻的机器人的控制量以及状态变量:
判断模块28:用于判断当前时间是否大于预设任务时间,若是,则结束,若否,返回获得所述机器人的当前状态信息步骤。
在本发明实施例中,实施本发明中的方法能够提高机器人的控制效率和控制精度;能够保证机器人不超过其物理约束;只需要求取机器人与障碍物之间的最近点,具有求解效率高、计算量小的优点;同时可变性能优化指标的设计尽在机器人靠近障碍物时起作用,在不必要时可以简化规划算法。
本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成,该程序可以存储于一计算机可读存储介质中,存储介质可以包括:只读存储器(ROM,Read Only Memory)、随机存取存储器(RAM,RandomAccess Memory)、磁盘或光盘等。
另外,以上对本发明实施例所提供的一种协作机器人的运动规划与实时避障方法及***进行了详细介绍,本文中应采用了具体个例对本发明的原理及实施方式进行了阐述,以上实施例的说明只是用于帮助理解本发明的方法及其核心思想;同时,对于本领域的一般技术人员,依据本发明的思想,在具体实施方式及应用范围上均会有改变之处,综上所述,本说明书内容不应理解为对本发明的限制。
Claims (10)
1.一种协作机器人的运动规划与实时避障方法,其特征在于,所述方法包括:
对机器人进行初始化,在初始化后获得所述机器人的当前状态信息;
获得所述机器人与障碍物之间的所形成的位置关系信息,所述位置关系信息包括所述机器人与障碍物之间的最近距离、所述机器人最靠近障碍物的位置、障碍物指向所述机器人最近障碍物位置的向量和所述位置对应的雅克比矩阵;
基于所述机器人的期望运动轨迹确立所述机器人的等式约束;
基于所述最近距离构建用于躲避障碍物的实时变化的优化函数;
基于所述机器人的物理约束以及所述优化函数构建所述机器人的不等式约束;
基于所述等式约束和不等式约束利用在线求解计算机器人的状态变量与控制量的更新率;
根据所述更新率获得所述机器人的更新状态变量和控制量,并发送至所述机器人;
判断当前时间是否大于预设任务时间,若是,则结束,若否,返回获得所述机器人的当前状态信息步骤。
2.根据权利要求1所述的运动规划与实时避障方法,其特征在于,所述当前状态信息包括所述机器人的当前关节角度、当前关节角速度、当前末端位置、当前速度及当前状态变量。
4.根据权利要求1所述的运动规划与实时避障方法,其特征在于,所述基于所述最近距离构建用于躲避障碍物的实时变化的优化函数,包括:
定义所述机器人与障碍物之间的安全距离,根据所述机器人与障碍物之间的最近距离与所述安全距离之间的大小关系确立一个实时变化的优化函数。
6.根据权利要求1所述的运动规划与实时避障方法,其特征在于,所述基于所述机器人的物理约束以及所述优化函数构建所述机器人的不等式约束,包括:
将所述机器人的物理约束量在加速度层进行归并,并根据所述归并结果设计所述机器人最靠近障碍物的位置的不等式约束;
其中,所述将所述机器人的物理约束量在加速度层进行归并过程如下:
所述不等式约束如下:
7.根据权利要求1所述的运动规划与实时避障方法,其特征在于,所述基于所述等式约束和不等式约束利用在线求解计算机器人的状态变量与控制量的更新率公式如下:
其中,∈表示正控制参数;表示所述机器人的关节角加速度;e表示所述机器人当前位置与期望位置的差值,e=xd-x,xd表示期望位置,x所述机器人当前位置;J表示所述机器人的雅克比矩阵;JT表示所述机器人的雅克比矩阵的转置;表示所述机器人的期望速度;PΩ(·)表示一个饱和函数;表示所述位置对应的雅克比矩阵的转置;Ji表示所述位置对应的雅克比矩阵;AT表示障碍物指向所述机器人最近障碍物位置的向量转置;A表示障碍物指向所述机器人最近障碍物位置的向量;k表示正的控制参数; d表示所述机器人与障碍物之间的最近距离,D表示所述机器人与障碍物之间的安全距离;表示所述机器人关节角速度;ρ表示正的控制参数,其中0<ρ<1;λ1,λ2分别表示第一状态变量和第二状态变量;分别表示第一状态变量和第二状态变量的一阶导数。
8.根据权利要求1所述的运动规划与实时避障方法,其特征在于,所述根据所述更新率获得所述机器人的更新状态变量和控制量,包括:
根据所述更新率计算所述机器人下一刻的控制量及状态变量,获得所述机器人的更新状态变量和控制量。
10.一种协作机器人的运动规划与实时避障***,其特征在于,所述***包括:
状态信息获得模块:用于对机器人进行初始化,在初始化后获得所述机器人的当前状态信息;
位置关系信息获得模块:用于获得所述机器人与障碍物之间的所形成的位置关系信息,所述位置关系信息包括所述机器人与障碍物之间的最近距离、所述机器人最靠近障碍物的位置、障碍物指向所述机器人最近障碍物位置的向量和所述位置对应的雅克比矩阵;
等式约束构建模块:基于所述机器人的期望运动轨迹确立所述机器人的等式约束;
优化函数构建模块:用于基于所述最近距离构建用于躲避障碍物的实时变化的优化函数;
不等式约束构建模块:用于基于所述机器人的物理约束以及所述优化函数构建所述机器人的不等式约束;
更新率计算模块:用于基于所述等式约束和不等式约束利用在线求解计算机器人的状态变量与控制量的更新率;
更新控制模块:用于根据所述更新率获得所述机器人的更新状态变量和控制量,并发送至所述机器人;
判断模块:用于判断当前时间是否大于预设任务时间,若是,则结束,若否,返回获得所述机器人的当前状态信息步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911382289.7A CN110977989B (zh) | 2019-12-27 | 2019-12-27 | 一种协作机器人的运动规划与实时避障方法及*** |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911382289.7A CN110977989B (zh) | 2019-12-27 | 2019-12-27 | 一种协作机器人的运动规划与实时避障方法及*** |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110977989A CN110977989A (zh) | 2020-04-10 |
CN110977989B true CN110977989B (zh) | 2021-04-23 |
Family
ID=70078286
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201911382289.7A Active CN110977989B (zh) | 2019-12-27 | 2019-12-27 | 一种协作机器人的运动规划与实时避障方法及*** |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110977989B (zh) |
Families Citing this family (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112873208A (zh) * | 2021-01-29 | 2021-06-01 | 佛山树客智能机器人科技有限公司 | 一种抗噪与动力学约束的机器人实时运动规划方法及装置 |
CN112965482B (zh) * | 2021-02-01 | 2023-03-10 | 广东省科学院智能制造研究所 | 一种多机器人运动避碰控制方法及*** |
CN113618742B (zh) * | 2021-08-24 | 2022-07-29 | 深圳市优必选科技股份有限公司 | 一种机器人避障方法、装置和机器人 |
CN113618739B (zh) * | 2021-08-24 | 2022-07-29 | 深圳市优必选科技股份有限公司 | 一种机器人动态避障方法、装置和机器人 |
CN114227686B (zh) * | 2021-12-28 | 2024-07-19 | 深圳市优必选科技股份有限公司 | 一种机器人避障控制方法、装置、终端设备及存储介质 |
CN115990874A (zh) * | 2022-11-05 | 2023-04-21 | 北京化工大学 | 一种超冗余机械臂分段式动态避障方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2003102497A1 (en) * | 2002-06-04 | 2003-12-11 | Alken Inc. Dba Polhemus | Method and apparatus for electromagnetic position and orientation tracking with distortion compensation employing modulated signal |
CN102773858A (zh) * | 2012-07-17 | 2012-11-14 | 北京航空航天大学 | 一种码垛机器人的避障方法 |
CN104392081A (zh) * | 2014-06-30 | 2015-03-04 | 管小清 | 一种冗余机械臂运动障碍物避障算法 |
CN105150210A (zh) * | 2015-05-13 | 2015-12-16 | 西北工业大学 | 一种用于遥操作人机交互的虚拟管道动态避障控制方法 |
CN108714894A (zh) * | 2018-05-03 | 2018-10-30 | 华南理工大学 | 一种求解双冗余机械臂互相碰撞的动力学方法 |
CN108908331A (zh) * | 2018-07-13 | 2018-11-30 | 哈尔滨工业大学(深圳) | 超冗余柔性机器人的避障方法及***、计算机存储介质 |
-
2019
- 2019-12-27 CN CN201911382289.7A patent/CN110977989B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2003102497A1 (en) * | 2002-06-04 | 2003-12-11 | Alken Inc. Dba Polhemus | Method and apparatus for electromagnetic position and orientation tracking with distortion compensation employing modulated signal |
CN102773858A (zh) * | 2012-07-17 | 2012-11-14 | 北京航空航天大学 | 一种码垛机器人的避障方法 |
CN104392081A (zh) * | 2014-06-30 | 2015-03-04 | 管小清 | 一种冗余机械臂运动障碍物避障算法 |
CN105150210A (zh) * | 2015-05-13 | 2015-12-16 | 西北工业大学 | 一种用于遥操作人机交互的虚拟管道动态避障控制方法 |
CN108714894A (zh) * | 2018-05-03 | 2018-10-30 | 华南理工大学 | 一种求解双冗余机械臂互相碰撞的动力学方法 |
CN108908331A (zh) * | 2018-07-13 | 2018-11-30 | 哈尔滨工业大学(深圳) | 超冗余柔性机器人的避障方法及***、计算机存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN110977989A (zh) | 2020-04-10 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110977989B (zh) | 一种协作机器人的运动规划与实时避障方法及*** | |
CN112859859B (zh) | 一种基于三维障碍物体素对象映射的动态栅格地图更新方法 | |
Sudhakara et al. | Trajectory planning of a mobile robot using enhanced A-star algorithm | |
CN109491389B (zh) | 一种具有速度约束的机器人轨迹跟踪方法 | |
CN110986953B (zh) | 路径规划方法、机器人及计算机可读存储介质 | |
CN110682286B (zh) | 一种协作机器人实时避障方法 | |
Dong et al. | A high-performance flight control approach for quadrotors using a modified active disturbance rejection technique | |
CN108444490B (zh) | 基于可视图和a*算法深度融合的机器人路径规划方法 | |
CN113618742B (zh) | 一种机器人避障方法、装置和机器人 | |
CN111596654A (zh) | 基于改进d星路径规划算法的电缆沟机器人导航避障方法 | |
CN111176272B (zh) | 一种基于运动约束的人工势场轨迹规划方法及*** | |
CN110561420B (zh) | 臂型面约束柔性机器人轨迹规划方法及装置 | |
CN108772835B (zh) | 一种障碍物与物理极限躲避方法 | |
CN114721275B (zh) | 一种基于预设性能的视觉伺服机器人自适应跟踪控制方法 | |
CN114460965B (zh) | 一种基于改进人工势场法的无人机三维避障方法 | |
CN114594788A (zh) | 一种未知环境下四旋翼无人飞行器航迹规划方法及*** | |
CN114564008A (zh) | 基于改进A-Star算法的移动机器人路径规划方法 | |
Kabutan et al. | Motion planning by T‐RRT with potential function for vertical articulated robots | |
CN113199481B (zh) | 机器人运动控制方法、装置、电子设备及介质 | |
WO2021068980A1 (zh) | 三维地图生成方法、移动装置和计算机可读存储介质 | |
Vukosavljev et al. | Safe and robust robot maneuvers based on reach control | |
Krafes et al. | Vision-based control of a flying spherical inverted pendulum | |
Bulgakov et al. | Dispersed cyber-physical coordination and path planning using unmanned aerial vehicle | |
Sun et al. | Multi-agent formation control based on artificial force with exponential form | |
Hu et al. | An obstacle avoidance design of UAV based on genetic algorithm |
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 | ||
CP01 | Change in the name or title of a patent holder |
Address after: 510070 Building No. 15, compound 100, martyrs Road, Guangzhou, Guangdong Patentee after: Institute of intelligent manufacturing, Guangdong Academy of Sciences Address before: 510070 Building No. 15, compound 100, martyrs Road, Guangzhou, Guangdong Patentee before: GUANGDONG INSTITUTE OF INTELLIGENT MANUFACTURING |
|
CP01 | Change in the name or title of a patent holder |