CN114489087B - 一种多无人驾驶车辆路径协同规划方法及系统 - Google Patents

一种多无人驾驶车辆路径协同规划方法及系统 Download PDF

Info

Publication number
CN114489087B
CN114489087B CN202210400906.7A CN202210400906A CN114489087B CN 114489087 B CN114489087 B CN 114489087B CN 202210400906 A CN202210400906 A CN 202210400906A CN 114489087 B CN114489087 B CN 114489087B
Authority
CN
China
Prior art keywords
model
unmanned vehicle
collision
environment
unmanned
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
CN202210400906.7A
Other languages
English (en)
Other versions
CN114489087A (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.)
Huidong Planet Beijing Technology Co ltd
Beijing Institute of Technology BIT
Original Assignee
Huidong Planet Beijing Technology Co ltd
Beijing Institute of Technology BIT
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 Huidong Planet Beijing Technology Co ltd, Beijing Institute of Technology BIT filed Critical Huidong Planet Beijing Technology Co ltd
Priority to CN202210400906.7A priority Critical patent/CN114489087B/zh
Publication of CN114489087A publication Critical patent/CN114489087A/zh
Application granted granted Critical
Publication of CN114489087B publication Critical patent/CN114489087B/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/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling
    • G05D1/0289—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling with means for avoiding collisions between vehicles
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

本发明涉及一种多无人驾驶车辆路径协同规划方法及系统。该方法包括建立多无人驾驶车辆行驶的环境模型；建立每一无人驾驶车辆的运动学模型，并根据运动模型建立无人驾驶车辆之间的防碰撞包裹圆模型；根据无人驾驶车辆的之间防碰撞包裹圆模型以及环境模型，采用分离超平面定理，建立无人驾驶车辆与环境中动静态障碍物防碰撞模型；根据环境模型、运动学模型、防碰撞包裹圆模型以及无人驾驶车辆与环境中动静态障碍物防碰撞模型，基于交替方向乘子法逐步迭代对多无人驾驶车辆进行解耦式运动规划，确定每一无人驾驶车辆的最优路径。本发明能够合理的避免与动静态障碍物发生碰撞，进而实现多无人驾驶车辆协同规划控制。

Description

一种多无人驾驶车辆路径协同规划方法及系统
技术领域
本发明涉及无人驾驶领域，特别是涉及一种多无人驾驶车辆路径协同规划方法及系统。
背景技术
随着无人驾驶技术快速的发展，无人驾驶技术逐渐由单车智能向多车智能方向发展。真正实现无人驾驶技术的全面落地。其中，多无人驾驶车辆规划技术在整个无人车技术领域中占据着重要地位。高效合理的无人车辆规划技术可以实现车与车之间、车与障碍物之间不发生碰撞。
目前，多无人驾驶车辆规划技术主要面临由运动学约束、动力学约束和障碍物约束等导致复杂的求解过程。在多无人驾驶车辆规划相关技术中，处理这些复杂的约束过于复杂，难以满足规划的实时性要求。另外，无人车行驶的环境不确定，存在动静态行驶的障碍物，合理的规避这些动态障碍物是满足无人车安全性的前提。因此，如何能够合理的避免与动静态障碍物发生碰撞，是现在亟待解决的问题。
发明内容
本发明的目的是提供一种多无人驾驶车辆路径协同规划方法及系统，能够合理的避免与动静态障碍物发生碰撞，进而实现多无人驾驶车辆协同规划控制。
为实现上述目的，本发明提供了如下方案：
一种多无人驾驶车辆路径协同规划方法，包括：
建立多无人驾驶车辆行驶的环境模型；所述环境模型包括：车辆行驶区域、静态障碍物和动态障碍物；
建立每一无人驾驶车辆的运动学模型，并根据运动模型建立无人驾驶车辆之间的防碰撞包裹圆模型；所述防碰撞包裹圆模型为两个无人驾驶车辆的包裹圆不重叠；所述包裹圆的半径是根据对应无人驾驶车辆的前悬长度、后悬长度、车宽以及前后轴之间的距离进行确定；所述包裹圆的圆心是根据对应无人驾驶车辆的后轴中间坐标、在大地坐标系中的姿态角以及前后轴之间的距离进行确定；
根据无人驾驶车辆的之间防碰撞包裹圆模型以及环境模型，采用分离超平面定理，建立无人驾驶车辆与环境中动静态障碍物防碰撞模型；
根据环境模型、运动学模型、防碰撞包裹圆模型以及无人驾驶车辆与环境中动静态障碍物防碰撞模型，基于交替方向乘子法逐步迭代对多无人驾驶车辆进行解耦式运动规划，确定每一无人驾驶车辆的最优路径。
可选地，所述建立多无人驾驶车辆行驶的环境模型，具体包括：
获取动态障碍物的起始点位置、运动时间以及运动速度；
根据动态障碍物的起始点位置、运动时间以及运动速度确定动态障碍物的运动轨迹。
可选地，所述建立每一无人驾驶车辆的运动学模型，并根据运动模型建立无人驾驶车辆之间的防碰撞包裹圆模型，具体包括：
利用公式
Figure 304150DEST_PATH_IMAGE001
确定无人驾驶车辆之间的防碰撞包裹圆模型；
其中，
Figure 740685DEST_PATH_IMAGE002
为第i个无人驾驶车辆的包裹圆的半径，
Figure 224887DEST_PATH_IMAGE003
为第j个无人驾驶车辆的包裹圆的半径，
Figure 37860DEST_PATH_IMAGE004
，
Figure 579700DEST_PATH_IMAGE005
为第i个无人驾驶车辆的包裹圆的圆心坐标，
Figure 688601DEST_PATH_IMAGE006
，
Figure 836598DEST_PATH_IMAGE007
为第j个无人驾驶车辆的包裹圆的圆心坐标，N为无人驾驶车辆的个数。
可选地，所述根据无人驾驶车辆的之间防碰撞包裹圆模型以及环境模型，采用分离超平面定理，建立无人驾驶车辆与环境中动静态障碍物防碰撞模型，具体包括：
利用公式
Figure 344939DEST_PATH_IMAGE008
确定无人驾驶车辆与环境中动静态障碍物防碰撞模型；
其中，
Figure 882231DEST_PATH_IMAGE009
为超平面的法向量，
Figure 67094DEST_PATH_IMAGE010
为超平面的偏移量，
Figure 181680DEST_PATH_IMAGE009
与
Figure 369079DEST_PATH_IMAGE010
被参数化为样条，超平面的运动被描述为时间的函数，
Figure 134779DEST_PATH_IMAGE011
为第i个无人驾驶车辆的运动轨迹，
Figure 585483DEST_PATH_IMAGE012
为第i个无人驾驶车辆的纵向速度，t为第i个无人驾驶车辆的运动时间，
Figure 921786DEST_PATH_IMAGE013
为超平面的法向量
Figure 145832DEST_PATH_IMAGE009
的转置矩阵，时间[0，T]为运动的总时间。
可选地，所述根据环境模型、运动学模型、防碰撞包裹圆模型以及无人驾驶车辆与环境中动静态障碍物防碰撞模型，基于交替方向乘子法逐步迭代对多无人驾驶车辆进行解耦式运动规划，确定每一无人驾驶车辆的最优路径，具体包括：
根据环境模型、运动学模型、防碰撞包裹圆模型以及无人驾驶车辆与环境中动静态障碍物防碰撞模型确定每一无人驾驶车辆的起点与终点，并根据每一无人驾驶车辆的起点与终点，采用B样条曲线，确定每一无人驾驶车辆的运动轨迹的B样条曲线函数；
根据每一无人驾驶车辆的运动轨迹的B样条曲线函数，建立以所有无人驾驶车辆的运动轨迹的总和最小为目标的总和函数；
根据总和函数建立增加二次惩罚项的增广拉格朗日函数；
根据增加二次惩罚项的增广拉格朗日函数，利用交替方向乘子法进行迭代更新，确定每一无人驾驶车辆的最优路径。
一种多无人驾驶车辆路径协同规划系统，包括：
环境模型建立模块，用于建立多无人驾驶车辆行驶的环境模型；所述环境模型包括：车辆行驶区域、静态障碍物和动态障碍物；
防碰撞包裹圆模型建立模块，用于建立每一无人驾驶车辆的运动学模型，并根据运动模型建立无人驾驶车辆之间的防碰撞包裹圆模型；所述防碰撞包裹圆模型为两个无人驾驶车辆的包裹圆不重叠；所述包裹圆的半径是根据对应无人驾驶车辆的前悬长度、后悬长度、车宽以及前后轴之间的距离进行确定；所述包裹圆的圆心是根据对应无人驾驶车辆的后轴中间坐标、在大地坐标系中的姿态角以及前后轴之间的距离进行确定；
无人驾驶车辆与环境中动静态障碍物防碰撞模型建立模块，用于根据无人驾驶车辆的之间防碰撞包裹圆模型以及环境模型，采用分离超平面定理，建立无人驾驶车辆与环境中动静态障碍物防碰撞模型；
最优路径确定模块，用于根据环境模型、运动学模型、防碰撞包裹圆模型以及无人驾驶车辆与环境中动静态障碍物防碰撞模型，基于交替方向乘子法逐步迭代对多无人驾驶车辆进行解耦式运动规划，确定每一无人驾驶车辆的最优路径。
可选地，所述环境模型建立模块具体包括：
动态障碍物的参数获取单元，用于获取动态障碍物的起始点位置、运动时间以及运动速度；
动态障碍物的运动轨迹确定单元，用于根据动态障碍物的起始点位置、运动时间以及运动速度确定动态障碍物的运动轨迹。
可选地，所述防碰撞包裹圆模型建立模块具体包括：
防碰撞包裹圆模型建立单元，用于利用公式
Figure 533082DEST_PATH_IMAGE001
确定无人驾驶车辆之间的防碰撞包裹圆模型；
其中，
Figure 810480DEST_PATH_IMAGE002
为第i个无人驾驶车辆的包裹圆的半径，
Figure 291138DEST_PATH_IMAGE003
为第j个无人驾驶车辆的包裹圆的半径，
Figure 430126DEST_PATH_IMAGE004
，
Figure 921150DEST_PATH_IMAGE005
为第i个无人驾驶车辆的包裹圆的圆心坐标，
Figure 743351DEST_PATH_IMAGE006
，
Figure 70558DEST_PATH_IMAGE007
为第j个无人驾驶车辆的包裹圆的圆心坐标，N为无人驾驶车辆的个数。
可选地，所述无人驾驶车辆与环境中动静态障碍物防碰撞模型建立模块具体包括：
无人驾驶车辆与环境中动静态障碍物防碰撞模型建立单元，用于利用公式
Figure 996926DEST_PATH_IMAGE008
确定无人驾驶车辆与环境中动静态障碍物防碰撞模型；
其中，
Figure 263828DEST_PATH_IMAGE009
为超平面的法向量，
Figure 991350DEST_PATH_IMAGE010
为超平面的偏移量，
Figure 55121DEST_PATH_IMAGE009
与
Figure 129387DEST_PATH_IMAGE010
被参数化为样条，超平面的运动被描述为时间的函数，
Figure 584552DEST_PATH_IMAGE011
为第i个无人驾驶车辆的运动轨迹，
Figure 250019DEST_PATH_IMAGE012
为第i个无人驾驶车辆的纵向速度，t为第i个无人驾驶车辆的运动时间，
Figure 535507DEST_PATH_IMAGE013
为超平面的法向量
Figure 584103DEST_PATH_IMAGE009
的转置矩阵，时间[0，T]为运动的总时间。
可选地，所述最优路径确定模块具体包括：
B样条曲线函数确定单元，用于根据环境模型、运动学模型、防碰撞包裹圆模型以及无人驾驶车辆与环境中动静态障碍物防碰撞模型确定每一无人驾驶车辆的起点与终点，并根据每一无人驾驶车辆的起点与终点，采用B样条曲线，确定每一无人驾驶车辆的运动轨迹的B样条曲线函数；
总和函数建立单元，用于根据每一无人驾驶车辆的运动轨迹的B样条曲线函数，建立以所有无人驾驶车辆的运动轨迹的总和最小为目标的总和函数；
增广拉格朗日函数建立单元，用于根据总和函数建立增加二次惩罚项的增广拉格朗日函数；
最优路径确定单元，用于根据增加二次惩罚项的增广拉格朗日函数，利用交替方向乘子法进行迭代更新，确定每一无人驾驶车辆的最优路径。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
本发明所提供的一种多无人驾驶车辆路径协同规划方法及系统，根据无人驾驶车辆的之间防碰撞包裹圆模型以及环境模型，采用分离超平面定理，建立无人驾驶车辆与环境中动静态障碍物防碰撞模型。利用车辆之间防碰撞包裹圆模型与防碰撞约束的分离超平面定理，避免了车辆与车辆之间、车辆与动静态障碍物之间的碰撞，基于交替方向乘子法逐步迭代对多无人驾驶车辆进行解耦式运动规划求解，最终为每一辆无人车求解出满足运动学约束、避免碰撞、光滑可通行的最优路径。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明所提供的一种多无人驾驶车辆路径协同规划方法流程示意图；
图2为无人驾驶车辆之间防碰撞包裹圆模型示意图；
图3为分离超平面防碰撞示意图；
图4为一种多无人驾驶车辆路径协同规划示意图；
图5为本发明所提供的一种多无人驾驶车辆路径协同规划系统结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是提供一种多无人驾驶车辆路径协同规划方法及系统，能够合理的避免与动静态障碍物发生碰撞，进而实现多无人驾驶车辆协同规划控制。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
图1为本发明所提供的一种多无人驾驶车辆路径协同规划方法流程示意图，如图1所示，本发明所提供的一种多无人驾驶车辆路径协同规划方法，包括：
S101，建立多无人驾驶车辆行驶的环境模型；所述环境模型包括：车辆行驶区域、静态障碍物和动态障碍物；车辆行驶区域的长宽分别为l与h，静态障碍物为半径r的圆；动态障碍物为半径为R的圆。
S101具体包括：
获取动态障碍物的起始点位置
Figure 514013DEST_PATH_IMAGE014
、运动时间
Figure 475016DEST_PATH_IMAGE015
以及运动速度
Figure 90543DEST_PATH_IMAGE016
；
根据动态障碍物的起始点位置
Figure 37770DEST_PATH_IMAGE014
、运动时间
Figure 196088DEST_PATH_IMAGE015
以及运动速度
Figure 593571DEST_PATH_IMAGE016
确定动态障碍物的运动轨迹
Figure 463438DEST_PATH_IMAGE017
，即
Figure 847978DEST_PATH_IMAGE018
。
S102，建立每一无人驾驶车辆的运动学模型，并根据运动模型建立无人驾驶车辆之间的防碰撞包裹圆模型；所述防碰撞包裹圆模型为两个无人驾驶车辆的包裹圆不重叠；所述包裹圆的半径是根据对应无人驾驶车辆的前悬长度、后悬长度、车宽以及前后轴之间的距离进行确定；所述包裹圆的圆心是根据对应无人驾驶车辆的后轴中间坐标、在大地坐标系中的姿态角以及前后轴之间的距离进行确定；
S102具体包括：
无人驾驶车辆运动学模型：
Figure 877114DEST_PATH_IMAGE019
（1）
如图2所示，无人驾驶车辆之间的防碰撞包裹圆模型：
Figure 55285DEST_PATH_IMAGE001
（2）
其中，
Figure 786350DEST_PATH_IMAGE002
为第i个无人驾驶车辆的包裹圆的半径，
Figure 340959DEST_PATH_IMAGE003
为第j个无人驾驶车辆的包裹圆的半径，
Figure 801765DEST_PATH_IMAGE004
，
Figure 275472DEST_PATH_IMAGE005
为第i个无人驾驶车辆的包裹圆的圆心坐标，
Figure 119931DEST_PATH_IMAGE006
，
Figure 852133DEST_PATH_IMAGE007
为第j个无人驾驶车辆的包裹圆的圆心坐标，N为无人驾驶车辆的个数，
Figure 855861DEST_PATH_IMAGE020
为第i (i=0,2,...,N-1) 辆无人驾驶车辆后轴中间坐标，
Figure 844676DEST_PATH_IMAGE021
为车体坐标系下车辆纵向速度；
Figure 680826DEST_PATH_IMAGE022
为无人驾驶车辆在大地坐标系下的纵向速度，
Figure 452604DEST_PATH_IMAGE023
为无人驾驶车辆在大地坐标系下的横向速度；
Figure 576418DEST_PATH_IMAGE024
为纵向加速度；
Figure 765829DEST_PATH_IMAGE025
为车辆在大地坐标系中的姿态角；
Figure 460246DEST_PATH_IMAGE026
为前轮偏转角，
Figure 284983DEST_PATH_IMAGE027
表示车辆前后轴之间的距离。
包裹圆的圆心为
Figure 371625DEST_PATH_IMAGE028
,其中圆心坐标可以表示为：
Figure 374348DEST_PATH_IMAGE029
（3）
包裹圆的半径
Figure 805329DEST_PATH_IMAGE030
为：
Figure 89549DEST_PATH_IMAGE031
（4）
S103，根据无人驾驶车辆的之间防碰撞包裹圆模型以及环境模型，采用分离超平面定理，建立无人驾驶车辆与环境中动静态障碍物防碰撞模型；如图3所示，在复杂环境中，静止和移动障碍物的碰撞避免会对车辆运动状态产生约束。在这种情况下，防碰撞约束采用分离超平面定理。这个定理表明两个不相交的凸集总是可以被一个超平面分开。这意味着在车辆和障碍物之间有一条分界线。假设障碍物可以被表示为顶点个数为
Figure 187955DEST_PATH_IMAGE032
的凸多面体表示，车辆与环境中动静态障碍物防碰撞约束（无人驾驶车辆与环境中动静态障碍物防碰撞模型）表示为：
Figure 736746DEST_PATH_IMAGE008
（5）
其中，
Figure 733652DEST_PATH_IMAGE009
为超平面的法向量，
Figure 149459DEST_PATH_IMAGE010
为超平面的偏移量，
Figure 102371DEST_PATH_IMAGE009
与
Figure 446896DEST_PATH_IMAGE010
被参数化为样条，超平面的运动被描述为时间的函数，
Figure 226371DEST_PATH_IMAGE011
为第i个无人驾驶车辆的运动轨迹，
Figure 931022DEST_PATH_IMAGE012
为第i个无人驾驶车辆的纵向速度，t为第i个无人驾驶车辆的运动时间，
Figure 817069DEST_PATH_IMAGE013
为超平面的法向量
Figure 831030DEST_PATH_IMAGE009
的转置矩阵，时间[0，T]为运动的总时间。
S104，根据环境模型、运动学模型、防碰撞包裹圆模型以及无人驾驶车辆与环境中动静态障碍物防碰撞模型，基于交替方向乘子法逐步迭代对多无人驾驶车辆进行解耦式运动规划，确定每一无人驾驶车辆的最优路径。
S104具体包括：
如图4所示，根据环境模型、运动学模型、防碰撞包裹圆模型以及无人驾驶车辆与环境中动静态障碍物防碰撞模型确定每一无人驾驶车辆的起点
Figure 333687DEST_PATH_IMAGE033
与终点
Figure 842029DEST_PATH_IMAGE034
（i=0,1,2...,N-1），并根据每一无人驾驶车辆的起点
Figure 352557DEST_PATH_IMAGE033
与终点
Figure 366780DEST_PATH_IMAGE035
（i=0,1,2...,N-1），采用B样条曲线，确定每一无人驾驶车辆的运动轨迹的B样条曲线函数；B样条曲线函数可以被表示成分段多项式：
Figure 730634DEST_PATH_IMAGE036
（6）
其中，
Figure 42667DEST_PATH_IMAGE037
为n个控制节点，
Figure 903307DEST_PATH_IMAGE038
为B样条基函数。
根据每一无人驾驶车辆的运动轨迹的B样条曲线函数，建立以所有无人驾驶车辆的运动轨迹的总和最小为目标的总和函数；
总和函数为：
Figure 118125DEST_PATH_IMAGE039
（7）
其中
Figure 736319DEST_PATH_IMAGE040
，
Figure 320884DEST_PATH_IMAGE041
为引入的中间变量，分别表示复制车辆i与车辆j的运动轨迹，Q表示所有无人驾驶车辆总轨迹，
Figure 331304DEST_PATH_IMAGE042
表示车辆之间不能有碰撞。
根据总和函数建立增加二次惩罚项的增广拉格朗日函数；其中系数为
Figure 687330DEST_PATH_IMAGE043
，对于增广拉格朗日函数可以表示为：
Figure 285496DEST_PATH_IMAGE044
（7）
其中
Figure 939331DEST_PATH_IMAGE045
，
Figure 181088DEST_PATH_IMAGE046
表示与对偶约束相关的对偶变量。
根据增加二次惩罚项的增广拉格朗日函数
Figure 894966DEST_PATH_IMAGE047
，利用交替方向乘子法进行迭代更新：
Figure 314184DEST_PATH_IMAGE048
（9）
第一步迭代式（9）为一个无人驾驶车辆标准的运动规划问题，通过引入对偶变量，避免了无人驾驶车辆下一步运动轨迹
Figure 115917DEST_PATH_IMAGE049
与
Figure 976295DEST_PATH_IMAGE050
，
Figure 861074DEST_PATH_IMAGE051
偏离太大，并把下一步运动轨迹
Figure 675578DEST_PATH_IMAGE049
通过无线通信技术分享给临近车辆。
Figure 671216DEST_PATH_IMAGE052
（10）
第二步迭代式（10）为车辆之间的交互约束，避免车辆之间的碰撞。
Figure 651679DEST_PATH_IMAGE053
（11）
第三步迭代式（11）为对偶变量权重更新，应用于式（10）、式（11）两步
Figure 520409DEST_PATH_IMAGE054
和
Figure 337055DEST_PATH_IMAGE055
之间的偏差计算，并把
Figure 250565DEST_PATH_IMAGE056
和
Figure 321421DEST_PATH_IMAGE057
分享给临近车辆。。
通过以上三步重复迭代计算，最终可以求得完整的交替方向乘子法完整序列，即多车全局最优解，使每辆车都可以得到一条满足运动学约束、避免碰撞、光滑可通行的最优路径。
图5为本发明所提供的一种多无人驾驶车辆路径协同规划系统结构示意图，如图5所示，本发明所提供的一种多无人驾驶车辆路径协同规划系统，包括：
环境模型建立模块501，用于建立多无人驾驶车辆行驶的环境模型；所述环境模型包括：车辆行驶区域、静态障碍物和动态障碍物；
防碰撞包裹圆模型建立模块502，用于建立每一无人驾驶车辆的运动学模型，并根据运动模型建立无人驾驶车辆之间的防碰撞包裹圆模型；所述防碰撞包裹圆模型为两个无人驾驶车辆的包裹圆不重叠；所述包裹圆的半径是根据对应无人驾驶车辆的前悬长度、后悬长度、车宽以及前后轴之间的距离进行确定；所述包裹圆的圆心是根据对应无人驾驶车辆的后轴中间坐标、在大地坐标系中的姿态角以及前后轴之间的距离进行确定；
无人驾驶车辆与环境中动静态障碍物防碰撞模型建立模块503，用于根据无人驾驶车辆的之间防碰撞包裹圆模型以及环境模型，采用分离超平面定理，建立无人驾驶车辆与环境中动静态障碍物防碰撞模型；
最优路径确定模块504，用于根据环境模型、运动学模型、防碰撞包裹圆模型以及无人驾驶车辆与环境中动静态障碍物防碰撞模型，基于交替方向乘子法逐步迭代对多无人驾驶车辆进行解耦式运动规划，确定每一无人驾驶车辆的最优路径。
所述环境模型建立模块501具体包括：
动态障碍物的参数获取单元，用于获取动态障碍物的起始点位置、运动时间以及运动速度；
动态障碍物的运动轨迹确定单元，用于根据动态障碍物的起始点位置、运动时间以及运动速度确定动态障碍物的运动轨迹。
所述防碰撞包裹圆模型建立模块502具体包括：
防碰撞包裹圆模型建立单元，用于利用公式
Figure 548003DEST_PATH_IMAGE001
确定无人驾驶车辆之间的防碰撞包裹圆模型；
其中，
Figure 101213DEST_PATH_IMAGE002
为第i个无人驾驶车辆的包裹圆的半径，
Figure 173074DEST_PATH_IMAGE003
为第j个无人驾驶车辆的包裹圆的半径，
Figure 957490DEST_PATH_IMAGE004
，
Figure 197716DEST_PATH_IMAGE005
为第i个无人驾驶车辆的包裹圆的圆心坐标，
Figure 739687DEST_PATH_IMAGE006
，
Figure 349660DEST_PATH_IMAGE007
为第j个无人驾驶车辆的包裹圆的圆心坐标，N为无人驾驶车辆的个数。
所述无人驾驶车辆与环境中动静态障碍物防碰撞模型建立模块503具体包括：
无人驾驶车辆与环境中动静态障碍物防碰撞模型建立单元，用于利用公式
Figure 159222DEST_PATH_IMAGE008
确定无人驾驶车辆与环境中动静态障碍物防碰撞模型；
其中，
Figure 727607DEST_PATH_IMAGE009
为超平面的法向量，
Figure 350349DEST_PATH_IMAGE010
为超平面的偏移量，
Figure 296438DEST_PATH_IMAGE009
与
Figure 789867DEST_PATH_IMAGE010
被参数化为样条，超平面的运动被描述为时间的函数，
Figure 529153DEST_PATH_IMAGE011
为第i个无人驾驶车辆的运动轨迹，
Figure 13093DEST_PATH_IMAGE012
为第i个无人驾驶车辆的纵向速度，t为第i个无人驾驶车辆的运动时间，
Figure 230448DEST_PATH_IMAGE013
为超平面的法向量
Figure 109542DEST_PATH_IMAGE009
的转置矩阵，时间[0，T]为运动的总时间。
所述最优路径确定模块504具体包括：
B样条曲线函数确定单元，用于根据环境模型、运动学模型、防碰撞包裹圆模型以及无人驾驶车辆与环境中动静态障碍物防碰撞模型确定每一无人驾驶车辆的起点与终点，并根据每一无人驾驶车辆的起点与终点，采用B样条曲线，确定每一无人驾驶车辆的运动轨迹的B样条曲线函数；
总和函数建立单元，用于根据每一无人驾驶车辆的运动轨迹的B样条曲线函数，建立以所有无人驾驶车辆的运动轨迹的总和最小为目标的总和函数；
增广拉格朗日函数建立单元，用于根据总和函数建立增加二次惩罚项的增广拉格朗日函数；
最优路径确定单元，用于根据增加二次惩罚项的增广拉格朗日函数，利用交替方向乘子法进行迭代更新，确定每一无人驾驶车辆的最优路径。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (4)

1.一种多无人驾驶车辆路径协同规划方法，其特征在于，包括：
建立多无人驾驶车辆行驶的环境模型；所述环境模型包括：车辆行驶区域、静态障碍物和动态障碍物；
建立每一无人驾驶车辆的运动学模型，并根据运动模型建立无人驾驶车辆之间的防碰撞包裹圆模型；所述防碰撞包裹圆模型为两个无人驾驶车辆的包裹圆不重叠；所述包裹圆的半径是根据对应无人驾驶车辆的前悬长度、后悬长度、车宽以及前后轴之间的距离进行确定；所述包裹圆的圆心是根据对应无人驾驶车辆的后轴中间坐标、在大地坐标系中的姿态角以及前后轴之间的距离进行确定；
根据无人驾驶车辆的之间防碰撞包裹圆模型以及环境模型，采用分离超平面定理，建立无人驾驶车辆与环境中动静态障碍物防碰撞模型；
根据环境模型、运动学模型、防碰撞包裹圆模型以及无人驾驶车辆与环境中动静态障碍物防碰撞模型，基于交替方向乘子法逐步迭代对多无人驾驶车辆进行解耦式运动规划，确定每一无人驾驶车辆的最优路径；
所述建立多无人驾驶车辆行驶的环境模型，具体包括：
获取动态障碍物的起始点位置、运动时间以及运动速度；
根据动态障碍物的起始点位置、运动时间以及运动速度确定动态障碍物的运动轨迹；
所述根据无人驾驶车辆的之间防碰撞包裹圆模型以及环境模型，采用分离超平面定理，建立无人驾驶车辆与环境中动静态障碍物防碰撞模型，具体包括：
利用公式
Figure DEST_PATH_IMAGE001
确定无人驾驶车辆与环境中动静态障碍物防碰撞模型；
其中，
Figure DEST_PATH_IMAGE002
为超平面的法向量，
Figure DEST_PATH_IMAGE003
为超平面的偏移量，
Figure 665814DEST_PATH_IMAGE002
与
Figure 520637DEST_PATH_IMAGE003
被参数化为样条，超平面的运动被描述为时间的函数，
Figure DEST_PATH_IMAGE004
为第i个无人驾驶车辆的运动轨迹，
Figure DEST_PATH_IMAGE005
为第i个无人驾驶车辆的纵向速度，t为第i个无人驾驶车辆的运动时间，
Figure DEST_PATH_IMAGE006
为超平面的法向量
Figure 720281DEST_PATH_IMAGE002
的转置矩阵，时间[0，T]为运动的总时间，
Figure DEST_PATH_IMAGE007
为第i个无人驾驶车辆的包裹圆的半径；
所述根据环境模型、运动学模型、防碰撞包裹圆模型以及无人驾驶车辆与环境中动静态障碍物防碰撞模型，基于交替方向乘子法逐步迭代对多无人驾驶车辆进行解耦式运动规划，确定每一无人驾驶车辆的最优路径，具体包括：
根据环境模型、运动学模型、防碰撞包裹圆模型以及无人驾驶车辆与环境中动静态障碍物防碰撞模型确定每一无人驾驶车辆的起点与终点，并根据每一无人驾驶车辆的起点与终点，采用B样条曲线，确定每一无人驾驶车辆的运动轨迹的B样条曲线函数；
根据每一无人驾驶车辆的运动轨迹的B样条曲线函数，建立以所有无人驾驶车辆的运动轨迹的总和最小为目标的总和函数；
根据总和函数建立增加二次惩罚项的增广拉格朗日函数；
根据增加二次惩罚项的增广拉格朗日函数，利用交替方向乘子法进行迭代更新，确定每一无人驾驶车辆的最优路径。
2.根据权利要求1所述的一种多无人驾驶车辆路径协同规划方法，其特征在于，所述建立每一无人驾驶车辆的运动学模型，并根据运动模型建立无人驾驶车辆之间的防碰撞包裹圆模型，具体包括：
利用公式
Figure DEST_PATH_IMAGE008
确定无人驾驶车辆之间的防碰撞包裹圆模型；
其中，
Figure 394451DEST_PATH_IMAGE007
为第i个无人驾驶车辆的包裹圆的半径，
Figure DEST_PATH_IMAGE009
为第j个无人驾驶车辆的包裹圆的半径，
Figure DEST_PATH_IMAGE010
，
Figure DEST_PATH_IMAGE011
为第i个无人驾驶车辆的包裹圆的圆心坐标，
Figure DEST_PATH_IMAGE012
，
Figure DEST_PATH_IMAGE013
为第j个无人驾驶车辆的包裹圆的圆心坐标，N为无人驾驶车辆的个数。
3.一种多无人驾驶车辆路径协同规划系统，其特征在于，包括：
环境模型建立模块，用于建立多无人驾驶车辆行驶的环境模型；所述环境模型包括：车辆行驶区域、静态障碍物和动态障碍物；
防碰撞包裹圆模型建立模块，用于建立每一无人驾驶车辆的运动学模型，并根据运动模型建立无人驾驶车辆之间的防碰撞包裹圆模型；所述防碰撞包裹圆模型为两个无人驾驶车辆的包裹圆不重叠；所述包裹圆的半径是根据对应无人驾驶车辆的前悬长度、后悬长度、车宽以及前后轴之间的距离进行确定；所述包裹圆的圆心是根据对应无人驾驶车辆的后轴中间坐标、在大地坐标系中的姿态角以及前后轴之间的距离进行确定；
无人驾驶车辆与环境中动静态障碍物防碰撞模型建立模块，用于根据无人驾驶车辆的之间防碰撞包裹圆模型以及环境模型，采用分离超平面定理，建立无人驾驶车辆与环境中动静态障碍物防碰撞模型；
最优路径确定模块，用于根据环境模型、运动学模型、防碰撞包裹圆模型以及无人驾驶车辆与环境中动静态障碍物防碰撞模型，基于交替方向乘子法逐步迭代对多无人驾驶车辆进行解耦式运动规划，确定每一无人驾驶车辆的最优路径；
所述环境模型建立模块具体包括：
动态障碍物的参数获取单元，用于获取动态障碍物的起始点位置、运动时间以及运动速度；
动态障碍物的运动轨迹确定单元，用于根据动态障碍物的起始点位置、运动时间以及运动速度确定动态障碍物的运动轨迹；
所述无人驾驶车辆与环境中动静态障碍物防碰撞模型建立模块具体包括：
无人驾驶车辆与环境中动静态障碍物防碰撞模型建立单元，用于利用公式
Figure 972938DEST_PATH_IMAGE001
确定无人驾驶车辆与环境中动静态障碍物防碰撞模型；
其中，
Figure 998663DEST_PATH_IMAGE002
为超平面的法向量，
Figure 203379DEST_PATH_IMAGE003
为超平面的偏移量，
Figure 543225DEST_PATH_IMAGE002
与
Figure 415366DEST_PATH_IMAGE003
被参数化为样条，超平面的运动被描述为时间的函数，
Figure 549675DEST_PATH_IMAGE004
为第i个无人驾驶车辆的运动轨迹，
Figure 241688DEST_PATH_IMAGE005
为第i个无人驾驶车辆的纵向速度，t为第i个无人驾驶车辆的运动时间，
Figure 179032DEST_PATH_IMAGE006
为超平面的法向量
Figure 843363DEST_PATH_IMAGE002
的转置矩阵，时间[0，T]为运动的总时间，
Figure 210890DEST_PATH_IMAGE007
为第i个无人驾驶车辆的包裹圆的半径；
所述最优路径确定模块具体包括：
B样条曲线函数确定单元，用于根据环境模型、运动学模型、防碰撞包裹圆模型以及无人驾驶车辆与环境中动静态障碍物防碰撞模型确定每一无人驾驶车辆的起点与终点，并根据每一无人驾驶车辆的起点与终点，采用B样条曲线，确定每一无人驾驶车辆的运动轨迹的B样条曲线函数；
总和函数建立单元，用于根据每一无人驾驶车辆的运动轨迹的B样条曲线函数，建立以所有无人驾驶车辆的运动轨迹的总和最小为目标的总和函数；
增广拉格朗日函数建立单元，用于根据总和函数建立增加二次惩罚项的增广拉格朗日函数；
最优路径确定单元，用于根据增加二次惩罚项的增广拉格朗日函数，利用交替方向乘子法进行迭代更新，确定每一无人驾驶车辆的最优路径。
4.根据权利要求3所述的一种多无人驾驶车辆路径协同规划系统，其特征在于，所述防碰撞包裹圆模型建立模块具体包括：
防碰撞包裹圆模型建立单元，用于利用公式
Figure 390199DEST_PATH_IMAGE008
确定无人驾驶车辆之间的防碰撞包裹圆模型；
其中，
Figure 868585DEST_PATH_IMAGE007
为第i个无人驾驶车辆的包裹圆的半径，
Figure 449739DEST_PATH_IMAGE009
为第j个无人驾驶车辆的包裹圆的半径，
Figure 925851DEST_PATH_IMAGE010
，
Figure 191042DEST_PATH_IMAGE011
为第i个无人驾驶车辆的包裹圆的圆心坐标，
Figure 410802DEST_PATH_IMAGE012
，
Figure 846463DEST_PATH_IMAGE013
为第j个无人驾驶车辆的包裹圆的圆心坐标，N为无人驾驶车辆的个数。
CN202210400906.7A 2022-04-18 2022-04-18 一种多无人驾驶车辆路径协同规划方法及系统 Active CN114489087B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210400906.7A CN114489087B (zh) 2022-04-18 2022-04-18 一种多无人驾驶车辆路径协同规划方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210400906.7A CN114489087B (zh) 2022-04-18 2022-04-18 一种多无人驾驶车辆路径协同规划方法及系统

Publications (2)

Publication Number Publication Date
CN114489087A CN114489087A (zh) 2022-05-13
CN114489087B true CN114489087B (zh) 2022-07-05

Family

ID=81489546

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210400906.7A Active CN114489087B (zh) 2022-04-18 2022-04-18 一种多无人驾驶车辆路径协同规划方法及系统

Country Status (1)

Country Link
CN (1) CN114489087B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114995465B (zh) * 2022-08-02 2022-11-15 北京理工大学 一种考虑车辆运动能力的多无人车运动规划方法和系统

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106406346A (zh) * 2016-11-01 2017-02-15 北京理工大学 一种多无人机协同快速覆盖搜索航迹规划方法
CN106926844A (zh) * 2017-03-27 2017-07-07 西南交通大学 一种基于实时环境信息的动态自动驾驶换道轨迹规划方法
CN108415413A (zh) * 2018-03-28 2018-08-17 华南农业大学 一种基于圆形有用域的智能叉车局部避障路径规划方法
CN108536149A (zh) * 2018-04-25 2018-09-14 吉林大学 一种基于Dubins路径的无人驾驶车辆避障控制装置及控制方法
CN109141450A (zh) * 2018-07-13 2019-01-04 广州杰赛科技股份有限公司 目标速度确定方法和系统、计算机设备、计算机存储介质
CN109407705A (zh) * 2018-12-14 2019-03-01 厦门理工学院 一种无人机躲避障碍物的方法、装置、设备和存储介质
CN112068445A (zh) * 2020-09-23 2020-12-11 北京理工大学 自动驾驶车辆路径规划与路径跟踪集成控制方法及系统
CN112526988A (zh) * 2020-10-30 2021-03-19 西安交通大学 一种自主移动机器人及其路径导航和路径规划方法、系统
CN112572472A (zh) * 2020-12-08 2021-03-30 重庆大学 一种基于Frenet坐标系的自动驾驶碰撞预测方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR102326062B1 (ko) * 2014-11-12 2021-11-12 현대모비스 주식회사 자율주행차량의 장애물 회피 시스템 및 방법
US11217107B2 (en) * 2017-10-05 2022-01-04 California Institute Of Technology Simultaneous representation of moving and static obstacles for automatically controlled vehicles

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106406346A (zh) * 2016-11-01 2017-02-15 北京理工大学 一种多无人机协同快速覆盖搜索航迹规划方法
CN106926844A (zh) * 2017-03-27 2017-07-07 西南交通大学 一种基于实时环境信息的动态自动驾驶换道轨迹规划方法
CN108415413A (zh) * 2018-03-28 2018-08-17 华南农业大学 一种基于圆形有用域的智能叉车局部避障路径规划方法
CN108536149A (zh) * 2018-04-25 2018-09-14 吉林大学 一种基于Dubins路径的无人驾驶车辆避障控制装置及控制方法
CN109141450A (zh) * 2018-07-13 2019-01-04 广州杰赛科技股份有限公司 目标速度确定方法和系统、计算机设备、计算机存储介质
CN109407705A (zh) * 2018-12-14 2019-03-01 厦门理工学院 一种无人机躲避障碍物的方法、装置、设备和存储介质
CN112068445A (zh) * 2020-09-23 2020-12-11 北京理工大学 自动驾驶车辆路径规划与路径跟踪集成控制方法及系统
CN112526988A (zh) * 2020-10-30 2021-03-19 西安交通大学 一种自主移动机器人及其路径导航和路径规划方法、系统
CN112572472A (zh) * 2020-12-08 2021-03-30 重庆大学 一种基于Frenet坐标系的自动驾驶碰撞预测方法

Non-Patent Citations (9)

* Cited by examiner, † Cited by third party
Title
An Obstacle Avoidance Method for Autonomous Vehicle in Straight Road Based on Expanded Circle;Penghang Feng etal.;《2020 Asia-Pacific Conference on Image Processing, Electronics and Computers (IPEC)》;20201231;第43-46页 *
Mengyin Fu ; Kai Zhang ; Yi Yang ; Hao Zhu ; Meiling Wang.Collision-free and kinematically feasible path planning along a reference path for autonomous vehicle.《2015 IEEE Intelligent Vehicles Symposium (IV)》.2015, *
Yongjun Yan ; Jinxiang Wang ; Kuoran Zhang ; Mingcong Cao ; Jiansong Che.Path Planning using a Kinematic Driver-Vehicle-Road Model with Consideration of Driver"s Characteristics.《2019 IEEE Intelligent Vehicles Symposium (IV)》.2019, *
Yue Li ; Qiang Huang ; Junyao Gao ; Xuandong Su ; Jingchao Zhao ; Liancun.A Simple Reactive Algorithm for Miniature Mobile Robot Navigation in the Unknown Environment with Static Obstacles.《2013 IEEE International Conference on Robotics and Biomimetics (ROBIO)》.2013, *
基于粗糙集和邻域系统的智能车避障控制方法;江怡;《中国优秀博硕士学位论文全文数据库(硕士)工程科技Ⅱ辑》;20170715;第C035-81页 *
基于车身圆避撞模型的交叉口合流工况智能车决策控制研究;刘永刚等;《系统仿真技术及其应用》;20211231;第371-376页 *
基于运动学车辆模型的人-车-路系统建模和轨迹规划研究;严永俊;《中国优秀博硕士学位论文全文数据库(硕士)工程科技Ⅱ辑》;20220115;第C035-209页 *
无人车全局路径规划与局部避障方法研究;刘依卓;《中国优秀博硕士学位论文全文数据库(硕士)工程科技Ⅱ辑》;20220315;第C035-521页 *
无人驾驶车辆路径规划算法研究;刘光中;《中国优秀博硕士学位论文全文数据库(硕士)工程科技Ⅱ辑》;20220315;第C035-511页 *

Also Published As

Publication number Publication date
CN114489087A (zh) 2022-05-13

Similar Documents

Publication Publication Date Title
Noguchi et al. Development of a master–slave robot system for farm operations
Guo et al. Lane changing trajectory planning and tracking controller design for intelligent vehicle running on curved road
Feraco et al. A local trajectory planning and control method for autonomous vehicles based on the RRT algorithm
Yoon et al. Spline-based RRT∗ using piecewise continuous collision-checking algorithm for Car-like vehicles
Ferranti et al. SafeVRU: A research platform for the interaction of self-driving vehicles with vulnerable road users
Frego et al. Semi-analytical minimum time solutions with velocity constraints for trajectory following of vehicles
Neto et al. On the generation of trajectories for multiple UAVs in environments with obstacles
CN111381597A (zh) 一种基于高斯伪谱法的车辆避障轨迹规划方法
CN104881030A (zh) 基于快速终端滑模原理的无人车侧纵向耦合跟踪控制方法
Wang et al. Longitudinal and lateral control of autonomous vehicles in multi‐vehicle driving environments
CN114489087B (zh) 一种多无人驾驶车辆路径协同规划方法及系统
Cao et al. An integrated MPC approach for FWIA autonomous ground vehicles with emergency collision avoidance
Xu et al. Distributed formation control of homogeneous vehicle platoon considering vehicle dynamics
Li et al. Dynamically integrated spatiotemporal‐based trajectory planning and control for autonomous vehicles
Yue et al. Automated hazard escaping trajectory planning/tracking control framework for vehicles subject to tire blowout on expressway
Wang et al. A novel autonomous trajectory control for vehicular cyber-physical systems with flocking control algorithms
Wu et al. Dynamic path planning for autonomous driving on branch streets with crossing pedestrian avoidance guidance
Tan et al. Design and development of a novel autonomous scaled multiwheeled vehicle
Wille et al. Comprehensive treated sections in a trajectory planner for realizing autonomous driving in Braunschweig's urban traffic
CN114995465B (zh) 一种考虑车辆运动能力的多无人车运动规划方法和系统
Zhang et al. Learning end-to-end inertial-wheel odometry for vehicle ego-motion estimation
Osman et al. Combined road following control and automatic lane keeping for automated guided vehicles
Yang et al. ESO-based lateral control for electrical vehicles with unmodeled tire dynamics on uneven road
CN112706770B (zh) 一种考虑线控转向迟滞的车辆汇入控制系统及方法
Coad et al. Safe trajectory planning using reinforcement learning for self driving

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