一种基于自适应ekf的无人侦察机同步定位与构图方法
【技术领域】
[0001] 本发明属于无人机技术领域，具体涉及一种基于自适应EKF的无人侦察机同步定 位与构图方法。
【背景技术】
[0002] 现代战争中，战场环境复杂，及时掌握战场情况对取得战争胜利起着决定性的作 用。无人侦察机应运而生，其能够潜入敌区上空进行侦查，为作战指挥中心提供实时的敌区 图像和信息，使得指挥官及时掌握敌军情况，进而制定合理的作战计划。其中，小型无人侦 查机以其体积小、重量轻、造价低、无伤亡等优势已经成为目前研究的热点。无人侦察机在 执行任务过程中要求其具有续航时间长、导航精度高、不易被敌方发现等特性。其中高精度 的导航是无人侦察机成功完成任何任务的基本前提，因而恰当的导航方法是无人侦察机完 成任务成败的关键所在。
[0003] 导航是指无人侦察机机借助外部信号或通过自身携带的传感器判断自身位置，从 而指导其航行、完成预定任务的过程。目前无人侦察机的导航方法主要分为两种，非自主导 航以及自主导航。
[0004] 非自主导航方法主要是卫星导航，利用GPS、欧米茄、罗兰、北斗等，能够获得位置、 速度、时间等导航信息，且精度不随时间推移而降低。然而GPS无法提供无人侦察机的姿态 信息，此外，目前只有GPS是覆盖全球的，这就使得无人侦察机在执行大范围任务时难免受 制于人，并且应用环境的适应性较差，容易受到干扰。自主导航不依赖外界信号源，仅仅通 过自身携带的速度、姿态及环境感知传感器即可实现相对准确的导航，但是对于导航算法 的要求较高，是目前无人侦察机导航领域的研究重点及难点。
[0005] 自主导航方法主要有航位推算、视觉导航、地球物理导航、多普勒导航、惯性导航、 同步定位与构图等。航位推算具有较强的自主性、保密性及抗干扰性，但是其推算误差随着 时间迅速积累，不适合长航时无人侦察机导航；视觉导航具有独立性、准确性，但是只能获 得相对的运动状态信息；地球物理导航具有无源性、无辐射、隐蔽性强，但是需要大量的地 磁数据，实时性受计算机处理能力限制，且受地形，天气影响；多普勒导航自主性好、测速精 度高、抗干扰性强，但是其工作时必须发射电波，隐蔽性不好；惯性导航，精度高、隐蔽性好， 短期内具有很高的导航定位精度，但惯导系统初始对准较为困难，且造价比较高。
[0006] 同步定位与地图构建（SimultaneousLocalizationandMapping，SLAM)技术的 提出，一定程度上缓解了上述导航方法中存在的不足。SLAM是在无人侦察机自身位置不确 定，航位推算的过程中位置误差逐渐累积的情况下，利用自身携带的环境感知传感器反复 探测环境中特征，从而完成自身及特征位置的校正，同时构建环境地图，无需预知地形信息 或外部辅助定位设备即可以得到较为可靠的无人侦察机位置信息及环境地图信息，详细描 述如图1所示。
【发明内容】
[0007] (一）要解决的技术问题
[0008] 本发明要解决的技术问题是：如何将自适应滤波方法与SLAM方法相结合以解决 无人侦查机自主导航过程中存在的位置误差累计、环境适应性差、初始对准困难及隐蔽性 差等问题。
[0009] (二）技术方案
[0010] 为解决上述技术问题，本发明提供一种基于自适应EKF的无人侦察机同步定位与 构图方法，该方法包括如下步骤：
[0011] 步骤S1 :对无人侦察机SLAM系统建模；
[0012] 1.1地图模型
[0013] 采用特征地图来构建环境地图模型，将环境中的目标用几何原型来描述；
[0014] 1.2坐标系统
[0015] 由于无人侦察机所携带的传感器与环境特征之间的感知行为发生在传感器坐标 系中，速度的测量发生在无人侦察机运动坐标系中，而最终环境地图的构建需要表示在全 局地图中，所以建立全局坐标系，无人侦察机运动坐标系及传感器坐标系，并表明他们之间 的关系，定义全局坐标系，G为地心，X指向正北，Y指向正东，Z垂直于X平面 并指向G;定义AXaYaZa为无人侦察机运动坐标系，A为无人侦察机质心，XA指向机头， 直于机头，ΖΑ垂直于ΧΑΥΑ平面并指向A;定义SXsYsZs为环境感知传感器坐标系，S为传感器 质心，Xs指向传感器电磁波发射方向，Ys垂直于电磁波发射方向，Zs垂直于XSYS平面并指向 S;三个坐标系均符合右手定则；
[0016] 1.3特征模型
[0017] 根据公式（1)，定义特征的运动学模型为：
[0018] Xfi (k) =Xfi (k~l)i= 1···η (1)
[0019] 其中，k为离散时间变量；
[0020] 特征在传感器坐标系中一般以极坐标的形式表示：
[0021]
[0022] 其中，Xfl表示第i个特征的状态，即位置；η为特征的个数；
[0023] Pi为特征i在极坐标系下的极径；Θi为特征i在极坐标系下的极角；
[0024] 1. 4无人侦察机运动模型
[0025] 通过建立一个四自由度的常速运动学模型来描述无人侦察机的运动变换趋势，如 公式（2)所示：
[0026] XA (k) =f(XA (k-1),n(k~l)) (2)
[0027] 无人侦察机XA=[xyzΦuvwr]T，[x,y,z,Φ]为全局坐标系中无人侦察机 的位置和艏向，[u，v，w，r]表示运动坐标系A中相应的XA、YA、ZA方向的线速度和转艏角速 度，n= [nu,nv,nw,nj表示以加速度的形式作用在速度上的高斯噪声；
[0028] 根据特征状态Xfl和无人侦察机状态X&可以获得SLAM系统的状态X，包括无人侦 察机的状态XA和特征的状态Xf:
[0029] X=[XAXf]T ⑶
[0030]
[0031] 其中，T为采样时间；
[0032] 1. 5传感器测量模型
[0033] (1)速度测量模型
[0034] 速度测量传感器可提供X、Υ、Ζ三个方向的速度，测量模型为：
[0035] Zv=HvX+sv (4)
[0036] 其中，Zv为速度的测量值；
[0037] 测量矩阵比为：
[0038] Ηγ -[03X4Ι3Χ303Χ103Χ2η]
[0039] 其中，0为全零矩阵，I为单位矩阵；
[0040] 速度测量噪声sv的协方差矩阵为：
[0041]
[0042] 其中，cjI为速度u的方差，4，为速度v的方差，为速度w的方差；
[0043] (2)高度测量模型
[0044] 通过高度测量传感器可以获得无人侦察机的高度，测量模型为：
[0045] Zh=HhX+sh (5)
[0046] 其中，ZH为高度的测量值；
[0047] 测量矩阵Hh为：
[0048] Hh= [0 0 1 0 0 0 0 0 01X2n]
[0049] 高度测量噪声的sH协方差矩阵表示为：
[0050]
[0051] ⑶艏向测量模型
[0052] 通过艏向测量传感器可以获得无人侦察机的艏向，测量模型为：
[0053] Zc=HcX+sc (6)
[0054] 其中，Zc为艏向的测量值；
[0055] 测量矩阵氏为：
[0056] Hc= [0 00100000 1X2n]
[0057] 艏向测量噪声sc的协方差为：
[0058] Rc 二
[0059] (4)环境测量模型
[0060] 通过环境感知传感器可以获得环境中特征相对于无人侦查机的距离和方位，测量 模型为：
[0061]
：(J);
[0062] 其中，Sl为特征测量噪声；
[0063]
[0064] 测量矩阵为：
[0065]
[0066] 其中，/^和贫4是点特征在运动坐标系下的参数表示，pJPΘ1是点特征在全局 坐标系下的参数表示；
[0067