[发明专利]一种非线性不确定系统的安全轨迹规划方法

权利要求书

1.一种非线性不确定系统的安全轨迹规划方法，其特征在于，包括以下步骤：

步骤一、开环轨迹库计算

采用PRM方法随机生成路径点，针对相邻路径点连线设计可连接的动力学轨迹，生成局部轨迹库为非线性规划问题，并进行计算得到开环轨迹和开环控制，开环轨迹和开环控制构成了轨迹库；

步骤二、计算局部不变集

采用非线性系统吸引域与不变集的算法进行离线计算，得到针对确定可达集的安全轨迹稳定的范围；

步骤三、计算误差传递

首先将非线性系统离散化，通过LQR算法得到局部反馈系数K_k得到控制率，得到误差椭圆方程；

步骤四、路径规划

管道在t_k时刻的值E(t_k)与误差椭圆ε(0,P_k)进行集差运算，获得误差情况下可信的管道，并通提供的估计值检测是否可以让真实值区间处于下一个管道的入口处构成序列连接，而完成路径规划拼接；

步骤二中，采用SOS算法进行离线计算，针对步骤一中轨迹库中的所有轨迹，求解时变黎卡提方程，得到时间区间中的lyapunov函数，作为SOS算法的初值；进而得到针对确定可达集的安全轨迹稳定的范围；

步骤二中，时变黎卡提方程为：

其中S(t)为待求解矩阵，A(t)为系统线性化以后的系数矩阵，Q，R为设计参数矩阵；

步骤三中得到误差椭圆方程具体步骤为：

定义：椭圆ε(q,Q)表示q为椭圆圆心矢量坐标，Q为椭圆参数，其为正定对阵矩阵，且<>表示内积；

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

x_k+1＝g(x_k,u_k,w_k)

y_k+1＝h(x_k,v_k)

其中，w_k和v_k+1分别为系统噪声和观测噪声，x与y分别为状态变量和观测量变量，u为系统输入，通过LQR线性二次型控制算法得到局部反馈系数K_k得到控制率：

假设为状态实际值与估计值的误差，误差椭圆：

e^e∈ε(0,P_k)

P_k为后验误差估计，假设为状态估计和标称状态的误差，

其中A_k为系统线性化系数，H_k为观测方程的雅克比矩阵，L_k+1为滤波增益；令实际状态表示为：

因此有：

其中：A_G＝A_k+B_kK_k，因此标称估计的预测值：其中椭圆参数为：

2.根据权利要求1所述的一种非线性不确定系统的安全轨迹规划方法，其特征在于，步骤四中，具体算法如下：

v,n表示随机路径节点，Q表示数据结构堆栈，v.path表示到达当前节点的路径，路径通过动力学估计进行连接，保存在v[MT]中，MT为一个元组{τ,F,c,u}分别表示{轨迹，funnel，耗费值，控制力}，FORECAST(n,v)为两个节点误差传递的计算，得到新的含有不确定性的funnel F，更新v[MT]中的值，SEQCOMPOSITION(F₁,F₂)表示两个funnel序列连接计算，可连接返回1否则为0。