CN106950976B - 基于卡尔曼和粒子滤波的室内飞艇三维定位装置及方法 - Google Patents

基于卡尔曼和粒子滤波的室内飞艇三维定位装置及方法 Download PDF

Info

Publication number
CN106950976B
CN106950976B CN201710113867.1A CN201710113867A CN106950976B CN 106950976 B CN106950976 B CN 106950976B CN 201710113867 A CN201710113867 A CN 201710113867A CN 106950976 B CN106950976 B CN 106950976B
Authority
CN
China
Prior art keywords
mea
airship
posterior
platform
indoor
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201710113867.1A
Other languages
English (en)
Other versions
CN106950976A (zh
Inventor
赵磊
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Infinite Space Technology Co ltd
Original Assignee
Beijing Infinite Space Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Infinite Space Technology Co ltd filed Critical Beijing Infinite Space Technology Co ltd
Priority to CN201710113867.1A priority Critical patent/CN106950976B/zh
Publication of CN106950976A publication Critical patent/CN106950976A/zh
Application granted granted Critical
Publication of CN106950976B publication Critical patent/CN106950976B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
    • G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
    • G01S15/06—Systems determining the position data of a target
    • G01S15/08—Systems for measuring distance only
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/08—Systems determining position data of a target for measuring distance only
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/10—Simultaneous control of position or course in three dimensions
    • G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Electromagnetism (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Acoustics & Sound (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
  • Navigation (AREA)

Abstract

本发明提出了基于卡尔曼和粒子滤波的室内飞艇三维定位装置和方法，该装置包括飞艇平台、飞行预估模块、激光测高模块、超声波测距模块、飞行管理与控制计算机，其中，飞行管理与控制计算机包括相互连接的卡尔曼滤波器和粒子滤波器。该方法包括采用卡尔曼滤波器对飞艇平台的高度测量值进行卡尔曼滤波并输出卡尔曼滤波高度估计值，采用粒子滤波器基于空间距离、卡尔曼滤波高度估计值、速度测量值和姿态测量值并且采用粒子滤波算法获得粒子滤波水平位置坐标值。通过本发明的装置和方法，获得了更加精确的室内定位结果。

Description

基于卡尔曼和粒子滤波的室内飞艇三维定位装置及方法
技术领域
本发明涉及无人机室内导航定位技术领域，具体涉及一种基于卡尔曼和粒子滤波的室内飞艇三维定位装置及方法。
背景技术
在室内立体监测，尤其在大型仓库管理上，人们目前越来越多地采用可低速飞行以及自主悬停的飞行器来完成任务。然而，类似于直升机、多旋翼等传统低速无人机存在系统架构复杂、续航时间差等缺点，无法执行长时间连续监察任务。而诸如CN201120573237.0所公开的小型室内飞艇，基于其浮空特性，可很好的解决上述问题。
目前主要的室内定位技术有红外技术、射频识别技术、超宽带技术、光跟踪技术以及超声波技术。由于采用不同的定位技术直接影响到定位系统的硬件水平以及导航算法的构建，因而人们研究了很多不同的无人机室内导航定位方案。目前应用较成熟的定位产品，其设计多采用超声波技术，其优点一是成本低、安装使用方便，二是不受光线、烟雾影响，并且时间信息直观。对于大型仓库管理，该技术具有明显的优势。
目前，许多学者都在探索如何将超声波技术与室内定位技术结合起来(例如，请参见CN201410330642.8)，制作一种基于超声波测距的室内无人机导航定位系统，但受制于超声波精度低，测量噪声大的限制，其在具体的算法设计、器件布局以及技术实现等方面缺乏有效统一，定位效果并不理想。
而且，传统的卡尔曼滤波算法结构简单，计算量小，但只适用于高斯噪声环境，对于非高斯噪声环境、非线性化程度高的场合，其状态估计偏差大，容易产生发散。尽管粒子滤波算法不受非线性程度与高斯噪声假定的限制，但其计算量大，对于大范围三维空间环境，需要大量粒子估计才能实现系统状态收敛，因而其计算效率低，系统实时性差。
因此，对于基于超声波的大型仓库定位这种非线性强，立体空间尺寸大的场合，传统单一滤波算法难以满足人们的需求，现实需要性能更好、效率更高的基于超声波的室内定位算法(例如，请参见CN201410191921.0)。为了在大的室内环境下获得更加精确的室内定位结果，需要提出新的技术方案。
发明内容
本发明的目的是通过以下技术方案实现的。
本发明提出了一种基于卡尔曼和粒子滤波的室内飞艇三维定位装置，包括：飞艇平台、飞行预估模块、激光测高模块、超声波测距模块、飞行管理与控制计算机。
根据本发明的室内飞艇三维定位装置，其中的飞行预估模块被安装在飞艇平台的顶部，且包括三轴加速度计、三轴陀螺仪、三轴磁罗盘，分别用于采集飞艇平台的速度测量值(umea,vmea,wmea)、姿态测量值(φmea,θmea,ψmea)和航向测量值；其中的激光测高模块被安装在飞艇平台的正下方，且包括激光雷达和云台，用于依据激光测距的原理测量并输出飞艇平台的高度测量值hmea；其中的超声波测距模块包括安装在飞艇平台的头部偏下位置的接收标签和飞艇平台增益天线、安装在位于飞艇平台下方且附近无障碍物的四个固定室内位置的发射基站和四个基站增益天线，用于获取并输出飞艇平台与四个固定室内位置的发射基站之间的空间距离l1、l2、l3、l4；其中的飞行管理与控制计算机被安装在飞艇平台的顶部，且包括相互连接的卡尔曼滤波器和粒子滤波器，其中的卡尔曼滤波器对飞艇平台的高度测量值hmea进行卡尔曼滤波并输出卡尔曼滤波高度估计值hposterior，其中的粒子滤波器基于空间距离l1、l2、l3、l4、卡尔曼滤波高度估计值hposterior、速度测量值(umea,vmea,wmea)和姿态测量值(φmea,θmea,ψmea)并且采用粒子滤波算法获得粒子滤波水平位置坐标值(xposterior,yposterior)，从而得到飞艇平台的位置定位的精确估计值(xposterior,yposterior,hposterior)。
本发明还提出了一种基于卡尔曼和粒子滤波的室内飞艇三维定位方法，其是采用上文所述的室内飞艇三维定位装置实现的，该方法包括：飞行预估模块采集并输出飞艇平台的速度测量值(umea,vmea,wmea)、姿态测量值(φmea,θmea,ψmea)和航向测量值；激光测高模块依据激光测距的原理测量并输出飞艇平台的高度测量值hmea；超声波测距模块获取并输出飞艇平台与四个固定室内位置的发射基站之间的空间距离l1、l2、l3、l4；以及，飞行管理与控制计算机中的卡尔曼滤波器对飞艇平台的高度测量值hmea进行卡尔曼滤波并输出卡尔曼滤波高度估计值hposterior，其中的粒子滤波器基于空间距离l1、l2、l3、l4、卡尔曼滤波高度估计值hposterior、速度测量值(umea,vmea,wmea)和姿态测量值(φmea,θmea,ψmea)并且采用粒子滤波算法获得粒子滤波水平位置坐标值(xposterior,yposterior)，从而得到飞艇平台的位置定位的精确估计值(xposterior,yposterior,hposterior)。
根据本发明的室内飞艇三维定位方法，其中，超声波测距模块获取并输出飞艇平台与四个固定室内位置的发射基站之间的空间距离l1、l2、l3、l4包括以下步骤：
超声波测距模块的四个发射基站通过四个基站增益天线发射超声波，接收标签和飞艇平台增益天线分别接收四个基站增益天线发射的超声波信号，获取飞艇平台与四个固定室内位置的发射基站之间的空间距离l1、l2、l3、l4；以及
输出飞艇平台与四个固定室内位置的发射基站之间的空间距离l1、l2、l3、l4。
本发明的优点在于：在所提出的室内飞艇三维定位装置和方法中结合了传统的卡尔曼滤波算法和粒子滤波算法，实现了(1)室内飞艇三维导航定位架构的构建；(2)通过卡尔曼滤波算法完成飞艇高度的更加精确的估计；(3)基于超声波测距结果和更加精确的卡尔曼高度估计，进一步通过粒子滤波算法完成室内飞艇的水平位置的更加精确的估计；(4)整合高度估计和位置估计结果，更精确地完成室内飞艇的三维定位任务。
附图说明
通过阅读下文具体实施方式的详细描述，各种其他的优点和益处对于本领域普通技术人员将变得清楚明了。附图仅用于示出具体实施方式的目的，而并不认为是对本发明的限制。而且在整个附图中，用相同的参考符号表示相同的部件。在附图中：
图1是根据本发明的基于卡尔曼和粒子滤波的室内飞艇三维定位装置的示意图。
图2是根据本发明的室内飞艇三维定位装置中的飞行预估模块与飞行管理与控制计算机的局部放大示意图。
图3是根据本发明的室内飞艇三维定位装置中的激光测高模块的局部放大示意图。
图4是根据本发明的室内飞艇三维定位装置中的系统运行原理图。
图5是根据本发明的室内飞艇三维定位装置中的多传感器信息采集融合系统的结构框图。
具体实施方式
下面将参照附图更详细地描述本发明的示例性实施方式。虽然附图中显示了本发明的示例性实施方式，然而应当理解，可以以各种形式实现本发明而不应被这里阐述的实施方式所限制。相反，提供这些实施方式是为了能够更透彻地理解本发明，并且能够将本发明的范围完整的传达给本领域的技术人员。
图1是根据本发明的基于卡尔曼和粒子滤波的室内飞艇三维定位装置的示意图。
如图1所示，所提出的室内飞艇三维定位装置包括飞艇平台1、飞行预估模块2、激光测高模块3、超声波测距模块4和飞行管理与控制计算机5。
飞艇平台1是整个室内飞艇的任务平台，可根据任务需求搭载相应的机载任务模块，如机械手、检测器、摄像机等。飞行预估模块2、激光测高模块3、超声波测距模块4中的接收标签405和飞艇平台增益天线410、飞行管理与控制计算机5都安装在飞艇平台1上。飞行预估模块2和飞行管理与控制计算机5一同安装在飞艇平台1顶部，激光测高模块3安装在飞艇平台1正下方，超声波测距模块4中的接收标签405和飞艇平台增益天线410安装在飞艇平台1头部偏下的位置。而超声波测距模块4中的四个发射基站401、402、403、404和增益天线406、407、408、409安装在室内的四个固定点，并保证其附近无障碍物，且位于飞艇平台1下方。
图2是根据本发明的室内飞艇三维定位装置中的飞行预估模块与飞行管理与控制计算机的局部放大示意图。
如图2所示，飞行预估模块2，其安装在飞艇平台1的顶部，且包括三轴加速度计201、三轴陀螺仪202、三轴磁罗盘203，分别用于采集飞艇平台1的速度测量值(umea,vmea,wmea)、姿态测量值(φmea,θmea,ψmea)和航向测量值，并传递给粒子滤波器502。由于室内没有高精度GPS信号的校准，上述传感器获取的飞艇平台状态测量值是噪声高、漂移大的不精确数据，需要后面介绍的卡尔曼-粒子组合滤波算法进行后验校准。如图2所示，飞行管理与控制计算机5包括卡尔曼滤波器501和粒子滤波器502。
图3是根据本发明的室内飞艇三维定位装置中的激光测高模块的局部放大示意图。
如图3所示，激光测高模块3，其安装在飞艇平台1的正下方，且包括激光雷达301和云台302，通过三自由度云台302保证激光雷达301在整个飞行过程中始终正对地面，且依据激光测距的原理测量并输出飞艇平台1的高度测量值hmea，并传递给卡尔曼滤波器501。由于飞艇平台1自身震动以及传感器的固有误差，该数据无法直接用于飞艇平台1的定位，卡尔曼滤波器501通过KF算法，完成对飞艇平台1的高度的先验预测与后验估计，得出飞艇平台1的高度的精确估计值hposterior。
如图3所示，超声波测距模块4中的接收标签405和飞艇平台增益天线410分别接收四个基站增益天线406、407、408、409发射的超声波信号，从而获取飞艇平台1与四个固定室内位置的发射基站401、402、403、404之间的空间距离l1、l2、l3、l4。
图4是根据本发明的室内飞艇三维定位装置中的系统运行的原理图，包括粒子滤波的原理。
如图4所示，飞行管理与控制计算机5所包括的卡尔曼滤波器501和粒子滤波器502相互连接。卡尔曼滤波器501对飞艇平台1的高度测量值hmea进行卡尔曼滤波并向粒子滤波器502输出卡尔曼滤波高度估计值hposterior。粒子滤波器502基于空间距离l1、l2、l3、l4、卡尔曼滤波高度估计值hposterior、速度测量值(umea,vmea,wmea)(作为图4中的先验预测值(uprior，vprior，wprior))和姿态测量值(φmea,θmea,ψmea)(作为图4中的先验预测值(φprior，θprior，Ψprior))并且采用粒子滤波算法获得粒子滤波水平位置坐标值(xposterior,yposterior)，从而得到飞艇平台1的位置定位的精确估计值(xposterior,yposterior,hposterior)(相当于图4中的xposterior，yposterior，zposterior))。
可选择地，卡尔曼滤波器501将计算出的飞艇高度的精确估计值hposterior传递给粒子滤波器502。当飞行预估模块2获取的飞艇平台1的测量状态值(umea,vmea,wmea)、(φmea,θmea,ψmea)传递给粒子滤波器502后，需要通过其他数据信息对该数据进行后验校准，得出飞艇平台1位置的精确估计值(xposterior,yposterior,zposterior)。具体操作原理如下：超声波测距模块4的四个发射基站401、402、403、404通过增益天线406、407、408、409发射超声波，安装在飞艇平台1头部偏下的接收标签405结合增益天线410分别接收来自四个发射基站401、402、403、404发出的超声波信号，获取飞艇平台1与四个基站的空间距离l1、l2、l3、l4，并发送给粒子滤波器502，粒子滤波器502通过l1、l2、l3、l4以及hposterior对(umea,vmea,wmea)、(φmea,θmea,ψmea)进行后验校准，通过PF算法计算出飞艇平台1的精确估计位置(xposterior,yposterior,zposterior)。
图5是根据本发明的室内飞艇三维定位装置中的多传感器信息采集融合系统的结构框图。
如图5所示，根据本发明的室内飞艇三维定位装置中的多传感器接收部分采用数字信号处理器DSP作为飞行管理与控制计算机5，采用德州仪器TMS320C6713芯片，多通道串口扩展板由两片四通道扩展芯片TL16C554A和一些辅助器件组成。处理器内嵌有传感器驱动模块、超声波信号处理模块、传感器数据融合模块、融合算法模块，通过片上接口与多通道串口扩展板串口与机载激光雷达301连接。其中，三轴加速度计201、三轴陀螺仪202通过I2C总线与DSP连接；三轴磁罗盘203通过A/D转换接口与DSP连接；激光雷达301与超声波接收标签405通过串口与多通道扩展板连接。各类传感器信息通过相应的接口与多通道串口扩展板的串口被传输至多传感器信息采集融合系统，一方面将数据信息采样封装，另一方面通过卡尔曼-粒子组合滤波算法，生成飞艇定位的重要数据，数据通过CAN总线传送至智能决策系统。
本发明还提出了一种基于卡尔曼和粒子滤波的室内飞艇三维定位方法，该方法采用上文所述的室内飞艇三维定位装置，下面将详细介绍。在这里首先给出相关符号说明(注：本算法基于国际通用的惯性坐标系与机体坐标系)：
(umea,vmea,wmea)：飞行预估模块2采集的速度测量值；
(φmea,θmea,ψmea)：飞行预估模块2采集的姿态测量值；
hmea：激光雷达301采集的高度测量值；
dt：两次采样时间间隔；
(xbi,ybi,zbi)(i＝1,2,3,4)：分别为超声波测距模块4中的四个发射基站(401、402、403、404)在室内空间的位置；
li(i＝1,2,3,4)：分别为超声波测距模块4中的接收标签405采集的飞艇与四个发射基站(401、402、403、404)直线距离；
Figure BDA0001235126930000071
KF状态向量；
Z＝hpiror：KF测量向量；
P：KF状态协方差矩阵；
A：KF状态转移矩阵；
Q：KF系统噪声对角阵；
H：KF测量矩阵；
R：测量噪声对角阵；
K：卡尔曼增益；
hprior：飞艇高度先验预测值；
hposterior：飞艇高度后验估计值；
(xprior,yprior)：飞艇水平位置先验预测值；
(xposterior,yposterior)：飞艇水平位置后验估计值；
N：PF粒子数；
na：PF系统高斯噪声；
ba：PF系统漂移；
Figure BDA0001235126930000081
P粒子先验观测值；
ωn：PF粒子权重值。
根据本发明的基于卡尔曼和粒子滤波的室内飞艇三维定位方法包括以下步骤：
飞行预估模块2采集并输出飞艇平台1的速度测量值(umea,vmea,wmea)和姿态测量值(φmea,θmea,ψmea)；激光测高模块3依据激光测距的原理测量并输出飞艇平台1的高度测量值hmea；超声波测距模块4获取并输出飞艇平台1与四个固定室内位置的发射基站之间的空间距离l1、l2、l3、l4；以及，飞行管理与控制计算机5中的卡尔曼滤波器对飞艇平台1的高度测量值hmea进行卡尔曼滤波并输出卡尔曼滤波高度估计值hposterior，其中的粒子滤波器基于空间距离l1、l2、l3、l4、卡尔曼滤波高度估计值hposterior、速度测量值(umea,vmea,wmea)和姿态测量值(φmea,θmea,ψmea)并且采用粒子滤波算法获得粒子滤波水平位置坐标值(xposterior,yposterior)，从而得到飞艇平台1的位置定位的精确估计值(xposterior,yposterior,hposterior)。
其中，超声波测距模块4获取并输出飞艇平台1与四个固定室内位置的发射基站之间的空间距离l1、l2、l3、l4包括以下步骤：
超声波测距模块4的四个发射基站通过四个基站增益天线发射超声波，接收标签和飞艇平台增益天线分别接收四个基站增益天线发射的超声波信号，获取飞艇平台1与四个固定室内位置的发射基站之间的空间距离l1、l2、l3、l4；以及
输出飞艇平台1与四个固定室内位置的发射基站之间的空间距离l1、l2、l3、l4。
其中，粒子滤波器基于空间距离l1、l2、l3、l4、卡尔曼滤波高度估计值hposterior、速度测量值(umea,vmea,wmea)和姿态测量值(φmea,θmea,ψmea)并且采用粒子滤波算法获得粒子滤波水平位置坐标值(xposterior,yposterior)包括以下步骤：
在整个室内水平面尺寸范围内随机生成N个离散粒子(x1,y1)，(x2,y2)…，(xN,yN)，每个离散粒子的位置坐标值(xi,yi)(i＝1～N)均为室内范围的随机值，用于估计飞艇平台1的水平位置坐标值；
搜集粒子滤波算法所需的以下状态输入，并完成采样封装：飞行预估模块2采集并输出的飞艇平台1的速度测量值(umea,vmea,wmea)和姿态测量值(φmea,θmea,ψmea)、超声波测距模块4获取并输出的空间距离l1、l2、l3、l4、以及卡尔曼滤波高度估计值hposterior；
计算每个离散粒子的虚拟运动量，使用飞行预估模块2采集并输出的飞艇平台1的速度测量值(umea,vmea,wmea)和姿态测量值(φmea,θmea,ψmea)根据下式计算飞艇平台1的先验速度，其中，s(.)表示sin(.)，c(.)表示cos(.)，
Figure BDA0001235126930000091
通过以下状态方程对下一时刻的离散粒子进行预测，得到离散粒子先验预测值
Figure BDA0001235126930000092
其中na、ba分别是随机设定的系统高斯噪声和漂移，
Figure BDA0001235126930000093
并且结合hposterior，使用下列公式求出每个离散粒子的先验距离观测值
Figure BDA0001235126930000094
结合超声波测距模块4获取并输出的空间距离l1、l2、l3、l4使用下式计算每个离散粒子的权重值，
Figure BDA0001235126930000101
并且使用下式归一化权重值，
Figure BDA0001235126930000102
使用下式获得粒子滤波水平位置坐标值(xposterior,yposterior)，
Figure BDA0001235126930000103
可假定总的离散粒子数N一定，每个离散粒子在下一时刻出现的概率为归一化权重值ωn，按照归一化权重值ωn的大小重新选择离散粒子，得到所有离散粒子的新位置坐标值后从第二个步骤开始重复。为了使本领域的技术人员更好地理解本发明，在一个具体实施例中，给出了整个卡尔曼-粒子组合滤波定位算法的示例性步骤：
步骤1：通过卡尔曼滤波算法得出飞艇精确的高度估计值hposterior。该过程通过卡尔曼滤波器501完成。
1)构建状态向量
Figure BDA0001235126930000104
以及测量向量Z＝hmea(作为图4中的飞艇高度先验预测值hprior)，状态协方差矩阵P，并根据实际情况给定X初始值X0，P初始值P0。
2)状态先验预测，构建状态转移矩阵A以及系统噪声对角阵Q，
Figure BDA0001235126930000105
其中
Figure BDA0001235126930000106
表示噪声分布协方差，根据飞艇自身运动状态而定。
计算状态先验预测值Xprior以及误差协方差矩阵的先验预测值Pprior：Xprior＝AXposterior，Pprior＝APposteriorAT+Q。第一次迭代时，Xposterior＝X0且Pposterior＝P0。
3)后验测量校准
构建测量矩阵H以及测量噪声对角阵R，
H＝[1,0]，
Figure BDA0001235126930000111
其中
Figure BDA0001235126930000112
表示噪声分布协方差，根据激光雷达301特性与飞艇运动状态而定。
计算卡尔曼增益K：K＝PpriorHT(HPpriorHT+R)-1。
完成系统状态的后验估计
Xposterior＝Xprior+K(Z-HXprior)。
完成误差协方差矩阵的更新
Pposterior＝(I-KH)Pprior。
4)每完成一次后验校准计算后，滤波器得出接近真值的飞艇高度后验估计值hposterior(Xposterior第一项)以及高度变化率
Figure BDA0001235126930000113
(Xposterior第二项)。将Xposterior传递给粒子滤波器502，用于下一步PF算法中。同时，每当激光雷达301获取新的高度测量值hmea时，重新执行上述2)。
步骤2：通过粒子滤波算法计算出飞艇精确的水平位置，该过程在粒子滤波器502中完成。
1)在整个室内水平面尺寸范围内随机生成N个离散粒子
(x1,y1)，(x2,y2)…，(xN,yN)，每个离散粒子的位置坐标值(xi,yi)均为室内范围的随机值，用于估计飞艇水平位置。
2)搜集PF算法所需的所有状态输入，并完成采样封装，具体包括：飞行预估模块2采集的速度测量值(umea,vmea,wmea)、和姿态测量值(φmea,θmea,ψmea)，超声波测距模块4获取的距离数据l1、l2、l3、l4以及步骤1计算出的飞艇高度后验估计值hposterior。
3)计算每个离散粒子的虚拟运动量，根据飞行预估模块2测量数据计算飞艇平台1的先验速度，其中，s(.)表示sin(.)，c(.)表示cos(.)。
Figure BDA0001235126930000121
4)通过状态方程对下一时刻粒子进行预测，得到粒子先验预测值
Figure BDA0001235126930000122
其中na、ba分别是随机设定的系统高斯噪声和漂移。
Figure BDA0001235126930000123
结合hposterior，求出每个离散粒子的先验观测值
Figure BDA0001235126930000124
5)结合超声波测距仪采集的数据计算每个离散粒子的权重值，
Figure BDA0001235126930000125
归一化权重值，
Figure BDA0001235126930000126
6)根据每个离散粒子的权重值ωn进行重采样。总粒子数一定，按照权重值的大小重新选择粒子，每个离散粒子在下一时刻出现的概率为ωn。得到粒子位置后验更新值
Figure BDA0001235126930000127
以及ωn。
7)对飞艇水平位置进行后验估计，得到粒子滤波结果(xposterior,yposterior)，
Figure BDA0001235126930000128
8)结合步骤1得出的hposterior，得出室内飞艇在立体空间的精确估计值(xposterior,yposterior,hposterior)，进而完成室内飞艇的定位任务，当粒子滤波器502接收到下一组采集数据时，重新迭代2)至8)，实现对飞艇位置的实时定位功能。
与先前技术相比，本发明可对完成室内飞艇大范围精确三维定位，显著提高飞艇的室内导航范围，增强立体监测效果，大大增加连续无人监测时间，同时简化导航装置，降低设备成本，提高定位精度，简化了调控时的计算与编程，使得定位的效果不受空间尺寸、仓库操作的影响，导航效果突出、算法简单高效、抗扰能力强大。
以上所述，仅为本发明示例性的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。

Claims (3)

1.一种基于卡尔曼和粒子滤波的室内飞艇三维定位装置，其特征在于，包括：飞艇平台(1)、飞行预估模块(2)、激光测高模块(3)、超声波测距模块(4)、飞行管理与控制计算机(5)，其特征在于，
飞行预估模块(2)，其安装在飞艇平台(1)的顶部，且包括三轴加速度计(201)、三轴陀螺仪(202)、三轴磁罗盘(203)，分别用于采集飞艇平台(1)的速度测量值(umea,vmea,wmea)、姿态测量值(φmea,θmea,ψmea)；
激光测高模块(3)，其安装在飞艇平台(1)的正下方，且包括激光雷达(301)和云台(302)，用于依据激光测距的原理测量并输出飞艇平台(1)的高度测量值hmea；
超声波测距模块(4)，其包括安装在飞艇平台(1)的头部偏下位置的接收标签(405)和飞艇平台增益天线(410)、安装在位于飞艇平台(1)下方且附近无障碍物的四个固定室内位置的发射基站(401、402、403、404)和四个基站增益天线(406、407、408、409)，用于获取并输出飞艇平台(1)与四个固定室内位置的发射基站(401、402、403、404)之间的空间距离l1、l2、l3、l4；
飞行管理与控制计算机(5)，其安装在飞艇平台(1)的顶部，且包括相互连接的卡尔曼滤波器(501)和粒子滤波器(502)，其中的卡尔曼滤波器(501)对飞艇平台(1)的高度测量值hmea进行卡尔曼滤波并输出卡尔曼滤波高度估计值hposterior，其中的粒子滤波器(502)基于空间距离l1、l2、l3、l4、卡尔曼滤波高度估计值hposterior、速度测量值(umea,vmea,wmea)和姿态测量值(φmea,θmea,ψmea)并且采用粒子滤波算法获得粒子滤波水平位置坐标值(xposterior,yposterior)，从而得到飞艇平台(1)的位置定位的精确估计值(xposterior,yposterior,hposterior)。
2.一种基于卡尔曼和粒子滤波的室内飞艇三维定位方法，其是采用权利要求1所述的室内飞艇三维定位装置实现的，所述方法包括：
飞行预估模块(2)采集并输出飞艇平台(1)的速度测量值(umea,vmea,wmea)、姿态测量值(φmea,θmea,ψmea)和航向测量值；
激光测高模块(3)依据激光测距的原理测量并输出飞艇平台(1)的高度测量值hmea；
超声波测距模块(4)获取并输出飞艇平台(1)与四个固定室内位置的发射基站(401、402、403、404)之间的空间距离l1、l2、l3、l4；以及
飞行管理与控制计算机(5)中的卡尔曼滤波器(501)对飞艇平台(1)的高度测量值hmea进行卡尔曼滤波并输出卡尔曼滤波高度估计值hposterior，其中的粒子滤波器(502)基于空间距离l1、l2、l3、l4、卡尔曼滤波高度估计值hposterior、速度测量值(umea,vmea,wmea)和姿态测量值(φmea,θmea,ψmea)并且采用粒子滤波算法获得粒子滤波水平位置坐标值(xposterior,yposterior)，从而得到飞艇平台(1)的位置定位的精确估计值(xposterior,yposterior,hposterior)。
3.根据权利要求2所述的室内飞艇三维定位方法，其特征在于，超声波测距模块(4)获取并输出飞艇平台(1)与四个固定室内位置的发射基站(401、402、403、404)之间的空间距离l1、l2、l3、l4包括以下步骤：
超声波测距模块(4)的四个发射基站(401、402、403、404)通过四个基站增益天线(406、407、408、409)发射超声波，接收标签(405)和飞艇平台增益天线(410)分别接收四个基站增益天线(406、407、408、409)发射的超声波信号，获取飞艇平台(1)与四个固定室内位置的发射基站(401、402、403、404)之间的空间距离l1、l2、l3、l4；以及
输出飞艇平台(1)与四个固定室内位置的发射基站(401、402、403、404)之间的空间距离l1、l2、l3、l4。
CN201710113867.1A 2017-02-28 2017-02-28 基于卡尔曼和粒子滤波的室内飞艇三维定位装置及方法 Active CN106950976B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710113867.1A CN106950976B (zh) 2017-02-28 2017-02-28 基于卡尔曼和粒子滤波的室内飞艇三维定位装置及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710113867.1A CN106950976B (zh) 2017-02-28 2017-02-28 基于卡尔曼和粒子滤波的室内飞艇三维定位装置及方法

Publications (2)

Publication Number Publication Date
CN106950976A CN106950976A (zh) 2017-07-14
CN106950976B true CN106950976B (zh) 2020-04-03

Family

ID=59467070

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710113867.1A Active CN106950976B (zh) 2017-02-28 2017-02-28 基于卡尔曼和粒子滤波的室内飞艇三维定位装置及方法

Country Status (1)

Country Link
CN (1) CN106950976B (zh)

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107976683B (zh) * 2017-11-24 2021-09-03 中国人民解放军63653部队 一种用激光雷达实时测量艇库内飞艇顶部离地高度的方法
EP3637214A4 (en) * 2017-12-18 2020-12-23 SZ DJI Technology Co., Ltd. CONTROL PROCESS OF UNPILOT AIR VEHICLE AND UNPILOT AIR VEHICLE
CN108427431A (zh) * 2018-04-24 2018-08-21 东莞理工学院 一种基于激光扫描地图构建系统的四轴飞行器及其方法
CN110673627A (zh) * 2019-09-16 2020-01-10 广东工业大学 一种森林无人机搜寻方法
CN113029158B (zh) * 2021-04-26 2023-09-22 常州大学 基于激光与声音融合定位的旋翼飞行器及其定位方法
CN114814727B (zh) * 2021-12-29 2024-05-10 山东科技大学 一种高定位精度和抗干扰能力的超宽带三维定位方法
CN115586531B (zh) * 2022-09-23 2026-02-27 湖南三一智能控制设备有限公司 超声测距信号的处理方法、系统、车辆及电子设备
CN119556276B (zh) * 2025-01-27 2025-03-28 北京瑞达恩科技股份有限公司 基于多目标跟踪的无人机侦测用相控阵雷达

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102818567A (zh) * 2012-08-08 2012-12-12 浙江大学 集合卡尔曼滤波－粒子滤波相结合的auv组合导航方法
JP2015060431A (ja) * 2013-09-19 2015-03-30 株式会社Ihi センサの指向制御方法と装置
CN105023277A (zh) * 2014-04-15 2015-11-04 南京理工大学 基于复杂动态场景的卡尔曼粒子滤波跟踪方法
EP3018447A1 (fr) * 2014-11-07 2016-05-11 Thales Procédé de détermination d'un rayon de protection associé à un paramètre de navigation d'un système hybride de navigation inertielle, et système associé
CN105704071A (zh) * 2015-07-07 2016-06-22 大连大学 基于信息序列的自适应渐消扩展卡尔曼粒子滤波的多普勒频移估计方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105573339B (zh) * 2016-01-16 2019-01-11 深圳先进技术研究院 一种基于旋翼飞艇的导航飞行系统
CN105718549A (zh) * 2016-01-16 2016-06-29 深圳先进技术研究院 一种基于飞艇的三维WiFi指纹制图系统和方法
CN105572638A (zh) * 2016-02-06 2016-05-11 向杰 一种基于惯性姿态与超声波测距的三维定位方法及装置
CN106017463B (zh) * 2016-05-26 2019-02-26 浙江大学 一种基于定位传感装置的飞行器定位方法
CN205971851U (zh) * 2016-08-09 2017-02-22 上海禾赛光电科技有限公司 具有自定位功能的无人机及气体遥测系统

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102818567A (zh) * 2012-08-08 2012-12-12 浙江大学 集合卡尔曼滤波－粒子滤波相结合的auv组合导航方法
JP2015060431A (ja) * 2013-09-19 2015-03-30 株式会社Ihi センサの指向制御方法と装置
CN105023277A (zh) * 2014-04-15 2015-11-04 南京理工大学 基于复杂动态场景的卡尔曼粒子滤波跟踪方法
EP3018447A1 (fr) * 2014-11-07 2016-05-11 Thales Procédé de détermination d'un rayon de protection associé à un paramètre de navigation d'un système hybride de navigation inertielle, et système associé
CN105704071A (zh) * 2015-07-07 2016-06-22 大连大学 基于信息序列的自适应渐消扩展卡尔曼粒子滤波的多普勒频移估计方法

Also Published As

Publication number Publication date
CN106950976A (zh) 2017-07-14

Similar Documents

Publication Publication Date Title
CN106950976B (zh) 基于卡尔曼和粒子滤波的室内飞艇三维定位装置及方法
US10914590B2 (en) Methods and systems for determining a state of an unmanned aerial vehicle
US10962655B2 (en) Systems and methods for positioning of UAV
CN106772493B (zh) 基于北斗差分定位的无人机航向测算系统及其测算方法
US10534068B2 (en) Localization system, vehicle control system, and methods thereof
CN105955302A (zh) 一种多旋翼无人机环境自主监测控制系统及方法
CN109945856A (zh) 基于惯性/雷达的无人机自主定位与建图方法
CN111983660A (zh) Gnss拒止环境下四旋翼无人机定位系统和方法
CN104820434A (zh) 一种无人机对地面运动目标的测速方法
CN113419235A (zh) 一种基于毫米波雷达的无人机定位方法
CN104316058B (zh) 一种采用交互多模型的移动机器人wsn/ins组合导航方法
WO2022000195A1 (zh) 作业方法、控制设备、作业无人机、系统及存储介质
CN110779496B (zh) 三维地图构建系统、方法、设备和存储介质
CN114035150A (zh) 一种基于无人机升空平台的无线电射频源测向装置及定位方法
CN110081882A (zh) 四旋翼无人机航向测量器及控制方法
CN113920186B (zh) 一种低空无人机多源融合定位方法
JP2019007964A (ja) Ｕａｖの位置を推定するシステム及び方法
CN105928512A (zh) 一种基于地磁场的室内定位方法
CN113721188B (zh) 拒止环境下的多无人机自身定位与目标定位方法
CN116929350A (zh) 一种基于数据链测距的快速临机重构协同导航系统及方法
CN113063401A (zh) 一种无人机航测系统
WO2022094962A1 (zh) 飞行器的悬停方法、飞行器及存储介质
CN212030555U (zh) 基于arinc429模拟器的自主导航与定位系统
CN111694006A (zh) 一种用于室内无人系统的导航传感系统
CN117452461A (zh) 基站位姿在线标定的视觉-惯性-单基站uwb融合导航方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
CB03 Change of inventor or designer information

Inventor after: Zhao Lei

Inventor after: Guo Ta

Inventor before: Zhao Lei

CB03 Change of inventor or designer information