一种基于不确定性预测的航迹规划方法
【技术领域】
[0001] 本发明属于航迹规划领域，涉及复杂环境下运动体受运动不确定性影响较大时， 通过预测航迹不确定性来规划航迹的方法。
【背景技术】
[0002] 航迹规划的目的是找到空间中从给定起点出发到目标区域的可行的航迹。现有的 航迹规划方法主要有概略图法、单元分解法、人工势场法和基于搜索的航迹规划方法，这些 方法的主要缺陷是不予考虑运动体的运动学约束，也不考虑运动体在实际运动过程中受到 的过程噪声和观测噪声的影响，因而无法预测运动体在沿所规划航迹快速运动时的偏差， 不能保证运动体不与障碍物发生碰撞。
【发明内容】
[0003] 本发明目的在于：针对上述存在的问题，提供一种基于不确定性预测的航迹规划 方法，从而可以在运动体高速运动状态下，能够准确预测运动体偏离所规划航迹的偏差，保 证航迹的可行性。
[0004] 本发明的技术方案如下：一种基于不确定性预测的航迹规划方法，其利用运动体 的动力学模型、过程噪声模型和观测噪声模型，通过将真实航迹的偏差划分为真实航迹偏 离估计航迹的偏差和估计航迹偏离所规划航迹的偏差之和，以概率形式预测真实航迹的不 确定性，在此基础上结合快速扩展随机树方法，规划出可行的航迹。
[0005] 所述的基于不确定性预测的航迹规划方法，其包括如下具体步骤：
[0006] 步骤1 :确定运动体的动力学约束和噪声模型以及其反馈控制器
[0007] 将运动体的动力学约束表示成如下的线性随机离散系统形式
[0008] Xt=AtXt^1+B tUt^1+wt (1)
[0009] zt=Ctxt+vt
[0010] 其中X为状态向量，U为控制输入，过程噪声Wt~N(0, Qt)服从正态分布，Z为测 量向量，观测噪声Vt~N(0, Rt)服从正态分布，At为系统矩阵，Bt为控制矩阵，Ct为控制矩 阵，
[0011] 真实航迹Xt由其估计值毛用稳定控制器K反馈跟踪某一所规划航迹<生成，满足
[0012]
【主权项】
1. 一种基于不确定性预测的航迹规划方法，其特征在于，利用运动体的动力学模型、过 程噪声模型和观测噪声模型，通过将真实航迹的偏差划分为真实航迹偏离估计航迹的偏差 和估计航迹偏离所规划航迹的偏差之和，以概率形式预测真实航迹的不确定性，在此基础 上结合快速扩展随机树方法，规划出可行的航迹。
2. 根据权利要求1所述的基于不确定性预测的航迹规划方法，其特征在于，包括如下 具体步骤： 步骤1 :确定运动体的动力学约束和噪声模型以及其反馈控制器 将运动体的动力学约束表示成如下的线性随机离散系统形式 XfAtXH+BtUH+Wt (1) zt=Ctxt+vt 其中X为状态向量，u为控制输入，过程噪声wt~N(0，Qt)服从正态分布，Z为测量向 量，观测噪声vt~N(0,Rt)服从正态分布，At为系统矩阵，Bt为控制矩阵，Ct为控制矩阵， 真实航迹xt由其估计值為用稳定控制器K反馈跟踪某一所规划航迹<生成，满足
步骤2 :初始化顶点集合与边集合 顶点集合V只包含起点，边集合E为空集，即 V={xinitial},E={} 其中，V为顶点集合，xinitial为起点，E为边集合，以用于保存搜索结果； 步骤3 :设置循环变量i的取值范围 步骤4至步骤10对于i=l，2, 3, 时执行，这里i为循环变量，M为最大的循环次数； 步骤4 :从无障碍的空间中任取一点，这里无障碍空间是指运动体不会与环境中的 障碍物发生碰撞的空间的集合； 步骤5 :从V中找到距离最近的点，记作4??.，； 步骤6 :根据r;aw/、和运动体动力学方程（1)，在给定时间步长条件下找到距离Acw.e彳尽可遗近的点知停5以及与?间的航迹； 步骤7 :用下述公式预测航迹的不确定性协方差矩阵 t L I I
其中，W是与Qt同维度的协方差矩阵，用来描述真实航迹围绕航迹乂^时的不确定 性，2t用来描述真实航迹与估计航迹间的偏差的不确定性，At用来描述估计航迹与航迹 的偏差的不确定性，艮为中间变量； 步骤8:在协方差矩阵< 的条件下，若满足P(xteX>Si表明航迹可行，继续 下一步，否则，返回步骤2 ; 步骤9 :将X。添加到V中，将〇"添加到E中； 步骤10 :判断循环是否终止。如果胃eJ-) >戌，贝U循环终止，终点xU记为x6nd， 继续下一步，这里xg°al为目标区域，否则，返回步骤2 ; 步骤11 :回溯找出所规划航迹 在V中从xmd开始依次回溯寻找上一个节点至起点xinitial,这条从xmd出发经过各个节 点到达xinitial的航迹即为所规划的可行航迹。
【专利摘要】本发明属于航迹规划领域，涉及复杂环境下运动体受运动不确定性影响较大时，通过预测航迹不确定性来规划航迹的方法。本发明利用运动体的动力学模型、过程噪声模型和观测噪声模型，通过将真实航迹的偏差划分为真实航迹偏离估计航迹的偏差和估计航迹偏离所规划航迹的偏差之和，以概率形式预测真实航迹的不确定性，在此基础上结合快速扩展随机树方法，规划出可行的航迹。本发明同时考虑高速运动体的动力学约束和所受过程噪声和观测噪声的影响，通过基于运动模型和误差模型的预测，事先预测所规划航迹的不确定性，从而保证在任意时刻运动体与障碍物发生碰撞的概率都小于(1-δ1)，且能够到达目标区域的概率大于δ2，因此在概率意义上保证所规划的路线是可行的。
【IPC分类】G06F19-00
【公开号】CN104699935
【申请号】CN201310674923
【发明人】顾天元, 张宁, 陈小龙
【申请人】中国航空工业第六一八研究所
【公开日】2015年6月10日
【申请日】2013年12月10日