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

说明书

技术领域

本发明属于航迹规划领域，涉及复杂环境下运动体受运动不确定性影响较大时，通过预测航迹不确定性来规划航迹的方法。

背景技术

航迹规划的目的是找到空间中从给定起点出发到目标区域的可行的航迹。现有的航迹规划方法主要有概略图法、单元分解法、人工势场法和基于搜索的航迹规划方法，这些方法的主要缺陷是不予考虑运动体的运动学约束，也不考虑运动体在实际运动过程中受到的过程噪声和观测噪声的影响，因而无法预测运动体在沿所规划航迹快速运动时的偏差，不能保证运动体不与障碍物发生碰撞。

发明内容

本发明目的在于：针对上述存在的问题，提供一种基于不确定性预测的航迹规划方法，从而可以在运动体高速运动状态下，能够准确预测运动体偏离所规划航迹的偏差，保证航迹的可行性。

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

所述的基于不确定性预测的航迹规划方法，其包括如下具体步骤：

步骤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={}