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

摘要

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

主权项

1.一种存在测量不确定性下的反馈路径规划方法，其特征在于，包括以下步骤：/n1)开环轨迹计算：/n开环轨迹计算采用PRM算法进行计算，然后采用PRM离散点作为初始估计；/n2)计算局部反馈吸引域：/n采用SOS算法进行计算，得到高阶较大的吸引域；/n考虑一般非线性动力学系统：/n /n假设开环轨迹和开环控制采用x₀,u₀表示，将坐标转变换到局部坐标系：/n /n假设吸引域中初始状态满足x₀(0)∈χ₀，在时间区间t∈[0,T]中，存在集合满足：/n /n定义：集合是参数化以后的吸引域的子集，其满足：/n /n使得/n采用SOS计算吸引域，假设局部吸引域的估计表示为：/n /n其中S_i＝S_i^T，因此/n求解时变黎卡提方程：得到每一时刻lyapunov函数的表达式；这个lyapunov函数作为SOS求解吸引域的初始估计：/n /n3)计算误差椭圆：/n定义：椭圆ε(q,Q)表示q为椭圆圆心，且<>表示内积；/n首先将非线性系统离散化：/n /n其中w_k和v_k+1分别为系统噪声和观测噪声；标称系统为：/nx_0k+1＝f(x_0k)+B_ku_0k (8)/n通过LQR算法得到局部反馈系数K_k得到控制律：/n /n假设为实际值与估计值的误差，e^e∈ε(0,P_k)，P_k为后验误差估计，ε(·)表示椭圆；后验估计误差采用ESMF、EKF或者其他滤波算法得到；假设为估计状态和标称状态的误差；/n /n令所以实际状态表示为：/n /n因此有：/n /n其中：/nA_G＝A_k+B_kK_k，/n因此标称估计的预测值：其中椭圆参数：/n /n4)在线计算：/n离线计算得到吸引域，采用GP进行在线估计，采用GPML工具箱进行计算，得到估计吸引与上界ρ，从而形成吸引与椭圆E(t_i)，与误差椭圆进行庞德里亚金集差；得到新的标称轨迹附近的吸引域。/n