CN113255967A - 信号时序逻辑约束下基于终点回溯的任务规划方法和装置 - Google Patents

信号时序逻辑约束下基于终点回溯的任务规划方法和装置 Download PDF

Info

Publication number
CN113255967A
CN113255967A CN202110466457.1A CN202110466457A CN113255967A CN 113255967 A CN113255967 A CN 113255967A CN 202110466457 A CN202110466457 A CN 202110466457A CN 113255967 A CN113255967 A CN 113255967A
Authority
CN
China
Prior art keywords
node
planning
task
agent
search tree
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.)
Pending
Application number
CN202110466457.1A
Other languages
English (en)
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.)
Beijing Institute of Technology BIT
Original Assignee
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 Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202110466457.1A priority Critical patent/CN113255967A/zh
Publication of CN113255967A publication Critical patent/CN113255967A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
    • G06Q10/00—Administration; Management
    • G06Q10/04—Forecasting or optimisation specially adapted for administrative or management purposes, e.g. linear programming or "cutting stock problem"
    • G06Q10/047—Optimisation of routes or paths, e.g. travelling salesman problem
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2111/00—Details relating to CAD techniques
    • G06F2111/04—Constraint-based CAD

Landscapes

  • Engineering & Computer Science (AREA)
  • Business, Economics & Management (AREA)
  • Physics & Mathematics (AREA)
  • Human Resources & Organizations (AREA)
  • Theoretical Computer Science (AREA)
  • Economics (AREA)
  • Strategic Management (AREA)
  • General Physics & Mathematics (AREA)
  • Game Theory and Decision Science (AREA)
  • Operations Research (AREA)
  • Quality & Reliability (AREA)
  • Tourism & Hospitality (AREA)
  • Marketing (AREA)
  • General Business, Economics & Management (AREA)
  • Entrepreneurship & Innovation (AREA)
  • Development Economics (AREA)
  • Computer Hardware Design (AREA)
  • Evolutionary Computation (AREA)
  • Geometry (AREA)
  • General Engineering & Computer Science (AREA)
  • Management, Administration, Business Operations System, And Electronic Commerce (AREA)

Abstract

本发明公开了一种信号时序逻辑约束下基于终点回溯的任务规划方法和装置，该方法首先离线构建搜索树：在智能体的二维位置工作空间中，加入时间维信息，构建三维空间；信号时序逻辑约束下，在三维空间中，以根据智能体给定任务预估的终点为根节点，向起始状态平面生长快速随机搜索树，直到起始状态平面中每个有可能的位置点都被叶子节点覆盖；在线任务规划时，给定智能体的初始状态，在快速随机搜索树中搜索智能体可达范围内的所有叶子节点，选择代价值最小的叶子节点作为规划节点；智能体从初始状态到规划节点再到根节点的路径就是路径规划结果。本发明能够遍历可达状态空间，提高收敛成功率，降低在线规划复杂度。

Description

信号时序逻辑约束下基于终点回溯的任务规划方法和装置
技术领域
本发明涉及智能体任务规划技术领域，具体涉及一种信号时序逻辑约束下基于终点回溯的任务规划方法和装置。
背景技术
信号时序逻辑(Signal Temporal Logic)是当前机器人规划领域具有研究前景的技术领域。信号时序逻辑能够用程式化语言，建模一系列具有时序关系，且带有严格时间约束的复杂任务，从而对机器人的控制与规划添加高阶时序约束，并能够给底层控制器提供接口。在机器人规划领域中，有时机器人任务复杂，具有时序关系上的耦合性的特点，一些传统方法难以进行建模，而信号时序逻辑，能够很好的建模多智能体系统的复杂任务。
针对信号时序逻辑规划任务，已有的解决方案有如下几种：
方案1：文献(Raman V,Alexandre Donzé,Maasoumy M,et al.Model PredictiveControl with Signal Temporal Logic Specifications[C].53rd IEEE Conference onDecision and Control.IEEE,2015.)对于离散系统在信号时序逻辑的约束下的规划问题，提出了将信号时序逻辑任务编码为整数约束，原最优控制问题被转化为混合整数优化问题，通过迭代求解该问题得到机器人的最优路径。
方案2：文献(Lars L,Dimarogonas D V.Control Barrier Functions forSignal Temporal Logic Tasks[J].IEEE Control Systems Letters,2018,3:96-101.)为了解决基于混合整数规划求解的信号时序逻辑约束规划问题的时间复杂度高的问题，提出了一个基于控制屏障函数的高效规划理论框架。该框架中，各个信号时序逻辑任务有各自对应的控制屏障函数，通过凸优化以及本地反馈控制，得到该问题的切换形式最优解。
方案3：文献(Balakrishnan A,Deshmukh J V.Structured Reward Shapingusing Signal Temporal Logic specifications[C].2019IEEE/RSJ InternationalConference on Intelligent Robots and Systems(IROS).IEEE,2019:3481-3486.)该文献通过引入强化学习思想，得到信号时序逻辑约束下的最优路径。为了解决信号时序逻辑约束的稀疏奖励问题，该文章提出了一种回报塑造方法，将稀疏的奖励处理为稠密奖励，从而指导强化学习算法快速收敛。
对于第三种方案，希望找到Reward最大的路径，但是由于需要探索的状态空间往往过大，且由于信号时序逻辑自带严格的时间约束，很多探索路径在还没有完成任务时便违背了时间约束，因此难以收敛，找不到最优的路径，导致路径规划成功率降低。
发明内容
有鉴于此，本发明提供了一种信号时序逻辑STL约束下基于终点回溯的任务规划方法，能够遍历可达状态空间，提高收敛成功率，降低在线规划复杂度，有助于智能体完成更复杂的任务。
为了解决上述技术问题，本发明是这样实现的。
一种信号时序逻辑STL约束下基于终点回溯的任务规划方法，其特征在于，包括：
步骤1、离线构建搜索树：在智能体的二维位置工作空间(x,y)中，加入时间维信息t，构建三维空间(x,y,t)；根据智能体的给定任务预估终点sd＝(xd,yd,h)，并放置到所述三维空间中；(xd,yd)和h分别为任务目标位置和任务对应的最大时长；将智能体给定任务的所有未来操作符与过去操作符互换，构建出转换后任务的变换器Transducer；在Transducer的约束下，以终点sd作为根节点，自终点向t＝0的起始状态平面生长快速随机搜索树，直到起始状态平面中每个有可能的位置点都被叶子节点s'覆盖，停止快速随机搜索树的生长；计算快速随机搜索树中每个节点到根节点的代价值；
步骤2、在线任务规划：给定智能体的初始状态(x0,y0,0)，在快速随机搜索树中搜索智能体可达范围内的所有叶子节点s'，选择代价值最小的叶子节点作为规划节点；智能体从初始状态到规划节点再到根节点的路径wd就是路径规划结果。
优选地，步骤1所述停止快速随机搜索树的生长的判断方式为：
构建以叶子节点s'为顶点、开口朝向起始状态平面、高度无限延伸、圆锥中轴线垂直于(x,y)平面的圆锥，圆锥斜面斜率的绝对值为智能体的最大速度；每个圆锥与所述起始状态平面相交，如果所有圆锥的圆形相交面的并集覆盖智能体的工作范围，则停止快速随机搜索树的生长。
优选地，在确定所有圆锥的圆形相交面的并集是否覆盖智能体的工作范围时，进一步设置一个覆盖率ε％，将智能体的工作范围SQ与覆盖率ε％相乘，获得覆盖阈值SQ·ε％；当所有圆锥的圆形相交面的并集达到所述覆盖阈值SQ·ε％，则确定所述并集覆盖智能体的工作范围。
优选地，步骤2中所述智能体可达范围的确定方式为：
构建以智能体的初始状态(x0,y0,0)为顶点、圆锥开口朝向根节点、高度无限延伸、圆锥中轴线垂直于(x,y)平面的圆锥，圆锥斜面斜率的绝对值为智能体的最大速度；将圆锥面包围中的叶子节点s'确定为智能体可达范围；
或者，构建以每个叶子节点s'为顶点、圆锥开口背离根节点、高度无限延伸、圆锥中轴线垂直于(x,y)平面的圆锥，圆锥斜面斜率的绝对值为智能体的最大速度；判断初始状态(x0,y0,0)落在哪些叶子节点s'的圆锥内，则将这些叶子节点s'确定为智能体可达范围。
优选地，步骤1中所述计算快速随机搜索树中每个节点到根节点的代价值为：
Jsa＝cost(sa)-λ·r(w)
其中，Jsa表示节点sa到根节点的代价值；cost(sa)表示节点sa到根节点的距离，λ为偏好因子，r(w)为路径w的鲁棒程度。
优选地，该方法进一步包括：在路径wd中以一定时间分辨率进行点的采样，获得一条由带有时间的采样点构成的路径pa；采用模型预测控制MPC算法对pa进行跟踪，生成最优控制量储存至对应的采样点中，完成控制量规划。
优选地，在进行控制量规划时，判断规划路径上是否已经有之前规划过的采样点，如果是，则重复利用已存储控制量，即判断智能体的状态表征数据与该采样点对应的状态表征数据之间的差异是否在设定范围内，如果是则直接应用之前存储的控制量进行控制。
优选地，在进行控制量规划时，如果规划路径上部分段落有之前规划过的采样点，对于已经规划过的采样点，执行所述重复利用已存储控制量的操作；对于没有规划过的采样点，则采用模型预测控制算法生成最优控制量储存至对应的采样点中。
优选地，所述状态表征数据包括智能体位置和转角。
本发明还提供了一种信号时序逻辑STL约束下基于终点回溯的任务规划装置，包括离线构建搜索树模块、代价计算模块和在线任务规划模块；
离线构建搜索树模块，用于在智能体的二维位置工作空间(x,y)中，加入时间维信息t，构建三维空间(x,y,t)；根据智能体的给定任务φn预估终点sd＝(xd,yd,h)并放置到所述三维空间中，其中(xd,yd)和h分别为任务目标位置和任务对应的最大时长；将智能体给定任务φn的所有未来操作符与过去操作符互换，构建出转换后任务φn'的变换器Transducer；在Transducer的约束下，以终点sd作为根节点，自终点向t＝0的起始状态平面生长快速随机搜索树，直到起始状态平面中每个有可能的位置点都被叶子节点s'覆盖，停止快速随机搜索树的生长；
代价计算模块，用于计算快速随机搜索树中每个节点到根节点的代价值，记录到节点中；
在线任务规划模块，用于根据给定的智能体的初始状态(x0,y0,0)，在快速随机搜索树中搜索智能体可达范围内的所有叶子节点s'，选择代价值最小的叶子节点作为规划节点；智能体从初始状态到规划节点再到根节点的路径wd就是路径规划结果。
有益效果：
本发明首先证明出快速随机搜索树生长的所有满足转换后任务的时间反序序列，在正向执行时一定满足原任务要求，这是本发明能够实施的基础。基于此本发明提出了信号时序逻辑约束下基于终点回溯的高效规划方法，其创新点主要体现在以下几方面：
(1)回溯模型的建立：本发明采用从预设的终点向起始状态平面进行回溯的方式构建搜索树，叶子节点能够覆盖整个起始状态空间，因此可达空间可以被完全遍历。当利用搜索树正向搜索时，总会找到一条路径能够到达任务节点，从而提高收敛成功率；而且对于任意的起始状态，仅需要选择到达根节点距离代价最小的叶子节点即可完成规划，减少了规划的时间，降低在线规划复杂度，有助于智能体完成更复杂的任务。
(2)控制量的存储：在控制机器人跟随结果路径时，可将最优控制量存储在各个状态节点中。当该状态节点在另一次规划任务中再次被选择，且机器人状态与存储的状态相近，则可以直接调用存储的控制量完成控制，不再需要迭代求解控制量，进一步减少了规划时间。
(3)在一优选实施例中，本发明以最大速度为斜面斜率绝对值的圆锥与初始平面取交集，每个叶子节点能够覆盖一部分初始状态空间。当覆盖范围到达一定范围时停止搜索。类似的，在线任务规划时也采用以最大速度为斜面斜率的圆锥来确定智能体可达范围，能够保证智能体能够在自身速度限制下到达规划节点，保证整个规划结果的可用性。
(4)在一优选实施例中，本发明可以通过调整覆盖率控制离线构建过程耗费时间，提高了本方案使用时的灵活度。
(5)在一优选实施例中，代价值采用Jsa＝cost(sa)-λ·r(w)，使用者可以通过调整偏好因子的大小来控制找到的最优路径是最短路径还是最为符合任务约束的路径。
附图说明
图1为终点回溯的规划算法框架；
图2为Transducer指导构建的快速随机搜索树；叶子节点采用三角形表示；
图3为判断叶子节点覆盖初始状态平面的示意图；
图4为确定可达空间的示意图；(a)为第一种方案，(b)为第二种方案；叶子节点采用三角形表示，初始状态采用五角星表示；
图5为最终所得路径以及MPC控制效果；(a)为最终所得路径；(b)为MPC控制效果；
图6为本发明基于终点回溯的任务规划装置的示意图。
具体实施方式
本发明提供了一种信号时序逻辑(STL)约束下基于终点回溯的任务规划方法，其基本思想是：在智能体工作空间中加入时间维信息构建三维空间，将智能体的任务终点加入该三维空间；在三维空间中，以任务终点为根节点，在信号时序逻辑约束下进行回溯，获得搜索树并存储路径代价，实现了基于终点回溯的离线规划。在线任务规划时，利用搜索树就可以找到代价最小的路径。
可见，本发明采用从预设的终点向起始状态平面进行回溯的方式构建搜索树，叶子节点能够覆盖整个起始状态空间，因此可达空间可以被完全遍历。当利用搜索树正向搜索时，总会找到一条路径能够到达任务节点，从而提高收敛成功率；而且对于任意的起始状态，仅需要选择到达根节点距离代价最小的叶子节点即可完成规划，减少了规划的时间，降低在线规划复杂度，有助于智能体完成更复杂的任务。
本发明可以进一步通过模型预测控制(Model Predictive Control)对所得路径进行跟踪。由模型预测控制生成的最优控制量可以储存至对应的节点中，当又一次规划至该节点，且控制对象的状态与该点预置状态相近时，可以直接应用之前预置好的控制量进行控制，进一步减少在线运算时间。
下面结合附图并举实施例对本发明进行详细描述。以下实施以机器人为例进行描述。
下面先对信号时序逻辑进行简要介绍。信号时序逻辑是一类形式化语言，能够表述时间与逻辑上的约束关系。其在机器人规划领域的应用价值在于其能够为人提供高阶任务的表述方法，来控制机器人完成一系列具有先后顺序关系的动作，比如，先打开门才能进入房间，以及机器人每30分钟要前去充电。STL的具体语法定义如式(1)：
Figure BDA0003044211280000071
其中，I＝[a,b]为一时间区域，p为原子命题，通常为环境的标签或者机器人的某一个动作。
Figure BDA0003044211280000072
表示所有按照(1)式定义的任务，
Figure BDA0003044211280000073
是对
Figure BDA0003044211280000074
取反。
Figure BDA0003044211280000075
为
Figure BDA0003044211280000076
或
Figure BDA0003044211280000077
Figure BDA0003044211280000078
是未来形式，指在
Figure BDA0003044211280000079
为假之后的I时刻内，
Figure BDA00030442112800000710
需要为真。
Figure BDA00030442112800000711
是上式的过去形式，指在
Figure BDA00030442112800000712
成真之后的I时刻内
Figure BDA00030442112800000713
需要为真。各公式的形式化定义如下，其中w为一带有时间的序列。
Figure BDA0003044211280000081
其中，iff表示当且仅当，t′和t″表示不同的时间。
此外，FI(Eventually)、GI(Always)以及它们对应的过去形式可以定义如式(3)。
Figure BDA0003044211280000082
所有STL公式均可以通过FI、
Figure BDA0003044211280000083
以及
Figure BDA0003044211280000084
构造得到。
定义h为能够断定轨迹是否满足公式所需要的最长时间，其定义如式(4)：
Figure BDA0003044211280000085
其中，k代表任务φ的最小时间间隔。F[a,b]φ意为“在a到b秒内最终φ成立”，G[a,b]φ意为“在a到b秒内φ始终成立”。
对于STL公式，存在度量一条路径w对于公式的满足程度的公制单位，称为鲁棒程度，
Figure BDA0003044211280000086
r采用迭代形式进行计算，如式(5)所示。
Figure BDA0003044211280000087
定义1.对于任意一个带有时间的序列，w＝(s0,t0)(s1,t1)...(sn,tn)，其时间反序序列定义为w'＝(sn,t0)(sn-1,t1)...(s0,tn)。其中，si表示ti时刻的状态，i＝1,2,…,n。若任务描述中p指到达距离某点m一定距离的范围d内，则f(w)指与m点距离。
定理1.对于任何一个任务，若存在一个序列满足未来(过去)形式任务，则此序列的时间反序序列满足该任务对应的过去(未来)形式。
证.证明仅以
Figure BDA0003044211280000091
以及
Figure BDA0003044211280000092
为例，其他对应公式证明方法类似。假设
Figure BDA0003044211280000093
则根据公式(2)，
Figure BDA0003044211280000094
则在构建时间反序序列时，
Figure BDA0003044211280000095
这恰好符合
Figure BDA0003044211280000096
的定义，因此如果
Figure BDA0003044211280000097
则有
Figure BDA0003044211280000098
成立。证毕。
上述证明结论说明当进行时间回溯的离线模型构建时，只要将未来(过去)形式任务变为对应的过去(未来)任务，满足转换后任务的路径，在正向执行时，满足转换前的原始任务。
在上述证明结论指导下，本发明提出的STL约束下基于终点回溯的任务规划方法具体包括如下步骤。其中，步骤S1～S3为离线构建搜索树过程，步骤S4～S5为在线任务规划步骤。
离线构建搜索树
步骤S1、在机器人的二维位置工作空间(x,y)中，加入时间维信息t，构建三维空间(x,y,t)；根据机器人的给定任务φn预估终点sd＝(xd,yd,h)，并在三维空间中t＝h的平面上放置所述终点sd。其中(xd,yd)为任务目标位置，h为任务对应的最大时长；图2中终点sd是位于搜索树底部的圆点。h是给定的完成任务的最长时间，在后续进行搜索树的生长过程中，如果时间已经到h了，探索路径还没有完成任务，那么这条路径就可以断定为不符合任务了。
步骤S2、将机器人给定任务φn的所有未来操作符与过去操作符互换，构建出转换后任务φn'的变换器Transducer，表示为TSn；在TSn的指导下，以终点sd作为根节点，自终点sd向t＝0的起始状态平面生长快速随机搜索树，直到起始状态平面中每个有可能的位置点都被叶子节点s'覆盖。根据定理1，快速随机搜索树生长的所有满足转换后任务φn的时间反序序列，在正向执行时一定满足原任务要求。
起始状态平面是指t＝0所在的平面。在快速随机搜索树的生长过程中，节点慢慢靠近起始状态平面，但是由于采样算法自身随机采样特性，可能节点不会完全落在起始状态平面内。当叶子节点靠近起始状态平面，且叶子节点足够多时，初始状态平面的每个有可能完成任务的状态点就将被覆盖到。也就是说，当快速随机搜索树离线采样到足够多的数据点后，离线构建过程结束，此时所有满足任务的叶子节点前往根节点的路径在正向执行时一定满足原任务。
本步骤中，起始状态平面中每个有可能的位置点都被叶子节点s'覆盖的判断方式为：构建以叶子节点s'为顶点、开口朝向起始状态平面、高度无限延伸、圆锥中轴线垂直于(x,y)平面的圆锥，圆锥斜面斜率的绝对值为机器人的最大速度；如图3所示，每个圆锥与所述起始状态平面相交，所有圆锥的圆形相交面的并集覆盖机器人的工作范围，则认为有足够多的叶子节点，无论机器人在其工作空间中的那个位置起始，均可以通过某一个叶子节点及其连接的路径到达根节点。如果还不满足覆盖机器人工作范围的条件，则继续生长搜索树。
在实际中，还可以设置一个覆盖率ε％，将机器人的工作范围SQ与该覆盖率ε％相乘，获得一个覆盖阈值SQ·ε％；当所有圆锥的圆形相交面的并集达到该工作范围覆盖量，则认为有足够多的叶子节点。这样，通过调节覆盖率ε％，可以调整离线搜索树的构建速度。ε％越大，所需时间越长，ε％越小，所需时间越短。
本步骤中，将机器人给定任务φn的所有未来操作符与过去操作符互换，是指将给定任务的所有未来操作符转换为过去操作符，将给定任务的所有过去操作符转换为未来操作符。其中包括FI与GI的互换，
Figure BDA0003044211280000111
与
Figure BDA0003044211280000112
的互换，例如
Figure BDA0003044211280000113
转化成
Figure BDA0003044211280000114
图2为快速随机搜索树的生长结果。其中，较细的圆柱体为障碍物，每一个采样点与之前的采样点连接时要考虑会不会与障碍物碰撞。较粗的圆柱体代表任务点，其横跨的时间轴代表按照任务描述，在哪段时间内到达该任务点是满足任务约束的，在根据障碍物生长快速随机搜索树的过程为现有技术，这里不赘述。
步骤S3、计算快速随机搜索树中每个节点到根节点的代价值，记录到节点中。
本步骤中，代价值可以为节点(叶子节点s’以及中间节点s)到根节点的距离。在一优选实施例中，节点到根节点的代价值为：
Jsa＝cost(sa)-λ·r(w) (6)
其中，Jsa表示节点sa到根节点的代价值；cost(sa)表示节点sa到根节点的距离，λ为偏好因子，r(w)为路径w的鲁棒程度。
在构建完搜索树之后，树上的每一个节点(包括叶子节点s’以及中间节点s)都有对应的一个代价值，表征着从这个节点开始到达终点的路径的整体代价。
当快速随机搜索树离线采样到足够多的数据点之后，离线构建过程结束，此时所有满足任务φn的叶子节点前往根节点的路径在正向执行时一定满足原任务。在这些叶子节点足够多时，初始状态平面的每个有可能完成任务的状态点就将被覆盖到。
在线任务规划
步骤S4、给定机器人的初始状态(x0,y0,0)，在快速随机搜索树中搜索机器人可达范围内的所有叶子节点s'，选择代价值最小的叶子节点作为规划节点。机器人从初始状态到规划节点再到根节点的路径wd就是路径规划结果。
本步骤中，机器人可达范围也是通过构建锥形实现的。具体来说，参见图4中的(a)，构建以机器人的初始状态(x0,y0,0)为顶点、圆锥开口朝向根节点、高度无限延伸、圆锥中轴线垂直于(x,y)平面的圆锥，圆锥斜面斜率的绝对值为机器人的最大速度。在时间t的限制下，初始状态点均可以通过导航移动至圆锥中所有叶子节点。因此，将圆锥面包围中的叶子节点s'确定为机器人可达范围内的叶子节点s'。从圆锥包围的叶子节点中选择代价值最小的叶子节点，作为规划节点。以初始状态点为顶点、经过规划节点再到搜索树根节点连接一条路径来生成满足任务约束的路径wd。
在实际中，也可以采用图4中的(b)方案，构建以各叶子节点为顶点、圆锥开口背离根节点、高度无限延伸、圆锥中轴线垂直于(x,y)平面的2n个圆锥，n为叶子节点的个数；判断初始状态(x0,y0,0)落在了哪些叶子节点为顶点的圆锥内，这些叶子节点作为候选点，即机器人可达范围。
步骤S5、在路径wd中以一定时间分辨率进行点的采样，获得一条由带有时间的采样点ss构成的路径pa；采用模型预测控制算法对pa进行跟踪，生成最优控制量储存至对应的采样点ss中，完成控制量规划。
本步骤中采用模型预测控制对pa进行跟踪，具体问题建模如式(7)，
Figure BDA0003044211280000121
其中，N表示采样点的个数，xi表示机器人第i个状态点，pai表示第i个采样点。
该式子可以根据不同的系统进行不同的定义，并可以额外加入其它的约束条件，比如加入控制屏障函数的约束以保证任意分辨率内路径都会满足任务约束。
在进行控制量规划时，判断规划路径上是否已经有之前规划过的采样点ss，如果是，则重复利用已存储控制量，即判断机器人的状态表征数据与该采样点对应的状态表征数据之间的差异是否在设定范围内，如果是则直接应用之前存储的控制量进行控制。在一优选实施例中，状态表征数据包括机器人位置和转角。在实际中，还可以包括速度等其他信息。
在进行控制量规划时，如果规划路径上部分段落有之前规划过的采样点ss，对于已经规划过的采样点，执行所述重复利用已存储控制量的操作；对于没有规划过的采样点ss，则采用模型预测控制算法生成最优控制量储存至对应的采样点中。
至此，离线构建以及在线规划阶段的算法都已完成，本方案整体框架如图1所示。最终构建的快速随机搜索树如图2所示，给定任意起始状态，最终选取的路径以及MPC控制结果如图5所示。
基于上述方法，本发明还提供了一种信号时序逻辑STL约束下基于终点回溯的任务规划装置，如图6所示，包括离线构建搜索树模块、代价计算模块和在线任务规划模块。
离线构建搜索树模块，用于在智能体的二维位置工作空间(x,y)中，加入时间维信息t，构建三维空间(x,y,t)；根据智能体的给定任务φn预估终点sd＝(xd,yd,h)，并在所述三维空间中t＝h的平面上放置所述终点sd；将智能体给定任务φn的所有未来操作符与过去操作符互换，构建出转换后任务φn'的变换器Transducer；在Transducer的约束下，以终点sd作为根节点，自终点向t＝0的起始状态平面生长快速随机搜索树，直到起始状态平面中每个有可能的位置点都被叶子节点s'覆盖，停止快速随机搜索树的生长；
其中，停止快速随机搜索树的生长的判断方式为：构建以叶子节点s'为顶点、开口朝向起始状态平面、高度无限延伸、圆锥中轴线垂直于(x,y)平面的圆锥，圆锥斜面斜率的绝对值为智能体的最大速度；每个圆锥与所述起始状态平面相交，如果所有圆锥的圆形相交面的并集覆盖智能体的工作范围，则停止快速随机搜索树的生长。
优选地，在确定所有圆锥的圆形相交面的并集是否覆盖智能体的工作范围时，进一步设置一个覆盖率ε％，将智能体的工作范围SQ与覆盖率ε％相乘，获得覆盖阈值SQ·ε％；当所有圆锥的圆形相交面的并集达到所述覆盖阈值SQ·ε％，则确定所述并集覆盖智能体的工作范围。
代价计算模块，用于计算快速随机搜索树中每个节点到根节点的代价值，记录到节点中。优选地，快速随机搜索树中每个节点到根节点的代价值的计算方法采用公式(6)。
在线任务规划模块，用于根据给定的智能体的初始状态(x0,y0,0)，在快速随机搜索树中搜索智能体可达范围内的所有叶子节点s'，选择代价值最小的叶子节点作为规划节点；智能体从初始状态到规划节点再到根节点的路径wd就是路径规划结果。
其中，智能体可达范围的确定方式为：构建以智能体的初始状态(x0,y0,0)为顶点、圆锥开口朝向根节点、高度无限延伸、圆锥中轴线垂直于(x,y)平面的圆锥，圆锥斜面斜率的绝对值为智能体的最大速度；将圆锥面包围中的叶子节点s'确定为智能体可达范围；
或者，构建以每个叶子节点s'为顶点、圆锥开口背离根节点、高度无限延伸、圆锥中轴线垂直于(x,y)平面的圆锥，圆锥斜面斜率的绝对值为智能体的最大速度；判断初始状态(x0,y0,0)落在哪些叶子节点s'的圆锥内，则将这些叶子节点s'确定为智能体可达范围。
在线任务规划模块进一步完成控制量的规划。具体来说，在路径wd中以一定时间分辨率进行点的采样，获得一条由带有时间的采样点构成的路径pa；采用模型预测控制MPC算法对pa进行跟踪，生成最优控制量储存至对应的采样点中，完成控制量规划。
在进行控制量规划时，判断规划路径上是否已经有之前规划过的采样点，如果是，则重复利用已存储控制量，即判断智能体的状态表征数据与该采样点对应的状态表征数据之间的差异是否在设定范围内，如果是则直接应用之前存储的控制量进行控制。
优选地，在进行控制量规划时，如果规划路径上部分段落有之前规划过的采样点，对于已经规划过的采样点，执行所述重复利用已存储控制量的操作；对于没有规划过的采样点，则采用模型预测控制算法生成最优控制量储存至对应的采样点中。
以上的具体实施例仅描述了本发明的设计原理，该描述中的部件形状，名称可以不同，不受限制。所以，本发明领域的技术人员可以对前述实施例记载的技术方案进行修改或等同替换；而这些修改和替换未脱离本发明创造宗旨和技术方案，均应属于本发明的保护范围。

Claims (10)

1.一种信号时序逻辑STL约束下基于终点回溯的任务规划方法，其特征在于，包括：
步骤1、离线构建搜索树：在智能体的二维位置工作空间(x,y)中，加入时间维信息t，构建三维空间(x,y,t)；根据智能体的给定任务预估终点sd＝(xd,yd,h)，并放置到所述三维空间中；其中(xd,yd)为任务目标位置，h为任务对应的最大时长；将智能体给定任务的所有未来操作符与过去操作符互换，构建出转换后任务的变换器Transducer；在Transducer的约束下，以终点sd作为根节点，自终点向t＝0的起始状态平面生长快速随机搜索树，直到起始状态平面中每个有可能的位置点都被叶子节点s'覆盖，停止快速随机搜索树的生长；计算快速随机搜索树中每个节点到根节点的代价值；
步骤2、在线任务规划：给定智能体的初始状态(x0,y0,0)，在快速随机搜索树中搜索智能体可达范围内的所有叶子节点s'，选择代价值最小的叶子节点作为规划节点；智能体从初始状态到规划节点再到根节点的路径wd就是路径规划结果。
2.如权利要求1所述的方法，其特征在于，步骤1所述停止快速随机搜索树的生长的判断方式为：
构建以叶子节点s'为顶点、开口朝向起始状态平面、高度无限延伸、圆锥中轴线垂直于(x,y)平面的圆锥，圆锥斜面斜率的绝对值为智能体的最大速度；每个圆锥与所述起始状态平面相交，如果所有圆锥的圆形相交面的并集覆盖智能体的工作范围，则停止快速随机搜索树的生长。
3.如权利要求2所述的方法，其特征在于，在确定所有圆锥的圆形相交面的并集是否覆盖智能体的工作范围时，进一步设置一个覆盖率ε％，将智能体的工作范围SQ与覆盖率ε％相乘，获得覆盖阈值SQ·ε％；当所有圆锥的圆形相交面的并集达到所述覆盖阈值SQ·ε％，则确定所述并集覆盖智能体的工作范围。
4.如权利要求1所述的方法，其特征在于，步骤2中所述智能体可达范围的确定方式为：
构建以智能体的初始状态(x0,y0,0)为顶点、圆锥开口朝向根节点、高度无限延伸、圆锥中轴线垂直于(x,y)平面的圆锥，圆锥斜面斜率的绝对值为智能体的最大速度；将圆锥面包围中的叶子节点s'确定为智能体可达范围；
或者，构建以每个叶子节点s'为顶点、圆锥开口背离根节点、高度无限延伸、圆锥中轴线垂直于(x,y)平面的圆锥，圆锥斜面斜率的绝对值为智能体的最大速度；判断初始状态(x0,y0,0)落在哪些叶子节点s'的圆锥内，则将这些叶子节点s'确定为智能体可达范围。
5.如权利要求1所述的方法，其特征在于，步骤1中所述计算快速随机搜索树中每个节点到根节点的代价值为：
Jsa＝cost(sa)-λ·r(w)
其中，Jsa表示节点sa到根节点的代价值；cost(sa)表示节点sa到根节点的距离，λ为偏好因子，r(w)为快速随机搜索树中路径w的鲁棒程度。
6.如权利要求1所述的方法，其特征在于，该方法进一步包括：在路径wd中以一定时间分辨率进行点的采样，获得一条由带有时间的采样点构成的路径pa；采用模型预测控制MPC算法对pa进行跟踪，生成最优控制量储存至对应的采样点中，完成控制量规划。
7.如权利要求6所述的方法，其特征在于，在进行控制量规划时，判断规划路径上是否已经有之前规划过的采样点，如果是，则重复利用已存储控制量，即判断智能体的状态表征数据与该采样点对应的状态表征数据之间的差异是否在设定范围内，如果是则直接应用之前存储的控制量进行控制。
8.如权利要求7所述的方法，其特征在于，在进行控制量规划时，如果规划路径上部分段落有之前规划过的采样点，对于已经规划过的采样点，执行所述重复利用已存储控制量的操作；对于没有规划过的采样点，则采用模型预测控制算法生成最优控制量储存至对应的采样点中。
9.如权利要求7所述的方法，其特征在于，所述状态表征数据包括智能体位置和转角。
10.一种信号时序逻辑STL约束下基于终点回溯的任务规划装置，其特征在于，包括离线构建搜索树模块、代价计算模块和在线任务规划模块；
离线构建搜索树模块，用于在智能体的二维位置工作空间(x,y)中，加入时间维信息t，构建三维空间(x,y,t)；根据智能体的给定任务预估终点sd＝(xd,yd,h)并放置到所述三维空间中；其中(xd,yd)为任务目标位置，h为任务对应的最大时长；将智能体给定任务的所有未来操作符与过去操作符互换，构建出转换后任务的变换器Transducer；在Transducer的约束下，以终点sd作为根节点，自终点向t＝0的起始状态平面生长快速随机搜索树，直到起始状态平面中每个有可能的位置点都被叶子节点s'覆盖，停止快速随机搜索树的生长；
代价计算模块，用于计算快速随机搜索树中每个节点到根节点的代价值，记录到节点中；
在线任务规划模块，用于根据给定的智能体的初始状态(x0,y0,0)，在快速随机搜索树中搜索智能体可达范围内的所有叶子节点s'，选择代价值最小的叶子节点作为规划节点；智能体从初始状态到规划节点再到根节点的路径wd就是路径规划结果。
CN202110466457.1A 2021-04-28 2021-04-28 信号时序逻辑约束下基于终点回溯的任务规划方法和装置 Pending CN113255967A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110466457.1A CN113255967A (zh) 2021-04-28 2021-04-28 信号时序逻辑约束下基于终点回溯的任务规划方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110466457.1A CN113255967A (zh) 2021-04-28 2021-04-28 信号时序逻辑约束下基于终点回溯的任务规划方法和装置

Publications (1)

Publication Number Publication Date
CN113255967A true CN113255967A (zh) 2021-08-13

Family

ID=77222052

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110466457.1A Pending CN113255967A (zh) 2021-04-28 2021-04-28 信号时序逻辑约束下基于终点回溯的任务规划方法和装置

Country Status (1)

Country Link
CN (1) CN113255967A (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115212575A (zh) * 2022-09-19 2022-10-21 南京惠积信息科技有限公司 连续状态空间智能体构建方法
CN115793657A (zh) * 2022-12-09 2023-03-14 常州大学 基于时态逻辑控制策略的配送机器人路径规划方法
WO2023207630A1 (zh) * 2022-04-24 2023-11-02 华为技术有限公司 一种任务求解方法及其装置

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2017139613A1 (en) * 2016-02-11 2017-08-17 Massachusetts Institute Of Technology Motion planning for robotic systems
CN109657868A (zh) * 2018-12-26 2019-04-19 北京理工大学 一种任务时序逻辑约束的概率规划识别方法
CN109828600A (zh) * 2019-01-09 2019-05-31 北京理工大学 时间最优快速三维避障路径规划方法
CN111340348A (zh) * 2020-02-21 2020-06-26 北京理工大学 一种基于线性时序逻辑的分布式多智能体任务协作方法
US20200379893A1 (en) * 2019-05-29 2020-12-03 Toyota Research Institute, Inc. Simulation-based technique to synthesize controllers that satisfy signal temporal logic specifications

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2017139613A1 (en) * 2016-02-11 2017-08-17 Massachusetts Institute Of Technology Motion planning for robotic systems
CN109657868A (zh) * 2018-12-26 2019-04-19 北京理工大学 一种任务时序逻辑约束的概率规划识别方法
CN109828600A (zh) * 2019-01-09 2019-05-31 北京理工大学 时间最优快速三维避障路径规划方法
US20200379893A1 (en) * 2019-05-29 2020-12-03 Toyota Research Institute, Inc. Simulation-based technique to synthesize controllers that satisfy signal temporal logic specifications
CN111340348A (zh) * 2020-02-21 2020-06-26 北京理工大学 一种基于线性时序逻辑的分布式多智能体任务协作方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
田戴荧 等: "信号时序逻辑约束下基于终点回溯的高效规划", 《无人系统技术》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2023207630A1 (zh) * 2022-04-24 2023-11-02 华为技术有限公司 一种任务求解方法及其装置
CN115212575A (zh) * 2022-09-19 2022-10-21 南京惠积信息科技有限公司 连续状态空间智能体构建方法
CN115212575B (zh) * 2022-09-19 2022-12-20 南京惠积信息科技有限公司 连续状态空间智能体构建方法
CN115793657A (zh) * 2022-12-09 2023-03-14 常州大学 基于时态逻辑控制策略的配送机器人路径规划方法

Similar Documents

Publication Publication Date Title
CN113255967A (zh) 信号时序逻辑约束下基于终点回溯的任务规划方法和装置
CN109491389B (zh) 一种具有速度约束的机器人轨迹跟踪方法
Yasuda Behavior-based autonomous cooperative control of intelligent mobile robot systems with embedded Petri nets
Dunlap et al. Motion planning for mobile robots via sampling-based model predictive optimization
Orthey et al. Sampling-based motion planning: A comparative review
Ceballos et al. Quantitative performance metrics for mobile robots navigation
Hu et al. Dynamic global path planning with uncertainty for mobile robots in manufacturing
Banerjee et al. Learning-based warm-starting for fast sequential convex programming and trajectory optimization
Martinsen et al. Optimal model-based trajectory planning with static polygonal constraints
CN110716574A (zh) 一种基于深度q网络的uuv实时避碰规划方法
CN118201742A (zh) 使用图神经网络的多机器人协调
Zhao et al. Trajectory smoothing using jerk bounded shortcuts for service manipulator robots
Zhang et al. Motion planning with uncertainty
Zhang et al. Cooperative navigation based on cross entropy: dual leaders
Krug et al. Representing movement primitives as implicit dynamical systems learned from multiple demonstrations
Saeidi et al. Trust-based leader selection for bilateral haptic teleoperation of multi-robot systems
CN114527759A (zh) 一种基于分层强化学习的端到端驾驶方法
Mao et al. Combined complete coverage path planning for autonomous mobile robot in indoor environment
Laroche et al. Mobile robotics planning using abstract markov decision processes
Yu et al. A Direct approach of path planning using environmental contours
Zhou et al. Research on the fuzzy algorithm of path planning of mobile robot
Jeong et al. Parameterized Fast and Safe Tracking (FaSTrack) using Deepreach
Hamdan et al. Maria Vasileva, and Boris Gurenko () ID Joint Stock Company" Scientific-Design Bureau of Robotics and Control Systems”, 154, Socialist St, Taganrog, Rostov Region 347900, Russia boris. gurenko@ gmail. com
CN114740870B (zh) 一种基于物联网技术的家居机器人路径规划系统及规划方法
Hasankhani et al. Reinforcement Learning for Underwater Spatiotemporal Path Planning, with Application to an Autonomous Marine Current Turbine

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
WD01 Invention patent application deemed withdrawn after publication
WD01 Invention patent application deemed withdrawn after publication

Application publication date: 20210813