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

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

A Dynamic Feedback Path Planning Method Using Invariant Sets

The invention relates to a dynamic feedback path planning method using invariant sets, which leads the state near different equilibrium points to the next equilibrium point until the target is reached. The selection of equilibrium points is based on asymptotically optimal fast search random tree combined with local system invariant set, and the tree structure is locally adjusted according to current obstacle constraints. The security path is formed by invariant set, so that the agent is always in a security channel, so as to ensure the security and accessibility of the planning.

【技术实现步骤摘要】
一种采用不变集的动态反馈路径规划方法
本专利技术属于自动控制领域，涉及一种采用不变集的动态反馈路径规划方法。
技术介绍
反馈路径规划算法不仅可以对自主机器人等智能体未来运动的安全路径进行规划，而且可以通过反馈信息的引入，在规划过程中考虑路径的可达性。1986年Khatib在文章《Real-TimeObstacleAvoidanceforManipulatorsandMobileRobots》中提出人工势场法，该方法是路径规划领域一种基本的反馈路径规划算法。因为在线实时避障的特性，机器人、机器臂、航天器等广泛采用该路径规划算法。2010年Tedrake在《LQR-trees:FeedbackMotionPlanningviaSums-of-SquaresVerification》中提出名为LQR-Trees的一种方法。该方法是一种采用反向可达集覆盖整个可行状态空间来实现反馈路径规划。其中不变集的思想被采用，以求解尽可能大的反向可达集，来减少覆盖状态空间的可达集个数。随后Weiss在文章《SafePositivelyInvariantSetsforSpacecraftObstacleAvoidance》中提出采用不变集组网的方式对受约束的输出空间进行覆盖，网络点为路径的稳定平衡点，通过最优路径搜索算法在网络点中进行路径搜索，得到最优轨迹。Weiss随后采用快速搜索随机树(Rapidly-exploringrandomtree,RRT)的策略代替网格法，基于安全的正不变集寻找最优轨迹。但是这些方法针对动态约束情况下的安全运动规划均没有完善的解决办法。因此，如果能提出一种保证最优或者渐近最优情况下的动态安全运动规划方法，则能满足智能体动态避障更好地完成安全最优轨迹设计。
技术实现思路
本专利技术解决的技术问题是：为了解决现有技术不能针对未知动态约束进行规划的安全规划的问题。针对自主智能体在动态障碍物存在情况下进行实时安全轨迹规划，提出了一种基于不变集的动态安全避障方法。可以在线进行动态安全反馈运动规划。本专利技术的技术方案是：一种采用不变集的动态反馈路径规划方法，包括以下步骤：步骤一、根据系统状态方程，设计控制律和不变集，包括以下子步骤：子步骤一：构建状态方程和输出方程，y＝Cx其中x为状态矢量，u为控制输入，y为输出矢量，且A、B分别为x和u的系数且(A,B)可控，rank(C)＝ny；控制输入满足约束：输出满足约束：子步骤二：因输出空间能够描述为其中为对应凸空间的指示标识；k为自然数，为任意一个凸空间；子步骤三：设可行的平衡点处于输出空间中，其中平衡点满足：在平衡点v处设计线性二次型最优调节器，得到中的最优指标，从而得到最优控制u＝-K(x-v)，由此在平衡点v形成了一个稳定闭环系统：子步骤四：在步骤一中的稳定闭环系统中定义一个椭圆形正不变集不变集中的任意值x始终处于该集合中且满足并逐渐趋向于平衡点v；子步骤五：定义障碍物空间为不变集空间为若存在则缩放不变集空间直到其中和均为指示标识；步骤二、采用修正渐近最优快速搜索随机树进行安全运动规划，包括以下子步骤：子步骤一：从目标状态出发，树结构将不变集空间反向链接至初始状态，智能体出发后，存在能够躲避障碍物的安全通道表示链接初始和目标状态平衡点的指示函数，其中安全通道能够保证智能体从初始状态安全到达目标状态；子步骤二：智能体在安全通道中运动时，为躲避障碍物，需对安全通道轨迹进行不断的在线修正，修正后，最终得到序列(x,u)，从而是的智能体安全轨迹保持在设计的安全通道中，最终导引智能体顺利到达终点。本专利技术的进一步技术方案是：所述修正安全通道轨迹的算法流程如下：1.输入当前状态x0，目标状态xT，障碍物空间2.RRT*生成随机树形成安全通道并连接x0和xT3.开始循环3.1.给定时间间隔，更新障碍物空间当前状态x03.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的父节点Fv的不变集中，切换控制律u＝-K(x-Fv)，向下一平衡点Fv运动.3.3.2超出时间间隔，结束循环3.5.如果当前状态x到达目标点xT，循环结束，得到序列(x,u)。专利技术效果本专利技术的技术效果在于：本专利技术提出了一种通过切换平衡点的方法，将处于不同平衡点附近的状态引向下一个平衡点，直到到达目标。平衡点的选取采用渐近最优快速搜索随机树结合局部系统不变集，并根据当前障碍物约束局部调整树结构。通过不变集形成安全通路，使得智能体时刻处于一个安全通道中，从而保证规划的安全性和可达性。附图说明图1为0s时航天器安全飞行通道和树结构图图2为1000s时航天器安全飞行通道和树结构图图3为2000s时航天器安全飞行通道和树结构图图4为3000s时航天器安全飞行通道和树结构图图5为4000s时航天器安全飞行通道和树结构图图6为5000s时航天器安全飞行通道和树结构图具体实施方式参见图1—图6，本专利技术提出了一种采用不变集和动态渐近最优搜索随机树(RRT*)的反馈路径规划算法。通过离线计算安全不变集，然后采用在线反向搜索建立随机树，并根据动态约束的情况进行局部树结构的修正，实现动态避障反馈路径规划。该专利技术的实施主要包括以下两个步骤：步骤一、根据系统状态方程，设计控制律并计算不变集。对于状态和输出方程：其中分别为状态矢量，控制输入，输出矢量。(A,B)可控且控制输入和初始满足约束：因为状态约束的存在，输出空间为非凸空间，我们将该空间描述为多个凸空间的并集：其中为对应凸空间的指示标识。假设可行的平衡点处于输出空间中，例如对于平衡点满足：在平衡点v附近设计线性二次型最优调节器(linearquadraticregulator，LQR)满足最优指标最小，得到最优控制u＝-K(x-v)。该线性反馈控制在平衡点v形成了一个稳定闭环系统：该闭环系统存在一个椭圆形正不变集该不变集表示进入此集合范围的状态x将保持在该集合中且满足并逐渐趋向于平衡点v。假设障碍物空间为所有不变集空间如果存在则采用不变集空间对多个局部系统进行约束可以保证运动的绝对安全。其中和对应不同障碍物和平衡点。步骤二、采用修正渐近最优快速搜索随机树进行安全运动规划通过修正渐近最优快速搜索随机树(RRT*)进行随机生成平衡点v，并组成树结构其中v为平衡点，ε为平衡点之间连接关系。平衡点v处对应不变集从目标状态出发，树结构将不变集空间反向链接至初始状态，则一定存在一条通道其中表示链接初始和目标状态平衡点的指示函数。形成一条安全通道，将智能体从初始状态安全的导向目标状态。算法流程如下：4.输入当前状态x0，目标状态xT，障碍物空间5.RRT*生成随机树形成安全通道并连接x0和xT6.开始循环6.1.给定时间间隔，更新障碍物空间当前状态x06.2.局部修正局部树节点3.2.1如果3.2.1.1找出对应的平衡点v，及其所有子节点3.2.1.2如果存在父节点保留3.2.1.2如果不存本文档来自技高网...

【技术保护点】
1.一种采用不变集的动态反馈路径规划方法，其特征在于，包括以下步骤：步骤一、根据系统状态方程，设计控制律和不变集，包括以下子步骤：子步骤一：构建状态方程和输出方程，

【技术特征摘要】
1.一种采用不变集的动态反馈路径规划方法，其特征在于，包括以下步骤：步骤一、根据系统状态方程，设计控制律和不变集，包括以下子步骤：子步骤一：构建状态方程和输出方程，y＝Cx其中x为状态矢量，u为控制输入，y为输出矢量，且A、B分别为x和u的系数且(A,B)可控，rank(C)＝ny；控制输入满足约束：输出满足约束：子步骤二：因输出空间能够描述为其中为对应凸空间的指示标识；k为自然数，为任意一个凸空间；子步骤三：设可行的平衡点处于输出空间中，其中平衡点满足：在平衡点v处设计线性二次型最优调节器，得到中的最优指标，从而得到最优控制u＝-K(x-v)，由此在平衡点v形成了一个稳定闭环系统：子步骤四：在步骤一中的稳定闭环系统中定义一个椭圆形正不变集不变集中的任意值x始终处于该集合中且满足并逐渐趋向于平衡点v；子步骤五：定义障碍物空间为不变集空间为若存在则缩放不变集空间直到其中和均为指示标识；步骤二、采用修正渐近最优快速搜索随机树进行安全运动规划，包括以下子步骤：子步骤一：从目标状态出发，树结构将不变集空间反向链接至初始状态，智能体出发后，存在能够躲避障碍物的安全通道表示链接初始和目标状态平衡点的指示函数，其中安全通道能够保...

【专利技术属性】
技术研发人员：罗建军，高登巍，马卫华，王明明，党朝辉，袁建平，
申请(专利权)人：西北工业大学，
类型：发明
国别省市：陕西,61