CN109669459B - 一种采用不变集的动态反馈路径规划方法 - Google Patents
一种采用不变集的动态反馈路径规划方法 Download PDFInfo
- Publication number
- CN109669459B CN109669459B CN201811622488.6A CN201811622488A CN109669459B CN 109669459 B CN109669459 B CN 109669459B CN 201811622488 A CN201811622488 A CN 201811622488A CN 109669459 B CN109669459 B CN 109669459B
- Authority
- CN
- China
- Prior art keywords
- invariant set
- space
- state
- substep
- safety
- 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
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/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及一种采用不变集的动态反馈路径规划方法,将处于不同平衡点附近的状态引向下一个平衡点,直到到达目标。平衡点的选取采用渐近最优快速搜索随机树结合局部系统不变集,并根据当前障碍物约束局部调整树结构。通过不变集形成安全通路,使得智能体时刻处于一个安全通道中,从而保证规划的安全性和可达性。
Description
技术领域
本发明属于自动控制领域,涉及一种采用不变集的动态反馈路径规划方法。
背景技术
反馈路径规划算法不仅可以对自主机器人等智能体未来运动的安全路径进行规划,而且可以通过反馈信息的引入,在规划过程中考虑路径的可达性。1986年Khatib在文章《Real-Time Obstacle Avoidance for Manipulators and Mobile Robots》中提出人工势场法,该方法是路径规划领域一种基本的反馈路径规划算法。因为在线实时避障的特性,机器人、机器臂、航天器等广泛采用该路径规划算法。2010年Tedrake在《LQR-trees:FeedbackMotion Planning via Sums-of-Squares Verification》中提出名为LQR-Trees的一种方法。该方法是一种采用反向可达集覆盖整个可行状态空间来实现反馈路径规划。其中不变集的思想被采用,以求解尽可能大的反向可达集,来减少覆盖状态空间的可达集个数。随后Weiss在文章《Safe Positively Invariant Sets for Spacecraft Obstacle Avoidance》中提出采用不变集组网的方式对受约束的输出空间进行覆盖,网络点为路径的稳定平衡点,通过最优路径搜索算法在网络点中进行路径搜索,得到最优轨迹。Weiss随后采用快速搜索随机树(Rapidly-exploring random tree,RRT)的策略代替网格法,基于安全的正不变集寻找最优轨迹。但是这些方法针对动态约束情况下的安全运动规划均没有完善的解决办法。因此,如果能提出一种保证最优或者渐近最优情况下的动态安全运动规划方法,则能满足智能体动态避障更好地完成安全最优轨迹设计。
发明内容
本发明解决的技术问题是:为了解决现有技术不能针对未知动态约束进行规划的安全规划的问题。针对自主智能体在动态障碍物存在情况下进行实时安全轨迹规划,提出了一种基于不变集的动态安全避障方法。可以在线进行动态安全反馈运动规划。
本发明的技术方案是:一种采用不变集的动态反馈路径规划方法,包括以下步骤:
步骤一、根据系统状态方程,设计控制律和不变集,包括以下子步骤:
子步骤一:构建状态方程和输出方程,
y=Cx
步骤二、采用修正渐近最优快速搜索随机树进行安全运动规划,包括以下子步骤:
子步骤一:从目标状态出发,树结构将不变集空间反向链接至初始状态,智能体出发后,存在能够躲避障碍物的安全通道 表示链接初始和目标状态平衡点的指示函数,其中安全通道能够保证智能体从初始状态安全到达目标状态;
子步骤二:智能体在安全通道中运动时,为躲避障碍物,需对安全通道轨迹进行不断的在线修正,修正后,最终得到序列(x,u),从而是的智能体安全轨迹保持在设计的安全通道中,最终导引智能体顺利到达终点。
本发明的进一步技术方案是:所述修正安全通道轨迹的算法流程如下:
3.开始循环
3.2.局部修正局部树节点
3.4.在给定的时间间隔,开始循环
3.3.2超出时间间隔,结束循环
3.5.如果当前状态x到达目标点xT,循环结束,得到序列(x,u)。
发明效果
本发明的技术效果在于:本发明提出了一种通过切换平衡点的方法,将处于不同平衡点附近的状态引向下一个平衡点,直到到达目标。平衡点的选取采用渐近最优快速搜索随机树结合局部系统不变集,并根据当前障碍物约束局部调整树结构。通过不变集形成安全通路,使得智能体时刻处于一个安全通道中,从而保证规划的安全性和可达性。
附图说明
图1为0s时航天器安全飞行通道和树结构图
图2为1000s时航天器安全飞行通道和树结构图
图3为2000s时航天器安全飞行通道和树结构图
图4为3000s时航天器安全飞行通道和树结构图
图5为4000s时航天器安全飞行通道和树结构图
图6为5000s时航天器安全飞行通道和树结构图
具体实施方式
参见图1—图6,本发明提出了一种采用不变集和动态渐近最优搜索随机树(RRT*)的反馈路径规划算法。通过离线计算安全不变集,然后采用在线反向搜索建立随机树,并根据动态约束的情况进行局部树结构的修正,实现动态避障反馈路径规划。该发明的实施主要包括以下两个步骤:
步骤一、根据系统状态方程,设计控制律并计算不变集。
对于状态和输出方程:
在平衡点v附近设计线性二次型最优调节器(linear quadratic regulator,LQR)满足最优指标最小,得到最优控制u=-K(x-v)。该线性反馈控制在平衡点v形成了一个稳定闭环系统:
该闭环系统存在一个椭圆形正不变集该不变集表示进入此集合范围的状态x将保持在该集合中且满足并逐渐趋向于平衡点v。假设障碍物空间为所有不变集空间如果存在则采用不变集空间对多个局部系统进行约束可以保证运动的绝对安全。其中和对应不同障碍物和平衡点。
步骤二、采用修正渐近最优快速搜索随机树进行安全运动规划
通过修正渐近最优快速搜索随机树(RRT*)进行随机生成平衡点v,并组成树结构其中v为平衡点,ε为平衡点之间连接关系。平衡点v处对应不变集从目标状态出发,树结构将不变集空间反向链接至初始状态,则一定存在一条通道其中表示链接初始和目标状态平衡点的指示函数。形成一条安全通道,将智能体从初始状态安全的导向目标状态。算法流程如下:
6.开始循环
6.2.局部修正局部树节点
6.4.在给定的时间间隔,开始循环
3.3.2超出时间间隔,结束循环
6.5.如果当前状态x到达目标点xT,循环结束。
以航天器避障运动路径规划为应用对象,采用CW线性方程为状态方程,将航天器导引到目标位置,并避开动态障碍物。LQR权重矩阵Q=diag([100,100,100,107,107,107]),R=2×diag([105,107,109])。每个轴向控制上限为5N,主动航天器重量为100kg。所有空间大小为[-500 -1000 -500]T×[500 1000 500]T。参考轨道轨道角速度0.011rad/s。附图中,灰色椭圆区域为安全不变集区域,黑色立方体为障碍物,灰色细线为树结构,黑色粗实线为规划轨迹,灰色粗实线为真实轨迹。为CW方程;C=[I3×3 03×3],y表示位置。附图中多个时刻的截图显示,生成的安全管道为航天器导引出了一条不与障碍物(黑色立方体)相碰撞的安全走廊(灰色区域),而通过文中控制器的切换,完成整个系统从初始位置到目标位置的安全运动轨迹(会色粗实线)。
Claims (1)
1.一种采用不变集的动态反馈路径规划方法,其特征在于,包括以下步骤:
步骤一、根据系统状态方程,设计控制律和不变集,包括以下子步骤:
子步骤一:构建状态方程和输出方程,
y=Cx
步骤二、采用修正渐近最优快速搜索随机树进行安全运动规划,包括以下子步骤:
子步骤一:从目标状态出发,树结构将不变集空间反向链接至初始状态,智能体出发后,存在能够躲避障碍物的安全通道表示链接初始和目标状态平衡点的指示函数,其中安全通道能够保证智能体从初始状态安全到达目标状态;
子步骤二:智能体在安全通道中运动时,为躲避障碍物,需对安全通道轨迹进行不断的在线修正,修正后,最终得到序列(x,u),从而使得智能体安全轨迹保持在设计的安全通道中,最终导引智能体顺利到达终点;
所述修正安全通道轨迹的算法流程如下:
3.开始循环
3.2.局部修正局部树节点
3.4.在给定的时间间隔,开始循环
3.3.2超出时间间隔,结束循环
3.5.如果当前状态x到达目标状态xT,循环结束,得到序列(x,u)。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811622488.6A CN109669459B (zh) | 2018-12-28 | 2018-12-28 | 一种采用不变集的动态反馈路径规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811622488.6A CN109669459B (zh) | 2018-12-28 | 2018-12-28 | 一种采用不变集的动态反馈路径规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109669459A CN109669459A (zh) | 2019-04-23 |
CN109669459B true CN109669459B (zh) | 2022-05-10 |
Family
ID=66146515
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201811622488.6A Active CN109669459B (zh) | 2018-12-28 | 2018-12-28 | 一种采用不变集的动态反馈路径规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109669459B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2022246750A1 (zh) * | 2021-05-27 | 2022-12-01 | 深圳市大疆创新科技有限公司 | 轨迹生成方法、装置、可移动平台及存储介质 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105353768A (zh) * | 2015-12-08 | 2016-02-24 | 清华大学 | 狭窄空间内基于随机采样的无人机轨迹规划方法 |
CN105786014A (zh) * | 2016-03-29 | 2016-07-20 | 沈阳上博智拓科技有限公司 | 无人设备的控制方法和装置 |
CN105911864A (zh) * | 2016-06-02 | 2016-08-31 | 西北工业大学 | 基于虚拟平衡点不变集的不确定受限系统鲁棒控制方法 |
CN106695802A (zh) * | 2017-03-19 | 2017-05-24 | 北京工业大学 | 基于多自由度机械臂的改进式rrt*的避障运动规划方法 |
CN108107890A (zh) * | 2017-12-11 | 2018-06-01 | 西北工业大学 | 一种非线性不确定系统的安全轨迹规划方法 |
CN108195383A (zh) * | 2018-03-13 | 2018-06-22 | 济南大学 | 一种基于改进rrt算法的地下无人铲运机路径规划方法 |
CN108458717A (zh) * | 2018-05-07 | 2018-08-28 | 西安电子科技大学 | 一种迭代的快速扩展随机树irrt的无人机路径规划方法 |
Family Cites Families (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7447593B2 (en) * | 2004-03-26 | 2008-11-04 | Raytheon Company | System and method for adaptive path planning |
US20100114338A1 (en) * | 2008-10-31 | 2010-05-06 | Gm Global Technology Operations, Inc. | Multi-goal path planning of welding robots with automatic sequencing |
CN102435188B (zh) * | 2011-09-15 | 2013-10-02 | 南京航空航天大学 | 一种用于室内环境的单目视觉/惯性全自主导航方法 |
CN103278164B (zh) * | 2013-06-13 | 2015-11-18 | 北京大学深圳研究生院 | 一种复杂动态场景下机器人仿生路径规划方法及仿真平台 |
US9696160B2 (en) * | 2013-07-15 | 2017-07-04 | Bae Systems Plc | Route planning |
CN104359473A (zh) * | 2014-10-24 | 2015-02-18 | 南京航空航天大学 | 一种动态环境下无人机编队飞行的协同航迹智能规划方法 |
CN105807760B (zh) * | 2014-12-30 | 2020-01-03 | Tcl集团股份有限公司 | 一种智能机器人及其自建路径的方法和装置 |
CN107085437A (zh) * | 2017-03-20 | 2017-08-22 | 浙江工业大学 | 一种基于eb‑rrt的无人机航迹规划方法 |
CN108444489A (zh) * | 2018-03-07 | 2018-08-24 | 北京工业大学 | 一种改进rrt算法的路径规划方法 |
CN108909703B (zh) * | 2018-06-27 | 2020-05-12 | 聊城大学 | 一种自动驾驶紧急避让的失稳可控域的判定方法 |
CN108983780A (zh) * | 2018-07-24 | 2018-12-11 | 武汉理工大学 | 一种基于改进rrt*算法的移动机器人路径规划方法 |
CN108981716B (zh) * | 2018-08-22 | 2020-06-16 | 集美大学 | 一种适用于内陆和近海无人船的路径规划方法 |
-
2018
- 2018-12-28 CN CN201811622488.6A patent/CN109669459B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105353768A (zh) * | 2015-12-08 | 2016-02-24 | 清华大学 | 狭窄空间内基于随机采样的无人机轨迹规划方法 |
CN105786014A (zh) * | 2016-03-29 | 2016-07-20 | 沈阳上博智拓科技有限公司 | 无人设备的控制方法和装置 |
CN105911864A (zh) * | 2016-06-02 | 2016-08-31 | 西北工业大学 | 基于虚拟平衡点不变集的不确定受限系统鲁棒控制方法 |
CN106695802A (zh) * | 2017-03-19 | 2017-05-24 | 北京工业大学 | 基于多自由度机械臂的改进式rrt*的避障运动规划方法 |
CN108107890A (zh) * | 2017-12-11 | 2018-06-01 | 西北工业大学 | 一种非线性不确定系统的安全轨迹规划方法 |
CN108195383A (zh) * | 2018-03-13 | 2018-06-22 | 济南大学 | 一种基于改进rrt算法的地下无人铲运机路径规划方法 |
CN108458717A (zh) * | 2018-05-07 | 2018-08-28 | 西安电子科技大学 | 一种迭代的快速扩展随机树irrt的无人机路径规划方法 |
Also Published As
Publication number | Publication date |
---|---|
CN109669459A (zh) | 2019-04-23 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
He et al. | Disturbance observer-based neural network control of cooperative multiple manipulators with input saturation | |
CN111562797B (zh) | 确保收敛的无人机飞行时间最优实时轨迹优化方法 | |
Wang et al. | Neural adaptive command filtered control for cooperative path following of multiple underactuated autonomous underwater vehicles along one path | |
CN106527129B (zh) | 一种并联机器人间接自适应模糊控制参数的确定方法 | |
CN103941728A (zh) | 一种无人机密集自主编队的队形变换方法 | |
Pan et al. | A virtual spring method for the multi-robot path planning and formation control | |
Fan et al. | Formation control of multiple unmanned surface vehicles using the adaptive null-space-based behavioral method | |
CN105759633A (zh) | 一种带强连通分支的多机器人系统可控包含控制方法 | |
Zhao et al. | A novel control strategy for quadrotors with variable mass and external disturbance | |
CN109669459B (zh) | 一种采用不变集的动态反馈路径规划方法 | |
Lan et al. | ADP-based intelligent decentralized control for multi-agent systems moving in obstacle environment | |
CN113371231B (zh) | 一种带约束的航天器姿态控制方法 | |
Kan et al. | Ensuring network connectivity during formation control using a decentralized navigation function | |
CN113341956B (zh) | 基于改进人工势场法的多智能体主从式编队控制方法 | |
Wang et al. | Formation control of multi-UAV with collision avoidance using artificial potential field | |
Hu et al. | Formation control for an UAV team with environment-aware dynamic constraints | |
Wei | A new formation control strategy based on the virtual-leader-follower and artificial potential field | |
Baghli et al. | Design and simulation of Adaptive Neuro Fuzzy Inference System (ANFIS) controller for a robot manipulator | |
Deng et al. | Multi-robot dynamic formation path planning with improved polyclonal artificial immune algorithm | |
Yu et al. | Bionic swarm control based on second-order communication topology | |
Harokopos | Optimal learning control of mechanical manipulators in repetitive motions | |
CN114265315A (zh) | 异构线性集群系统时变输出编队跟踪控制方法及系统 | |
Kazerooni et al. | Semi-decentralized optimal control technique for a leader-follower team of unmanned systems with partial availability of the leader command | |
Emile et al. | A decentralized control of multiple unmanned aerial vehicles formation flight considering obstacle avoidance | |
Amano et al. | Tracking Control foe Multi-Agent Systems Using Broadcast Signals Based on Positive Realness |
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 |