小型无人机自主导航定位方法技术

本发明专利技术属于无人机技术领域，具体涉及一种小型无人机自主导航定位方法，其利用改进建议分布粒子滤波实现无人机的同步定位与地图构建方法框架的设计。并针对无人机运动模型、观测模型线性化导致的模型不匹配及粒子的退化问题加以研究。提出利用自适应EKF及RTS产生建议分布，融入最新的观测量，设计无人机的同步定位与构图方法，产生更接近真实分布的粒子集，通过重采样，对粒子的状态进行更新，从而从根本上避免模型线性化的影响，提高估计精度，同时由于不需要计算系统状态矩阵的协方差，降低了计算量，最终达到提高无人机长航时导航定位与地图构建精度及效率的目的。

【技术实现步骤摘要】
小型无人机自主导航定位方法
本专利技术属于无人机
，具体涉及一种小型无人机自主导航定位方法。
技术介绍
现代社会人力资源成本价值越来越高，尤其对于环境比较恶劣或者比较耗时的任务，例如危险的楼内火灾救援现场，或枯燥耗时的楼内墙壁粉刷等，利用机器来替代人工是非常不错的选择，于是各种小型无人机应运而生，而由于自主导航定位技术是小型无人机完成任何任务的先决条件，所以导航定位技术成为限制小型无人机发展的瓶颈技术。传统的小型无人机大都采取航位推算配合GPS的组合导航方法实现导航定位，随着时间推移，其位置估计误差增大，需要定期采用GPS或北斗对其位置进行校正。这种方法对于GPS或北斗的卫星信号要求比较高，一旦信号受到遮挡或者变弱，将无法进行位置校正，导致位置估计误差累计较大。同步定位与构图(SimultaneousLocalizationandMapping，SLAM)技术的提出，一定程度上缓解了传统导航方法中存在的不足。SLAM是在无人机自身初始位置不确定、航位推算过程中位置误差逐渐累积的情况下，利用自身携带的环境感知传感器反复探测环境中特征，从而完成自身及特征位置的校正，同时构建环境地图，无需借助外部辅助定位设备(GPS/北斗)即可以得到较为可靠的无人机位置信息及环境地图信息。同步定位与构图技术的主要框架依赖于滤波估计方法，比较流行的是基于EKF框架。然而，EKF一般通过对系统方程进行一阶泰勒展开的方式实现非线性方程的线性化，这一操作不可避免的会产生线性化后的截断误差，而小型无人机的运动模型本来就不是精确的，这就势必导致其导航定位结果受到严重影响，并且由于需要对系统状态误差的协方差进行估计，导致其计算量较大，实时性受到影响；基于粒子滤波的SLAM框架是另一种比较流行的框架，但是传统的粒子滤波采用先验分布作为建议分布，没有考虑最新的测量，导致粒子退化问题严重，限制了无人机导航定位精度。
技术实现思路
(一)要解决的技术问题本专利技术要解决的技术问题是：如何将自适应滤波方法及RTS平滑过程与粒子滤波相结合以降低粒子退化程度，同时将这种改进的粒子滤波与SLAM方法相结合以解决无人机自主导航过程中存在的位置误差累积、线性化截断误差及粒子退化等问题。(二)技术方案为解决上述技术问题，本专利技术提供一种小型无人机自主导航定位方法，该方法包括如下步骤：步骤S1：对无人机SLAM系统建模；1.1地图模型采用特征地图来构建环境地图模型，将环境中的目标用几何原型来描述；1.2坐标系统由于无人机所携带的传感器与环境特征之间的感知行为发生在传感器坐标系中，速度的测量发生在无人机运动坐标系中，而最终环境地图的构建需要表示在全局地图中，所以建立全局坐标系，无人机运动坐标系及传感器坐标系，并表明他们之间的关系，定义GXGYGZG为全局坐标系，G为地心，XG指向正北，YG指向正东，ZG垂直于XGYG平面并指向G；定义AXAYAZA为无人机运动坐标系，A为无人机质心，XA指向机头，YA垂直于机头，ZA垂直于XAYA平面并指向A；定义SXSYSZS为环境感知传感器坐标系，S为传感器质心，XS指向传感器电磁波发射方向，YS垂直于电磁波发射方向，ZS垂直于XSYS平面并指向S；三个坐标系均符合右手定则；1.3特征模型根据公式(1)，定义特征的运动学模型为：其中，k为离散时间变量；1.4无人机运动模型通过建立一个四自由度的常速运动学模型来描述无人机的运动变换趋势，如公式(2)所示：XA(k)＝f(XA(k-1),n(k-1))(2)无人机XA＝[xyzψuvwr]T，[x,y,z,ψ]为全局坐标系中无人机的位置和艏向，[u,v,w,r]表示运动坐标系A中相应的XA、YA、ZA方向的线速度和航向角速度，n＝[nu,nv,nw,nr]表示以加速度的形式作用在速度上的高斯噪声；根据特征状态和无人机状态XA可以获得SLAM系统的状态X，包括无人机的状态XA和特征的状态Xf：X＝[XAXf]T(3)1.5传感器测量模型(1)速度测量模型速度测量传感器可提供X、Y、Z三个方向的速度，测量模型为：ZV＝HVX+sV(4)其中，ZV为速度的测量值，sV为速度测量噪声；(2)高度测量模型通过高度测量传感器可以获得无人机的高度，测量模型为：ZH＝HHX+sH(5)其中，ZH为高度的测量值，sH为高度测量噪声；(3)航向测量模型通过艏向测量传感器可以获得无人机的航向，测量模型为：ZC＝HCX+sC(6)其中，ZC为航向的测量值，sC为航向测量噪声；(4)环境测量模型通过环境感知传感器可以获得环境中特征相对于无人机的距离和方位，测量模型为：其中，si为特征测量噪声；步骤S2：基于步骤S1所建立的无人机SLAM系统模型，执行基于自适应EKF-RTS建议分布粒子滤波的无人机同步定位与构图操作；其中，建立无人机SLAM系统的状态方程和观测方程如下所示，被估计状态以及观测变量与状态向量的关系呈现非线性：X(k)＝f(X(k-1),m(k))Z(k)＝h(X(k),n(k))(8)式中，X(k)是无人机SLAM系统的状态向量；Z(k)是系统的观测向量；m(k)是系统的过程噪声序列；n(k)是系统的观测噪声序列；f()和h()表示系统的非线性状态方程和非线性观测方程；通过对粒子滤波的建议分布进行改进，获得更加接近粒子真实分布的粒子集合，通过重采样，计算新的粒子权值，并获得更新后的粒子状态；进而按照下述基于自适应EKFRTS平滑粒子滤波SLAM(EKF-RTS-PF-SLAM)基本步骤流程执行：步骤A：初始化含有无人机状态及特征状态的粒子集；步骤B：粒子状态预测：步骤C：传感器观测及数据关联首先获得速度、姿态及环境感知等传感器观测值，如果数据关联结果表明所观测特征为地图中已存在特征，则根据对该特征的实际测量值与预测测量值之间的差异来实现无人机状态及整个地图中所有特征位置的校正；具体如下：步骤C-1：生成AEKF-RTS建议分布，融入最新的观测量，对粒子进行重要性采样；步骤C-1-1：AEKF粒子状态更新步骤C-1-2：滤波估计状态值存储：存储时间区域[0，T]内各时刻的滤波结果用于下一步的最优平滑。步骤C-1-3：RTS平滑：步骤C-1-3-1：初始化利用k＝T时刻的滤波估计状态值初始化平滑器的状态及方差：步骤C-1-3-2：平滑修正；步骤C-1-4：AEKF-RTS建议分布生成；根据对各个时刻的状态向量及协方差进行预测更新及平滑修正得到的状态估计值及协方差序列为：从而构造AEKF-RTS建议分布函数如下：步骤C-2：计算粒子权值，对粒子状态进行更新；步骤C-3：进行重采样，提取权值大于设定阈值的粒子；步骤D：地图扩张如果数据关联结果表明该次测量为新线特征，则进行地图扩张。(三)有益效果与现有技术相比较，本专利技术是一种无人机仅通过自身携带的速度、姿态、环境感知传感器等实现自主导航的方法。导航定位是无人机完成预定使命的必备条件，是决定其能否自主完成任务的关键因素。本专利技术所述方法中，无人机摆脱室内GPS/北斗信号弱的限制及捷联惯导等造价高昂的外部导航设备的依赖，在运动的同时利用自身携带的环境感知传感器，不断探测结构化室内环境中的线特征，动态地增量式构建环境特征地图，同时利用对环境中特征的重复本文档来自技高网...

【技术保护点】
1.一种小型无人机自主导航定位方法，其特征在于，该方法包括如下步骤：步骤S1：对无人机SLAM系统建模；1.1地图模型采用特征地图来构建环境地图模型，将环境中的目标用几何原型来描述；1.2坐标系统由于无人机所携带的传感器与环境特征之间的感知行为发生在传感器坐标系中，速度的测量发生在无人机运动坐标系中，而最终环境地图的构建需要表示在全局地图中，所以建立全局坐标系，无人机运动坐标系及传感器坐标系，并表明他们之间的关系，定义GXGYGZG为全局坐标系，G为地心，XG指向正北，YG指向正东，ZG垂直于XGYG平面并指向G；定义AXAYAZA为无人机运动坐标系，A为无人机质心，XA指向机头，YA垂直于机头，ZA垂直于XAYA平面并指向A；定义SXSYSZS为环境感知传感器坐标系，S为传感器质心，XS指向传感器电磁波发射方向，YS垂直于电磁波发射方向，ZS垂直于XSYS平面并指向S；三个坐标系均符合右手定则；1.3特征模型根据公式(1)，定义特征的运动学模型为：

【技术特征摘要】
1.一种小型无人机自主导航定位方法，其特征在于，该方法包括如下步骤：步骤S1：对无人机SLAM系统建模；1.1地图模型采用特征地图来构建环境地图模型，将环境中的目标用几何原型来描述；1.2坐标系统由于无人机所携带的传感器与环境特征之间的感知行为发生在传感器坐标系中，速度的测量发生在无人机运动坐标系中，而最终环境地图的构建需要表示在全局地图中，所以建立全局坐标系，无人机运动坐标系及传感器坐标系，并表明他们之间的关系，定义GXGYGZG为全局坐标系，G为地心，XG指向正北，YG指向正东，ZG垂直于XGYG平面并指向G；定义AXAYAZA为无人机运动坐标系，A为无人机质心，XA指向机头，YA垂直于机头，ZA垂直于XAYA平面并指向A；定义SXSYSZS为环境感知传感器坐标系，S为传感器质心，XS指向传感器电磁波发射方向，YS垂直于电磁波发射方向，ZS垂直于XSYS平面并指向S；三个坐标系均符合右手定则；1.3特征模型根据公式(1)，定义特征的运动学模型为：其中，k为离散时间变量；1.4无人机运动模型通过建立一个四自由度的常速运动学模型来描述无人机的运动变换趋势，如公式(2)所示：XA(k)＝f(XA(k-1),n(k-1))(2)无人机XA＝[xyzψuvwr]T，[x,y,z,ψ]为全局坐标系中无人机的位置和艏向，[u,v,w,r]表示运动坐标系A中相应的XA、YA、ZA方向的线速度和航向角速度，n＝[nu,nv,nw,nr]表示以加速度的形式作用在速度上的高斯噪声；根据特征状态和无人机状态XA可以获得SLAM系统的状态X，包括无人机的状态XA和特征的状态Xf：X＝[XAXf]T(3)1.5传感器测量模型(1)速度测量模型速度测量传感器可提供X、Y、Z三个方向的速度，测量模型为：ZV＝HVX+sV(4)其中，ZV为速度的测量值，sV为速度测量噪声；(2)高度测量模型通过高度测量传感器可以获得无人机的高度，测量模型为：ZH＝HHX+sH(5)其中，ZH为高度的测量值，sH为高度测量噪声；(3)航向测量模型通过艏向测量传感器可以获得无人机的航向，测量模型为：ZC＝HCX+sC(6)其中，ZC为航向的测量值，sC为航向测量噪声...

【专利技术属性】
技术研发人员：王晶，所玉君，崔建飞，刘振业，
申请(专利权)人：天津津航计算技术研究所，
类型：发明
国别省市：天津,12