[发明专利]一种基于不确定性预测的航迹规划方法

权利要求书

1.一种基于不确定性预测的航迹规划方法，其特征在于，利用运动体的动力学模型、过程噪声模型和观测噪声模型，通过将真实航迹的偏差划分为真实航迹偏离估计航迹的偏差和估计航迹偏离所规划航迹的偏差之和，以概率形式预测真实航迹的不确定性，在此基础上结合快速扩展随机树方法，规划出可行的航迹。

2.根据权利要求1所述的基于不确定性预测的航迹规划方法，其特征在于，包括如下具体步骤：

步骤1：确定运动体的动力学约束和噪声模型以及其反馈控制器

将运动体的动力学约束表示成如下的线性随机离散系统形式

x_t=A_tx_t-1+B_tu_t-1+w_t  （1）

z_t=C_tx_t+v_t

其中x为状态向量，u为控制输入，过程噪声w_t～N(0,Q_t)服从正态分布，z为测量向量，观测噪声v_t～N(0,R_t)服从正态分布，A_t为系统矩阵，B_t为控制矩阵，C_t为控制矩阵，

真实航迹x_t由其估计值用稳定控制器K反馈跟踪某一所规划航迹生成，满足

xt=Atxt-1+Bt(ut-1-K(x^t-1-xt-1n))+wt,∀t=1,2,...,T;]]>

步骤2：初始化顶点集合与边集合

顶点集合V只包含起点，边集合E为空集，即

V={x_initial},E={}

其中，V为顶点集合，x_initial为起点，E为边集合，以用于保存搜索结果；

步骤3：设置循环变量i的取值范围

步骤4至步骤10对于i=1,2,3,…M时执行，这里i为循环变量，M为最大的循环次数；

步骤4：从无障碍的空间中任取一点这里无障碍空间是指运动体不会与环境中的障碍物发生碰撞的空间的集合；

步骤5：从V中找到距离最近的点，记作

步骤6：根据和运动体动力学方程（1），在给定时间步长条件下找到距离尽可能近的点以及与之间的航迹

步骤7：用下述公式预测航迹的不确定性协方差矩阵

Σ‾t=AtΣt-1AtT+QtΣt=Σ‾t-Σ‾tCtT(CtΣ‾tCtT+Rt)-1CtΣ‾tΛt=(At-BtK)Λt-1(At-BtK)T+Σ‾tCtT(CtΣ‾tCtT+Rt)-1CtΣ‾tψti=Σt+Λt---(2)]]>

其中，是与Q_t同维度的协方差矩阵，用来描述真实航迹围绕航迹时的不确定性，Σ_t用来描述真实航迹与估计航迹间的偏差的不确定性，Λ_t用来描述估计航迹与航迹的偏差的不确定性，为中间变量；

步骤8：在协方差矩阵的条件下，若满足P(x_t∈χ^free)>δ₁表明航迹可行，继续下一步，否则，返回步骤2；

步骤9：将添加到V中，将添加到E中；

步骤10：判断循环是否终止。如果则循环终止，终点记为x_end，继续下一步，这里χ^goal为目标区域，否则，返回步骤2；

步骤11：回溯找出所规划航迹

在V中从x_end开始依次回溯寻找上一个节点至起点x_initial，这条从x_end出发经过各个节点到达x_initial的航迹即为所规划的可行航迹。