CN110487267B - 一种基于vio＆uwb松组合的无人机导航系统及方法 - Google Patents

一种基于vio＆uwb松组合的无人机导航系统及方法 Download PDF

Info

Publication number
CN110487267B
CN110487267B CN201910621591.7A CN201910621591A CN110487267B CN 110487267 B CN110487267 B CN 110487267B CN 201910621591 A CN201910621591 A CN 201910621591A CN 110487267 B CN110487267 B CN 110487267B
Authority
CN
China
Prior art keywords
unmanned aerial
aerial vehicle
navigation
information
coordinate system
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
CN201910621591.7A
Other languages
English (en)
Other versions
CN110487267A (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.)
Hunan Jiaogong Intelligent Technology Co ltd
Original Assignee
Hunan Jiaogong Intelligent 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 Hunan Jiaogong Intelligent Technology Co ltd filed Critical Hunan Jiaogong Intelligent Technology Co ltd
Priority to CN201910621591.7A priority Critical patent/CN110487267B/zh
Publication of CN110487267A publication Critical patent/CN110487267A/zh
Application granted granted Critical
Publication of CN110487267B publication Critical patent/CN110487267B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
    • G01C11/04—Interpretation of pictures
    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/46—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being of a radio-wave signal type
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • 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
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0257—Hybrid positioning
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04B—TRANSMISSION
    • H04B7/00—Radio transmission systems, i.e. using radiation field
    • H04B7/14—Relay systems
    • H04B7/15—Active relay systems
    • H04B7/185—Space-based or airborne stations; Stations for satellite systems
    • H04B7/18502—Airborne stations
    • H04B7/18506—Communications with or from aircraft, i.e. aeronautical mobile service

Abstract

本申请公开了一种基于VIO&UWB松组合的无人机导航系统及方法，该基于VIO&UWB松组合的无人机导航系统包括：主控电路、视觉传感器、UWB定位模块、姿态传感器以及组合滤波器，视觉传感器、UWB定位模块、姿态传感器、组合滤波器分别与主控电路信号连接；本申请结合各传感器间的互补性，综合利用了视觉、惯性导航、UWB等传感器的测量信息，能够实现稳定、可靠的导航功能，建立复杂环境下稳定可靠的导航系统，实现旋翼无人机在传统导航盲区作业飞行，同时运算量大大减小，可以在较低性能的计算平台上实现，满足低成本无人机的应用需求。

Description

一种基于VIO＆UWB松组合的无人机导航系统及方法
技术领域
本申请涉及无人机导航领域，尤其涉及一种基于VIO(Visual InertialOdometry，视觉惯性里程计)&UWB(Ultra Wide Band，超宽带)松组合的无人机导航系统及方法。
背景技术
现有的INS(Inertial Navigation System，惯性导航系统)/GPS(GlobalPositioning System，全球定位系统)组合导航技术，在GPS信号覆盖良好的区域能够保证高效、稳定的导航性能。但在诸如桥梁下方、隧道内、房屋内等应用场景下，GPS信号被极大的屏蔽，无法利用传统的INS/GPS组合导航技术实现有效的导航功能。而现有的即时定位与地图构建(SLAM技术)其计算复杂、对设备要求高，而旋翼无人机无法携带高性能的计算机实现该种技术。
发明内容
本发明主要解决的技术问题是提供一种基于VIO&UWB松组合的旋翼无人机导航系统及方法，利用相对有限的运算资源，实现视觉、惯性导航、UWB定位技术的组合导航。
为解决上述技术问题，本发明提供一种基于VIO&UWB松组合的旋翼无人机导航系统及方法，其中，无人机导航系统包括：主控电路、视觉传感器、UWB定位模块、姿态传感器以及组合滤波器，视觉传感器、UWB定位模块、姿态传感器、组合滤波器分别与主控电路信号连接；主控电路用于控制视觉传感器、UWB定位模块、姿态传感器、组合式滤波器；视觉传感器用于采集图像的特征点获取图像特征点深度信息，根据图像特征点深度信息计算得到无人机旋转和平移信息；UWB定位模块用于获取移动标签之间的位置信息；姿态传感器用于获取无人机的角速度和加速度信息；组合滤波器通过无人机旋转和平移信息、移动标签之间的位置信息以及无人机的角速度和加速度信息计算导航信息。
其中，视觉传感器包括双目相机。
其中，UWB定位模块包括基站和移动标签。
其中，述姿态传感器包括陀螺仪和加速度计。
其中，所述组合式滤波器包括集中式滤波器。
其中，组合式滤波器包括集中式滤波器。
为解决上述技术问题，本申请还提供了一种基于VIO&UWB松组合的旋翼无人机导航方法，包括：获取无人机位置信息；通过双目相机对周围环境进行拍照获取图像特征点信息，根据图像特征点信息进行重投影计算，得到无人机旋转和平移信息；获取无人机的角速度和加速度信息；根据无人机位置信息、无人机旋转和平移信息以及无人机的角速度和加速度信息计算得到导航信息。
其中，获取所述无人机位置信息的步骤包括：获取所述基站的位置信息；通过所述基站的位置信息计算得到所述无人机的位置信息。
其中，述获取所述基站的位置信息的步骤包括：
建立无人机坐标系b和导航坐标系n；
其中，通过双目相机对周围环境进行拍照获取图像特征点信息，根据图像特征点信息进行重投影计算，得到无人机旋转和平移信息的步骤包括：通过双目相机采集左右两个相机的图像；获取左右两幅图像的特征点计算特征点的深度信息；根据特征点深度信息计算无人机旋转和平移信息。
其中，根据无人机位置信息、无人机旋转和平移信息以及无人机的角速度和加速度信息计算得到导航信息具体包括：
无人机坐标系b相对于导航坐标系n的转动四元数Q可表示为：
Figure BDA0002125655140000021
或
Figure BDA0002125655140000022
其中
Figure BDA0002125655140000031
是陀螺仪在无人机坐标系下三轴的测量数据，
Figure BDA0002125655140000032
Figure BDA0002125655140000033
是陀螺仪三轴的常值偏差；
以导航坐标系n为基准，设定无人机所在位置为(xn,yn,zn)，速度为
Figure BDA0002125655140000034
易得其位置微分方程为
Figure BDA0002125655140000035
无人机在导航坐标系n中的速度可通过姿态传感器更新，其关系为
Figure BDA0002125655140000036
其中
Figure BDA0002125655140000037
是加速度计在无人机坐标系下三轴的测量数据，
Figure BDA0002125655140000038
Figure BDA0002125655140000039
是加速度计三轴的常值偏差，G为导航坐标系n下的重力加速度；
陀螺仪三轴常值偏差微分方程为：
Figure BDA00021256551400000310
加速度计三轴常值偏差微分方程
Figure BDA00021256551400000311
结合公式(1)、(2)、(3)、(4)、(5)，将状态向量X定义为
Figure BDA00021256551400000312
状态微分方程为：
Figure BDA0002125655140000041
本发明的有益效果是：区别于现有技术，本申请无人机导航系统包括：主控电路、视觉传感器、UWB定位模块、姿态传感器以及组合滤波器，视觉传感器、UWB定位模块、姿态传感器、组合滤波器分别与主控电路信号连接；主控电路用于控制视觉传感器、UWB定位模块、姿态传感器、组合式滤波器；视觉传感器用于采集图像的特征点获取图像特征点深度信息，根据图像特征点深度信息计算得到无人机旋转和平移信息；UWB定位模块用于获取移动标签之间的位置信息；姿态传感器用于获取无人机的角速度和加速度信息；组合滤波器通过无人机旋转和平移信息、移动标签之间的位置信息以及无人机的角速度和加速度信息计算导航信息，本申请结合各传感器间的互补性，综合利用了视觉、惯性导航、UWB等传感器的测量信息，能够实现稳定、可靠的导航功能，建立复杂环境下稳定可靠的导航系统，实现旋翼无人机在传统导航盲区作业飞行，同时运算量大大减小，可以在较低性能的计算平台上实现，满足低成本无人机的应用需求。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图，其中：
图1是本申请基于VIO&UWB松组合的无人机导航系统的一实施例的传感器组成结构示意图；
图2是本申请基于VIO&UWB松组合的无人机导航方法一实施例的流程示意图。
具体实施方式
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，均属于本申请保护的范围。
请参阅图1，图1是基于VIO&UWB松组合的无人机导航系统的一实施例的传感器组成及数据融合结构示意图，具体地包括：
主控电路101、视觉传感器102、UWB定位模块103、姿态传感器104以及组合滤波器105。
在一具体实施例中，主控电路101分别与视觉传感器102、UWB定位模块103、姿态传感器104、组合滤波器105信号连接，用于控制控制视觉传感器102、UWB定位模块103、姿态传感器104、组合式滤波器105，其中，主控电路101中安装有无人机机载计算平台和UWB机载端。
视觉传感器102用于采集图像的特征点获取图像特征点深度信息，根据图像特征点深度信息计算得到无人机旋转和平移信息。
在一具体的实施里中，视觉传感器102采用双目相机，双目相机安装在无人机左右两侧，无人机通过双目相机同时采集左右两个相机的图像，实时提取左右两个相机拍摄的图像特征点并估计特征点深度信息，并基于最小重投影误差原理对无人机的旋转和平移信息进行计算。
UWB定位模块103用于获取移动标签之间的位置信息。
在一具体的实施例中，UWB定位模块103包括基站和移动标签两个部分，无人机可以实时获取移动标签之间的位置信息与位置变化。在GPS信号良好的地点安装差分GPS地面站和UWB基站，利用差分GPS地面站标定UWB基站的位置，将UWB基站的位置信息输入无人机机载计算平台，各个UWB基站向UWB机载端发射测距信号，使无人机获取自身与各个UWB基站的距离信息。其中UWB基站至少设置4个，具体设置数量在此不做限定。移动标签安装在UWB基站以及无人机上。
为了避免基于视觉的导航算法在复杂环境下失效(视场中有效特征丢失、相机过度曝光等)，进一步集成UWB定位模块103的位置信息，从而增加导航系统的整体可观性。
姿态传感器104用于获取无人机的角速度和加速度信息。
在一具体的实施例中，姿态传感器104包括陀螺仪和加速度计，陀螺仪和加速度计可以敏感无人机的角速度和加速度信息，其中加速度计可以测量重力加速度，在无人机静止或匀速运动过程中，利用加速度所测量的重力加速度来保存无人机俯仰角和横滚角的稳定。
陀螺仪和加速度计在计算无人机运动状态时，会受到漂移的影响产生累计误差，相比于惯性传感器，相机数据基本基本不会有漂移，通过双目相机获取载体相对与上一时刻状态的旋转和平移增量，结合上一时刻各个传感器数据融合得到的位姿信息，解算到当前时刻的图像位姿测量信息，与惯性传感器进行融合，有效的估计并修正惯性传感器读数的漂移。
组合滤波器105通过无人机旋转和平移信息、移动标签之间的位置信息以及无人机的角速度和加速度信息计算导航信息。
在一具体的实施例中，组合滤波器105采用集中式滤波器。考虑到测量模型和状态模型均为非线性，故采用非线性滤波器作为组合导航的滤波器算法。通过设计松组合滤波器，融合双目相机、惯性导航和UWB定位系统的测量信息，在有限的计算资源条件下，实现稳定的导航功能。
相较于分散式滤波器，即使用多个卡尔曼滤波器对目标状态量进行估计，本申请采用集中式滤波器的方法，结构简单，工程中容易实现。前者算法最优估计是基于各个测量量是不相关的假设为前提，这在工程中是难以保证的；后者则可以给出最优的误差估计。
本发明的有益效果是：本申请无人机导航系统包括：主控电路、视觉传感器、UWB定位模块、姿态传感器以及组合滤波器，视觉传感器、UWB定位模块、姿态传感器、组合滤波器分别与主控电路信号连接，本申请结合各传感器间的互补性，综合利用了视觉、惯性导航、UWB等传感器的测量信息，能够实现稳定、可靠的导航功能，建立复杂环境下稳定可靠的导航系统，实现旋翼无人机在传统导航盲区作业飞行，同时运算量大大减小，可以在较低性能的计算平台上实现，满足低成本无人机的应用需求。
请参阅图2，图2是基于VIO&UWB松组合的无人机导航方法一实施例的流程示意图。在本实施例中基于VIO&UWB松组合的无人机导航方法包括如下步骤：
S201：获取无人机位置信息。
在诸如桥梁下方、隧道内、房屋内等应用场景下，GPS信号被极大的屏蔽，无法利用传统的INS/GPS组合导航技术实现有效的导航功能。此时，可通过UWB定位模块获取无人机的位置信息，具体地，无人机获取基站的位置信息，然后通过基站的位置信息计算得到无人机的位置信息。
在一具体的实施例中，UWB定位模块包括基站和移动标签两个部分，无人机可以实时获取移动标签之间的位置信息与位置变化。在GPS信号良好的地点安装差分GPS地面站和UWB基站，利用差分GPS地面站标定UWB基站的位置，将UWB基站的位置信息输入无人机机载计算平台，各个UWB基站向UWB机载端发射测距信号，使无人机获取自身与各个UWB基站的距离信息。其中UWB基站至少设置4个，具体设置数量在此不做限定。移动标签安装在UWB基站以及无人机上。
无人机通过移动标签获取UWB基站的位置信息，然后建立无人机坐标系b和导航坐标系n。
具体地，无人机坐标系b--是固联在无人机上的坐标系，坐标原点设在无人机的重心处，xb轴沿无人机横轴指向前，yb轴沿无人机纵轴指向右，zb轴与xb轴和yb轴构成右手笛卡尔坐标系。简记为：前(X)-右(Y)-下(Z)。导航坐标系n——是在导航时根据导航系统工作的需要而选取的作为导航基准的坐标系，此处选取北(N)-东(E)-地(D)。
将导航坐标系n转换到无人机坐标系b；
设导航坐标系b按照旋转顺序Z-Y-X变换到无人机坐标系n；
绕X轴旋转，对应横滚角roll：φ；
绕Y轴旋转，对应俯仰角pitch：θ；
绕Z轴旋转，对应航向角yaw：ψ；
基本旋转矩阵分别为
Figure BDA0002125655140000081
姿态旋转矩阵为：(旋转顺序Z-Y-X)
Figure BDA0002125655140000082
S202：通过双目相机对周围环境进行拍照获取图像特征点信息，根据图像特征点信息进行重投影计算，得到无人机旋转和平移信息。
具体地，无人机通过双目相机采集左右两个相机的图像；获取左右两幅图像的特征点计算特征点的深度信息；根据特征点深度信息计算无人机旋转和平移信息
在一具体的实施例中，无人机上安装有视觉传感器，其中，视觉传感器可设置为双目相机，双目相机安装在无人机左右两侧，无人机通过双目相机对周围的环境拍照获取左右两个相机拍摄图像的特征点信息，时提取左右两个相机拍摄的图像特征点并估计特征点深度信息，并基于最小重投影误差原理对无人机的旋转和平移信息进行计算。
S203；获取无人机的角速度和加速度信息；根据无人机位置信息、无人机旋转和平移信息以及无人机的角速度和加速度信息计算得到导航信息。
通过设计松组合滤波器，融合双目相机、惯性导航和UWB定位系统的测量信息，在有限的计算资源条件下，实现稳定的导航功能。
通过设计EKF算法，融合三种导航传感器的导航测量信息，获取稳定可靠的导航信息。
导航解算模型说明：
无人机坐标系b相对于导航坐标系n的转动四元数Q可表示为：
Figure BDA0002125655140000091
或
Figure BDA0002125655140000092
其中
Figure BDA0002125655140000093
是所述陀螺仪无人机坐标系下三轴的测量数据，
Figure BDA0002125655140000094
Figure BDA0002125655140000095
是所述陀螺仪三轴的常值偏差；
以导航坐标系n为基准，设定所述无人机所在位置为(xn,yn,zn)，速度为
Figure BDA0002125655140000096
易得其位置微分方程为
Figure BDA0002125655140000097
所述无人机在所述导航坐标系n中的速度可通过所述姿态传感器更新，其关系为
Figure BDA0002125655140000098
其中
Figure BDA0002125655140000099
是所述加速度计在所述无人机坐标系下三轴的测量数据，
Figure BDA00021256551400000910
是所述加速度计三轴的常值偏差，G为所述导航坐标系n下的重力加速度；
所述陀螺仪三轴常值偏差微分方程为：
Figure BDA0002125655140000101
所述加速度计三轴常值偏差微分方程
Figure BDA0002125655140000102
结合公式(1)、(2)、(3)、(4)、(5)，将状态向量X定义为
Figure BDA0002125655140000103
状态微分方程为：
Figure BDA0002125655140000104
在一具体的实施例中，记桥下作业无人机接收到的对应某架无人机i的测量伪距值：
Figure BDA0002125655140000105
式中(x,y,z)为桥下作业无人机的位置坐标，(xsi,ysi,zsi)为基站i的位置坐标。
当n(n≥4)时，系统的测量向量为：
Zuwb＝[ρB1,ρB2,…,ρBn]
为了提高俯仰角和横滚角的可观性，测量模型中引入了三轴加速度计的测量值，其测量模型为：
Figure BDA0002125655140000111
设定从双目相机中直接计算得到的位姿信息为Rcam和Tcam，其中Rcam表示相机当前时刻相对于上一时刻的姿态变换矩阵，Tcam表示相机当前时刻相对于上一时刻的位移变化矩阵。
若组合滤波器在上一时刻估计得到的四元数为
Figure BDA0002125655140000112
结合当前时刻从双目相机中计算得到的位姿变换信息，可以得到导航坐标系下的姿态信息和位移信息并将其表示为：
Figure BDA0002125655140000113
Figure BDA0002125655140000114
在获取到当前时刻的位置增量观测值后，结合组合滤波器结果，可以得到当前位置增量的观测值：
Δx＝ΔP(1),Δy＝ΔP(2),Δz＝ΔP(3)
由
Figure BDA0002125655140000115
可以获取当前姿态角的观测值为φ、θ、ψ，则姿态观测值为：
Figure BDA0002125655140000116
位置观测值可以记为：
Figure BDA0002125655140000117
由此，我们得到了组合导航系统的测量的数学模型，其表示为：
Figure BDA0002125655140000121
根据状态方程和量测方程，将其抽象表示为
Figure BDA0002125655140000122
Z(t)＝h[X(t),t]+v(t)
式中f(·)、h(·)是非线性函数，w(t)和v(t)是彼此不相关的零均值白噪声，它们与初始状态也不相关，满足以下关系
E[w(t)]＝0，E[w(t)wT(t)]＝q(t)δ(t-τ)
E[v(t)]＝0，E[v(t)vT(t)]＝r(t)δ(t-τ)
E[w(t)vT(τ)]＝0，E[X(0)wT(τ)]＝0E[X(0)VT(τ)＝0]
将状态方程和量测方程在状态最优轨迹附近展开成泰勒级数，并取其一次近似值，得
Figure BDA0002125655140000123
Figure BDA0002125655140000124
可以将上式改写成
Figure BDA0002125655140000125
Z(t)＝H(t)X(t)+Y(t)+v(t)
其中
Figure BDA0002125655140000131
Figure BDA0002125655140000132
Figure BDA0002125655140000133
Figure BDA0002125655140000134
将改写后的状态方程和观测方程离散化，有
Xk＝Φk,k-1Xk-1+Mk-1+Γk-1Wk-1
Zk＝HkXk+Nk+Vk
其中
Figure BDA0002125655140000135
T表示滤波时间间隔；
Wk～N(0,Qd)，Vk～N(0,Rd)
Figure BDA0002125655140000136
Figure BDA0002125655140000137
Figure BDA0002125655140000138
结合以上定义，可以得到简化后的系统状态方程和观测方程
Xk＝Φk,k-1Xk-1+Mk-1+Γk-1Wk-1
Zk＝HkXk+Nk+Vk
根据上述表达式和卡尔曼滤波原理，可以得到离散扩展卡尔曼滤波方程：
状态一步预测方程：
Figure BDA0002125655140000139
或者
Figure BDA00021256551400001310
(T为滤波时间间隔)
状态估计方程：
Figure BDA00021256551400001311
滤波增益方程：
Figure BDA00021256551400001312
一步预测均方误差方程：
Figure BDA0002125655140000141
估计均方误差方程
Figure BDA0002125655140000142
或
Pk|k＝(I-KkHk)Pk|k-1
本发明的有益效果是：区别于现有技术，本申请无人机导航系统包括：主控电路、视觉传感器、UWB定位模块、姿态传感器以及组合滤波器，视觉传感器、UWB定位模块、姿态传感器、组合滤波器分别与主控电路信号连接；主控电路用于控制视觉传感器、UWB定位模块、姿态传感器、组合式滤波器；视觉传感器用于采集图像的特征点获取图像特征点深度信息，根据图像特征点深度信息计算得到无人机旋转和平移信息；UWB定位模块用于获取移动标签之间的位置信息；姿态传感器用于获取无人机的角速度和加速度信息；组合滤波器通过无人机旋转和平移信息、移动标签之间的位置信息以及无人机的角速度和加速度信息计算导航信息，本申请结合各传感器间的互补性，综合利用了视觉、惯性导航、UWB等传感器的测量信息，能够实现稳定、可靠的导航功能，建立复杂环境下稳定可靠的导航系统，实现旋翼无人机在传统导航盲区作业飞行，同时运算量大大减小，可以在较低性能的计算平台上实现，满足低成本无人机的应用需求。
以上所述仅为本发明的实施方式，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (8)

1.一种基于VIO&UWB松组合的无人机导航系统，其特征在于，所述无人机导航系统包括：主控电路、视觉传感器、UWB定位模块、姿态传感器以及组合滤波器，所述视觉传感器、UWB定位模块、姿态传感器、组合滤波器分别与所述主控电路信号连接；
所述主控电路用于控制所述视觉传感器、UWB定位模块、姿态传感器、组合式滤波器；
所述视觉传感器用于采集图像的特征点获取所述图像特征点深度信息，根据所述图像特征点深度信息计算得到无人机旋转和平移信息；
所述UWB定位模块用于获取移动标签之间的位置信息；
所述姿态传感器用于获取所述无人机的角速度和加速度信息；
所述组合滤波器通过所述无人机旋转和平移信息、所述移动标签之间的位置信息以及所述无人机的角速度和加速度信息计算导航信息；
所述组合滤波器通过导航解算模型计算所述导航信息，所述导航解算模型包括：
无人机坐标系b相对于导航坐标系n的转动四元数Q可表示为：
Figure FDA0002992644350000011
其中
Figure FDA0002992644350000012
是陀螺仪无人机坐标系下三轴的测量数据，
Figure FDA0002992644350000013
Figure FDA0002992644350000014
是所述陀螺仪三轴的常值偏差；
以导航坐标系n为基准，设定所述无人机所在位置为(xn,yn,zn)，速度为
Figure FDA0002992644350000015
易得其位置微分方程为
Figure FDA0002992644350000016
所述无人机在所述导航坐标系n中的速度可通过所述姿态传感器更新，其关系为
Figure FDA0002992644350000021
其中
Figure FDA0002992644350000022
是加速度计在所述无人机坐标系下三轴的测量数据，
Figure FDA0002992644350000023
是所述加速度计三轴的常值偏差，G为所述导航坐标系n下的重力加速度；
所述陀螺仪三轴常值偏差微分方程为：
Figure FDA0002992644350000024
所述加速度计三轴常值偏差微分方程
Figure FDA0002992644350000025
结合公式(1)、(2)、(3)、(4)、(5)，将状态向量X定义为
Figure FDA0002992644350000026
状态微分方程为：
Figure FDA0002992644350000031
2.根据权利要求1所述的无人机导航系统，其特征在于，所述视觉传感器包括双目相机。
3.根据权利要求1所述的无人机导航系统，其特征在于，所述UWB定位模块包括基站和移动标签。
4.根据权利要求1所述的无人机导航系统，其特征在于，所述组合式滤波器包括集中式滤波器。
5.一种基于VIO&UWB松组合的无人机导航方法，其特征在于，所述无人机导航方法包括:
获取所述无人机位置信息；
通过双目相机对周围环境进行拍照获取图像特征点信息，根据所述图像特征点信息进行重投影计算，得到所述无人机旋转和平移信息；
获取所述无人机的角速度和加速度信息；
根据所述无人机位置信息、无人机旋转和平移信息以及所述无人机的角速度和加速度信息计算得到导航信息；
无人机坐标系b相对于导航坐标系n的转动四元数Q可表示为：
Figure FDA0002992644350000041
其中
Figure FDA0002992644350000042
是陀螺仪无人机坐标系下三轴的测量数据，
Figure FDA0002992644350000043
Figure FDA0002992644350000044
是所述陀螺仪三轴的常值偏差；
以导航坐标系n为基准，设定所述无人机所在位置为(xn,yn,zn)，速度为
Figure FDA0002992644350000045
易得其位置微分方程为
Figure FDA0002992644350000046
所述无人机在所述导航坐标系n中的速度可通过所述姿态传感器更新，其关系为
Figure FDA0002992644350000047
其中
Figure FDA0002992644350000048
是加速度计在所述无人机坐标系下三轴的测量数据，
Figure FDA0002992644350000049
是所述加速度计三轴的常值偏差，G为所述导航坐标系n下的重力加速度；
所述陀螺仪三轴常值偏差微分方程为：
Figure FDA00029926443500000410
所述加速度计三轴常值偏差微分方程
Figure FDA00029926443500000411
结合公式(1)、(2)、(3)、(4)、(5)，将状态向量X定义为
Figure FDA0002992644350000051
状态微分方程为：
Figure FDA0002992644350000052
6.根据权利要求5所述的无人机导航方法，其特征在于，所述获取所述无人机位置信息的步骤包括：
获取基站的位置信息；
通过所述基站的位置信息计算得到所述无人机的位置信息。
7.根据权利要求6所述的无人机导航方法，其特征在于，所述获取所述基站的位置信息的步骤包括：
建立无人机坐标系b和导航坐标系n；
将导航坐标系n转换到无人机坐标系b；
设导航坐标系b按照旋转顺序Z-Y-X变换到无人机坐标系n；
绕X轴旋转，对应横滚角roll：φ；
绕Y轴旋转，对应俯仰角pitch：θ；
绕Z轴旋转，对应航向角yaw：ψ；
基本旋转矩阵分别为
Figure FDA0002992644350000061
Figure FDA0002992644350000062
姿态旋转矩阵为：旋转顺序Z-Y-X
Figure FDA0002992644350000063
8.根据权利要求5所述的无人机导航方法，其特征在于，所述通过所述双目相机对周围环境进行拍照获取图像特征点信息，根据所述图像特征点信息进行重投影计算，得到所述无人机旋转和平移信息的步骤包括：
通过所述双目相机采集左右两个相机的图像；
获取左右两幅图像的特征点计算所述特征点的深度信息；
根据所述特征点深度信息计算无人机旋转和平移信息。
CN201910621591.7A 2019-07-10 2019-07-10 一种基于vio＆uwb松组合的无人机导航系统及方法 Active CN110487267B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910621591.7A CN110487267B (zh) 2019-07-10 2019-07-10 一种基于vio＆uwb松组合的无人机导航系统及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910621591.7A CN110487267B (zh) 2019-07-10 2019-07-10 一种基于vio＆uwb松组合的无人机导航系统及方法

Publications (2)

Publication Number Publication Date
CN110487267A CN110487267A (zh) 2019-11-22
CN110487267B true CN110487267B (zh) 2021-06-04

Family

ID=68545965

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910621591.7A Active CN110487267B (zh) 2019-07-10 2019-07-10 一种基于vio＆uwb松组合的无人机导航系统及方法

Country Status (1)

Country Link
CN (1) CN110487267B (zh)

Families Citing this family (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110954066B (zh) * 2019-12-19 2021-09-28 陕西长岭电子科技有限责任公司 基于超宽带定位的直升机吊挂摆动监测系统及方法
CN111238486B (zh) * 2020-03-12 2021-11-19 北京三快在线科技有限公司 用于无人设备的导航方法、装置、存储介质及无人设备
WO2021253195A1 (en) * 2020-06-15 2021-12-23 Robert Bosch Gmbh Positioning system and method for operating the positioning system
CN111812584A (zh) * 2020-06-22 2020-10-23 中国科学院重庆绿色智能技术研究院 一种无人机定位系统和定位方法
CN112130579A (zh) * 2020-09-24 2020-12-25 国网智能科技股份有限公司 一种隧道无人机巡检方法及系统
CN112229392B (zh) * 2020-09-25 2022-11-18 福建华电可门发电有限公司 一种高冗余的室内煤场导航方法及系统
WO2022086446A1 (en) * 2020-10-22 2022-04-28 Nanyang Technological University Uwb anchor deployment
CN112378396A (zh) * 2020-10-29 2021-02-19 江苏集萃未来城市应用技术研究所有限公司 基于抗差lm视觉惯性里程计与uwb混合高精度室内定位方法
CN112556695B (zh) * 2020-11-30 2023-09-19 北京建筑大学 室内定位与三维建模方法、系统、电子设备及存储介质
CN112837374B (zh) * 2021-03-09 2023-11-03 中国矿业大学 一种空间定位方法及系统
CN113758488B (zh) * 2021-09-27 2023-08-29 同济大学 一种基于uwb与vio的室内定位方法及设备

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106679648A (zh) * 2016-12-08 2017-05-17 东南大学 一种基于遗传算法的视觉惯性组合的slam方法
CN108873038A (zh) * 2018-09-10 2018-11-23 芜湖盟博科技有限公司 自主泊车定位方法及定位系统
CN109084760A (zh) * 2018-07-11 2018-12-25 北京壹氢科技有限公司 一种楼宇间导航系统
CN109813311A (zh) * 2019-03-18 2019-05-28 南京航空航天大学 一种无人机编队协同导航方法
CN109991636A (zh) * 2019-03-25 2019-07-09 启明信息技术股份有限公司 基于gps、imu以及双目视觉的地图构建方法及系统

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106679648A (zh) * 2016-12-08 2017-05-17 东南大学 一种基于遗传算法的视觉惯性组合的slam方法
CN109084760A (zh) * 2018-07-11 2018-12-25 北京壹氢科技有限公司 一种楼宇间导航系统
CN108873038A (zh) * 2018-09-10 2018-11-23 芜湖盟博科技有限公司 自主泊车定位方法及定位系统
CN109813311A (zh) * 2019-03-18 2019-05-28 南京航空航天大学 一种无人机编队协同导航方法
CN109991636A (zh) * 2019-03-25 2019-07-09 启明信息技术股份有限公司 基于gps、imu以及双目视觉的地图构建方法及系统

Also Published As

Publication number Publication date
CN110487267A (zh) 2019-11-22

Similar Documents

Publication Publication Date Title
CN110487267B (zh) 一种基于vio＆uwb松组合的无人机导航系统及方法
CN107727079B (zh) 一种微小型无人机全捷联下视相机的目标定位方法
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
CN109931926B (zh) 一种基于站心坐标系的小型无人机无缝自主式导航方法
CN110160542B (zh) 车道线的定位方法和装置、存储介质、电子装置
Tardif et al. A new approach to vision-aided inertial navigation
CN109991636A (zh) 基于gps、imu以及双目视觉的地图构建方法及系统
US10352959B2 (en) Method and system for estimating a path of a mobile element or body
CN103175524B (zh) 一种无标识环境下基于视觉的飞行器位置与姿态确定方法
WO2021127968A1 (zh) 一种gnss/imu测绘系统和方法
CN111426320B (zh) 一种基于图像匹配/惯导/里程计的车辆自主导航方法
US20170074678A1 (en) Positioning and orientation data analysis system and method thereof
CN110057356B (zh) 一种隧道内车辆定位方法及装置
CN112562077A (zh) 一种融合pdr和先验地图的行人室内定位方法
Andert et al. Optical-aided aircraft navigation using decoupled visual SLAM with range sensor augmentation
EP3227634B1 (en) Method and system for estimating relative angle between headings
US20220057517A1 (en) Method for constructing point cloud map, computer device, and storage medium
CN113009816B (zh) 时间同步误差的确定方法及装置、存储介质及电子装置
Niu et al. Camera-based lane-aided multi-information integration for land vehicle navigation
Chiang et al. Semantic proximity update of GNSS/INS/VINS for Seamless Vehicular Navigation using Smartphone sensors
CN112860823B (zh) 终端指向的显示方法、装置和存储介质及电子设备
Forno et al. Techniques for improving localization applications running on low-cost IoT devices
CN116817927B (zh) 双滤波器组合导航定位与测姿方法、电子设备及介质
WO2022179047A1 (zh) 一种状态信息估计方法及装置
CN116678391A (zh) 自移动设备及其多传感器的标定方法、存储介质

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