[发明专利]一种存在测量不确定性下的反馈路径规划方法

说明书

本发明公开了一种存在测量不确定性下的反馈路径规划方法，包括以下步骤：1)开环轨迹计算；2)计算局部反馈吸引域；3)计算误差椭圆；4)在线计算；本发明考虑测量不确定性对于反馈吸引域的影响，采用庞德里亚金集差缩小了吸引域的大小，重新评估路径规划过程中的安全避障问题，并采用GP算法在线估计吸引域大小从而克服吸引域计算缓慢的缺陷。本发明重新修正局部反馈控制吸引域的大小，并在线进行路径规划的算法。本发明考虑测量不确定性对标称轨迹的影响，对吸引域进行重新修正，形成更小且更安全的吸引域，从而保证路径规划的快速性和安全性。

【技术领域】

本发明属于路径规划与动力学控制领域，涉及一种存在测量不确定性下的反馈路径规划方法。

【背景技术】

复杂环境下的路径规划算法研究由来已久，已经有很多方法可以实现，比如APF算法，PRM算法，RRT算法。但是传统的算法在动力学层面上并不能真实保证安全轨迹的运行，因此又诞生了一些考虑动力学因素的路径规划算法比如。MIT的EECS实验室的RussTedrake教授提出采用LQR-trees方法。这种方法类似于IHMPC方法，在局部计算线性反馈控制，采用路径规划算法RRT将局部稳定控制扩展到全局，从而实现全局稳定控制。

由于求取反馈控制吸引域需要复杂的凸优化方法：sums of squares，因此计算速度缓慢，A.Majumdar采用离线计算局部路径的算法，然后在线进行整合优化，求解全局路径。并将动力学系统的不确定性考虑到整个算法中，最终形成具有一定鲁棒性的路径规划算法。

【发明内容】

本发明针对反馈路径规划中存在干扰和测量不确定性问题，考虑测量和系统误差，提供一种采用庞德里亚金集差的存在测量不确定性下的反馈路径规划方法。

为达到上述目的，本发明采用以下技术方案予以实现：

一种存在测量不确定性下的反馈路径规划方法，包括以下步骤：

1)开环轨迹计算：

开环轨迹计算采用PRM算法进行计算，然后采用PRM离散点作为初始估计；

2)计算局部反馈吸引域：

采用SOS算法进行计算，得到高阶较大的吸引域；

考虑一般非线性动力学系统：

假设开环轨迹和开环控制采用x₀,u₀表示，将坐标转变换到局部坐标系：

假设吸引域中初始状态满足x₀(0)∈χ₀，在时间区间t∈[0,T]中，存在集合满足：

定义：集合是参数化以后的吸引域的子集，其满足：

使得

采用SOS计算吸引域，假设局部吸引域的估计表示为：

其中S_i＝S_i^T，因此

求解时变黎卡提方程：得到每一时刻lyapunov函数的表达式；这个lyapunov函数作为SOS求解吸引域的初始估计：

3)计算误差椭圆：

定义：椭圆ε(q,Q)表示q为椭圆圆心，且<>表示内积；

首先将非线性系统离散化：