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

摘要

本发明公开了一种非线性不确定系统的安全轨迹规划方法，本发明是一种新的非线性动力学路径规划方法，考虑测量和系统误差，提出了采用庞德里亚金集差的方法重新修正局部反馈控制不变集大小，并在线进行路径规划的算法。具体步骤包括：步骤一、开环轨迹库计算；步骤二、计算局部不变集；步骤三、计算误差传递；步骤四、路径规划。从仿真结果可以看出本发明所提出方法和算法的有效性，本发明可以形成全局鲁棒的安全路径，从而满足非线性不确定系统安全轨迹规划的要求。

主权项

1.一种非线性不确定系统的安全轨迹规划方法，其特征在于，包括以下步骤：/n步骤一、开环轨迹库计算/n采用PRM方法随机生成路径点，针对相邻路径点连线设计可连接的动力学轨迹，生成局部轨迹库为非线性规划问题，并进行计算得到开环轨迹和开环控制，开环轨迹和开环控制构成了轨迹库；/n步骤二、计算局部不变集/n采用非线性系统吸引域与不变集的算法进行离线计算，得到针对确定可达集的安全轨迹稳定的范围；/n步骤三、计算误差传递/n首先将非线性系统离散化，通过LQR算法得到局部反馈系数K_k得到控制率，得到误差椭圆方程；/n步骤四、路径规划/n管道在t_k时刻的值E(t_k)与误差椭圆ε(0,P_k)进行集差运算，获得误差情况下可信的管道，并通提供的估计值检测是否可以让真实值区间处于下一个管道的入口处构成序列连接，而完成路径规划拼接；/n步骤二中，采用SOS算法进行离线计算，针对步骤一中轨迹库中的所有轨迹，求解时变黎卡提方程，得到时间区间中的lyapunov函数，作为SOS算法的初值；进而得到针对确定可达集的安全轨迹稳定的范围；/n步骤二中，时变黎卡提方程为：/n /n其中S(t)为待求解矩阵，A(t)为系统线性化以后的系数矩阵，Q，R为设计参数矩阵；/n步骤三中得到误差椭圆方程具体步骤为：/n定义：椭圆ε(q,Q)表示q为椭圆圆心矢量坐标，Q为椭圆参数，其为正定对阵矩阵，且<>表示内积；/n首先将非线性系统离散化：/nx_k+1＝g(x_k,u_k,w_k)/ny_k+1＝h(x_k,v_k)/n其中，w_k和v_k+1分别为系统噪声和观测噪声，x与y分别为状态变量和观测量变量，u为系统输入，通过LQR线性二次型控制算法得到局部反馈系数K_k得到控制率：/n /n假设为状态实际值与估计值的误差，误差椭圆：/ne^e∈ε(0,P_k)/nP_k为后验误差估计，假设为状态估计和标称状态的误差，/n /n其中A_k为系统线性化系数，H_k为观测方程的雅克比矩阵，L_k+1为滤波增益；令实际状态表示为：/n /n因此有：/n /n其中：A_G＝A_k+B_kK_k，因此标称估计的预测值：其中椭圆参数为：/n /n