CN113253744A - 多机器人协同轨迹规划方法、装置、电子设备和存储介质 - Google Patents

多机器人协同轨迹规划方法、装置、电子设备和存储介质 Download PDF

Info

Publication number
CN113253744A
CN113253744A CN202110803758.9A CN202110803758A CN113253744A CN 113253744 A CN113253744 A CN 113253744A CN 202110803758 A CN202110803758 A CN 202110803758A CN 113253744 A CN113253744 A CN 113253744A
Authority
CN
China
Prior art keywords
robot
planned
trajectory
track
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.)
Granted
Application number
CN202110803758.9A
Other languages
English (en)
Other versions
CN113253744B (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.)
Institute of Automation of Chinese Academy of Science
Original Assignee
Institute of Automation of Chinese Academy of Science
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 Institute of Automation of Chinese Academy of Science filed Critical Institute of Automation of Chinese Academy of Science
Priority to CN202110803758.9A priority Critical patent/CN113253744B/zh
Publication of CN113253744A publication Critical patent/CN113253744A/zh
Application granted granted Critical
Publication of CN113253744B publication Critical patent/CN113253744B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明提供一种多机器人协同轨迹规划方法、装置、电子设备和存储介质，其中方法包括：将机器人划分为轨迹规划组和不变轨迹组；根据轨迹规划组中各个待规划机器人在当前时刻的位置、速度以及目标时刻的位置、速度，分别构建各个待规划机器人对应的包含待优化参数的时空贝塞尔曲线轨迹方程；基于粒子群算法以及多机器人需满足的物理约束，依次求解各个待规划机器人对应的时空贝塞尔曲线轨迹方程中的待优化参数，得到各个待规划机器人的同步时间轨迹，以供各个待规划机器人沿所述轨迹协同运动。本发明扩大了轨迹规划方法的适用范围，可用于地面移动机器人、无人机、水下机器人等多种类型机器人的二维或三维实时轨迹规划。

Description

多机器人协同轨迹规划方法、装置、电子设备和存储介质
技术领域
本发明涉及自动化技术领域，尤其涉及一种多机器人协同轨迹规划方法、装置、电子设备和存储介质。
背景技术
当前，机器人广泛应用于仓储服务、航空航天、水下作业等领域。面对愈加复杂的任务和多变的工作环境，单机器人系统由于其自身感知处理信息能力、决策能力和任务执行能力的限制，在面对一些大型的需要高效并行完成的任务时往往难以胜任。受到群体智能的启发，利用多机器人组成协作系统，有利于提高作业效率和系统的鲁棒性。
多机器人协同轨迹规划是多机器人协作的关键，轨迹规划通常可分为路径规划和沿路径点的时间优化两部分，针对路径规划问题，目前方法较为成熟，常用的算法有 D*算法、A*算法、RRT*算法、人工势场法等。但是这些方法给出的一系列路径点，往往不满足机器人的物理约束，还需进行路径平滑并给出路径点的时间信息，因此难以应用于地面移动机器人、无人机、水下机器人等二维或三维实时轨迹规划场合。
在进行多机器人三维实时协同轨迹规划时，需综合考虑机器人的空间结构和运动过程中的约束条件，规划出多条平滑且满足多种物理约束的轨迹。一类常见的轨迹规划方法是基于参数化曲线轨迹规划法，其中有代表性的方法包括贝塞尔曲线法、Dubins曲线法、多项式轨迹平滑法等。其中，贝塞尔曲线作为一种速度、加速度和曲率均连续的参数化曲线，适合作为多机器人轨迹规划的工具。例如，胡峰等人提出基于贝塞尔曲线的多机器人路径规划和队形重构方法；王宇等人提出基于时空贝塞尔曲线的多水下机器人以最短时间同时到达目标点为目标的参数搜索方法。尽管上述方法在特定条件下的多机器人轨迹规划效果较好，但这些方法只考虑在二维平面进行轨迹规划，且未考虑地形等环境因素以及实时避障问题，应用场景受到较大制约。
因此，根据多机器人自身携带的传感器获取周围环境信息，综合多机器人运动需满足的约束条件，进行多机器人实时协同轨迹规划是一个待解决的技术问题。
发明内容
本发明提供一种多机器人协同轨迹规划方法、装置、电子设备和存储介质，用以解决现有技术中应用场景受到较大制约的缺陷。
本发明提供一种多机器人协同轨迹规划方法，包括：
将机器人划分为轨迹规划组和不变轨迹组；
根据所述轨迹规划组中各个待规划机器人在当前时刻的位置、速度以及目标时刻的位置、速度，分别构建各个待规划机器人对应的包含待优化参数的时空贝塞尔曲线轨迹方程；
基于粒子群算法以及多机器人需满足的物理约束，依次求解各个待规划机器人对应的时空贝塞尔曲线轨迹方程中的待优化参数，得到各个待规划机器人的同步时间轨迹，以供各个待规划机器人沿所述轨迹协同运动；
其中，求解任一待规划机器人对应的时空贝塞尔曲线轨迹方程中的待优化参数时，所述任一待规划机器人需与所述不变轨迹组中的各个已规划机器人满足避碰安全距离约束，并在求解完毕后将所述任一待规划机器人移动到所述不变轨迹组中，直到轨迹规划组为空时，全部待规划机器人完成轨迹规划。
根据本发明提供一种的多机器人协同轨迹规划方法，任一待规划机器人对应的包含待优化参数的时空贝塞尔曲线轨迹方程为：
Figure 453553DEST_PATH_IMAGE001
其中，P(τ)为时空贝塞尔曲线轨迹方程，自变量τ的取值范围为[0, 1]，伯恩斯坦基函数B(τ)为：
Figure 936487DEST_PATH_IMAGE002
其中，P(τ)包含四个控制点P i (x i , y i , z i , t i )，i=0、1、2、3；
(x i , y i , z i )表示P i 的三维空间位置，t i 表示P i 的时刻；P 0(x 0, y 0, z 0, t 0)表示时空贝塞尔曲线的初始位置和时刻，P 3(x 3, y 3, z 3, t 3)表示时空贝塞尔曲线的目标位置和时刻；中间控制点P 1(x 1, y 1, z 1)和P 2(x 2, y 2, z 2)的位置分别表示为：
Figure 720641DEST_PATH_IMAGE003
其中，(v x0, v y0, v z0)表示时空贝塞尔曲线的初始速度，(v x3, v y3, v z3)表示时空贝塞尔曲线的目标速度；所述待优化参数为t 1和t 2。
根据本发明提供的一种多机器人协同轨迹规划方法，所述基于粒子群算法以及多机器人需满足的物理约束，依次求解各个待规划机器人对应的时空贝塞尔曲线轨迹方程中的待优化参数，得到各个待规划机器人的同步时间轨迹，具体包括：
初始化步骤：随机初始化粒子群算法中各个粒子的位置和速度；其中，各个粒子的位置对应t 1和t 2的一组取值；
优化步骤：更新各个粒子的位置和速度，基于各个粒子的位置求解得到所述任一待规划机器人的多个时空贝塞尔曲线轨迹方程，离散化各个轨迹方程，并基于所述物理约束计算各个粒子对应的目标函数值，从而确定整个种群中当前最优解对应粒子的位置；
迭代步骤：重复执行所述优化步骤，不断优化各个粒子的位置，直至达到终止条件；基于整个种群中最优解对应粒子的位置，确定所述任一待规划机器人对应的最优时空贝塞尔曲线轨迹方程，即得到所述任一待规划机器人的最优轨迹；
其中，求解各个待规划机器人的同步时间轨迹时，根据如下关系：
Figure 37353DEST_PATH_IMAGE004
其中，t为时间，当满足t 1, t 2∈[t 0, t 3]时，时间函数t(τ)恒为增函数，因此，基于一组离散化t值计算出各个待规划机器人对应的离散化τ值，再代入时空贝塞尔曲线轨迹方程，可满足各个机器人轨迹离散点的时刻对应相等，即求得各个待规划机器人的同步时间轨迹。
根据本发明提供的一种多机器人协同轨迹规划方法，任一粒子对应的目标函数值基于如下公式计算：
Figure 24900DEST_PATH_IMAGE005
其中，f(t 1, t 2)为所述任一粒子对应的目标函数值，所述任一粒子的位置为(t 1,t 2)；S为(t 1, t 2)对应的时空贝塞尔曲线轨迹的路程，M k 为预设数值，其中k∈[1, 6], k∈N*，δ k 为判别系数，包含L的积分项表示沿路径点的线积分，实际利用各个相邻离散点的距离总和近似该项。
根据本发明提供的一种多机器人协同轨迹规划方法，所述判别系数基于如下步骤确定：
基于所述任一粒子的位置，对所述任一待规划机器人的时空贝塞尔曲线轨迹方程进行离散化；
基于离散化的时空贝塞尔曲线轨迹，计算所述任一待规划机器人在每个离散点处的速度、加速度、曲率、与障碍物表面间的距离、运动轨迹高度以及与不变轨迹组中任一机器人间的距离；
基于所述任一待规划机器人在每个离散点处的速度、加速度、曲率、与障碍物表面间的距离、运动轨迹高度以及与不变轨迹组中任一机器人间的距离，以及所述物理约束，确定所述判别系数。
根据本发明提供的一种多机器人协同轨迹规划方法，所述物理约束包括最大速度约束、最大加速度约束、最大曲率约束、避障安全距离约束、地形安全高度约束和避碰安全距离约束。
根据本发明提供的一种多机器人协同轨迹规划方法，所述将机器人划分为轨迹规划组和不变轨迹组，具体包括：
在初始阶段，所有机器人在初始位置等待轨迹规划，因此将所有机器人划分到所述轨迹规划组中，所述不变轨迹组为空；
在中间阶段，所有机器人沿已规划好的轨迹运动，在运动过程中的任一时刻，若任一或多个机器人观测且将碰触到新的障碍物，则将所述任一或多个机器人划分到所述轨迹规划组，其余机器人划分到所述不变轨迹组；若任一或多个机器人轨迹规划失败，则将所有机器人划分到所述轨迹规划组中，所述不变轨迹组为空。
本发明还提供一种多机器人协同轨迹规划装置，包括：
待规划机器人确定单元，用于将机器人划分为轨迹规划组和不变轨迹组；
轨迹方程构建单元，用于根据所述轨迹规划组中各个待规划机器人在当前时刻的位置、速度以及目标时刻的位置、速度，分别构建各个待规划机器人对应的包含待优化参数的时空贝塞尔曲线轨迹方程；
协同轨迹规划单元，用于基于粒子群算法以及多机器人需满足的物理约束，依次求解各个待规划机器人对应的时空贝塞尔曲线轨迹方程中的待优化参数，得到各个待规划机器人的同步时间轨迹，以供各个待规划机器人沿所述轨迹协同运动；
其中，求解任一待规划机器人对应的时空贝塞尔曲线轨迹方程中的待优化参数时，所述任一待规划机器人需与所述不变轨迹组中的各个已规划机器人满足避碰安全距离约束，并在求解完毕后将所述任一待规划机器人移动到所述不变轨迹组中，直到轨迹规划组为空时，全部待规划机器人完成轨迹规划。
本发明还提供一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如上述任一种所述多机器人协同轨迹规划方法的步骤。
本发明还提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现如上述任一种所述多机器人协同轨迹规划方法的步骤。
本发明提供的多机器人协同轨迹规划方法、装置、电子设备和存储介质，根据各个待规划机器人在当前时刻的位置、速度以及目标时刻的位置、速度，基于粒子群算法在满足多种物理约束的情况下，依次优化得到各个待规划机器人的时空贝塞尔曲线轨迹方程，并且满足全部机器人的轨迹具有时间同步性，扩大了轨迹规划方法的适用范围，可用于地面移动机器人、无人机、水下机器人等多种类型机器人的二维或三维实时轨迹规划。
附图说明
为了更清楚地说明本发明或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明提供的多机器人协同轨迹规划方法的流程示意图；
图2为本发明提供的复杂地形下机器人运动轨迹及其投影轨迹示意图；
图3为本发明提供的机器人运动过程中实时避障示意图；
图4为本发明提供的多机器人协同轨迹规划方法的详细流程示意图；
图5为本发明提供的多机器人协同轨迹规划装置的结构示意图；
图6-图9为本发明提供的多机器人三维空间轨迹规划运动过程示意图；
图10-图17为本发明提供的多机器人运动过程中各物理量随时间变化关系图；
图18为本发明提供的电子设备的结构示意图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明中的附图，对本发明中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
图1为本发明实施例提供的多机器人协同轨迹规划方法的流程示意图，如图1所示，该方法包括：
步骤110，将机器人划分为轨迹规划组和不变轨迹组。
具体地，将多个机器人划分为轨迹规划组和不变轨迹组，其中，轨迹规划组中包括需要进行轨迹规划的待规划机器人，不变轨迹组包括已规划好轨迹的已规划机器人。在初始阶段，轨迹规划组包含全体机器人；在中间阶段，轨迹规划组包含部分需要避障的机器人，在这部分机器人轨迹规划失败时，轨迹规划组包含全体机器人。在此之前，需进行初始化和实时采集操作，确定全局地形图信息，给定多机器人运动需满足的物理约束以及各个待规划机器人在初始时刻及目标时刻的位置和速度，通过定位系统和观测系统，采集每个机器人实时位置、速度，并观测当前位置一定半径范围内的障碍物信息。
步骤120，根据轨迹规划组中各个待规划机器人在当前时刻的位置、速度以及目标时刻的位置、速度，分别构建各个待规划机器人对应的包含待优化参数的时空贝塞尔曲线轨迹方程。
具体地，将轨迹规划组中任一待规划机器人对应的当前时刻、当前位置和当前速度作为初始时刻、初始位置和初始速度，结合其对应的目标时刻、目标位置和目标速度，可以构建该待规划机器人对应的包含待优化参数的时空贝塞尔曲线轨迹方程。其中，待优化参数的取值范围由初始时刻和目标时刻确定。
步骤130，基于粒子群算法以及多机器人需满足的物理约束，依次求解各个待规划机器人对应的时空贝塞尔曲线轨迹方程中的待优化参数，得到各个待规划机器人的同步时间轨迹，以供各个待规划机器人沿轨迹协同运动；
其中，求解任一待规划机器人对应的时空贝塞尔曲线轨迹方程中的待优化参数时，该待规划机器人需与不变轨迹组中的各个已规划机器人满足避碰安全距离约束，并在求解完毕后将该待规划机器人移动到不变轨迹组中，直到轨迹规划组为空时，全部待规划机器人完成轨迹规划。
具体地，依次利用粒子群算法对每个待规划机器人对应的时空贝塞尔曲线轨迹方程进行全局寻优，求解时空贝塞尔曲线轨迹方程中的待优化参数，从而得到各个待规划机器人的轨迹。其中，在利用粒子群算法寻找待优化参数的最优解时，将多机器人需满足的物理约束作为约束条件，优化目标为轨迹路程最短。
针对轨迹规划组的每个待规划机器人利用上述方法依次规划其轨迹。在每次轨迹规划过程中，轨迹规划组的待规划机器人与所有不变轨迹组机器人均需满足避碰安全距离约束，即满足该机器人与所有不变轨迹组机器人的同步时间轨迹离散点在相同时刻的距离始终大于避碰安全距离。当该待规划机器人完成轨迹规划时，将该待规划机器人从轨迹规划组移动到不变轨迹组，直到轨迹规划组的待规划机器人数目减为0，完成多机器人协同轨迹规划。
本发明实施例提供的方法，根据各个待规划机器人在当前时刻的位置、速度以及目标时刻的位置、速度，基于粒子群算法在满足多种物理约束的情况下，依次优化得到各个待规划机器人的时空贝塞尔曲线轨迹方程，并且满足全部机器人的轨迹具有时间同步性，扩大了轨迹规划方法的适用范围，可用于地面移动机器人、无人机、水下机器人等多种类型机器人的二维或三维实时轨迹规划。
基于上述任一实施例，在轨迹规划前的初始化和信息采集操作具体包括：
确定全局地形图信息，通过创建稀疏网格，在网格点处输入对应高度值，再利用三次插值法，得到包含地形高度的稠密网格。图2为本发明实施例提供的复杂地形下机器人运动轨迹及其投影轨迹示意图，如图2所示，利用稠密网格表示地形面的轮廓，得到全局地形图信息。
给定多机器人运动需满足的物理约束，包括最大速度约束v max、最大加速度约束a max、最大曲率约束K max、避障安全距离约束B safe、地形安全高度约束H safe和避碰安全距离约束D safe。其中，v max, a max, K max根据机器人的运动约束而定，D safe根据机器人的空间结构大小而定，B safe根据不同的障碍物的三维空间大小而有所不同，H safe表示机器人运动轨迹沿地形面的投影高度，如图2所示，机器人i从起始点F s,i 沿轨迹C i 运动到目标点F g,i ，运动轨迹C i 沿地形面的投影轨迹为H i ，H i 的高度即为H safe，因此，H safe的数值因投影点位置的不同而改变。
给定多机器人总数为N，并给定每个机器人的初始时刻t s 、初始位置(x s,i , y s,i ,z s,i )、初始速度(v xs,i , v ys,i , v zs,i )、目标时刻t g 、目标位置(x g,i , y g,i , z g,i )以及目标速度(v xg,i , v yg,i , v zg,i )，其中i为机器人编号(i∈[1, N], i∈N*)。
在初始化操作之后，通过定位系统和观测系统，采集每个机器人的实时位置和实时速度，并观测当前位置一定半径范围内的障碍物信息。在非预知环境中，机器人在运动过程中随时会遇到障碍物，假设在多机器人运动过程中的t m 时刻(t m ∈[t s , t g ])，利用定位系统采集每个机器人的实时位置(x m,i , y m,i , z m,i )和实时速度(v xm,i , v ym,i , v zm,i )，每个机器人通过观测系统在半径d范围内实时观测当前时刻周围的障碍物信息。设障碍物j的中心位置为(x j , y j , z j )，避障安全距离为B safe(j)，B safe(j)同时作为障碍物j的中心到其表面的距离。图3为本发明实施例提供的机器人运动过程中实时避障示意图，如图3所示，将观测到障碍物表面作为观测到障碍物的依据，即满足如下关系视为机器人i观测到障碍物j：
Figure 62258DEST_PATH_IMAGE006
基于上述任一实施例，步骤110具体包括：
在初始阶段，所有N个机器人均需要进行轨迹规划，因此将所有机器人划分到轨迹规划组中。在中间阶段，多机器人在沿轨迹运动过程中，若未观测到障碍物，或观测到的障碍物与全部机器人所规划的轨迹无干涉，则全部机器人正常运动。若部分机器人观测到并将碰触新的障碍物，则将这部分机器人视为待规划机器人，进行轨迹重规划。设轨迹规划组中有N k 个机器人需进行轨迹规划，则N k 的取值如下
Figure 186072DEST_PATH_IMAGE007
其中，n表示观测到并将碰触新的障碍物的机器人数(n∈[1, N], n∈N*)。在中间轨迹重规划阶段，将n个机器人划分到轨迹规划组，即式(2)中，N k =n。
上述n个机器人的轨迹重规划结束之后，若仍存在机器人规划的轨迹不满足全部物理约束，则将全部N个机器人划分到轨迹规划组，即式(2)中，N k =N。
当轨迹规划组的N k 个机器人确定以后，剩余N-N k 个机器人被自然划分到不变轨迹组。
在中间阶段，任一机器人实时避障的具体过程如图3所示，机器人i规划出一条从初始点P 0到目标点P 3的轨迹C i ，障碍物Block(j)与该机器人所规划的轨迹相交，机器人的观测半径为d。机器人在运动过程中的中间点P 0,new处观测并得到障碍物Block(j)的中心位置(x j , y j , z j )和避障安全距离B safe(j)，此时以中间点P 0,new为起点，将该点处的速度设为新的初始速度，仍以到达目标点P 3为目标，机器人重新规划出一条轨迹C i ,new，通过以上方法实现实时避障的目的。
基于上述任一实施例，步骤120具体包括：
根据任一待规划机器人在当前时刻的位置、速度以及目标时刻的位置和速度，构建包含待优化参数的时空贝塞尔曲线轨迹方程P(τ)：
Figure 782007DEST_PATH_IMAGE008
其中，P(τ)为三次时空贝塞尔曲线轨迹方程，伯恩斯坦基函数B(τ)具体如下
Figure 256851DEST_PATH_IMAGE009
如式(3)所示，P(τ)包含四个控制点P i (x i , y i , z i , t i ) (i=0, 1, 2, 3)，其中，(x i , y i , z i )表示P i 的三维空间位置，t i 表示P i 的时刻。P 0(x 0, y 0, z 0, t 0)表示时空贝塞尔曲线的初始位置和初始时刻，即待规划机器人i的当前实时位置(x m,i , y m,i , z m,i )和当前时刻t m ，P 3(x 3, y 3, z 3, t 3)表示时空贝塞尔曲线的目标位置和目标时刻，即待规划机器人的目标位置(x g,i , y g,i , z g,i )和目标时刻t g 。
据式(3)，对其求一阶导数，当τ分别取0和1时
Figure 363478DEST_PATH_IMAGE010
设(v x0, v y0, v z0)为时空贝塞尔曲线的初始速度，即待规划机器人的当前速度(v xm,i , v ym,i , v zm,i )，(v x3, v y3, v z3)为时空贝塞尔曲线的目标速度，即待规划机器人的目标速度(v xg,i , v yg,i , v zg,i )。据式(5)，当τ=0和τ=1时，(v x0, v y0, v z0)和(v x3, v y3, v z3)分别为
Figure 607377DEST_PATH_IMAGE011
因此，P 1(x 1, y 1, z 1)，P 2(x 2, y 2, z 2)的空间位置可分别表示如下
Figure 108635DEST_PATH_IMAGE012
其中，t1, t2为待优化参数，另据式(3)，t与τ存在如下关系
Figure 70775DEST_PATH_IMAGE013
可以证明当满足t 1, t 2∈[t 0, t 3]时，式(8)中t(τ)恒为增函数，满足时间的不可逆性。因此，将t 1, t 2∈[t 0, t 3]为待优化参数的取值范围，即粒子群算法中每个粒子位置的取值范围。
基于上述任一实施例，步骤130具体包括：
在初始化过程中，设粒子群算法中粒子数为M，最大迭代次数为T，在区间[t 0, t 3]中随机初始化M个粒子的位置x j,1和速度v j,1, (j∈[1, M], j∈N*)，每个粒子的维数为2，对应t 1, t 2。
在优化过程中，选取P(τ)的路程最短作为优化目标，所采用的目标函数为：
Figure 449934DEST_PATH_IMAGE014
其中，f(t 1, t 2)为所述任一粒子对应的目标函数值，所述任一粒子的位置为(t 1,t 2)；S为(t 1, t 2)对应的时空贝塞尔曲线轨迹的路程，M k 为预设数值，当M k 为大数时，可保证f值足够大。其中k∈[1, 6], k∈N*，δ k 为判别系数，包含L的积分项表示沿路径点的线积分，实际利用各个相邻离散点的距离总和近似该项。
δ k 可以基于物理约束，通过以下方式确定得到：
Figure 813920DEST_PATH_IMAGE015
具体地，计算δ k 时，考虑到检查条件1至6时，需计算相应表达式的极值，其计算难度较大，因此对轨迹方程P(τ)进行离散化。再考虑到条件6中比较机器人两两之间距离时，时间必须保持同步性，因此需要确定各个机器人的同步时间轨迹。
据式(8)可以证明当满足t 1, t 2∈[t 0, t 3]时，t(τ)恒为增函数，故t与τ满足一一对应关系，基于一组离散化t值计算出各个机器人对应的离散化τ值，再代入时空贝塞尔曲线轨迹方程P(τ)，即可满足各个机器人轨迹离散点的时刻对应相等，即求得各个机器人的同步时间轨迹。
根据离散化后的轨迹，计算该待规划机器人在每个离散点处的速度v、加速度a、曲率K、与全部已知障碍物表面间的距离B、运动轨迹高度H以及与不变轨迹组机器人间的距离D，并据此判断条件1至条件6是否始终满足，从而得到判别系数。同时，根据每个离散点的位置可以计算出P(τ)的路程S，最终求出f(t 1, t 2)的值。
在迭代过程中，根据M个粒子前一次迭代的速度和位置，更新其速度和位置，具体如下：
Figure 491938DEST_PATH_IMAGE016
其中，x j,k 和v j,k 分别表示粒子j在第k次迭代时的位置和速度，p j,k 表示粒子j前k次迭代搜索到的最优解的位置，g k 是整个种群前k次迭代搜索到最优解的位置。初始时，M个粒子的目标函数f j,1与p j,1相等。c 0, c 1, c 2表示群体认知系数，可以根据实际需求调节，使得种群按理想速度收敛。
在轨迹规划组的任一待规划机器人的轨迹规划完毕后，将所述任一待规划机器人移动到所述不变轨迹组中，直到轨迹规划组为空时，全部待规划机器人完成轨迹规划。
基于上述任一实施例，图4为本发明实施例提供的多机器人协同轨迹规划方法的详细流程示意图，图5为本发明实施例提供的多机器人协同轨迹规划装置的结构示意图。如图4和图5所示，核心部分包括待规划机器人确定单元510、轨迹方程构建单元520和协同轨迹规划单元530，次要部分包括初始化及实时采集单元。
具体地，初始化单元用于：输入全局地形图、多机器人需满足的物理约束，以及多机器人初始及目标时刻、位置、速度；
待规划机器人确定单元510用于：指定需要进行轨迹规划的机器人，初始阶段指定全部机器人进行规划，将待规划轨迹的机器人编入轨迹规划组，其余机器人编入不变轨迹组；
实时采集单元用于：采集每个机器人实时位置和速度；
轨迹方程构建单元520用于：根据轨迹规划组内任一机器人的始末位置、速度、时间，构建含待优化参数的时空贝塞尔曲线轨迹方程P(τ)，其中，在中间重规划阶段，机器人的当前状态作为初始状态，即将当前时刻、位置、速度作为初始时刻、位置、速度；
根据时空贝塞尔曲线特性，得到P(τ)中待优化参数的取值范围；
协同轨迹规划单元530用于：运用粒子群算法，将每个粒子的位置作为待优化参数取值，每次迭代更新所有粒子的位置，根据一个粒子的位置可以求得对应的一个轨迹方程P(τ)；
将轨迹方程P(τ)离散化；
根据离散化的轨迹计算路程，并根据离散点是否满足速度、加速度、曲率、避障、地形、与所有不变轨迹组机器人的避碰约束，计算目标函数；
重复上述粒子群算法的迭代步骤，直至迭代次数大于最大迭代次数或多次目标值波动范围小于一定阈值；
规划出上述机器人的优化轨迹P(τ)，将该机器人从轨迹规划组移至不变轨迹组；
重复上述轨迹规划步骤，直至轨迹规划组的机器人数目减为0；
待规划机器人确定单元510还用于：全部机器人沿规划的轨迹运动，在运动过程中观测周围是否有新的障碍物，若存在新障碍物，则判断是否部分机器人轨迹即将碰触新障碍物，并针对即将碰触新障碍物的机器人进行轨迹重规划，必要时针对全部机器人进行轨迹重规划，直至所有机器人到达目标点。
基于上述任一实施例，为验证上述实施例提供的基于时空贝塞尔曲线的多机器人协同轨迹规划方法的可行性，利用数值仿真进行实验验证。此处，选取四个机器人进行从各自初始状态到目标状态的协同轨迹规划，图6-图9为本发明实施例提供的多机器人三维空间轨迹规划运动过程示意图，如图6-图9所示，四个机器人的运动过程共分为以下三个阶段：
第一阶段：从0 s到4.7 s，四个机器人分别沿着四条所规划出的时空贝塞尔曲线同时运动，在4.7 s末，机器人2因为观测到障碍物2且即将触碰，因此机器人2重新规划轨迹；
第二阶段：从4.7 s到12.8 s，四个机器人沿各自规划轨迹运动。在12.8 s末，机器人3观测到障碍物3且即将触碰，因此机器人3重新规划轨迹；
第三阶段：从12.8 s到30 s，四个机器人沿各自规划轨迹运动。在30 s末，四个机器人以各自预定的目标速度同时到达各自目标位置。
图10-17为本发明实施例提供的多机器人运动过程中各物理量随时间变化关系图，从图10-图12中可以看出，四个机器人的速度、加速度、曲率均满足运动约束要求。
图13-图15表示多机器人实时避障过程。其中，障碍物1在初始阶段已经被观测到，因此初始阶段即对障碍物1进行避障，而障碍物2和3在中间时刻被观测到，因此机器人2和3分别在4.7 s和12.8 s进行轨迹重规划。
图16表示四个机器人两两之间的距离随时间变化关系，从图中可知，四个机器人两两之间的距离也满足避碰约束条件。
图17表示四个机器人距离地形高度变化关系，由图可知，四个机器人运动轨迹的高度始终高于各自地形安全高度，满足地形约束条件。
图18示例了一种电子设备的实体结构示意图，如图18所示，该电子设备可以包括：处理器(processor)1810、通信接口(Communications Interface)1820、存储器(memory)1830和通信总线1840，其中，处理器1810，通信接口1820，存储器1830通过通信总线1840完成相互间的通信。处理器1810可以调用存储器1830中的逻辑指令，以执行多机器人协同轨迹规划方法。
此外，上述的存储器1830中的逻辑指令可以通过软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备（可以是个人计算机，服务器，或者网络设备等）执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，Random Access Memory）、磁碟或者光盘等各种可以存储程序代码的介质。
另一方面，本发明还提供一种计算机程序产品，所述计算机程序产品包括存储在非暂态计算机可读存储介质上的计算机程序，所述计算机程序包括程序指令，当所述程序指令被计算机执行时，计算机能够执行上述各方法所提供的多机器人协同轨迹规划方法。
又一方面，本发明还提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现以执行上述各提供的多机器人协同轨迹规划方法。
以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备（可以是个人计算机，服务器，或者网络设备等）执行各个实施例或者实施例的某些部分所述的方法。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

1.一种多机器人协同轨迹规划方法，其特征在于，包括：
将机器人划分为轨迹规划组和不变轨迹组；
根据所述轨迹规划组中各个待规划机器人在当前时刻的位置、速度以及目标时刻的位置、速度，分别构建各个待规划机器人对应的包含待优化参数的时空贝塞尔曲线轨迹方程；
基于粒子群算法以及多机器人需满足的物理约束，依次求解各个待规划机器人对应的时空贝塞尔曲线轨迹方程中的待优化参数，得到各个待规划机器人的同步时间轨迹，以供各个待规划机器人沿所述轨迹协同运动；
其中，求解任一待规划机器人对应的时空贝塞尔曲线轨迹方程中的待优化参数时，所述任一待规划机器人需与所述不变轨迹组中的各个已规划机器人满足避碰安全距离约束，并在求解完毕后将所述任一待规划机器人移动到所述不变轨迹组中，直到轨迹规划组为空时，全部待规划机器人完成轨迹规划。
2.根据权利要求1所述的多机器人协同轨迹规划方法，其特征在于，任一待规划机器人对应的包含待优化参数的时空贝塞尔曲线轨迹方程为：
Figure 750769DEST_PATH_IMAGE001
其中，P(τ)为时空贝塞尔曲线轨迹方程，自变量τ的取值范围为[0, 1]，伯恩斯坦基函数B(τ)为：
Figure 763725DEST_PATH_IMAGE002
其中，P(τ)包含四个控制点P i (x i , y i , z i , t i )，i=0、1、2、3；
(x i , y i , z i )表示P i 的三维空间位置，t i 表示P i 的时刻；P 0(x 0, y 0, z 0, t 0)表示时空贝塞尔曲线的初始位置和时刻，P 3(x 3, y 3, z 3, t 3)表示时空贝塞尔曲线的目标位置和时刻；中间控制点P 1(x 1, y 1, z 1)和P 2(x 2, y 2, z 2)的位置分别表示为：
Figure 223394DEST_PATH_IMAGE003
其中，(v x0, v y0, v z0)表示时空贝塞尔曲线的初始速度，(v x3, v y3, v z3)表示时空贝塞尔曲线的目标速度；所述待优化参数为t 1和t 2。
3.根据权利要求2所述的多机器人协同轨迹规划方法，其特征在于，所述基于粒子群算法以及多机器人需满足的物理约束，依次求解各个待规划机器人对应的时空贝塞尔曲线轨迹方程中的待优化参数，得到各个待规划机器人的同步时间轨迹，具体包括：
初始化步骤：随机初始化粒子群算法中各个粒子的位置和速度；其中，各个粒子的位置对应t 1和t 2的一组取值；
优化步骤：更新各个粒子的位置和速度，基于各个粒子的位置求解得到所述任一待规划机器人的多个时空贝塞尔曲线轨迹方程，离散化各个轨迹方程，并基于所述物理约束计算各个粒子对应的目标函数值，从而确定整个种群中当前最优解对应粒子的位置；
迭代步骤：重复执行所述优化步骤，不断优化各个粒子的位置，直至达到终止条件；基于整个种群中最优解对应粒子的位置，确定所述任一待规划机器人对应的最优时空贝塞尔曲线轨迹方程，即得到所述任一待规划机器人的最优轨迹；
其中，求解各个待规划机器人的同步时间轨迹时，根据如下关系：
Figure 638195DEST_PATH_IMAGE004
其中，t为时间，当满足t 1, t 2∈[t 0, t 3]时，时间函数t(τ)恒为增函数，因此基于一组离散化t值计算出各个待规划机器人对应的离散化τ值，再代入时空贝塞尔曲线轨迹方程，可满足各个机器人轨迹离散点的时刻对应相等，即求得各个待规划机器人的同步时间轨迹。
4.根据权利要求3所述的多机器人协同轨迹规划方法，其特征在于，任一粒子对应的目标函数值基于如下公式计算：
Figure 393792DEST_PATH_IMAGE005
其中，f(t 1, t 2)为所述任一粒子对应的目标函数值，所述任一粒子的位置为(t 1, t 2)；S为(t 1, t 2)对应的时空贝塞尔曲线轨迹的路程，M k 为预设数值，其中k∈[1, 6], k∈N*，δ k 为判别系数，包含L的积分项表示沿路径点的线积分，实际利用各个相邻离散点的距离总和近似该项。
5.根据权利要求4所述的多机器人协同轨迹规划方法，其特征在于，所述判别系数基于如下步骤确定：
基于所述任一粒子的位置，对所述任一待规划机器人的时空贝塞尔曲线轨迹方程进行离散化；
基于离散化的时空贝塞尔曲线轨迹，计算所述任一待规划机器人在每个离散点处的速度、加速度、曲率、与障碍物表面间的距离、运动轨迹高度以及与不变轨迹组中任一机器人间的距离；
基于所述任一待规划机器人在每个离散点处的速度、加速度、曲率、与障碍物表面间的距离、运动轨迹高度以及与不变轨迹组中任一机器人间的距离，以及所述物理约束，确定所述判别系数。
6.根据权利要求1至5任一项所述的多机器人协同轨迹规划方法，其特征在于，所述物理约束包括最大速度约束、最大加速度约束、最大曲率约束、避障安全距离约束、地形安全高度约束和避碰安全距离约束。
7.根据权利要求1至5任一项所述的多机器人协同轨迹规划方法，其特征在于，所述将机器人划分为轨迹规划组和不变轨迹组，具体包括：
在初始阶段，所有机器人在初始位置等待轨迹规划，因此将所有机器人划分到所述轨迹规划组中，所述不变轨迹组为空；
在中间阶段，所有机器人沿已规划好的轨迹运动，在运动过程中的任一时刻，若任一或多个机器人观测且将碰触到新的障碍物，则将所述任一或多个机器人划分到所述轨迹规划组，其余机器人划分到所述不变轨迹组；若任一或多个机器人轨迹规划失败，则将所有机器人划分到所述轨迹规划组中，所述不变轨迹组为空。
8.一种多机器人协同轨迹规划装置，其特征在于，包括：
待规划机器人确定单元，用于将机器人划分为轨迹规划组和不变轨迹组；
轨迹方程构建单元，用于根据所述轨迹规划组中各个待规划机器人在当前时刻的位置、速度以及目标时刻的位置、速度，分别构建各个待规划机器人对应的包含待优化参数的时空贝塞尔曲线轨迹方程；
协同轨迹规划单元，用于基于粒子群算法以及多机器人需满足的物理约束，依次求解各个待规划机器人对应的时空贝塞尔曲线轨迹方程中的待优化参数，得到各个待规划机器人的同步时间轨迹，以供各个待规划机器人沿所述轨迹协同运动；
其中，求解任一待规划机器人对应的时空贝塞尔曲线轨迹方程中的待优化参数时，所述任一待规划机器人需与所述不变轨迹组中的各个已规划机器人满足避碰安全距离约束，并在求解完毕后将所述任一待规划机器人移动到所述不变轨迹组中，直到轨迹规划组为空时，全部待规划机器人完成轨迹规划。
9.一种电子设备，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1至7任一项所述多机器人协同轨迹规划方法的步骤。
10.一种非暂态计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至7任一项所述多机器人协同轨迹规划方法的步骤。
CN202110803758.9A 2021-07-16 2021-07-16 多机器人协同轨迹规划方法、装置、电子设备和存储介质 Active CN113253744B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110803758.9A CN113253744B (zh) 2021-07-16 2021-07-16 多机器人协同轨迹规划方法、装置、电子设备和存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110803758.9A CN113253744B (zh) 2021-07-16 2021-07-16 多机器人协同轨迹规划方法、装置、电子设备和存储介质

Publications (2)

Publication Number Publication Date
CN113253744A true CN113253744A (zh) 2021-08-13
CN113253744B CN113253744B (zh) 2021-10-01

Family

ID=77180472

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110803758.9A Active CN113253744B (zh) 2021-07-16 2021-07-16 多机器人协同轨迹规划方法、装置、电子设备和存储介质

Country Status (1)

Country Link
CN (1) CN113253744B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113467487A (zh) * 2021-09-06 2021-10-01 中国科学院自动化研究所 路径规划模型的训练、路径规划方法、装置及电子设备
CN113759970A (zh) * 2021-09-04 2021-12-07 杭州西湖喷泉设备成套有限公司 多漂浮水秀平台共同表演的控制方法、系统及存储介质
CN116372964A (zh) * 2023-06-05 2023-07-04 西湖大学 具有可切换末端执行机构集的空中机器人系统

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2014190766A (ja) * 2013-03-26 2014-10-06 Bridgestone Corp 挙動解析装置および挙動解析方法
CN107490965A (zh) * 2017-08-21 2017-12-19 西北工业大学 一种空间自由漂浮机械臂的多约束轨迹规划方法
CN107608372A (zh) * 2017-08-14 2018-01-19 广西师范大学 一种基于改进rrt算法与改进ph曲线相结合的多无人机协同航迹规划方法
CN109799822A (zh) * 2019-01-30 2019-05-24 中国石油大学(华东) 移动机器人全局平滑路径规划方法
CN109839953A (zh) * 2019-02-19 2019-06-04 上海交通大学 基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2014190766A (ja) * 2013-03-26 2014-10-06 Bridgestone Corp 挙動解析装置および挙動解析方法
CN107608372A (zh) * 2017-08-14 2018-01-19 广西师范大学 一种基于改进rrt算法与改进ph曲线相结合的多无人机协同航迹规划方法
CN107490965A (zh) * 2017-08-21 2017-12-19 西北工业大学 一种空间自由漂浮机械臂的多约束轨迹规划方法
CN109799822A (zh) * 2019-01-30 2019-05-24 中国石油大学(华东) 移动机器人全局平滑路径规划方法
CN109839953A (zh) * 2019-02-19 2019-06-04 上海交通大学 基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
LI, QINGYANG: "3D Cubic Bezier Curves for Multi-target Path Planning for Autonomous Underwater Vehicles", 《OCEANS 2019-MARSEILLE》 *
WANG YU ETC.: "Generation of temporal–spatial Bezier curve for simultaneous arrival of multiple unmanned vehicles", 《INFORMATION SCIENCES》 *
胡峰等: "基于Bezier曲线的多无人机路径规划算法", 《第二十九届中国控制会议论文集》 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113759970A (zh) * 2021-09-04 2021-12-07 杭州西湖喷泉设备成套有限公司 多漂浮水秀平台共同表演的控制方法、系统及存储介质
CN113759970B (zh) * 2021-09-04 2024-02-02 杭州西湖喷泉设备成套有限公司 多漂浮水秀平台共同表演的控制方法、系统及存储介质
CN113467487A (zh) * 2021-09-06 2021-10-01 中国科学院自动化研究所 路径规划模型的训练、路径规划方法、装置及电子设备
CN116372964A (zh) * 2023-06-05 2023-07-04 西湖大学 具有可切换末端执行机构集的空中机器人系统
CN116372964B (zh) * 2023-06-05 2024-02-20 西湖大学 具有可切换末端执行机构集的空中机器人系统

Also Published As

Publication number Publication date
CN113253744B (zh) 2021-10-01

Similar Documents

Publication Publication Date Title
CN113253744B (zh) 多机器人协同轨迹规划方法、装置、电子设备和存储介质
Gao et al. Gradient-based online safe trajectory generation for quadrotor flight in complex environments
CN109885891B (zh) 一种智能车gpu并行加速轨迹规划方法
CN110703768B (zh) 一种改进型动态rrt*的移动机器人运动规划方法
Rösmann et al. Efficient trajectory optimization using a sparse model
WO2018176596A1 (zh) 基于权重改进粒子群算法的无人自行车路径规划方法
CN109508035A (zh) 基于分布式控制的多区域分级式无人机编队路径规划方法
CN114967744A (zh) 一种多无人机协同避障的规划方法
CN113156954A (zh) 一种基于增强学习的多智能体集群避障方法
CN115033016A (zh) 一种异构无人集群编队避障方法及系统
CN114932549A (zh) 空间冗余机械臂的运动规划方法与装置
Zollars et al. Optimal SUAS path planning in three-dimensional constrained environments
Pérez et al. Path planning based on parametric curves
Malayjerdi et al. Mobile robot navigation based on fuzzy cognitive map optimized with grey wolf optimization algorithm used in augmented reality
CN112504279B (zh) 适用于无人机的无碰撞路径规划方法、系统及介质
CN113885567B (zh) 一种基于冲突搜索的多无人机的协同路径规划方法
CN114815899A (zh) 基于3d激光雷达传感器的无人机三维空间路径规划方法
CN111596668B (zh) 基于逆向强化学习的移动机器人拟人化路径规划方法
CN115855086A (zh) 基于自旋转的室内场景自主重建方法、系统及介质
CN112161626B (zh) 一种基于航路跟踪映射网络的高可飞性航路规划方法
CN111176324B (zh) 一种多无人机分布式协同编队规避动态障碍的方法
Nikitenko et al. RRTs postprocessing for uncertain environments
Akmandor et al. Reactive navigation framework for mobile robots by heuristically evaluated pre-sampled trajectories
Yassine et al. A robust synergetic controller for Quadrotor obstacle avoidance using Bézier curve versus B-spline trajectory generation
Bulut The optimal path of robot end effector based on hierarchical clustering and Bézier curve with three shape parameters

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