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

权利要求书

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

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为椭圆圆心，且<>表示内积；

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

其中w_k和v_k+1分别为系统噪声和观测噪声；标称系统为：

x_0k+1＝f(x_0k)+B_ku_0k (8)

通过LQR算法得到局部反馈系数K_k得到控制律：

假设为实际值与估计值的误差，e^e∈ε(0,P_k)，P_k为后验误差估计，ε(·)表示椭圆；后验估计误差采用ESMF、EKF或者其他滤波算法得到；假设为估计状态和标称状态的误差；

令所以实际状态表示为：

因此有：

其中：

A_G＝A_k+B_kK_k，

因此标称估计的预测值：其中椭圆参数：

4)在线计算：

离线计算得到吸引域，采用GP进行在线估计，采用GPML工具箱进行计算，得到估计吸引与上界ρ，从而形成吸引与椭圆E(t_i)，与误差椭圆进行庞德里亚金集差；得到新的标称轨迹附近的吸引域。

2.根据权利要求1所述的存在测量不确定性下的反馈路径规划方法，其特征在于，步骤1)采用GPOPS-II软件进行非线性轨迹计算。

3.根据权利要求1所述的存在测量不确定性下的反馈路径规划方法，其特征在于，步骤2)中，求解吸引域的具体方法如下：

2-1)初始化LQR反馈控制得到的lyapunov函数V和初始猜测值ρ(t)；

2-2)初始先验指标函数为正无穷，即cost_prev＝∞，且当前状态不收敛；

2-3)如果不收敛，开始循环：

a.最小化函数：固定V和ρ(t)找(L,L_t,L_0,i,L_E,i)与S_i；

b.最小化函数：固定L,L_E,i找(V,ρ,L_t,L_0,iS_i)；

2-4)令当前指标函数

2-5)如果则收敛，结束如果；

2-6)先验指标等于当前指标：cost_prev＝cost；

2-7)结束循环；

其中，L,L_t,L_0,i,L_E,i均为拉格朗日乘子，为状态量x的函数，系数待定；ε为预设一小实数。

4.根据权利要求3所述的存在测量不确定性下的反馈路径规划方法，其特征在于，步骤4)的具体方法如下：

4-1)初始化并构建地图；

4-2)采用PRM算法进行初始路径估计，得到可行路径；

4-3)基于步骤4-2)中PRM方法得到初始路径参考值，采用GPOPS-II计算开环路经与开环控制律；

4-4)采用时变黎卡提方程计算初始lyapunov函数；

4-5)开始循环；

4-6)获取当前状态估计值；

4-7)采用GP方法估计吸引域半径，训练数据来源于吸引域的离线计算；

4-8)检查误差椭圆与吸引域椭圆距离，如果小于吸引域椭圆半径，则采用庞德里亚金集差方法计算新的鲁棒吸引域，具体为吸引域椭圆与误差椭圆做差；否则回到步骤4-2)重规划；

4-9)检查当前鲁棒吸引域与障碍物是否无碰撞，如果碰撞则增加GPOPS-II的约束升级开环控制u₀；

4-10)结束循环。