CN113465596A - 一种基于多传感器融合的四旋翼无人机定位方法 - Google Patents

一种基于多传感器融合的四旋翼无人机定位方法 Download PDF

Info

Publication number
CN113465596A
CN113465596A CN202110712202.9A CN202110712202A CN113465596A CN 113465596 A CN113465596 A CN 113465596A CN 202110712202 A CN202110712202 A CN 202110712202A CN 113465596 A CN113465596 A CN 113465596A
Authority
CN
China
Prior art keywords
data
aerial vehicle
unmanned aerial
quad
positioning
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.)
Granted
Application number
CN202110712202.9A
Other languages
English (en)
Other versions
CN113465596B (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.)
University of Electronic Science and Technology of China
Original Assignee
University of Electronic Science and Technology of China
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 University of Electronic Science and Technology of China filed Critical University of Electronic Science and Technology of China
Priority to CN202110712202.9A priority Critical patent/CN113465596B/zh
Publication of CN113465596A publication Critical patent/CN113465596A/zh
Application granted granted Critical
Publication of CN113465596B publication Critical patent/CN113465596B/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
    • 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
    • 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
    • G01C21/1656—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 with passive imaging devices, e.g. cameras
    • 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
    • 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/485—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an optical system or imaging system
    • 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于多传感器融合的四旋翼无人机定位方法，通过对分布式多传感器的模块化操作，以惯性测量单元IMU的测量值、视觉传感器的测量值、GPS传感器的测量值以及超声波传感器的测量值作为主要的状态，并且以松耦合的方式进行扩展卡尔曼滤波的定位信息融合，最终输出一个频率稳定且准确的位置和姿态，最终将位姿发给飞控单元，完成定位任务。

Description

一种基于多传感器融合的四旋翼无人机定位方法
技术领域
本发明属于四旋翼飞行器技术领域，更为具体地讲，涉及一种基于多传感器融合的四旋翼无人机定位方法。
背景技术
近年来，四旋翼飞行器在日常生活、工业生产等多个领域的应用呈现多点开花的局面，比如航拍、搜救、监控、物流等。但随着四轴飞行器应用领域的不断发展以及其飞行场景的不断增加，不可避免地会对飞行器的环境适应性提出更高的要求，希望飞行器能在各种不同的环境中实现高效精准的位姿估计。因此四轴飞行器如何在不同的场景中实现高效且准确的定位已成为研究的热点和难点。
无人机不同环境下实现对自身的定位极具挑战，根据无人机所处环境不同，位姿估计的传感器也各不相同，常见的室外定位方式有全球定位系统(GPS)和惯性测量单元(IMU)等，室内定位方式有相机(SLAM)。目前大部分传感器通常只能针对某个特定的场景，不具有通用性，应用场景大大受限。
基于此，本发明采用了多种传感器融合的方法，从多个传感器中采集尽可能多的信息，利用改进的扩展卡尔曼滤波算法进行数据融合以达到对自身位姿估计最优并在不同场景某个传感器失效的情况下系统也能正常工作。该系统具有鲁棒性好，容错率高，环境适应性强等特点。
发明内容
本发明的目的在于克服现有技术的不足，提供一种基于多传感器融合的四旋翼无人机定位方法，通过改进的扩展卡尔曼滤波器融合相机视觉(SLAM)，全球定位系统(GPS)，惯性测量单元(IMU)，超声波多个传感器的数据，将多种传感器进行数据融合，实现在不同环境下准确估计四轴飞行器的位姿并进行导航。
为实现上述发明目的，本发明一种基于多传感器融合的四旋翼无人机定位方法，其特征在于，包括以下步骤：
(1)、利用机载IMU传感器估计四旋翼无人机在t1时刻的三轴速度
Figure BDA0003133353560000021
三轴角速度
Figure BDA0003133353560000022
三轴加速度
Figure BDA0003133353560000023
三轴位置
Figure BDA0003133353560000029
和三轴姿态
Figure BDA0003133353560000025
(2)、利用视觉传感器估计四旋翼无人机在t2时刻的三轴位置
Figure BDA0003133353560000026
和三轴姿态
Figure BDA0003133353560000027
(3)、利用机载GPS传感器估计四旋翼无人机在t3时刻的三轴位置
Figure BDA0003133353560000028
(4)、利用超声波传感器计算估计四旋翼无人机在t4时刻的飞行高度h；
(4.1)、四旋翼无人机通过串口解析超声波信号，获得超声波信号中提供的高度h'；
(4.2)、根据四旋翼无人机的姿态，对此飞行高度h进行修正：h＝cos(Pitch)*cos(Roll)*h'，Pitch为四旋翼无人机的俯仰角，Roll为四旋翼无人机的横滚角；
(5)、使用卡尔曼滤波进行视觉惯导定位；
(5.1)、设置卡尔曼滤波器的系统状态量X＝[p,q,v,ba,bω]，其中，p,q,v,ba,bω分别为四旋翼无人机的位置、姿态、速度、IMU的偏置、IMU的随机游走噪声；设置卡尔曼滤波器中相机传感器的测量噪声R，R为六维单位矩阵；设置状态协方差矩阵P，初始值为15维的单位阵；设置过程噪声误差矩阵
Figure BDA0003133353560000031
其中，σp为位置误差因子，为速度误差因子，σq为姿态误差因子，σba为IMU偏置误差；
(5.2)、计算机载IMU传感器在t1时刻及t1-1时刻采集数据的时间差Δt，建立四旋翼无人机的运动学预测矩阵F；
Figure BDA0003133353560000032
I为单位阵；
(5.3)、将IMU在t1时刻测量的数据q1,a,ω代入运动学预测矩阵F及公式X(t1)＝F*X(t1-1)，从而预测出四旋翼无人机在下一个时刻的位置和姿态；
再根据协方差预测方程P(t1)＝FP(t1-1)FT+Q，预测出t1时刻的协方差矩阵P；
(5.4)、匹配IMU数据p1、q1和视觉定位数据p2、q2的时间，当两个时间戳之差|t1-t2|＜Δt时，则认为匹配成功，然后转至步骤(5.5)；否则，将视觉定位数据p2、q2剔除；
(5.5)、对视觉定位数据先进行离群滤波，再计算视觉定位数据的马氏范数，如果马氏范数超过设定阈值ε，则判定视觉定位数据p2、q2异常，并拒绝接受，返回至步骤(5.3)；否则，直接进入步骤(5.6)；
(5.6)、构建视觉定位的观测矩阵
Figure BDA0003133353560000033
然后计算卡尔曼滤波增益K＝PH/(HPHT+R)；
记视觉传感器的测量值为Z＝[p2,q2]，然后更新卡尔曼滤波器的系统状态量X'＝X+K(Z-HX)；更新状态协方差矩阵P'＝(I-KH)P；
(5.7)、输出更新后的系统状态量X'，然后提取X'中的p,q，作为融合后的视觉惯导定位数据；
(6)、基于图优化方法融合视觉惯导定位及GPS和超声波定位；
(6.1)、连续记录L个时刻的视觉惯导定位数据，记为pi,qi，i＝1,2,…,L；然后根据六点法将这L个时刻的视觉惯导定位数据转换至GPS的东北天坐标系下的数据，记为
Figure BDA0003133353560000041
(6.2)、设置图优化方法的状态节点集合S，具体包含L个状态节点以及对应的时间，其中，第i个状态节点记为：
Figure BDA0003133353560000042
其中，
Figure BDA0003133353560000043
表示第i个状态节点，ti表示第i个状态节点对应的时间；
(6.3)、在L个视觉惯导定位数据中，分别计算相邻两组数据的位姿变换量Tij，i≠j，j∈[1,L]；计算图优化方法的状态节点集合S的局部约束因子f(S)1＝∑Tij；
(6.4)、读入GPS定位数据p3以及对应时间t3，在图优化状态节点集合S中寻找与时间t3匹配对应的状态节点，如果找到匹配对应的状态节点Mi，则先取Mi中的位置数据
Figure BDA0003133353560000044
再计算状态节点集合S的GPS约束因子
Figure BDA0003133353560000045
若无法找到匹配对应的状态节点，则记f2(S)＝0；
(6.5)、读入超声波定位数据h以及对应时间t4，在图优化状态节点集合S中寻找与时间t4匹配对应的状态节点，如果找到匹配对应的状态节点Mi，则先取Mi中的位置数据
Figure BDA0003133353560000046
再提取
Figure BDA0003133353560000047
中z轴值
Figure BDA0003133353560000048
作为高度hi，最后计算状态节点集合S的GPS约束因子f3(S)＝hi-h；若无法找到匹配对应的状态节点，则记f3(S)＝0；
(6.6)、构建滑动窗口的优化函数h(S)＝min||ρ1f1(S)+ρ2f2(S)+ρ3f3(S)||2，其中，ρ1、ρ2、ρ3为柯西鲁棒核函数因子；
(6.7)、采用高斯牛顿法对h(S)进行迭代求解，当相邻两轮迭代后的差值小于预设阈值ψ时，停止迭代，并输出本轮迭代后的状态节点，得到四旋翼无人机融合后的位姿；
(6.8)、当第L+1个时刻的视觉惯导定位数据到来时，将滑动窗口后移一位，按照步骤(6)所述方法对第2至L+1个时刻的视觉惯导定位数据进行融合定位，得到四旋翼无人机在第L+1个时刻融合后的位姿。
本发明的发明目的是这样实现的：
本发明基于多传感器融合的四旋翼无人机定位方法，通过对分布式多传感器的模块化操作，以惯性测量单元IMU的测量值、视觉传感器的测量值、GPS传感器的测量值以及超声波传感器的测量值作为主要的状态，并且以松耦合的方式进行扩展卡尔曼滤波的定位信息融合，最终输出一个频率稳定且准确的位置和姿态，最终将位姿发给飞控单元，完成定位任务。
同时，本发明基于多传感器融合的四旋翼无人机定位方法还具有以下有益效果：
(1)、目前四旋翼无人机的位置估计和姿态解算多为单传感器，存在手段单一，环境适应性差的特点，无法应对复杂环境下的定位和导航任务，本发明利用多传感器信息的融合，最大限度地获取周围环境信息，增加系统的鲁棒性和容错性。
(2)、在有GPS的环境，利用视觉惯导融合GPS和超声波，采取滑动窗口优化的方式，大大减小优化节点数量，提高了算法的稳定性及快速性。
附图说明
图1是本发明基于多传感器融合的四旋翼无人机定位方法流程图；
图2是状态信息预测和更新示意图；
图3是图优化的一种具体实施方式结构图。
具体实施方式
下面结合附图对本发明的具体实施方式进行描述，以便本领域的技术人员更好地理解本发明。需要特别提醒注意的是，在以下的描述中，当已知功能和设计的详细描述也许会淡化本发明的主要内容时，这些描述在这里将被忽略。
实施例
图1是本发明基于多传感器融合的四旋翼无人机定位方法流程图。
在本实施例中，如图1所示，本发明一种基于多传感器融合的四旋翼无人机定位方法，包括以下步骤：
S1、利用机载IMU估计四旋翼无人机的三轴速度、三轴位置和三轴姿态角；
S1.1、给四旋翼无人机上电，通过集成在四旋翼无人机上的惯性测量单元测量出四旋翼无人机在当前时刻t1时的三轴加速度
Figure BDA0003133353560000051
三轴角速度
Figure BDA0003133353560000052
和频率f0，其中，x,y,z代表坐标轴；
S1.2、估计四旋翼无人机的三轴速度三轴速度
Figure BDA0003133353560000061
估计四旋翼无人机的三轴位置
Figure BDA0003133353560000062
估计四旋翼无人机的三轴姿态角
Figure BDA0003133353560000063
S2、利用视觉传感器估计四旋翼无人机在t2时刻的三轴位置
Figure BDA0003133353560000064
和三轴姿态
Figure BDA0003133353560000065
S2.1、利用机载的RGB-D深度相机拍摄连续的多帧原始彩色图像及对应的深度图像；
S2.2、将每一帧原始彩色图像按照不同比例缩放进行降采样，得到8张缩放比例不同的图像；再将每一张图像均匀分成多个图像网格，然后检测每个图像网格的FAST角点，将8张图像提取的所有FAST角点组成角点集合，即每一帧原始彩色图像构建一个角点集合；
S2.3、提取相邻两帧深度图像，以及对应原始彩色图像构建的角点集合，将角点集合中的每个角点进行ORB特征描述子；然后基于每个角点的ORB特征描述子，对两个角点集合中的每个角点进行特征点映射，得到3D-2D的匹配点对，ORB特征点作为地图点，生成初始局部地图；
S2.4、利用PNP算法求解出这两帧图像间的位姿变化，计算得到四轴飞行器当前位姿的估计值。
S2.5、将前后相邻两帧图像中的后一帧图像作为当前帧，然后根据当前位姿的估计值跟踪初始局部地图，将初始局部地图的地图点投影到当前帧，得到地图点与当前帧的ORB特征点的匹配点对，根据匹配点对，通过局部BA算法对当前位姿的估计值进行优化，得到优化后的空间位置
Figure BDA0003133353560000075
和旋转姿态
Figure BDA0003133353560000076
；
S2.6、利用D-BOW2词袋模型对实时图像进行回环检测，当检测到无人机重新经过曾经到达过的场景时，回环检测模块计算SIM3变换，随后采用BA优化模型，进一步优化位姿，消除视觉定位系统的累积误差，获得一个更为理想的三轴位置
Figure BDA0003133353560000071
和三轴姿态
Figure BDA0003133353560000072
S3、利用机载GPS传感器估计四旋翼无人机在t3时刻的三轴位置
Figure BDA0003133353560000073
S3.1、四旋翼无人机通过串口解析GPS信号，获得GPS信号中提供的经纬度和高度；
S3.2、以GPS信号的第一帧定位信号作为原点，将经纬度和高度转换为东北天坐标系下的位置，估计出四旋翼无人机在t3时刻的三轴位置
Figure BDA0003133353560000074
S4、利用超声波传感器计算估计四旋翼无人机在t4时刻的飞行高度h；
S4.1、四旋翼无人机通过串口解析超声波信号，获得超声波信号中提供的高度h'；
S4.2、根据四旋翼无人机的姿态，对此飞行高度h进行修正：h＝cos(Pitch)*cos(Roll)*h'，Pitch为四旋翼无人机的俯仰角，Roll为四旋翼无人机的横滚角；
S5、使用卡尔曼滤波进行视觉惯导定位；
S5.1、设置卡尔曼滤波器的系统状态量X＝[p,q,v,ba,bω]，其中，p,q,v,ba,bω分别为四旋翼无人机的位置、姿态、速度、IMU的偏置、IMU的随机游走噪声；设置卡尔曼滤波器中相机传感器的测量噪声R，R为六维单位矩阵；设置状态协方差矩阵P，初始值为15维的单位阵；设置过程噪声误差矩阵
Figure BDA0003133353560000081
其中，σp为位置误差因子，为速度误差因子，σq为姿态误差因子，σba为IMU偏置误差，在本实施例中所有的误差因子取2，可根据实际运行情况适当调整；
S5.2、计算机载IMU传感器在t1时刻及t1-1时刻采集数据的时间差Δt，建立四旋翼无人机的运动学预测矩阵F；
Figure BDA0003133353560000082
I为单位阵；
S5.3、将IMU在t1时刻测量的数据q1,a,ω代入运动学预测矩阵F及公式X(t1)＝F*X(t1-1)，从而预测出四旋翼无人机在下一个时刻的位置和姿态；
再根据协方差预测方程P(t1)＝FP(t1-1)FT+Q，预测出t1时刻的协方差矩阵P；
S5.4、如图2中的(a)所示，黑色状态表示未经过视觉定位更新的状态，如图2中的(b)所示，匹配IMU数据p1、q1和视觉定位数据p2、q2的时间，当两个时间戳之差|t1-t2|＜Δt时，Δt一般取0.1s，则认为匹配成功，然后转至步骤S5.5；否则，将视觉定位数据p2、q2剔除；
S5.5、对视觉定位数据先进行离群滤波，再计算视觉定位数据的马氏范数，如果马氏范数超过设定阈值ε＝100，则判定视觉定位数据p2、q2异常，并拒绝接受，返回至步骤S5.3；否则，直接进入步骤S5.6；
S5.6、构建视觉定位的观测矩阵
Figure BDA0003133353560000083
然后计算卡尔曼滤波增益K＝PH/(HPHT+R)；
记视觉传感器的测量值为Z＝[p2,q2]，然后更新卡尔曼滤波器的系统状态量X'＝X+K(Z-HX)；更新状态协方差矩阵P'＝(I-KH)P，如图2中的(c)所示，在更新完状态后重新利用预测方程将系统状态预测到最新一帧的状态上；
S5.7、输出更新后的系统状态量X'，然后提取X'中的p,q，作为融合后的视觉惯导定位数据；
S6、基于图优化方法融合视觉惯导定位及GPS和超声波定位；
S6.1、图优化结构如图3所示，连续记录L个时刻的视觉惯导定位数据，记为pi,qi，i＝1,2,…,L；然后根据六点法将这L个时刻的视觉惯导定位数据转换至GPS的东北天坐标系下的数据，记为
Figure BDA0003133353560000091
S6.2、设置图优化方法的状态节点集合S，具体包含L个状态节点以及对应的时间，其中，第i个状态节点记为：
Figure BDA0003133353560000092
其中，
Figure BDA0003133353560000093
表示第i个状态节点，ti表示第i个状态节点对应的时间；
S6.3、在L个视觉惯导定位数据中，分别计算相邻两组数据的位姿变换量Tij，i≠j，j∈[1,L]；计算图优化方法的状态节点集合S的局部约束因子f(S)1＝ΣTij；
S6.4、读入GPS定位数据p3以及对应时间t3，在图优化状态节点集合S中寻找与时间t3匹配对应的状态节点，如果找到匹配对应的状态节点Mi，则先取Mi中的位置数据
Figure BDA0003133353560000095
再计算状态节点集合S的GPS约束因子
Figure BDA0003133353560000096
若无法找到匹配对应的状态节点，则记f2(S)＝0；
S6.5、读入超声波定位数据h以及对应时间t4，在图优化状态节点集合S中寻找与时间t4匹配对应的状态节点，如果找到匹配对应的状态节点Mi，则先取Mi中的位置数据
Figure BDA0003133353560000097
再提取
Figure BDA0003133353560000098
中z轴值
Figure BDA0003133353560000099
作为高度hi，最后计算状态节点集合S的GPS约束因子f3(S)＝hi-h；若无法找到匹配对应的状态节点，则记f3(S)＝0；
S6.6、构建滑动窗口的优化函数h(S)＝min||ρ1f1(S)+ρ2f2(S)+ρ3f3(S)||2，其中，ρ1、ρ2、ρ3为各个约束因子的柯西鲁棒核函数因子；
S6.7、采用高斯牛顿法对h(S)进行迭代求解，当相邻两轮迭代后的差值小于预设阈值ψ时，停止迭代，并输出本轮迭代后的状态节点，得到四旋翼无人机融合后的位姿；
S6.8、当第L+1个时刻的视觉惯导定位数据到来时，将滑动窗口后移一位，按照步骤(6)所述方法对第2至L+1个时刻的视觉惯导定位数据进行融合定位，得到四旋翼无人机在第L+1个时刻融合后的位姿。
尽管上面对本发明说明性的具体实施方式进行了描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

Claims (3)

1.一种基于多传感器融合的四旋翼无人机定位方法，其特征在于，包括以下步骤：
(1)、利用机载IMU传感器估计四旋翼无人机在t1时刻的三轴速度
Figure FDA0003133353550000011
三轴角速度
Figure FDA0003133353550000012
三轴加速度
Figure FDA0003133353550000013
三轴位置
Figure FDA0003133353550000014
和三轴姿态
Figure FDA0003133353550000015
(2)、利用视觉传感器估计四旋翼无人机在t2时刻的三轴位置
Figure FDA0003133353550000016
和三轴姿态
Figure FDA0003133353550000017
(3)、利用机载GPS传感器估计四旋翼无人机在t3时刻的三轴位置
Figure FDA0003133353550000018
(4)、利用超声波传感器计算估计四旋翼无人机在t4时刻的飞行高度h；
(4.1)、四旋翼无人机通过串口解析超声波信号，获得超声波信号中提供的高度h'；
(4.2)、根据四旋翼无人机的姿态，对此飞行高度h进行修正：h＝cos(Pitch)*cos(Roll)*h'，Pitch为四旋翼无人机的俯仰角，Roll为四旋翼无人机的横滚角；
(5)、使用卡尔曼滤波进行视觉惯导定位；
(6)、基于图优化方法融合视觉惯导定位及GPS和超声波定位，得到四旋翼无人机的最终定位；
2.根据权利要求1所述的一种基于多传感器融合的四旋翼无人机定位方法，其特征在于，所述视觉惯导定位的具体步骤为：
(2.1)、设置卡尔曼滤波器的系统状态量X＝[p,q,v,ba,bω]，其中，p,q,v,ba,bω分别为四旋翼无人机的位置、姿态、速度、IMU的偏置、IMU的随机游走噪声；设置卡尔曼滤波器中相机传感器的测量噪声R，R为六维单位矩阵；设置状态协方差矩阵P，初始值为15维的单位阵；设置过程噪声误差矩阵
Figure FDA0003133353550000021
其中，σp为位置误差因子，为速度误差因子，σq为姿态误差因子，σba为IMU偏置误差；
(2.2)、计算机载IMU传感器在t1时刻及t1-1时刻采集数据的时间差Δt，建立四旋翼无人机的运动学预测矩阵F；
Figure FDA0003133353550000022
I为为单位阵；
(2.3)、将IMU在t1时刻测量的数据q1,a,ω代入运动学预测矩阵F及公式X(t1)＝F*X(t1-1)，从而预测出四旋翼无人机在下一个时刻的位置和姿态；
再根据协方差预测方程P(t1)＝FP(t1-1)FT+Q，预测出t1时刻的协方差矩阵P；
(2.4)、匹配IMU数据p1、q1和视觉定位数据p2、q2的时间，当两个时间戳之差|t1-t2|＜Δt时，则认为匹配成功，然后转至步骤(2.5)；否则，将视觉定位数据p2、q2剔除；
(2.5)、对视觉定位数据先进行离群滤波，再计算视觉定位数据的马氏范数，如果马氏范数超过设定阈值ε，则判定视觉定位数据p2、q2异常，并拒绝接受，返回至步骤(2.3)；否则，直接进入步骤(2.6)；
(2.6)、构建视觉定位的观测矩阵
Figure FDA0003133353550000023
然后计算卡尔曼滤波增益K＝PH/(HPHT+R)；
记视觉传感器的测量值为Z＝[p2,q2]，然后更新卡尔曼滤波器的系统状态量X'＝X+K(Z-HX)；更新状态协方差矩阵P'＝(I-KH)P；
(2.7)、输出更新后的系统状态量X'，然后提取X'中的p,q，作为融合后的视觉惯导定位数据。
3.根据权利要求1所述的一种基于多传感器融合的四旋翼无人机定位方法，其特征在于，所述的基于图优化方法融合视觉惯导定位及GPS和超声波定位的具体过程为：
(3.1)、连续记录L个时刻的视觉惯导定位数据，记为pi,qi，i＝1,2,…,L；然后根据六点法将这L个时刻的视觉惯导定位数据转换至GPS的东北天坐标系下的数据，记为
Figure FDA0003133353550000031
(3.2)、设置图优化方法的状态节点集合S，具体包含L个状态节点以及对应的时间，其中，第i个状态节点记为：
Figure FDA0003133353550000032
其中，
Figure FDA0003133353550000033
表示第i个状态节点，ti表示第i个状态节点对应的时间；
(3.3)、在L个视觉惯导定位数据中，分别计算相邻两组数据的位姿变换量Tij，i≠j，j∈[1,L]；计算图优化方法的状态节点集合S的局部约束因子f(S)1＝∑Tij；
(3.4)、读入GPS定位数据p3以及对应时间t3，在图优化状态节点集合S中寻找与时间t3匹配对应的状态节点，如果找到匹配对应的状态节点Mi，则先取Mi中的位置数据
Figure FDA0003133353550000034
再计算状态节点集合S的GPS约束因子
Figure FDA0003133353550000035
若无法找到匹配对应的状态节点，则记f2(S)＝0；
(3.5)、读入超声波定位数据h以及对应时间t4，在图优化状态节点集合S中寻找与时间t4匹配对应的状态节点，如果找到匹配对应的状态节点Mi，则先取Mi中的位置数据
Figure FDA0003133353550000036
再提取
Figure FDA0003133353550000037
中z轴值
Figure FDA0003133353550000038
作为高度hi，最后计算状态节点集合S的GPS约束因子f3(S)＝hi-h；若无法找到匹配对应的状态节点，则记f3(S)＝0；
(3.6)、构建滑动窗口的优化函数h(S)＝min||ρ1f1(S)+ρ2f2(S)+ρ3f3(S)||2，其中，ρ1、ρ2、ρ3为柯西鲁棒核函数因子；
(3.7)、采用高斯牛顿法对h(S)进行迭代求解，当相邻两轮迭代后的差值小于预设阈值ψ时，停止迭代，并输出本轮迭代后的状态节点，得到四旋翼无人机融合后的位姿；
(3.8)、当第L+1个时刻的视觉惯导定位数据到来时，将滑动窗口后移一位，按照步骤(3.1)-(3.7)所述方法对第2至L+1个时刻的视觉惯导定位数据进行融合定位，得到四旋翼无人机在第L+1个时刻融合后的位姿。
CN202110712202.9A 2021-06-25 2021-06-25 一种基于多传感器融合的四旋翼无人机定位方法 Active CN113465596B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110712202.9A CN113465596B (zh) 2021-06-25 2021-06-25 一种基于多传感器融合的四旋翼无人机定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110712202.9A CN113465596B (zh) 2021-06-25 2021-06-25 一种基于多传感器融合的四旋翼无人机定位方法

Publications (2)

Publication Number Publication Date
CN113465596A true CN113465596A (zh) 2021-10-01
CN113465596B CN113465596B (zh) 2023-05-30

Family

ID=77873009

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110712202.9A Active CN113465596B (zh) 2021-06-25 2021-06-25 一种基于多传感器融合的四旋翼无人机定位方法

Country Status (1)

Country Link
CN (1) CN113465596B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114419109A (zh) * 2022-03-29 2022-04-29 中航金城无人系统有限公司 一种基于视觉和气压信息融合的飞行器定位方法
CN114755693A (zh) * 2022-06-15 2022-07-15 天津大学四川创新研究院 基于多旋翼无人机的基建设施测量系统和方法

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106679648A (zh) * 2016-12-08 2017-05-17 东南大学 一种基于遗传算法的视觉惯性组合的slam方法
WO2018049581A1 (zh) * 2016-09-14 2018-03-22 浙江大学 一种同时定位与地图构建方法
CN107869989A (zh) * 2017-11-06 2018-04-03 东北大学 一种基于视觉惯导信息融合的定位方法及系统
CN109147058A (zh) * 2018-08-31 2019-01-04 腾讯科技（深圳）有限公司 用于视觉惯导信息融合的初始化方法和装置以及存储介质
CN109376785A (zh) * 2018-10-31 2019-02-22 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN111121767A (zh) * 2019-12-18 2020-05-08 南京理工大学 一种融合gps的机器人视觉惯导组合定位方法
CN111880573A (zh) * 2020-07-31 2020-11-03 电子科技大学 一种基于视觉惯导融合的四旋翼自主导航方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018049581A1 (zh) * 2016-09-14 2018-03-22 浙江大学 一种同时定位与地图构建方法
CN106679648A (zh) * 2016-12-08 2017-05-17 东南大学 一种基于遗传算法的视觉惯性组合的slam方法
CN107869989A (zh) * 2017-11-06 2018-04-03 东北大学 一种基于视觉惯导信息融合的定位方法及系统
CN109147058A (zh) * 2018-08-31 2019-01-04 腾讯科技（深圳）有限公司 用于视觉惯导信息融合的初始化方法和装置以及存储介质
CN109376785A (zh) * 2018-10-31 2019-02-22 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN111121767A (zh) * 2019-12-18 2020-05-08 南京理工大学 一种融合gps的机器人视觉惯导组合定位方法
CN111880573A (zh) * 2020-07-31 2020-11-03 电子科技大学 一种基于视觉惯导融合的四旋翼自主导航方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
郑国贤;张磊;张华希;: "机器人室内环境自主探索与地图构建方法" *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114419109A (zh) * 2022-03-29 2022-04-29 中航金城无人系统有限公司 一种基于视觉和气压信息融合的飞行器定位方法
CN114755693A (zh) * 2022-06-15 2022-07-15 天津大学四川创新研究院 基于多旋翼无人机的基建设施测量系统和方法
CN114755693B (zh) * 2022-06-15 2022-09-16 天津大学四川创新研究院 基于多旋翼无人机的基建设施测量系统和方法

Also Published As

Publication number Publication date
CN113465596B (zh) 2023-05-30

Similar Documents

Publication Publication Date Title
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
CN109887057B (zh) 生成高精度地图的方法和装置
US10295365B2 (en) State estimation for aerial vehicles using multi-sensor fusion
Kaiser et al. Simultaneous state initialization and gyroscope bias calibration in visual inertial aided navigation
CN109885080B (zh) 自主控制系统及自主控制方法
US8942964B2 (en) Optical state estimation and simulation environment for unmanned aerial vehicles
CN111338383B (zh) 基于gaas的自主飞行方法及系统、存储介质
CN108759826B (zh) 一种基于手机和无人机多传感参数融合的无人机运动跟踪方法
CN109238277B (zh) 视觉惯性数据深度融合的定位方法及装置
US20180075614A1 (en) Method of Depth Estimation Using a Camera and Inertial Sensor
Mercado et al. Gps/ins/optic flow data fusion for position and velocity estimation
CN112577493B (zh) 一种基于遥感地图辅助的无人机自主定位方法及系统
CN113465596A (zh) 一种基于多传感器融合的四旋翼无人机定位方法
CN114216454A (zh) 一种gps拒止环境下基于异源图像匹配的无人机自主导航定位方法
CN114719848B (zh) 基于视觉与惯性导航信息融合神经网络的无人机高度估算方法
CN114323033A (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN108444468B (zh) 一种融合下视视觉与惯导信息的定向罗盘
CN116007609A (zh) 一种多光谱图像和惯导融合的定位方法和计算系统
Deng et al. Visual–inertial estimation of velocity for multicopters based on vision motion constraint
CN108827287B (zh) 一种复杂环境下的鲁棒视觉slam系统
CN114777768A (zh) 一种卫星拒止环境高精度定位方法、系统及电子设备
CN116989772B (zh) 一种空地多模态多智能体协同定位与建图方法
Chen et al. Aerial robots on the way to underground: An experimental evaluation of VINS-mono on visual-inertial odometry camera
Cao et al. Visual-Inertial-Laser SLAM Based on ORB-SLAM3
CN114723920A (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