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

说明书

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

技术领域

本发明涉及非线性系统路径规划与动力学控制领域，特别涉及一种非线性不确定系统的安全轨迹规划方法。

背景技术

如果机器人在不能完全知道自己状态的情况下进行路径规划，采用估计算法来预测可能的路径以寻找安全且误差较小的路径，比如BRM(belief roadmap)算法，RRBT(Rapidly-exploring Random Belief Trees)算法。相比于传统的路径算法，考虑不确定因素为安全的路径提供一个很好的保障，避免因为不确定性的存在影响实际规划路径运行的安全。但是动力学系统有时并不能保证安全轨迹的运行，比如执行能力饱和，系统偏差等，因此诞生了符合动力学约束的路径规划算法比如。而对于非线性系统，满足线性控制算法的动力学预测只能存在于很小的邻域，MIT的EECS实验室的Russ Tedrake教授提出采用LQR-trees方法，将非线性控制问题转化为局部线性反馈控制全局采用随机搜索树算法连接的形式，形成了符合动力学反馈控制要求的次优轨迹，这是一种非线性控制算法。吸引域和不变集的引入完美解决了局部线性化的可行性问题，混合系统切换完成全局稳定。非线性系统吸引域和不变集的计算非常耗时，因此A.Majumda引入了路径库进行离线计算，并采用中的序列组合方法案进行在线合并完成不变集的连接。

发明内容

为了克服以上缺点，本发明提供了一种新的非线性动力学路径规划方法，该方法在考虑反馈路径规划中现存测量不确定性，并在误差存在情况下反馈控制依然能稳定的到达目标状态。

本发明是通过以下技术方案来实现：

一种非线性不确定系统的安全轨迹规划方法，包括以下步骤：

步骤一、开环轨迹库计算

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

步骤二、计算局部不变集

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

步骤三、计算误差传递

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

步骤四、路径规划

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

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

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

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

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