CN105203108A - 基于动态威胁状态估计和威胁概率评估的auv在线航路规划方法 - Google Patents

Abstract

本发明提供的是一种基于动态威胁状态估计和威胁概率评估的AUV在线航路规划方法。步骤一：通过状态估计方法对动态威胁的运动状态进行估计；步骤二：遍历未来N个时刻AUV所能采取的所有控制量，通过控制方程得到其可能位置；步骤三：结合动态威胁的运动状态，利用贝叶斯网络对未来可能位置的威胁概率进行评估；步骤四：综合考虑AUV未来位置的威胁概率和与目标点的距离，选择最优的控制量并实施，在下一个时刻点重复以上步骤，直至到达目标点。本发明能够保证AUV有效规避动态威胁，且具有较高的实时性。

Description

基于动态威胁状态估计和威胁概率评估的AUV在线航路规划方法

技术领域

本发明涉及的是一种在线航路规划方法，特别是一种基于状态估计和威胁概率评估的AUV在线航路规划方法。

背景技术

根据对AUV航行空间中环境信息的已知程度，可将航路规划分为两类：一种是全局航路规划，此时整个规划空间中的环境信息均为已知；另一种是局部航路规划，此时航行器周围的环境情况为未知或仅能获得部分信息。在线航路规划是一种局部规划方法，通过将传感器探测到的实时环境信息和先验知识相结合，实时地优化修正航路。与全局航路规划方法相比，在线航路规划方法更适合于存在动态威胁或者环境信息部分或者全部未知的应用场景。常用的在线航路规划方法有混合整型线性规划方法、人工势场法、启发式算法等。如在公开号为CN103077425A的专利文件中，提出了一种基于免疫遗传算法的AUV实时路径规划方法，该专利利用优化算法来求解实时航路规划问题，可以获得有效的AUV全局路径。但是，上述方法并不适合存在动态威胁的情况。在存在动态威胁的航行空间中，为了及时规避动态威胁以保证航行安全，有必要对动态威胁的运动状态和威胁程序进行实时估计，并在此基础上进行在线航路规划。

发明内容

本发明的目的在于提供一种能够保证AUV有效规避动态威胁，且具有较高的实时性的基于动态威胁状态估计和威胁概率评估的AUV在线航路规划方法。

本发明的目的是这样实现的：

步骤一：通过状态估计方法对动态威胁的运动状态进行估计；

步骤二：遍历未来N个时刻AUV所能采取的所有控制量，通过控制方程得到其可能位置；

步骤三：结合动态威胁的运动状态，利用贝叶斯网络对未来可能位置的威胁概率进行评估；

步骤四：综合考虑AUV未来位置的威胁概率和与目标点的距离，选择最优的控制量并实施，在下一个时刻点重复以上步骤，直至到达目标点。

本发明针对动态威胁机动性强的特点，将状态估计和威胁概率评估引入在线航路规划方法，提出一种基于状态估计和威胁概率评估的AUV在线航路规划方法。

本发明是为了解决动态威胁存在情况下的AUV在线航路规划的问题，首先对动态威胁的运动状态进行估计，并对其威胁进行分析评估，在此基础上进行AUV航路规划。本发明优点在于：

针对水下环境中动态威胁速度较快，机动性较强的特点，利用状态估计方法估计动态威胁的运动状态，利用贝叶斯网络评估航路威胁程度，并采用滚动优化的方法求解出航路。该方法能够保证AUV有效规避动态威胁，且具有较高的实时性。

附图说明

图1：在线航路规划方法流程图。

图2：模糊离散动态贝叶斯推理网络结构图。

具体实施方式

下面举例对本发明做更详细的描述。

结合图1，对具体的步骤说明如下：

步骤一：通过状态估计方法对动态威胁的运动状态进行估计。

步骤1.1利用AUV对动态威胁的观测量得到动态威胁位置坐标。

利用AUV自身的传感器获得动态威胁的运动状况，具体包括动态威胁相对于AUV的距离R，俯仰角A和偏向角E。

利用观测量得到动态威胁的绝对坐标，结合AUV自身位置(x,y,z)，建立动态威胁的世界坐标系坐标

$\left\{\begin{array}{c}\mathrm{\ξ}=R\cos E\cos A+x\\ \mathrm{\η}=R\sin E+y\\ \mathrm{\ζ}=R\cos E\sin A+z\end{array}\right.---\left(1\right)$

式中，R是动态威胁相对于AUV的距离，A是动态威胁相对于AUV的平面偏向角，E是动态威胁相对于AUV的垂直偏向角，(ξ,η,ζ)为动态威胁的世界坐标系坐标。

步骤1.2通过无迹卡尔曼滤波方法对当前时刻至N个采样时间内动态威胁的位置信息进行估计。

使用无迹卡尔曼滤波方法，对未来时刻的动态威胁位置进行估计。

无迹卡尔曼滤波的状态方程为：

$\left(\begin{array}{c}{\mathrm{\ξ}}_k\\ {\stackrel{\·}{\mathrm{\ξ}}}_k\\ {\mathrm{\η}}_k\\ {\stackrel{\·}{\mathrm{\η}}}_k\\ {\mathrm{\ζ}}_k\\ {\stackrel{\·}{\mathrm{\ζ}}}_k\end{array}\right)=\left(\begin{array}{cccccc}1& T& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& T& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& T\\ 0& 0& 0& 0& 0& 1\end{array}\right)\left(\begin{array}{c}{\mathrm{\ξ}}_{k-1}\\ {\stackrel{\·}{\mathrm{\ξ}}}_{k-1}\\ {\mathrm{\η}}_{k-1}\\ {\stackrel{\·}{\mathrm{\η}}}_{k-1}\\ {\mathrm{\ζ}}_{k-1}\\ {\stackrel{\·}{\mathrm{\ζ}}}_{k-1}\end{array}\right)+W_k---\left(2\right)$

观测方程

y_k＝HX_k+V_k(3)

更新无迹卡尔曼滤波器的参数。

计算Sigma点对应的观测值：

${\mathrm{\γ}}_{k|k-1}=h\left({\widetilde{\mathrm{\χ}}}_{k-1}\right)---\left(4\right)$

加权计算观测值的估计值：

${\hat{y}}_{k|k-1}=\underset{i=0}{\overset{2n}{\Σ}}{w}_i^m{\mathrm{\γ}}_{i,k|k-1}---\left(5\right)$

计算观测估计值的协方差：

$P_{y_k}=\underset{i=0}{\overset{2n}{\Σ}}{w}_i^c({\mathrm{\γ}}_{i,k|k-1}-{\hat{y}}_{k|k-1}){({\mathrm{\γ}}_{i,k|k-1}-{\hat{y}}_{k|k-1})}^T+R_{k-1}---\left(6\right)$

计算观测估计值与预测估计值的联合分布的协方差：

$P_{x_k{y}_k}=\underset{i=0}{\overset{2n}{\Σ}}{w}_i^c({\mathrm{\χ}}_{i,k|k-1}-{\hat{x}}_{k|k-1}){({\mathrm{\γ}}_{i,k|k-1}-{\hat{y}}_{k|k-1})}^T---\left(7\right)$

计算卡尔曼增益：

$K_k=P_{x_k{y}_k}{P_{y_k}}^{-1}---\left(8\right)$

计算基于实际观测值的协方差：

$P_k=P_{k|k-1}-K_k{P}_{y_k}{K_k}^T---\left(9\right)$

获得基于实际观测值的状态值：

${\hat{x}}_k={\hat{x}}_{k|k-1}+K_k\left(y_k-{\hat{y}}_{k|k+1}\right)---\left(10\right)$

最后，预测未来时刻动态威胁的位置：

计算Sigma点：

${\mathrm{\χ}}_k=\left[\begin{array}{ccc}{\hat{x}}_k& {\hat{x}}_k+{\left(\sqrt{(n+\mathrm{\λ})P_k}\right)}_i& {\hat{x}}_k-{\left(\sqrt{(n+\mathrm{\λ})P_k}\right)}_i\end{array}\right]---\left(11\right)$

预测：首先计算每个Sigma点的函数值，然后利用Sigma点的函数值进行加权，得到函数预测值，最后计算估计协方差。

x_k+1|k＝f_k(χ_k)(12)

${\hat{x}}_{k+1|k}=\underset{i=0}{\overset{2n}{\Σ}}{w}_i^m{\mathrm{\χ}}_{i,k+1|k}---\left(13\right)$

$P_{k+1|k}=\underset{i=0}{\overset{2n}{\Σ}}{w}_i^c\left({\mathrm{\χ}}_{i,k+1|k}-{\hat{x}}_{k+1|k}\right){\left({\mathrm{\χ}}_{i,k1|k}-{\hat{x}}_{k+1|k}\right)}^T+Q_k---\left(14\right)$

在未获取观测信息时，可以将估计值作为输入，通过以上方程，得到k+n时刻的动态威胁位置。

步骤二：遍历未来N个时刻AUV所能采取的所有控制量，通过控制方程得到AUV的所有可能位置。

视在单个时间片内的AUV的运动为匀速直线运动，建立AUV的运动模型如下：

当存在海流时，其运动模型如下：

遍历未来N个时刻AUV所能采取的控制量u_θ、和u_s的所有值，通过控制方程确定其位置。

由于AUV机械性能的约束，其控制量约束如下：

步骤三：结合动态威胁的运动状态，利用贝叶斯网络对未来可能位置的威胁概率进行评估。

步骤3.1构建用于威胁概率评估的模糊离散动态贝叶斯推理网络。

建立模糊离散动态贝叶斯推理网络如附图2所示。当前时刻的威胁概率由前一时刻的威胁概率、动态威胁与AUV的相对距离、动态威胁与AUV的相对距离变化均有关。

步骤3.2选择用于对观测量进行模糊化的模糊函数以及用于威胁概率评估的模糊离散动态贝叶斯推理网络的参数。

(1)相对距离采取采用三角型隶属函数，其计算方法如下：

(2)距离变化采取高斯型隶属函数，其计算方法如下：

(3)选择贝叶斯网络推理参数，即条件概率表和时间片间转移概率表如下：

表1威胁距离与威胁等级的条件概率表

	距离近	距离中	距离远
				威胁概率高	0.8	0.15	0.05
威胁概率中	0.2	0.6	0.2
				威胁概率低	0.05	0.15	0.8

表2运动趋势与威胁等级的条件概率表

	接近	保持	远离
				威胁概率高	0.7	0.2	0.1
威胁概率中	0.2	0.6	0.2
				威胁概率低	0.1	0.2	0.7

表3时间片间转移概率表

步骤3.3将动态威胁的观测数据、估计数据和AUV的未来可能位置模糊化，带入模糊离散动态贝叶斯网络，获得该位置的威胁概率。

利用动态威胁的观测数据和估计数据，以及AUV的未来可能位置，计算AUV与动态威胁的相对距离及其相邻时刻间的距离变化。

利用式(18)和(19)，计算AUV与动态威胁的相对距离和其变化量在各个模糊集合中的隶属度，将其带入模糊离散动态贝叶斯网络进行概率推理，推理方程如下：

$\begin{array}{c}P\left(x_{11},x_{12},\mathrm{...},x_{1n},\mathrm{...},x_{T1},x_{T2},\mathrm{...},x_{Tn}|y_{11o},y_{12o},\mathrm{...},y_{1mo},\mathrm{...},y_{T1o},y_{T2o},\mathrm{...},y_{Tmo}\right)\\ =\underset{y_{11},y_{12},\mathrm{...},y_{1m}}{\Σ}\frac{\underset{i,k}{\Π}P\left(x_{ik}|Pa\left(X_{it}\right)\right)\underset{i,j}{\Π}P\left(y_{ij}|Pa\left(Y_{ij}\right)\right)}{\underset{x_{11},x_{12},\mathrm{...},x_{T1},\mathrm{...},x_{T_n}}{\Σ}\underset{i,k}{\Π}P\left(x_{ik}|Pa\left(X_{it}\right)\right)\underset{i,j}{\Π}P\left(Pa\left(Y_{ij}\right)\right)}\\ \underset{ij}{\Π}P\left(Y_{ij}=y_{ijo}\right)i\∈\[1,n\],j\∈\[1,m\],K\∈\[1,n\]\end{array}---\left(20\right)$

这样，即可得到对于该位置的威胁概率。

步骤四：利用AUV未来位置的威胁概率和与目标点的距离，选择最优的控制量并实施，在下一个时刻点重复以上步骤，直至到达目标点。

步骤4.1利用威胁概率与AUV和目标点的距离，构建代价函数。

某一未来航路点的代价函数即为，如果AUV下一刻运动到该点，那么下一时刻动态威胁对于AUV的威胁程度和该点与目标点的距离的加权和。可以用公式表示为：

J＝C_pP_{(威胁＝高)}+Des(5-3)

式中，P_{(威胁＝高)}为若AUV下一刻运动到该点，动态威胁对其威胁代价为高的概率，即步骤四得到的结果。C_p为其加权系数。Des为该点到目标点的距离。

其中，C_p可以选取Des的倍数，即C_p＝c×Des，c一般取0.5～1.5之间，其值越大，AUV对威胁躲避的趋势越强。

步骤4.2将所有AUV的未来可能位置和威胁概率带入目标函数，求取其代价值。

步骤4.3选取代价值最小的可能位置对应的控制量并执行，并在下一个时刻点重复以上步骤，直至到达目标点。

通过以上步骤，即可在线规划处AUV的安全航路。

Claims (4)

1.一种基于动态威胁状态估计和威胁概率评估的AUV在线航路规划方法，其特征是：

步骤一：通过状态估计方法对动态威胁的运动状态进行估计；

步骤二：遍历未来N个时刻AUV所能采取的所有控制量，通过控制方程得到其可能位置；

步骤三：结合动态威胁的运动状态，利用贝叶斯网络对未来可能位置的威胁概率进行评估；

步骤四：综合考虑AUV未来位置的威胁概率和与目标点的距离，选择最优的控制量并实施，在下一个时刻点重复以上步骤，直至到达目标点。

2.根据权利要求1所述的基于动态威胁状态估计和威胁概率评估的AUV在线航路规划方法，其特征是所述的通过状态估计方法对动态威胁的运动状态进行估计具体包括：

步骤1.1利用AUV对动态威胁的观测量得到动态威胁的位置坐标；

步骤1.2通过无迹卡尔曼滤波方法对之后一到N个采样时间的动态威胁位置进行估计。

3.根据权利要求2所述的基于动态威胁状态估计和威胁概率评估的AUV在线航路规划方法，其特征是步骤1.1具体包括：

利用AUV自身的传感器获得动态威胁的运动状况，包括动态威胁相对于AUV的距离R，俯仰角A和偏向角E，

利用观测量得到动态威胁的绝对坐标，结合AUV自身位置(x,y,z)，建立动态威胁的世界坐标系坐标

$\left\{\begin{array}{c}\mathrm{\ξ}=R\cos E\cos A+x\\ \mathrm{\η}=R\sin E+y\\ \mathrm{\ζ}=R\cos E\sin A+z\end{array}\right.---\left(1\right)$

式中，R是动态威胁相对于AUV的距离，A是动态威胁相对于AUV的平面偏向角，E是动态威胁相对于AUV的垂直偏向角，(ξ,η,ζ)为动态威胁的世界坐标系坐标。

4.根据权利要求1、2或3所述的基于动态威胁状态估计和威胁概率评估的AUV在线航路规划方法，其特征是所述利用贝叶斯网络对未来可能位置的威胁概率进行评估具体包括：

步骤3.1构建用于威胁概率评估的模糊离散动态贝叶斯推理网络；

步骤3.2选择用于威胁概率评估的模糊离散动态贝叶斯推理网络的参数；

步骤3.3将动态威胁的观测数据、估计数据和AUV的未来可能位置带入模糊离散动态贝叶斯网络，评估该位置的威胁概率。