CN111123904B - 路径跟踪方法及终端设备 - Google Patents

路径跟踪方法及终端设备 Download PDF

Info

Publication number
CN111123904B
CN111123904B CN201811289515.2A CN201811289515A CN111123904B CN 111123904 B CN111123904 B CN 111123904B CN 201811289515 A CN201811289515 A CN 201811289515A CN 111123904 B CN111123904 B CN 111123904B
Authority
CN
China
Prior art keywords
path
look
distance
point
path point
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
CN201811289515.2A
Other languages
English (en)
Other versions
CN111123904A (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.)
Ubtech Robotics Corp
Original Assignee
Ubtech Robotics Corp
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 Ubtech Robotics Corp filed Critical Ubtech Robotics Corp
Priority to CN201811289515.2A priority Critical patent/CN111123904B/zh
Priority to US16/576,794 priority patent/US11287825B2/en
Publication of CN111123904A publication Critical patent/CN111123904A/zh
Application granted granted Critical
Publication of CN111123904B publication Critical patent/CN111123904B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0234—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using optical markers or beacons
    • 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
    • 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
    • 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
    • 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

Abstract

本发明涉及计算机技术领域，提供了一种路径跟踪方法及终端设备。该方法包括：获取预设路径和运动目标的当前位置；在所述预设路径上确定所述当前位置对应的前瞻路径点；获取所述前瞻路径点对应的路径曲率；根据所述前瞻路径点对应的路径曲率确定所述运动目标在当前位置的调整速度。本发明能够根据路径的曲率确定运动目标的调整速度，以便对运动目标进行速度调整，提高运动目标在不同路径曲率情况下的路径跟踪稳定性。

Description

路径跟踪方法及终端设备
技术领域
本发明涉及计算机技术领域，尤其涉及一种路径跟踪方法及终端设备。
背景技术
目前对于机器人、汽车、移动物体等运动目标的路径跟踪方法通常是将路径与前瞻距离的交会点选取作为前瞻点，通过控制运动目标不断地向更新后的前瞻点运动的方法使得运动目标在路径轨道上行走。但是，在一些曲率较大的路径点附近，这种路径跟踪方法往往会将原本应该到达的路径点略过，而且在这些路径点附近也会偶发运动目标脱轨无法恢复的情况。
发明内容
有鉴于此，本发明实施例提供了路径跟踪方法及终端设备，以解决目前运动目标的路径跟踪稳定性差的问题。
本发明实施例的第一方面提供了路径跟踪方法，包括：
获取预设路径和运动目标的当前位置；
在所述预设路径上确定所述当前位置对应的前瞻路径点；
获取所述前瞻路径点对应的路径曲率；
根据所述前瞻路径点对应的路径曲率确定所述运动目标在当前位置的调整速度。
本发明实施例的第二方面提供了终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现第一方面中的路径跟踪方法。
本发明实施例的第三方面提供了计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现第一方面中的路径跟踪方法。
本发明实施例与现有技术相比存在的有益效果是：通过在预设路径上确定运动目标当前位置对应的前瞻路径点，并根据前瞻路径点对应的路径曲率确定运动目标在当前位置的调整速度，能够根据路径的曲率确定运动目标的调整速度，以便对运动目标进行速度调整，提高运动目标在不同路径曲率情况下的路径跟踪稳定性。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的路径跟踪方法的实现流程图；
图2是本发明实施例提供的路径跟踪方法中确定当前位置对应的前瞻路径点的实现流程图；
图3是本发明实施例提供的路径跟踪方法中确定调整速度的实现流程图；
图4是本发明实施例提供的路径跟踪方法中选取路径点的实现流程图；
图5是本发明实施例提供的路径跟踪方法中确定调整系数的实现流程图；
图6是本发明实施例提供的路径跟踪方法中根据调整系数和运动目标的当前速度计算调整速度的实现流程图；
图7是本发明实施例提供的路径跟踪方法中计算调整角速度的实现流程图；
图8是本发明实施例提供的实施示例的一个场景示意图；
图9是本发明实施例提供的实施示例的另一场景示意图；
图10是本发明实施例提供的路径跟踪装置的示意图；
图11是本发明实施例提供的终端设备的示意图。
具体实施方式
以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、技术之类的具体细节，以便透彻理解本发明实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施例中也可以实现本发明。在其它情况中，省略对众所周知的系统、装置、电路以及方法的详细说明，以免不必要的细节妨碍本发明的描述。
为了说明本发明所述的技术方案，下面通过具体实施例来进行说明。
图1为本发明实施例提供的路径跟踪方法的实现流程图，详述如下：
在S101中，获取预设路径和运动目标的当前位置。
在本实施例中，预设路径为运动目标行走所要跟踪的路径。例如预设路径可以为指定的路径或经过路径规划得到的路径等，在此不作限定。可以获取预设路径的路径信息，例如，可以获取预设路径在指定坐标系中的路径信息。
运动目标的当前位置为运动目标当前所在的位置。可以获取运动目标的当前位置的位置信息，例如，可以对运动目标进行定位，获取运动目标的当前位置在指定坐标系中的位置坐标。容易想到的，运动目标的当前位置可能在预设路径上，也可能不在预设路径上。
可选地，可以按照预设时间间隔、预设行走路程间隔或其他预设方式在运动目标行走过程中获取运动目标的所在位置，在此不作限定。
在S102中，在所述预设路径上确定所述当前位置对应的前瞻路径点。
在本实施例中，在运动目标行走过程中，可以在预设路径上选取一个在运动目标所在位置前方的路径点，作为前瞻路径点，以便根据该前瞻路径点调整运动目标的运动状态。通过选取运动目标行走过程中不同位置对应的前瞻路径点以及相应调整运动目标的运动状态，进行路径跟踪。如图8和图9中的P点即为前瞻路径点。
作为本发明的一个实施例，如图2所示，S102可以包括：
在S201中，获取所述当前位置对应的前瞻距离。
在本实施例中，运动目标在不同位置对应的前瞻距离可以相同，也可以不同，在此不作限定。前瞻距离用于确定运动目标当前位置对应的前瞻路径点。
可选地，S201可以包括：
获取预设前瞻距离，将所述预设前瞻距离作为所述当前位置对应的前瞻距离；或者，
获取所述运动目标的上一位置对应的调整前瞻距离，将所述上一位置对应的调整前瞻距离作为所述当前位置对应的前瞻距离。
在本实施例中，作为一种实现方式，可以获取预设前瞻距离，将预设前瞻距离作为当前位置对应的前瞻距离，以便确定当前位置对应的前瞻路径点。其中，预设前瞻距离可以是一个固定的距离值，这种情况下运动目标在任何位置对应的前瞻距离都相等；或者预先生成预设前瞻距离与位置点的对应关系表，根据运动目标的当前位置在对应关系表中查找对应的预设前瞻距离，这种情况下运动目标在不同位置对应的前瞻距离可能相等，也可能不相等，由对应关系表决定。还可以有其他对预设前瞻距离的设置方式，在此不作限定。
作为另一实现方式，可以在路径跟踪过程中对前瞻距离进行调整，将运动目标在上一个位置的前瞻距离进行调整，得到上一位置对应的调整前瞻距离，并将该调整前瞻距离作为运动目标在当前位置对应的前瞻距离，以便确定运动目标当前位置对应的前瞻路径点。
例如，设置一个初始前瞻距离，位置A1、位置A2、位置A3为依次前后相邻的运动目标的所在位置。当运动目标在位置A1时，以初始前瞻距离确定位置A1对应的前瞻路径点，并对初始前瞻距离进行调整，得到位置A1对应的调整前瞻距离；当运动目标在位置A2时，以位置A1对应的调整前瞻距离作为位置A2的前瞻距离，确定位置A2对应的前瞻路径点，并对位置A2的前瞻距离进行调整，得到位置A2对应的调整前瞻距离；当运动目标在位置A3时，以位置A2对应的调整前瞻距离作为位置A3的前瞻距离，确定位置A3对应的前瞻路径点，并对位置A3的前瞻距离进行调整，得到位置A3对应的调整前瞻距离；之后运动目标所在位置对应的前瞻距离按照此方式依次递推，在此不再赘述。
本实施例通过在路径跟踪过程中对前瞻距离不断进行调整，将上一位置对应的调整前瞻距离作为当前位置对应的前瞻距离，能够根据不同的路径状态及时调整前瞻距离，使前瞻距离更适于当前的路径状态，从而使由前瞻距离确定出的前瞻路径点更为恰当，提高路径跟踪的稳定性。
在S202中，在所述预设路径上选取第一路径点作为所述前瞻路径点；所述第一路径点与所述当前位置之间的距离为所述当前位置对应的前瞻距离。
在本实施例中，可以在预设路径上选取与运动目标当前位置之间的距离等于当前位置对应的前瞻距离的路径点作为前瞻路径点。若存在多个第一路径点，可以从多个第一路径点中选取相对位置靠后的一个路径点作为前瞻路径点。其中相对位置靠后是指按照预设路径的起点至预设路径的终点方向排列多个第一路径点，将其中排列在最后位置的第一路径点确定为当前位置对应的前瞻路径点。
在S103中，获取所述前瞻路径点对应的路径曲率。
在本实施例中，一个路径点的路径曲率为预设路径曲线在该路径点处的曲率，用于表征预设路径在该路径点处的弯曲程度。前瞻路径点对应的路径曲率可以为前瞻路径点的路径曲率，或者前瞻路径点所在路段内的一个或多个路径点的路径曲率，或者前瞻路径点所在路段内的一个或多个路径点的路径曲率的数据运算结果，在此不作限定。其中数据运算可以为相加、作差、相乘、加权求和等运算，在此不作限定。可以根据预设路径的路径信息计算前瞻路径点对应的路径曲率。
在S104中，根据所述前瞻路径点对应的路径曲率确定所述运动目标在当前位置的调整速度。
在本实施例中，调整速度用于指示对运动目标的速度进行控制调整，使运动目标按照调整速度行走。前瞻路径点对应的路径曲率可以表征运动目标前方路径的弯曲程度。本实施例在运动目标的每个位置确定每个位置对应的调整速度，能够根据路径状况及时调整运动目标的运动状态，进而提高运动目标的路径跟踪稳定性。通过确定出的调整速度，可以在路径曲率较大时调小运动目标的速度，避免运动目标的脱轨情况，提高稳定性。
本发明实施例通过在预设路径上确定运动目标当前位置对应的前瞻路径点，并根据前瞻路径点对应的路径曲率确定运动目标在当前位置的调整速度，能够根据路径的曲率确定运动目标的调整速度，以便对运动目标进行速度调整，提高运动目标在不同路径曲率情况下的路径跟踪稳定性。
作为本发明的一个实施例，如图3所示，S104可以包括：
在S301中，选取所述前瞻路径点所在路段内的至少一个路径点。
在本实施例中，可以在预设路径上前瞻路径点所在路段内选取一个或多个路径点。其中，预设路径上路段的划分可以根据实际需求而定，在此不作限定。
作为本发明的一个实施例，如图4所示，S301可以包括：
在S401中，查找所述当前位置对应的第二路径点；所述第二路径点为所述预设路径上所述当前位置对应的垂点。
在本实施例中，可以在预设路径上查找当前位置对应的垂点作为当前位置对应的第二路径点。若运动目标的当前位置位于预设路径上，可以将运动目标的当前位置作为第二路径点；若运动目标的当前位置不在预设路径上，可以在预设路径上查找当前位置对应的垂点作为第二路径点。其中，当前位置对应的垂点与当前位置的连线方向可以垂直于预设路径在该垂点处的切线方向。如图8中的D点即为当前位置在预设路径上对应的垂点，可以作为第二路径点。
在S402中，在所述预设路径上选取所述第二路径点与所述前瞻路径点之间的路段内的至少一个路径点。
在本实施例中，可以将第二路径点和前瞻路径点作为路段的两个端点，将在预设路径上的该路段作为前瞻路径点的所在路段，从该路段中选取一个或多个路径点。例如，可以每隔预设距离间隔选取一个路径点，或者在该路段均匀选取预设数量的路径点，或者按照其他的选取方式进行路径点的选取，在此不作限定。如图8中可以在D点与P点之间路段内选取至少一个路径点。
本实施例通过从预设路径上当前位置对应的垂点与前瞻路径点之间的路段选取路径点，使选取到的路径点能够更准确反映运动目标前方一段路径的路径状况，从而使确定的调整速度更加准确，能够更准确及时的调整运动目标的运动状态，提高路径跟踪的稳定性。
在S302中，根据各个路径点对应的路径曲率计算所述运动目标在当前位置的调整速度。
在本实施例中，一个路径点对应的路径曲率可以为该路径点的路径曲率，或者该路径点的路径曲率与其它路径点的路径曲率的数据运算结果，在此不作限定。例如，数据运算可以为相加、作差、相乘、加权求和等运算。
本实施例通过根据前瞻路径点所在路段的多个路径点对应的路径曲率计算调整速度，能够使计算出的调整速度准确度更高，进而提高路径跟踪的稳定性。
作为本发明的一个实施例，如图5所示，S302可以包括：
在S501中，计算各个路径点对应的路径曲率及各个路径点对应的第一距离；所述任一路径点对应的第一距离为该任一路径点与前一个路径点之间的距离。
在本实施例中，每个路径点对应一个第一距离。以一个路径点为例，该路径点与其前一个路径点之间的距离为该路径点对应的第一距离。例如，可以按照预设次序排列各个路径点，该路径点的前一个路径点可以为与该路径点相邻的排列在该路径点之前的一个路径点。其中，预设次序可以为从前瞻路径点所在路段的一个端点到另一端点的次序，也可以为其他的排列次序，在此不作限定。
可选地，S501中“计算各个路径点对应的路径曲率”可以包括：
计算任一路径点的路径曲率与前一路径点的路径曲率之间的差值作为该任一路径点对应的路径曲率。
在本实施例中，每个路径点对应一个路径曲率。以一个路径点为例，将该路径点的路径曲率与其相邻的前一个路径点的路径曲率之间的差值作为该路径点对应的路径曲率。
本实施例通过将一个路径点与其前一个路径点的路径曲率差值作为该路径点对应的路径曲率，能够使各个路径点对应的路径曲率准确反映各个路径点处路径曲率的变化量，从曲率变化量的角度反映运动目标前方路径的路径状况，能够根据路径曲率的变化量确定运动目标的调整速度，提高路径跟踪的稳定性。
在S502中，计算各个路径点对应的路径曲率的绝对值与对应第一距离的乘积值，并将各个路径点对应的乘积值求和，得到乘积值之和。
在本实施例中，对于一个路径点，可以首先计算该路径点对应的路径曲率的绝对值。例如，若一个路径点对应的路径曲率为该路径点的路径曲率与其相邻的前一个路径点的路径曲率之间的差值，则可以计算该差值的绝对值。然后计算该绝对值与该路径点对应的第一距离的乘积值。分别计算各个路径点对应的乘积值，再将各个路径点对应的乘积值相加求和，得到乘积值之和。其中，将各个路径点对应的乘积值求和可以是将各个路径点对应的乘积值直接求和，或者将各个路径点对应的乘积值加权求和，在此不作限定。
在S503中，根据所述乘积值之和确定调整系数。
在本实施例中，可以根据乘积值之和来确定调整系数。调整系数用于计算运动目标的调整速度。
作为本发明的一个实施例，如图6所示，S503可以包括：
在S601中，计算所述乘积值之和与前瞻距离的第一比值；所述前瞻距离为所述运动目标的当前位置与所述前瞻路径点之间的距离。
在本实施例中，可以获取当前位置的坐标值和前瞻路径点的坐标值，根据上述两个坐标值计算前瞻距离；或者若在S102中当前位置对应的前瞻路径点是由当前位置对应的前瞻距离确定的，则在S601中可以直接获取该前瞻距离。可以将乘积值之和除以前瞻距离，得到第一比值。
在S602中，根据所述第一比值确定调整系数。
本实施例通过将前瞻距离添加到确定调整系数的考虑因素中，能够在不同前瞻距离时对运动目标的运动状态所作的调整有所不同，通过综合考虑路径曲率和前瞻距离两个因素，使确定出的调整系数更为恰当，从而使确定出的调整速度更适合于运动目标前方的路径状况，提高路径跟踪的稳定性。
可选地，S602可以包括：
根据惩罚函数公式及所述第一比值计算所述调整系数；所述惩罚函数公式为：
其中，P为所述调整系数，χ为所述第一比值。
本实施例通过惩罚函数，使得随着第一比值的增大，调整系数相应减小，能够在路径曲率较大时得到较小的调整系数，从而减慢运动目标的速度，提升运动目标路径跟踪的稳定性和鲁棒性。
在S504中，根据所述调整系数和所述运动目标的当前速度计算所述运动目标在当前位置的调整速度。
在本实施例中，可以获取运动目标在当前位置的速度作为当前速度，根据调整系数和当前速度计算运动目标在当前位置的调整速度。
本实施例通过根据路径曲率、路径点之间的距离以及前瞻距离三个与路径状况相关的因素确定调整系数，再根据调整系数和当前速度计算运动目标的调整速度，能够使得计算出的调整速度更适合于路径状况，增加路径跟踪的稳定性。
作为本发明的一个实施例，所述当前速度包括当前线速度，所述调整速度包括调整线速度和调整角速度。如图7所示，S504可以包括：
在S701中，计算所述运动目标的当前线速度与所述调整系数的乘积，得到所述调整线速度。
在本实施例中，当前线速度为运动目标在当前位置的线速度。可以将当前线速度与调整系数相乘，得到调整线速度。调整线速度用于指示运动目标进行线速度调整，将线速度调整到该调整线速度。
在S702中，计算前瞻距离与所述调整系数的乘积，得到调整前瞻距离；所述前瞻距离为所述运动目标的当前位置与所述前瞻路径点之间的距离。
在本实施例中，可以将运动目标在当前位置的前瞻距离与调整系数相乘，得到调整前瞻距离。调整前瞻距离用于确定运动目标下一个位置对应的前瞻路径点。
本实施例通过调整系数对前瞻距离进行调整，在路径跟踪过程中及时根据路径状况调整前瞻距离，从而使前瞻距离及前瞻路径点的选取更适合前方的路径状况，提升路径跟踪的稳定性。例如，可以在路径曲率较大时缩短前瞻距离，从而提升运动目标在大曲率路径处的跟踪稳定性。
在S703中，根据所述调整线速度、所述调整前瞻距离和偏差角度计算调整角速度；所述偏差角度为所述运动目标的当前位置到所述前瞻路径点的向量方向与所述运动目标的当前线速度的方向之间的夹角角度。
在本实施例中，可以以运动目标的当前位置为起点，前瞻路径点为终点得到一个向量，该向量与运动目标当前线速度存在一个夹角，该夹角的角度即为运动目标在当前位置的偏差角度。可以根据调整线速度、调整前瞻距离和偏差角度计算得到调整角速度。调整角速度用于指示运动目标进行角速度的调整。如图8中速度V与向量OP之间的夹角η即为偏差角度。
本实施例通过调整系数计算调整线速度、调整前瞻距离和调整角速度，在路径跟踪过程中通过上述三方面对运动目标的运动状态进行调整，能够从多个角度出发，共同提升路径追踪的稳定性。
可选地，S703可以包括：
计算所述调整线速度与所述调整前瞻距离之间的第二比值；
计算所述偏差角度的正弦值与所述第二比值的乘积，得到所述调整角速度。
在本实施例中，可以将调整线速度除以调整前瞻距离得到第二比值，再计算偏差角度的正弦值，将偏差角度的正弦值与第二比值相乘，得到调整角速度。
下面以一个移动机器人为例进行说明。如图8所示，一个移动机器人(图8中圆形物体)在以速度V在跟踪路径时候，难免会与路径产生一点微小的距离偏差d和角度偏差η。如何控制机器人不断的减小d与η的绝对值就是路径跟踪问题。传统的做法就是以机器人几何中心为圆点，以L长度为半径划一段圆弧，使得圆弧与路径有交叉点P。所以设计的控制器可以由下面的公式(2)和(3)来描述：
其中，为O点至P点的向量，ω为角速度。
在上面的控制器的引导下，机器人会以V的线速度，以ω的角速度来运行，保证了机器人在一定的范围内跟踪路径。
然而，当路径的曲率较大时候，L很可能将一段路径略过，如图9所示。而在这样的情况下，机器人很容易发生控制效果不稳定的情况，导致脱轨。
本发明实施例将路径的曲率考虑进来，在前瞻距离L上与机器人的速度上都增加了一个以路径曲率为变量的惩罚函数，使得机器人在曲率较大的地方可以慢速行驶，并且与路径贴的更加紧密。其中，惩罚函数如公式(5)所示：
其中，NL为路径上D点至前瞻路径点P点形成的路段内的所有路径点的个数；Δκi＝κi-κi-1为第i个路径点的路径曲率与第i-1个路径点的路径曲率之间的变化值，为第i个路径点的路径曲率，yi为路径的曲线方程；Δsi＝si-si-1为第i个路径点的坐标值与第i-1个路径点的坐标值之差。L为运动目标在当前位置的前瞻距离，该前瞻距离等于由上一位置点计算出的调整前瞻距离。
可以分别通过公式(6)、(7)和(8)计算调整线速度、调整前瞻距离及调整角速度。
Vc＝V·P(L) (6)
Lc＝L·P(L) (7)
其中，V为当前线速度，Vc为计算得到的调整线速度；L为当前位置的前瞻距离，Lc为计算得到的调整前瞻距离，用于作为运动目标在下一位置时的前瞻距离；ωc为调整线速度，η为偏差角度。
本实施例通过对原本固定的前瞻距离增加惩罚函数(该惩罚函数与轨迹的曲率相关)的方法，提高轨迹跟踪控制的稳定性。当机器人运行到曲率较大的地点附近时，通过增加惩罚函数减小前瞻距离的方法让机器人对轨迹跟踪的更紧，速度更慢来提高轨迹跟踪控制的稳定性；当机器人运行到曲率较小的地点附近时，这个惩罚函数可以让机器人的速度提升进而保证此方法的效率。本实施例根据机器人附近路径的曲率，通过以曲率为变量的惩罚函数来优化前瞻距离及运行速度，使得机器人在大曲率路径情况下减慢速度，缩短前瞻距离，从而明显提升了机器人轨迹跟踪的鲁棒性。
本发明实施例通过在预设路径上确定运动目标当前位置对应的前瞻路径点，并根据前瞻路径点对应的路径曲率确定运动目标在当前位置的调整速度，能够根据路径的曲率确定运动目标的调整速度，以便对运动目标进行速度调整，提高运动目标在不同路径曲率情况下的路径跟踪稳定性。
应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。
对应于上文实施例所述的路径跟踪方法，图10示出了本发明实施例提供的路径跟踪装置的示意图。为了便于说明，仅示出了与本实施例相关的部分。
参照图10，该装置包括第一获取模块101、第一处理模块102、第二获取模块103和第二处理模块104。
第一获取模块101，用于获取预设路径和运动目标的当前位置。
第一处理模块102，用于在所述预设路径上确定所述当前位置对应的前瞻路径点。
第二获取模块103，用于获取所述前瞻路径点对应的路径曲率。
第二处理模块104，用于根据所述前瞻路径点对应的路径曲率确定所述运动目标在当前位置的调整速度。
可选地，所述第一处理模块102用于：
获取所述当前位置对应的前瞻距离；
在所述预设路径上选取第一路径点作为所述前瞻路径点；所述第一路径点与所述当前位置之间的距离为所述当前位置对应的前瞻距离。
可选地，所述第一处理模块102用于：
获取预设前瞻距离，将所述预设前瞻距离作为所述当前位置对应的前瞻距离；或者，
获取所述运动目标的上一位置对应的调整前瞻距离，将所述上一位置对应的调整前瞻距离作为所述当前位置对应的前瞻距离。
可选地，所述第二处理模块104用于：
选取所述前瞻路径点所在路段内的至少一个路径点；
根据各个路径点对应的路径曲率计算所述运动目标在当前位置的调整速度。
可选地，所述第二处理模块104用于：
查找所述当前位置对应的第二路径点；所述第二路径点为所述预设路径上所述当前位置对应的垂点；
在所述预设路径上选取所述第二路径点与所述前瞻路径点之间的路段内的至少一个路径点。
可选地，所述第二处理模块104用于：
计算各个路径点对应的路径曲率及各个路径点对应的第一距离；所述任一路径点对应的第一距离为该任一路径点与前一个路径点之间的距离；
计算各个路径点对应的路径曲率的绝对值与对应第一距离的乘积值，并将各个路径点对应的乘积值求和，得到乘积值之和；
根据所述乘积值之和确定调整系数；
根据所述调整系数和所述运动目标的当前速度计算所述运动目标在当前位置的调整速度。
可选地，所述第二处理模块104用于：
计算任一路径点的路径曲率与前一路径点的路径曲率之间的差值作为该任一路径点对应的路径曲率。
可选地，所述第二处理模块104用于：
计算所述乘积值之和与前瞻距离的第一比值；所述前瞻距离为所述运动目标的当前位置与所述前瞻路径点之间的距离；
根据所述第一比值确定调整系数。
可选地，所述第二处理模块104用于：
根据惩罚函数公式及所述第一比值计算所述调整系数；所述惩罚函数公式为：
其中，P为所述调整系数，χ为所述第一比值。
可选地，所述当前速度包括当前线速度，所述调整速度包括调整线速度和调整角速度；所述第二处理模块104用于：
计算所述运动目标的当前线速度与所述调整系数的乘积，得到所述调整线速度；
计算前瞻距离与所述调整系数的乘积，得到调整前瞻距离；所述前瞻距离为所述运动目标的当前位置与所述前瞻路径点之间的距离；
根据所述调整线速度、所述调整前瞻距离和偏差角度计算调整角速度；所述偏差角度为所述运动目标的当前位置到所述前瞻路径点的向量方向与所述运动目标的当前线速度的方向之间的夹角角度。
可选地，所述第二处理模块104用于：
计算所述调整线速度与所述调整前瞻距离之间的第二比值；
计算所述偏差角度的正弦值与所述第二比值的乘积，得到所述调整角速度。
本发明实施例通过在预设路径上确定运动目标当前位置对应的前瞻路径点，并根据前瞻路径点对应的路径曲率确定运动目标在当前位置的调整速度，能够根据路径的曲率确定运动目标的调整速度，以便对运动目标进行速度调整，提高运动目标在不同路径曲率情况下的路径跟踪稳定性。
图11是本发明一实施例提供的终端设备的示意图。如图11所示，该实施例的终端设备11包括：处理器110、存储器111以及存储在所述存储器111中并可在所述处理器110上运行的计算机程序112，例如程序。所述处理器110执行所述计算机程序112时实现上述各个方法实施例中的步骤，例如图1所示的步骤101至104。或者，所述处理器110执行所述计算机程序112时实现上述各装置实施例中各模块/单元的功能，例如图10所示模块101至104的功能。
示例性的，所述计算机程序112可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器111中，并由所述处理器110执行，以完成本发明。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序112在所述终端设备11中的执行过程。
所述终端设备11可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述终端设备可包括，但不仅限于，处理器110、存储器111。本领域技术人员可以理解，图11仅仅是终端设备11的示例，并不构成对终端设备11的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述终端设备还可以包括输入输出设备、网络接入设备、总线、显示器等。
所称处理器110可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
所述存储器111可以是所述终端设备11的内部存储单元，例如终端设备11的硬盘或内存。所述存储器111也可以是所述终端设备11的外部存储设备，例如所述终端设备11上配备的插接式硬盘，智能存储卡(Smart Media Card，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)等。进一步地，所述存储器111还可以既包括所述终端设备11的内部存储单元也包括外部存储设备。所述存储器111用于存储所述计算机程序以及所述终端设备所需的其他程序和数据。所述存储器111还可以用于暂时地存储已经输出或者将要输出的数据。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
在本发明所提供的实施例中，应该理解到，所揭露的装置/终端设备和方法，可以通过其它的方式实现。例如，以上所描述的装置/终端设备实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
所述集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(Read-Only Memory，ROM)、随机存取存储器(RandomAccess Memory，RAM)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括是电载波信号和电信信号。
以上所述实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围，均应包含在本发明的保护范围之内。

Claims (11)

1.一种路径跟踪方法，其特征在于，包括：
获取预设路径和运动目标的当前位置；
在所述预设路径上确定所述当前位置对应的前瞻路径点；
获取所述前瞻路径点对应的路径曲率；
根据所述前瞻路径点对应的路径曲率确定所述运动目标在当前位置的调整速度，包括：选取所述前瞻路径点所在路段内的至少一个路径点；计算各个路径点对应的路径曲率及各个路径点对应的第一距离；所述任一路径点对应的第一距离为该任一路径点与前一个路径点之间的距离；计算各个路径点对应的路径曲率的绝对值与对应第一距离的乘积值，并将各个路径点对应的乘积值求和，得到乘积值之和；根据所述乘积值之和确定调整系数；根据所述调整系数和所述运动目标的当前速度计算所述运动目标在当前位置的调整速度。
2.如权利要求1所述的路径跟踪方法，其特征在于，所述在所述预设路径上确定所述当前位置对应的前瞻路径点包括：
获取所述当前位置对应的前瞻距离；
在所述预设路径上选取第一路径点作为所述前瞻路径点；所述第一路径点与所述当前位置之间的距离为所述当前位置对应的前瞻距离。
3.如权利要求2所述的路径跟踪方法，其特征在于，所述获取所述当前位置对应的前瞻距离包括：
获取预设前瞻距离，将所述预设前瞻距离作为所述当前位置对应的前瞻距离；或者，
获取所述运动目标的上一位置对应的调整前瞻距离，将所述上一位置对应的调整前瞻距离作为所述当前位置对应的前瞻距离。
4.如权利要求1所述的路径跟踪方法，其特征在于，所述选取所述前瞻路径点所在路段内的至少一个路径点包括：
查找所述当前位置对应的第二路径点；所述第二路径点为所述预设路径上所述当前位置对应的垂点；
在所述预设路径上选取所述第二路径点与所述前瞻路径点之间的路段内的至少一个路径点。
5.如权利要求1所述的路径跟踪方法，其特征在于，所述计算各个路径点对应的路径曲率包括：
计算任一路径点的路径曲率与前一路径点的路径曲率之间的差值作为该任一路径点对应的路径曲率。
6.如权利要求1所述的路径跟踪方法，其特征在于，所述根据所述乘积值之和确定调整系数包括：
计算所述乘积值之和与前瞻距离的第一比值；所述前瞻距离为所述运动目标的当前位置与所述前瞻路径点之间的距离；
根据所述第一比值确定调整系数。
7.如权利要求6所述的路径跟踪方法，其特征在于，所述根据所述第一比值确定调整系数包括：
根据惩罚函数公式及所述第一比值计算所述调整系数；所述惩罚函数公式为：
其中，P为所述调整系数，χ为所述第一比值。
8.如权利要求1、5至7任一项所述的路径跟踪方法，其特征在于，所述当前速度包括当前线速度，所述调整速度包括调整线速度和调整角速度；
所述根据所述调整系数和所述运动目标的当前速度计算所述运动目标在当前位置的调整速度包括：
计算所述运动目标的当前线速度与所述调整系数的乘积，得到所述调整线速度；
计算前瞻距离与所述调整系数的乘积，得到调整前瞻距离；所述前瞻距离为所述运动目标的当前位置与所述前瞻路径点之间的距离；
根据所述调整线速度、所述调整前瞻距离和偏差角度计算调整角速度；所述偏差角度为所述运动目标的当前位置到所述前瞻路径点的向量方向与所述运动目标的当前线速度的方向之间的夹角角度。
9.如权利要求8所述的路径跟踪方法，其特征在于，所述根据所述调整线速度、所述调整前瞻距离和偏差角度计算调整角速度包括：
计算所述调整线速度与所述调整前瞻距离之间的第二比值；
计算所述偏差角度的正弦值与所述第二比值的乘积，得到所述调整角速度。
10.一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至9任一项所述方法的步骤。
11.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至9任一项所述方法的步骤。
CN201811289515.2A 2018-10-31 2018-10-31 路径跟踪方法及终端设备 Active CN111123904B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN201811289515.2A CN111123904B (zh) 2018-10-31 2018-10-31 路径跟踪方法及终端设备
US16/576,794 US11287825B2 (en) 2018-10-31 2019-09-20 Path tracking method and mobile robot using the same

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811289515.2A CN111123904B (zh) 2018-10-31 2018-10-31 路径跟踪方法及终端设备

Publications (2)

Publication Number Publication Date
CN111123904A CN111123904A (zh) 2020-05-08
CN111123904B true CN111123904B (zh) 2023-07-18

Family

ID=70325343

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811289515.2A Active CN111123904B (zh) 2018-10-31 2018-10-31 路径跟踪方法及终端设备

Country Status (2)

Country Link
US (1) US11287825B2 (zh)
CN (1) CN111123904B (zh)

Families Citing this family (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112461239A (zh) * 2020-11-10 2021-03-09 北京京东乾石科技有限公司 移动体路径规划方法、装置、设备及存储介质
CN112711252A (zh) * 2020-12-08 2021-04-27 深圳市优必选科技股份有限公司 移动机器人及其路径跟踪方法、计算机可读存储介质
CN112731933A (zh) * 2020-12-24 2021-04-30 江苏新冠亿科技有限公司 一种自主规划路径的agv跟踪控制方法、设备及存储介质
CN112612289B (zh) * 2021-03-08 2021-09-14 浙江大华技术股份有限公司 轨迹跟踪控制方法、移动机器人、控制设备及存储介质
CN113063423B (zh) * 2021-03-23 2023-01-10 广东工业大学 一种基于lanelet框架的全局路径规划切割方法和系统
CN113156962B (zh) * 2021-04-29 2023-11-17 深圳银星智能集团股份有限公司 运动控制方法、装置、机器人及存储介质
CN113608525A (zh) * 2021-06-23 2021-11-05 北京迈格威科技有限公司 机器人运动状态的指示方法、装置、电子设备及存储介质
CN113253741B (zh) * 2021-06-29 2021-11-23 浙江华睿科技股份有限公司 一种agv叉车的控制方法、装置及计算机可读介质
CN113467460B (zh) * 2021-07-09 2024-03-12 江苏大学 一种基于双圆形前视距离的农业机械路径跟踪方法及系统
CN113495571B (zh) * 2021-07-26 2024-02-27 中国人民解放军63629部队 飞行器路径跟踪方法、装置、设备和介质
CN114234968B (zh) * 2021-12-17 2023-12-05 江西洪都航空工业集团有限责任公司 一种基于a星算法的移动机器人自主导航方法
CN114325229A (zh) * 2021-12-27 2022-04-12 深圳供电局有限公司 一种基于北斗的输电线路故障点定位方法与系统
CN115009266B (zh) * 2022-08-08 2022-11-01 国汽智控(北京)科技有限公司 车辆控制方法、装置及存储介质
CN116300409B (zh) * 2023-05-17 2023-08-15 季华实验室 一种轨迹跟踪控制方法、装置、电子设备及存储介质
CN116295447B (zh) * 2023-05-23 2023-08-15 未来机器人（深圳）有限公司 路径跟踪方法及自动导航车
CN117519212B (zh) * 2024-01-03 2024-04-12 杭州华橙软件技术有限公司 路径跟踪控制方法、装置、终端及计算机可读存储介质

Family Cites Families (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3430832B2 (ja) * 1997-01-27 2003-07-28 日産自動車株式会社 道路曲率推定装置
JP3961378B2 (ja) * 2002-09-19 2007-08-22 アルパイン株式会社 車載用ナビゲーション装置
JP2009126255A (ja) * 2007-11-21 2009-06-11 Aisin Aw Co Ltd 車両制御装置、車両制御方法及びコンピュータプログラム
KR20140025151A (ko) * 2012-08-21 2014-03-04 현대모비스 주식회사 스마트 크루즈 컨트롤 장치 및 방법
CN104460665A (zh) * 2014-10-13 2015-03-25 上海交通大学 基于道路曲率地图的磁导航无人车及其地图的建立方法
JP6536064B2 (ja) * 2015-02-10 2019-07-03 株式会社デンソー 走行軌跡選定装置、走行軌跡選定方法
KR102623680B1 (ko) * 2015-02-10 2024-01-12 모빌아이 비젼 테크놀로지스 엘티디. 자율 주행을 위한 약도
GB2539676B (en) * 2015-06-23 2020-11-25 Bentley Motors Ltd A method of controlling speed of a vehicle
KR102560681B1 (ko) * 2016-01-07 2023-07-28 한화에어로스페이스 주식회사 최적 경로 생성 방법 및 이를 위한 장치
CN107092249A (zh) * 2016-02-18 2017-08-25 苏州青飞智能科技有限公司 一种按需调度的无轨道磁导航无人车系统
GB201613105D0 (en) * 2016-07-29 2016-09-14 Tomtom Navigation Bv Methods and systems for map matching
CN106740868B (zh) * 2016-12-30 2019-03-29 东软集团股份有限公司 一种车速规划的方法、装置和设备
CN107153420B (zh) * 2017-05-25 2021-01-22 广州汽车集团股份有限公司 路径跟踪控制方法、装置及智能汽车
CN107097785B (zh) * 2017-05-25 2019-08-27 江苏大学 一种预瞄距离自适应的智能车辆横向控制方法
CN107368079B (zh) * 2017-08-31 2019-09-06 珠海市一微半导体有限公司 机器人清扫路径的规划方法及芯片
JP6969962B2 (ja) * 2017-10-05 2021-11-24 トヨタ自動車株式会社 車両の運転支援及び／又は走行制御のための地図情報提供システム
CN108388241B (zh) * 2018-01-05 2021-02-12 广州科语机器人有限公司 移动机器人的路径跟踪方法

Also Published As

Publication number Publication date
US11287825B2 (en) 2022-03-29
US20200133289A1 (en) 2020-04-30
CN111123904A (zh) 2020-05-08

Similar Documents

Publication Publication Date Title
CN111123904B (zh) 路径跟踪方法及终端设备
US11161246B2 (en) Robot path planning method and apparatus and robot using the same
CN110316193B (zh) 预瞄距离的设置方法、装置、设备及计算机可读存储介质
CN113715814B (zh) 碰撞检测方法、装置、电子设备、介质及自动驾驶车辆
CN111813101B (zh) 机器人路径规划方法、装置、终端设备及存储介质
US20190375099A1 (en) Motion target direction angle obtaining method, apparatus and robot using the same
CN110471409B (zh) 机器人巡检方法、装置、计算机可读存储介质及机器人
CN111158365A (zh) 一种路径规划方法、装置、机器人及存储介质
CN109974699B (zh) 一种机器人及其地图自主探索方法和装置
CN111208809B (zh) 机器人步态规划方法及终端设备
KR102560681B1 (ko) 최적 경로 생성 방법 및 이를 위한 장치
CN112051864A (zh) 用于移动目标轨迹跟踪的方法及装置、设备和可读介质
CN113548041B (zh) 应用于垂直车位的泊车控制方法、电子设备及车辆
CN111761581B (zh) 路径规划方法及装置、狭窄空间行进方法及装置
US20210088345A1 (en) System and method for autonomous motion planning
CN113191342A (zh) 车道定位方法及电子设备
CN115597620A (zh) 路径规划方法、装置、电子设备及存储介质
CN115309170A (zh) 一种考虑舒适性约束的轨迹规划方法、装置和系统
CN113306570A (zh) 用于控制自动驾驶车辆的方法、装置和自动驾驶配送车
CN113848899A (zh) 车辆横向控制方法、装置、设备及存储介质
CN111380556B (zh) 用于自动驾驶车辆的信息处理方法和装置
CN113156962B (zh) 运动控制方法、装置、机器人及存储介质
WO2024016395A1 (zh) 车辆的控制方法、装置和车辆
CN111402148B (zh) 用于自动驾驶车辆的信息处理方法和装置
CN114248761A (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