CN109669459A - 一种采用不变集的动态反馈路径规划方法 - Google Patents

Abstract

本发明涉及一种采用不变集的动态反馈路径规划方法，将处于不同平衡点附近的状态引向下一个平衡点，直到到达目标。平衡点的选取采用渐近最优快速搜索随机树结合局部***不变集，并根据当前障碍物约束局部调整树结构。通过不变集形成安全通路，使得智能体时刻处于一个安全通道中，从而保证规划的安全性和可达性。

Description

一种采用不变集的动态反馈路径规划方法

技术领域

本发明属于自动控制领域，涉及一种采用不变集的动态反馈路径规划方法。

背景技术

反馈路径规划算法不仅可以对自主机器人等智能体未来运动的安全路径进行规划，而且可以通过反馈信息的引入，在规划过程中考虑路径的可达性。1986年Khatib 在文章《Real-Time Obstacle Avoidance for Manipulators and Mobile Robots》中提出人工势场法，该方法是路径规划领域一种基本的反馈路径规划算法。因为在线实时避障的特性，机器人、机器臂、航天器等广泛采用该路径规划算法。2010年Tedrake在《LQR-trees:Feedback Motion Planning via Sums-of-Squares Verification》中提出名为 LQR-Trees的一种方法。该方法是一种采用反向可达集覆盖整个可行状态空间来实现反馈路径规划。其中不变集的思想被采用，以求解尽可能大的反向可达集，来减少覆盖状态空间的可达集个数。随后Weiss在文章《Safe Positively Invariant Sets for SpacecraftObstacle Avoidance》中提出采用不变集组网的方式对受约束的输出空间进行覆盖，网络点为路径的稳定平衡点，通过最优路径搜索算法在网络点中进行路径搜索，得到最优轨迹。Weiss随后采用快速搜索随机树(Rapidly-exploring random tree,RRT) 的策略代替网格法，基于安全的正不变集寻找最优轨迹。但是这些方法针对动态约束情况下的安全运动规划均没有完善的解决办法。因此，如果能提出一种保证最优或者渐近最优情况下的动态安全运动规划方法，则能满足智能体动态避障更好地完成安全最优轨迹设计。

发明内容

本发明解决的技术问题是：为了解决现有技术不能针对未知动态约束进行规划的安全规划的问题。针对自主智能体在动态障碍物存在情况下进行实时安全轨迹规划，提出了一种基于不变集的动态安全避障方法。可以在线进行动态安全反馈运动规划。

本发明的技术方案是：一种采用不变集的动态反馈路径规划方法，包括以下步骤：

步骤一、根据***状态方程，设计控制律和不变集，包括以下子步骤：

子步骤一：构建状态方程和输出方程，

y＝Cx

其中x为状态矢量，u为控制输入，y为输出矢量，且A、B 分别为x和u的系数且(A,B)可控，rank(C)＝n_y；控制输入满足约束：输出满足约束：

子步骤二：因输出空间能够描述为其中为对应凸空间的指示标识；k为自然数，为任意一个凸空间；

子步骤三：设可行的平衡点处于输出空间中，其中平衡点满足：

在平衡点v处设计线性二次型最优调节器，得到中的最优指标，从而得到最优控制u＝-K(x-v)，由此在平衡点v形成了一个稳定闭环***：

子步骤四：在步骤一中的稳定闭环***中定义一个椭圆形正不变集不变集中的任意值x始终处于该集合中且满足并逐渐趋向于平衡点v；

子步骤五：定义障碍物空间为不变集空间为若存在则缩放不变集空间直到其中和均为指示标识；

步骤二、采用修正渐近最优快速搜索随机树进行安全运动规划，包括以下子步骤：

子步骤一：从目标状态出发，树结构将不变集空间反向链接至初始状态，智能体出发后，存在能够躲避障碍物的安全通道 表示链接初始和目标状态平衡点的指示函数，其中安全通道能够保证智能体从初始状态安全到达目标状态；

子步骤二：智能体在安全通道中运动时，为躲避障碍物，需对安全通道轨迹进行不断的在线修正，修正后，最终得到序列(x,u)，从而是的智能体安全轨迹保持在设计的安全通道中，最终导引智能体顺利到达终点。

本发明的进一步技术方案是：所述修正安全通道轨迹的算法流程如下：

1.输入当前状态x₀，目标状态x_T，障碍物空间

2.RRT*生成随机树形成安全通道并连接x₀和x_T

3.开始循环

3.1.给定时间间隔，更新障碍物空间当前状态x₀

3.2.局部修正局部树节点

3.2.1如果

3.2.1.1找出对应的平衡点v，及其所有子节点

3.2.1.2如果存在父节点保留

3.2.1.2如果不存在父节点从中删除

3.2.2如果继续下一步

3.3.持续进行RRT*采样新的平衡点并更新树

3.4.在给定的时间间隔，开始循环

3.3.1若当前状态处于中，采用u＝-K(x-v)向对应平衡点v运动

3.3.2如果当前状态x处于v的父节点F_v的不变集中，切换控制律 u＝-K(x-F_v)，向下一平衡点F_v运动.

3.3.2超出时间间隔，结束循环

3.5.如果当前状态x到达目标点x_T，循环结束，得到序列(x,u)。

发明效果

本发明的技术效果在于：本发明提出了一种通过切换平衡点的方法，将处于不同平衡点附近的状态引向下一个平衡点，直到到达目标。平衡点的选取采用渐近最优快速搜索随机树结合局部***不变集，并根据当前障碍物约束局部调整树结构。通过不变集形成安全通路，使得智能体时刻处于一个安全通道中，从而保证规划的安全性和可达性。

附图说明

图1为0s时航天器安全飞行通道和树结构图

图2为1000s时航天器安全飞行通道和树结构图

图3为2000s时航天器安全飞行通道和树结构图

图4为3000s时航天器安全飞行通道和树结构图

图5为4000s时航天器安全飞行通道和树结构图

图6为5000s时航天器安全飞行通道和树结构图

具体实施方式

参见图1—图6，本发明提出了一种采用不变集和动态渐近最优搜索随机树(RRT*)的反馈路径规划算法。通过离线计算安全不变集，然后采用在线反向搜索建立随机树，并根据动态约束的情况进行局部树结构的修正，实现动态避障反馈路径规划。该发明的实施主要包括以下两个步骤：

步骤一、根据***状态方程，设计控制律并计算不变集。

对于状态和输出方程：

其中分别为状态矢量，控制输入，输出矢量。(A,B)可控且控制输入和初始满足约束：因为状态约束的存在，输出空间为非凸空间，我们将该空间描述为多个凸空间的并集：

其中为对应凸空间的指示标识。假设可行的平衡点处于输出空间中，例如对于平衡点满足：

在平衡点v附近设计线性二次型最优调节器(linear quadratic regulator，LQR)满足最优指标最小，得到最优控制u＝-K(x-v)。该线性反馈控制在平衡点v形成了一个稳定闭环***：

该闭环***存在一个椭圆形正不变集该不变集表示进入此集合范围的状态x将保持在该集合中且满足并逐渐趋向于平衡点v。假设障碍物空间为所有不变集空间如果存在则采用不变集空间对多个局部***进行约束可以保证运动的绝对安全。其中和对应不同障碍物和平衡点。

步骤二、采用修正渐近最优快速搜索随机树进行安全运动规划

通过修正渐近最优快速搜索随机树(RRT*)进行随机生成平衡点v，并组成树结构其中v为平衡点，ε为平衡点之间连接关系。平衡点v处对应不变集从目标状态出发，树结构将不变集空间反向链接至初始状态，则一定存在一条通道其中表示链接初始和目标状态平衡点的指示函数。形成一条安全通道，将智能体从初始状态安全的导向目标状态。算法流程如下：

4.输入当前状态x₀，目标状态x_T，障碍物空间

5.RRT*生成随机树形成安全通道并连接x₀和x_T

6.开始循环

6.1.给定时间间隔，更新障碍物空间当前状态x₀

6.2.局部修正局部树节点

3.2.1如果

3.2.1.1找出对应的平衡点v，及其所有子节点

3.2.1.2如果存在父节点保留

3.2.1.2如果不存在父节点从中删除

3.2.2如果继续下一步

6.3.持续进行RRT*采样新的平衡点并更新树

6.4.在给定的时间间隔，开始循环

3.3.1若当前状态处于中，采用u＝-K(x-v)向对应平衡点v运动

3.3.2如果当前状态x处于v的父节点F_v的不变集中，切换控制律 u＝-K(x-F_v)，向下一平衡点F_v运动.

3.3.2超出时间间隔，结束循环

6.5.如果当前状态x到达目标点x_T，循环结束。

以航天器避障运动路径规划为应用对象，采用CW线性方程为状态方程，将航天器导引到目标位置，并避开动态障碍物。LQR权重矩阵Q＝diag([100,100,100,10⁷,10⁷,10⁷])，R＝2×diag([10⁵,10⁷,10⁹])。每个轴向控制上限为5N，主动航天器重量为100kg。所有空间大小为[-500 -1000 -500]^T×[500 1000 500]^T。参考轨道轨道角速度0.011rad/s。附图中，灰色椭圆区域为安全不变集区域，黑色立方体为障碍物，灰色细线为树结构，黑色粗实线为规划轨迹，灰色粗实线为真实轨迹。为CW方程； C＝[I_3×3 0_3×3]，y表示位置。附图中多个时刻的截图显示，生成的安全管道为航天器导引出了一条不与障碍物(黑色立方体)相碰撞的安全走廊(灰色区域)，而通过文中控制器的切换，完成整个***从初始位置到目标位置的安全运动轨迹(会色粗实线)。

Claims (2)

1.一种采用不变集的动态反馈路径规划方法，其特征在于，包括以下步骤：

步骤一、根据***状态方程，设计控制律和不变集，包括以下子步骤：

子步骤一：构建状态方程和输出方程，

y＝Cx

其中x为状态矢量，u为控制输入，y为输出矢量，且A、B分别为x和u的系数且(A,B)可控，rank(C)＝n_y；控制输入满足约束：输出满足约束：

子步骤二：因输出空间能够描述为其中为对应凸空间的指示标识；k为自然数，为任意一个凸空间；

子步骤三：设可行的平衡点处于输出空间中，其中平衡点满足：

在平衡点v处设计线性二次型最优调节器，得到中的最优指标，从而得到最优控制u＝-K(x-v)，由此在平衡点v形成了一个稳定闭环***：

子步骤四：在步骤一中的稳定闭环***中定义一个椭圆形正不变集不变集中的任意值x始终处于该集合中且满足并逐渐趋向于平衡点v；

子步骤五：定义障碍物空间为不变集空间为若存在则缩放不变集空间直到其中和均为指示标识；

步骤二、采用修正渐近最优快速搜索随机树进行安全运动规划，包括以下子步骤：

子步骤一：从目标状态出发，树结构将不变集空间反向链接至初始状态，智能体出发后，存在能够躲避障碍物的安全通道 表示链接初始和目标状态平衡点的指示函数，其中安全通道能够保证智能体从初始状态安全到达目标状态；

子步骤二：智能体在安全通道中运动时，为躲避障碍物，需对安全通道轨迹进行不断的在线修正，修正后，最终得到序列(x,u)，从而是的智能体安全轨迹保持在设计的安全通道中，最终导引智能体顺利到达终点。

2.如权利要求1所述的一种采用不变集的动态反馈路径规划方法，其特征在于，

所述修正安全通道轨迹的算法流程如下：

1.输入当前状态x₀，目标状态x_T，障碍物空间

2.RRT*生成随机树形成安全通道并连接x₀和x_T

3.开始循环

3.1.给定时间间隔，更新障碍物空间当前状态x₀

3.2.局部修正局部树节点

3.2.1如果

3.2.1.1找出对应的平衡点v，及其所有子节点

3.2.1.2如果存在父节点保留

3.2.1.2如果不存在父节点从中删除

3.2.2如果继续下一步

3.3.持续进行RRT*采样新的平衡点并更新树

3.4.在给定的时间间隔，开始循环

3.3.1若当前状态处于中，采用u＝-K(x-v)向对应平衡点v运动

3.3.2如果当前状态x处于v的父节点F_v的不变集中，切换控制律u＝-K(x-F_v)，向下一平衡点F_v运动.

3.3.2超出时间间隔，结束循环

3.5.如果当前状态x到达目标点x_T，循环结束，得到序列(x,u)。