CN105222770A - 一种基于边界的烟田无人机自主导航算法 - Google Patents

Abstract

本发明提供一种基于边界的烟田无人机自主导航算法，该算法在无人机的探测区域内产生候选边界点，通过建立合理的目标函数，从候选边界点中选择目标点，建立的无人机运动状态模型，控制无人机朝该目标点方向运动，再预测和更新无人机的运动状态，该算法从确定的边界点中选择下一步运动目标点，目标点的选择范围可更宽，并在一定程度上避免陷入局部最优。

Description

一种基于边界的烟田无人机自主导航算法

技术领域

本发明涉及无人机导航算法领域，更具体地，涉及一种基于边界的烟田无人机自主导航算法。

背景技术

目前烟田无人机导航一般采用的方法是同步定位与地图构建，该方法是假定运动轨迹预先已知的，这种被动形式的方法与环境的未知性相矛盾，现有的主动的同步定位与地图构建方法主要是最优控制方法、局部子图法，而最优控制方法建立的目标函数是选择航迹规划点的基础，但是容易陷入局部最优，局部子图的方法更适用于多无人机协同的同步定位与地图构建航迹规划，对于单个无人机而言，除了可以减小一定计算量外，并没有明显的优势。

发明内容

本发明提供一种基于边界的烟田无人机自主导航算法，该算法目标点的选择范围较宽，可在一定程度上避免陷入局部最优。

为了达到上述技术效果，本发明的技术方案如下：

一种基于边界的烟田无人机自主导航算法，包括以下步骤：

S1：在烟田中设置若干地标，并取候选边界点：

S2：对候选的边界点进行评价确定边界点；

S3：使无人机朝确定的边界点运动并对无人机运动状态进行预测和更新。

进一步地，所述步骤S1的过程如下：

将无人机作业区域分为若干大小相等的小单元，每一个单元内的边界点分为相同的组，取每组中的边界点的重心的位置作为候选边界点。

进一步地，所述步骤S2的过程如下：

构建无人机观测地标的目标函数：

J＝ω₁ΔI+ω₂ΔS

式中P_jj(k|k)表示k时刻第j个地标的方差，N为地标总数，是无人机在边界点位置观测到的新的地标数L占总地标数的百分比，ω₁、ω₂是根据经验选取的权值；k时刻J值最大的候选边界点确定为一个边界点。

进一步地，所述步骤S3的过程如下：

1)令无人机的运动状态模型为：

X_v＝[xyψ]^T

式中x、y是无人机在二维平面中的位置，ψ是方位角；

令控制量U＝[ΔdΔψ]^T，Δd是一个时间步长对应的无人机运动距离，Δψ为姿态变化量，其中：

$\left\{\begin{array}{c}{x}_k=x_{k-1}+\mathrm{\Δ}{d}_{k}cos({\mathrm{\ψ}}_{k-1}+\mathrm{\Δ}{\mathrm{\ψ}}_k)\\ y_k=y_{k-1}+\mathrm{\Δ}{d}_{k}cos({\mathrm{\ψ}}_{k-1}+\mathrm{\Δ}{\mathrm{\ψ}}_k)\\ {\mathrm{\ψ}}_k={\mathrm{\ψ}}_{k-1}+\mathrm{\Δ}{\mathrm{\ψ}}_k\end{array}\right.$

则无人的运动状态模型方程为：

$X_{{\mathrm{\υ}}_k}=f(X_{{\mathrm{\υ}}_{k-1}},U_k)+W_k$

无人机与第i个地标的测量方程为：

$Z_{i,k}=h(X_{{\mathrm{\υ}}_k},m_{i,k})+{\mathrm{\υ}}_k$

式中m_i,k表示第i个地标的状态，m_i＝[m_xm_y]^T，地标是静止的则m_i,k＝m_i,k-1，W_k和υ_k运动噪声向量；

2)无人机的运动状态预测过程为：

$\hat{X}(k+1|k)=f\left(\hat{X}\right(k|k),U(k),0)$

$P(k+1|k)={\▿}_{X}f\·P\left(k\right|k){\▿}_X{f}^T+{\▿}_{X}f\·Q\left(k\right){\▿}_{\mathrm{\υ}}{f}^T$

$\hat{Z}(k+1|k)=h\left(\hat{X}\right(k+1|k),0)$

${\▿}_{X}f=\frac{\∂f}{\∂X}{|}_{X=\hat{X}}=\left[\begin{array}{cccc}\frac{\∂f^1}{\∂x_k^1}& \frac{\∂f^1}{\∂x_k^2}& \mathrm{...}& \frac{\∂f^1}{\∂x_k^n}\\ \frac{\∂f^2}{\∂x_k^1}& \frac{\∂f^2}{\∂x_k^2}& \mathrm{...}& \frac{\∂f^2}{\∂x_k^n}\\ .& .& & .\\ .& .& & .\\ .& .& & .\\ \frac{\∂f^n}{\∂x_k^1}& \frac{\∂f^n}{\∂x_k^2}& \mathrm{...}& \frac{\∂f^n}{\∂x_k^n}\end{array}\right]$

${\▿}_{\mathrm{\υ}}f=\frac{\∂f}{\∂X}{|}_{\mathrm{\υ}=0}=\left[\begin{array}{cccc}\frac{\∂f^1}{\∂{\mathrm{\υ}}_k^1}& \frac{\∂f^1}{\∂{\mathrm{\υ}}_k^2}& \mathrm{...}& \frac{\∂f^1}{\∂{\mathrm{\υ}}_k^n}\\ \frac{\∂f^2}{\∂{\mathrm{\υ}}_k^1}& \frac{\∂f^2}{\∂{\mathrm{\υ}}_k^2}& \mathrm{...}& \frac{\∂f^2}{\∂{\mathrm{\υ}}_k^n}\\ .& .& & .\\ .& .& & .\\ .& .& & .\\ \frac{\∂f^n}{\∂{\mathrm{\υ}}_k^1}& \frac{\∂f^n}{\∂{\mathrm{\υ}}_k^2}& \mathrm{...}& \frac{\∂f^n}{\∂{\mathrm{\υ}}_k^n}\end{array}\right]$

式中和分别是f(·)相对无人机状态变量X(k)和运动噪声向量υ(k)的雅克比矩阵，Q(k)是运动噪声向量W(k)的协方差；

3)无人机的运动状态更新过程为：

$\hat{X}(k+1|k+1)=\hat{X}(k+1|k)+W(k+1)\·\mathrm{\υ}(k+1)$

P(k+1|k+1)＝P(k+1|k)-W(k+1)·S_γγ(k+1|k)·W^T(k+1)

$\mathrm{\υ}(k+1)=Z(k+1)-h\left(\hat{X}\right(k+1|k),0)$

$S_{\mathrm{\γ}\mathrm{\γ}}(k+1|k)={\▿}_{X}h\·P\left(k\right|k)\·{\▿}_X{h}^T+R(k+1)$

$W(k+1)=P(k+1|k)\·{\▿}_X{h}^T\·S_{\mathrm{\γ}\mathrm{\γ}}^{-1}(k+1|k)$

式中R(k)是运动噪声向量υ(k)的协方差，表示h(·)相对于状态变变量X(k)的雅克比矩阵。

与现有技术相比，本发明技术方案的有益效果是：

本发明算法在无人机的探测区域内产生候选边界点，通过建立合理的目标函数，从候选边界点中选择目标点，建立的无人机运动状态模型，控制无人机朝该目标点方向运动，再预测和更新更新无人机的运动状态，该算法从确定的边界点中选择下一步运动目标点，目标点的选择范围可更宽，并在一定程度上避免陷入局部最优。

附图说明

图1为本发明算法的流程图；

图2为本发明算法的仿真实验图；

图3为最优控制法的仿真实验图。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；

为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；

对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

下面结合附图和实施例对本发明的技术方案做进一步的说明。

实施例1

一种基于边界的烟田无人机自主导航算法，包括以下步骤：

S1：在烟田中设置若干地标，并取候选边界点：

S2：对候选的边界点进行评价确定边界点；

S3：使无人机朝确定的边界点运动并对无人机运动状态进行预测和更新。

进一步地，所述步骤S1的过程如下：

将无人机作业区域分为若干大小相等的小单元，每一个单元内的边界点分为相同的组，取每组中的边界点的重心的位置作为候选边界点。

步骤S2的过程如下：

构建无人机观测地标的目标函数：

J＝ω₁ΔI+ω₂ΔS

式中P_jj(k|k)表示k时刻第j个地标的方差，N为地标总数，是无人机在边界点位置观测到的新的地标数L占总地标数的百分比，ω₁、ω₂是根据经验选取的权值；k时刻J值最大的候选边界点确定为一个边界点。

步骤S3的过程如下：

1)令无人机的运动状态模型为：

X_v＝[xyψ]^T

式中x、y是无人机在二维平面中的位置，ψ是方位角；

令控制量U＝[ΔdΔψ]^T，Δd是一个时间步长对应的无人机运动距离，Δψ为姿态变化量，其中：

$\left\{\begin{array}{c}{x}_k=x_{k-1}+\mathrm{\Δ}{d}_{k}cos({\mathrm{\ψ}}_{k-1}+\mathrm{\Δ}{\mathrm{\ψ}}_k)\\ y_k=y_{k-1}+\mathrm{\Δ}{d}_{k}cos({\mathrm{\ψ}}_{k-1}+\mathrm{\Δ}{\mathrm{\ψ}}_k)\\ {\mathrm{\ψ}}_k={\mathrm{\ψ}}_{k-1}+\mathrm{\Δ}{\mathrm{\ψ}}_k\end{array}\right.$

则无人的运动状态模型方程为：

$X_{{\mathrm{\υ}}_k}=f(X_{{\mathrm{\υ}}_{k-1}},U_k)+W_k$

无人机与第i个地标的测量方程为：

$Z_{i,k}=h(X_{{\mathrm{\υ}}_k},m_{i,k})+{\mathrm{\υ}}_k$

式中m_i,k表示第i个地标的状态，m_i＝[m_xm_y]^T，地标是静止的则m_i,k＝m_i,k-1，W_k和υ_k运动噪声向量；

2)无人机的运动状态预测过程为：

$\hat{X}(k+1|k)=f\left(\hat{X}\right(k|k),U(k),0)$

$P(k+1|k)={\▿}_{X}f\·P\left(k\right|k){\▿}_X{f}^T+{\▿}_{X}f\·Q\left(k\right){\▿}_{\mathrm{\υ}}{f}^T$

$\hat{Z}(k+1|k)=h\left(\hat{X}\right(k+1|k),0)$

${\▿}_{X}f=\frac{\∂f}{\∂X}{|}_{X=\hat{X}}=\left[\begin{array}{cccc}\frac{\∂f^1}{\∂x_k^1}& \frac{\∂f^1}{\∂x_k^2}& \mathrm{...}& \frac{\∂f^1}{\∂x_k^n}\\ \frac{\∂f^2}{\∂x_k^1}& \frac{\∂f^2}{\∂x_k^2}& \mathrm{...}& \frac{\∂f^2}{\∂x_k^n}\\ .& .& & .\\ .& .& & .\\ .& .& & .\\ \frac{\∂f^n}{\∂x_k^1}& \frac{\∂f^n}{\∂x_k^2}& \mathrm{...}& \frac{\∂f^n}{\∂x_k^n}\end{array}\right]$

${\▿}_{\mathrm{\υ}}f=\frac{\∂f}{\∂X}{|}_{\mathrm{\υ}=0}=\left[\begin{array}{cccc}\frac{\∂f^1}{\∂{\mathrm{\υ}}_k^1}& \frac{\∂f^1}{\∂{\mathrm{\υ}}_k^2}& \mathrm{...}& \frac{\∂f^1}{\∂{\mathrm{\υ}}_k^n}\\ \frac{\∂f^2}{\∂{\mathrm{\υ}}_k^1}& \frac{\∂f^2}{\∂{\mathrm{\υ}}_k^2}& \mathrm{...}& \frac{\∂f^2}{\∂{\mathrm{\υ}}_k^n}\\ .& .& & .\\ .& .& & .\\ .& .& & .\\ \frac{\∂f^n}{\∂{\mathrm{\υ}}_k^1}& \frac{\∂f^n}{\∂{\mathrm{\υ}}_k^2}& \mathrm{...}& \frac{\∂f^n}{\∂{\mathrm{\υ}}_k^n}\end{array}\right]$

式中和分别是f(·)相对无人机状态变量X(k)和运动噪声向量υ(k)的雅克比矩阵，Q(k)是运动噪声向量W(k)的协方差；

3)无人机的运动状态更新过程为：

$\hat{X}(k+1|k+1)=\hat{X}(k+1|k)+W(k+1)\·\mathrm{\υ}(k+1)$

P(k+1|k+1)＝P(k+1|k)-W(k+1)·S_γγ(k+1|k)·W^T(k+1)

$\mathrm{\υ}(k+1)=Z(k+1)-h\left(\hat{X}\right(k+1|k),0)$

$S_{\mathrm{\γ}\mathrm{\γ}}(k+1|k)={\▿}_{X}h\·P\left(k\right|k)\·{\▿}_X{h}^T+R(k+1)$

$W(k+1)=P(k+1|k)\·{\▿}_X{h}^T\·S_{\mathrm{\γ}\mathrm{\γ}}^{-1}(k+1|k)$

式中R(k)是运动噪声向量υ(k)的协方差，表示h(·)相对于状态变变量X(k)的雅克比矩阵。

利用本算法与最优控制法进行仿真对比，如图2和图3所示最优控制法的探测环境有限，并且无人机的运动状态呈现混乱无规律的特点，经常会陷入某个区域而不能离开，而采用本发明算法航迹规划，则能够较好地克服这种情况，无人机航迹平滑有序，显然，采用本发明算法时，无人机可以探测到更大范围的环境，并得到了较高的地标估计值。

相同或相似的标号对应相同或相似的部件；

附图中描述位置关系的用于仅用于示例性说明，不能理解为对本专利的限制；

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (4)

1.一种基于边界的烟田无人机自主导航算法，其特征在于，包括以下步骤：

S1：在烟田中设置若干地标，并取候选边界点：

S2：对候选的边界点进行评价确定边界点；

S3：使无人机朝确定的边界点运动并对无人机运动状态进行预测和更新。

2.根据权利要求1所述的基于边界的烟田无人机自主导航算法，其特征在于，所述步骤S1的过程如下：

将无人机作业区域分为若干大小相等的小单元，每一个单元内的边界点分为相同的组，取每组中的边界点的重心的位置作为候选边界点。

3.根据权利要求1所述的基于边界的烟田无人机自主导航算法，其特征在于，所述步骤S2的过程如下：

构建无人机观测地标的目标函数：

J＝ω₁ΔI+ω₂ΔS

式中P_jj(k|k)表示k时刻第j个地标的方差，N为地标总数，是无人机在边界点位置观测到的新的地标数L占总地标数的百分比，ω₁、ω₂是根据经验选取的权值；k时刻J值最大的候选边界点确定为一个边界点。

4.根据权利要求1所述的基于边界的烟田无人机自主导航算法，其特征在于，所述步骤S3的过程如下：

1)令无人机的运动状态模型为：

X_v＝[xyψ]^T

式中x、y是无人机在二维平面中的位置，ψ是方位角；

令控制量U＝[ΔdΔψ]^T，Δd是一个时间步长对应的无人机运动距离，Δψ为姿态变化量，其中：

$\left\{\begin{array}{c}{x}_k=x_{k-1}+\mathrm{\Δ}{d}_{k}cos({\mathrm{\ψ}}_{k-1}+\mathrm{\Δ}{\mathrm{\ψ}}_k)\\ y_k=y_{k-1}+\mathrm{\Δ}{d}_{k}cos({\mathrm{\ψ}}_{k-1}+\mathrm{\Δ}{\mathrm{\ψ}}_k)\\ {\mathrm{\ψ}}_k={\mathrm{\ψ}}_{k-1}+\mathrm{\Δ}{\mathrm{\ψ}}_k\end{array}\right.$

则无人的运动状态模型方程为：

$X_{{\mathrm{\υ}}_k}=f(X_{{\mathrm{\υ}}_{k-1}},U_k)+W_k$

无人机与第i个地标的测量方程为：

$Z_{i,k}=h(X_{{\mathrm{\υ}}_k},m_{i,k})+{\mathrm{\υ}}_k$

式中m_i,k表示第i个地标的状态，m_i＝[m_xm_y]^T，地标是静止的则m_i,k＝m_i,k-1，W_k和υ_k运动噪声向量；

2)无人机的运动状态预测过程为：

$\hat{X}(k+1|k)=f\left(\hat{X}\right(k|k),U(k),0)$

$P(k+1|k)={\▿}_{X}f\·P\left(k\right|k){\▿}_X{f}^T+{\▿}_{X}f\·Q\left(k\right){\▿}_{\mathrm{\υ}}{f}^T$

$\hat{Z}(k+1|k)=h\left(\hat{X}\right(k+1|k),0)$

${\▿}_{X}f=\frac{\∂f}{\∂X}{|}_{X=\hat{X}}=\left[\begin{array}{cccc}\frac{\∂f^1}{\∂x_k^1}& \frac{\∂f^1}{\∂x_k^2}& \mathrm{...}& \frac{\∂f^1}{\∂x_k^n}\\ \frac{\∂f^2}{\∂x_k^1}& \frac{\∂f^2}{\∂x_k^2}& \mathrm{...}& \frac{\∂f^2}{\∂x_k^n}\\ .& .& & .\\ .& .& & .\\ .& .& & .\\ \frac{\∂f^n}{\∂x_k^1}& \frac{\∂f^n}{\∂x_k^2}& \mathrm{...}& \frac{\∂f^n}{\∂x_k^n}\end{array}\right]$

${\▿}_{\mathrm{\υ}}f=\frac{\∂f}{\∂X}{|}_{\mathrm{\υ}=0}=\left[\begin{array}{cccc}\frac{\∂f^1}{\∂{\mathrm{\υ}}_k^1}& \frac{\∂f^1}{\∂{\mathrm{\υ}}_k^2}& \mathrm{...}& \frac{\∂f^1}{\∂{\mathrm{\υ}}_k^n}\\ \frac{\∂f^2}{\∂{\mathrm{\υ}}_k^1}& \frac{\∂f^2}{\∂{\mathrm{\υ}}_k^2}& \mathrm{...}& \frac{\∂f^2}{\∂{\mathrm{\υ}}_k^n}\\ .& .& & .\\ .& .& & .\\ .& .& & .\\ \frac{\∂f^n}{\∂{\mathrm{\υ}}_k^1}& \frac{\∂f^n}{\∂{\mathrm{\υ}}_k^2}& \mathrm{...}& \frac{\∂f^n}{\∂{\mathrm{\υ}}_k^n}\end{array}\right]$

式中和分别是f(·)相对无人机状态变量X(k)和运动噪声向量υ(k)的雅克比矩阵，Q(k)是运动噪声向量W(k)的协方差；

3)无人机的运动状态更新过程为：

$\hat{X}(k+1|k+1)=\hat{X}(k+1|k)+W(k+1)\·\mathrm{\υ}(k+1)$

P(k+1|k+1)＝P(k+1|k)-W(k+1)·S_γγ(k+1|k)·W^T(k+1)

$\mathrm{\υ}(k+1)=Z(k+1)-h\left(\hat{X}\right(k+1|k),0)$

$S_{\mathrm{\γ}\mathrm{\γ}}(k+1|k)={\▿}_{X}h\·P\left(k\right|k)\·{\▿}_X{h}^T+R(k+1)$

$W(k+1)=P(k+1|k)\·{\▿}_X{h}^T\·S_{\mathrm{\γ}\mathrm{\γ}}^{-1}(k+1|k)$

式中R(k)是运动噪声向量υ(k)的协方差，表示h(·)相对于状态变变量X(k)的雅克比矩阵。