CN113465596B - 一种基于多传感器融合的四旋翼无人机定位方法 - Google Patents
一种基于多传感器融合的四旋翼无人机定位方法 Download PDFInfo
- Publication number
- CN113465596B CN113465596B CN202110712202.9A CN202110712202A CN113465596B CN 113465596 B CN113465596 B CN 113465596B CN 202110712202 A CN202110712202 A CN 202110712202A CN 113465596 B CN113465596 B CN 113465596B
- Authority
- CN
- China
- Prior art keywords
- aerial vehicle
- unmanned aerial
- state node
- data
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 41
- 230000004927 fusion Effects 0.000 title claims abstract description 20
- 230000000007 visual effect Effects 0.000 claims abstract description 51
- 238000005259 measurement Methods 0.000 claims abstract description 12
- 238000001914 filtration Methods 0.000 claims abstract description 9
- 239000011159 matrix material Substances 0.000 claims description 26
- 238000005457 optimization Methods 0.000 claims description 23
- 108010063499 Sigma Factor Proteins 0.000 claims description 6
- 230000008569 process Effects 0.000 claims description 4
- 230000009466 transformation Effects 0.000 claims description 4
- 230000002159 abnormal effect Effects 0.000 claims description 3
- 238000005295 random walk Methods 0.000 claims description 3
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical group C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 abstract description 2
- 230000008878 coupling Effects 0.000 abstract description 2
- 238000010168 coupling process Methods 0.000 abstract description 2
- 238000005859 coupling reaction Methods 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 3
- 238000001514 detection method Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 101100365794 Schizosaccharomyces pombe (strain 972 / ATCC 24843) sim3 gene Proteins 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 230000008124 floral development Effects 0.000 description 1
- 238000009776 industrial production Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
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)
- Navigation (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
Abstract
本发明公开了一种基于多传感器融合的四旋翼无人机定位方法,通过对分布式多传感器的模块化操作,以惯性测量单元IMU的测量值、视觉传感器的测量值、GPS传感器的测量值以及超声波传感器的测量值作为主要的状态,并且以松耦合的方式进行扩展卡尔曼滤波的定位信息融合,最终输出一个频率稳定且准确的位置和姿态,最终将位姿发给飞控单元,完成定位任务。
Description
技术领域
本发明属于四旋翼飞行器技术领域,更为具体地讲,涉及一种基于多传感器融合的四旋翼无人机定位方法。
背景技术
近年来,四旋翼飞行器在日常生活、工业生产等多个领域的应用呈现多点开花的局面,比如航拍、搜救、监控、物流等。但随着四轴飞行器应用领域的不断发展以及其飞行场景的不断增加,不可避免地会对飞行器的环境适应性提出更高的要求,希望飞行器能在各种不同的环境中实现高效精准的位姿估计。因此四轴飞行器如何在不同的场景中实现高效且准确的定位已成为研究的热点和难点。
无人机不同环境下实现对自身的定位极具挑战,根据无人机所处环境不同,位姿估计的传感器也各不相同,常见的室外定位方式有全球定位系统(GPS)和惯性测量单元(IMU)等,室内定位方式有相机(SLAM)。目前大部分传感器通常只能针对某个特定的场景,不具有通用性,应用场景大大受限。
基于此,本发明采用了多种传感器融合的方法,从多个传感器中采集尽可能多的信息,利用改进的扩展卡尔曼滤波算法进行数据融合以达到对自身位姿估计最优并在不同场景某个传感器失效的情况下系统也能正常工作。该系统具有鲁棒性好,容错率高,环境适应性强等特点。
发明内容
本发明的目的在于克服现有技术的不足,提供一种基于多传感器融合的四旋翼无人机定位方法,通过改进的扩展卡尔曼滤波器融合相机视觉(SLAM),全球定位系统(GPS),惯性测量单元(IMU),超声波多个传感器的数据,将多种传感器进行数据融合,实现在不同环境下准确估计四轴飞行器的位姿并进行导航。
为实现上述发明目的,本发明一种基于多传感器融合的四旋翼无人机定位方法,其特征在于,包括以下步骤:
(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维的单位阵;设置过程噪声误差矩阵其中,σp为位置误差因子,为速度误差因子,σq为姿态误差因子,σba为IMU偏置误差;
(5.2)、计算机载IMU传感器在t1时刻及t1-1时刻采集数据的时间差Δt,建立四旋翼无人机的运动学预测矩阵F;
(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);
记视觉传感器的测量值为Z=[p2,q2],然后更新卡尔曼滤波器的系统状态量X'=X+K(Z-HX);更新状态协方差矩阵P'=(I-KH)P;
(5.7)、输出更新后的系统状态量X',然后提取X'中的p,q,作为融合后的视觉惯导定位数据;
(6)、基于图优化方法融合视觉惯导定位及GPS和超声波定位;
(6.3)、在L个视觉惯导定位数据中,分别计算相邻两组数据的位姿变换量Tij,i≠j,j∈[1,L];计算图优化方法的状态节点集合S的局部约束因子f(S)1=∑Tij;
(6.4)、读入GPS定位数据p3以及对应时间t3,在图优化状态节点集合S中寻找与时间t3匹配对应的状态节点,如果找到匹配对应的状态节点Mi,则先取Mi中的位置数据再计算状态节点集合S的GPS约束因子若无法找到匹配对应的状态节点,则记f2(S)=0;
(6.5)、读入超声波定位数据h以及对应时间t4,在图优化状态节点集合S中寻找与时间t4匹配对应的状态节点,如果找到匹配对应的状态节点Mi,则先取Mi中的位置数据再提取中z轴值作为高度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估计四旋翼无人机的三轴速度、三轴位置和三轴姿态角;
S2.1、利用机载的RGB-D深度相机拍摄连续的多帧原始彩色图像及对应的深度图像;
S2.2、将每一帧原始彩色图像按照不同比例缩放进行降采样,得到8张缩放比例不同的图像;再将每一张图像均匀分成多个图像网格,然后检测每个图像网格的FAST角点,将8张图像提取的所有FAST角点组成角点集合,即每一帧原始彩色图像构建一个角点集合;
S2.3、提取相邻两帧深度图像,以及对应原始彩色图像构建的角点集合,将角点集合中的每个角点进行ORB特征描述子;然后基于每个角点的ORB特征描述子,对两个角点集合中的每个角点进行特征点映射,得到3D-2D的匹配点对,ORB特征点作为地图点,生成初始局部地图;
S2.4、利用PNP算法求解出这两帧图像间的位姿变化,计算得到四轴飞行器当前位姿的估计值。
S2.5、将前后相邻两帧图像中的后一帧图像作为当前帧,然后根据当前位姿的估计值跟踪初始局部地图,将初始局部地图的地图点投影到当前帧,得到地图点与当前帧的ORB特征点的匹配点对,根据匹配点对,通过局部BA算法对当前位姿的估计值进行优化,得到优化后的空间位置和旋转姿态;
S2.6、利用D-BOW2词袋模型对实时图像进行回环检测,当检测到无人机重新经过曾经到达过的场景时,回环检测模块计算SIM3变换,随后采用BA优化模型,进一步优化位姿,消除视觉定位系统的累积误差,获得一个更为理想的三轴位置和三轴姿态
S3.1、四旋翼无人机通过串口解析GPS信号,获得GPS信号中提供的经纬度和高度;
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维的单位阵;设置过程噪声误差矩阵其中,σp为位置误差因子,为速度误差因子,σq为姿态误差因子,σba为IMU偏置误差,在本实施例中所有的误差因子取2,可根据实际运行情况适当调整;
S5.2、计算机载IMU传感器在t1时刻及t1-1时刻采集数据的时间差Δt,建立四旋翼无人机的运动学预测矩阵F;
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;
记视觉传感器的测量值为Z=[p2,q2],然后更新卡尔曼滤波器的系统状态量X'=X+K(Z-HX);更新状态协方差矩阵P'=(I-KH)P,如图2中的(c)所示,在更新完状态后重新利用预测方程将系统状态预测到最新一帧的状态上;
S5.7、输出更新后的系统状态量X',然后提取X'中的p,q,作为融合后的视觉惯导定位数据;
S6、基于图优化方法融合视觉惯导定位及GPS和超声波定位;
S6.3、在L个视觉惯导定位数据中,分别计算相邻两组数据的位姿变换量Tij,i≠j,j∈[1,L];计算图优化方法的状态节点集合S的局部约束因子f(S)1=ΣTij;
S6.4、读入GPS定位数据p3以及对应时间t3,在图优化状态节点集合S中寻找与时间t3匹配对应的状态节点,如果找到匹配对应的状态节点Mi,则先取Mi中的位置数据再计算状态节点集合S的GPS约束因子若无法找到匹配对应的状态节点,则记f2(S)=0;
S6.5、读入超声波定位数据h以及对应时间t4,在图优化状态节点集合S中寻找与时间t4匹配对应的状态节点,如果找到匹配对应的状态节点Mi,则先取Mi中的位置数据再提取中z轴值作为高度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 (1)
1.一种基于多传感器融合的四旋翼无人机定位方法,其特征在于,包括以下步骤:
(4)、利用超声波传感器计算估计四旋翼无人机在t4时刻的飞行高度h;
(4.1)、四旋翼无人机通过串口解析超声波信号,获得超声波信号中提供的高度h';
(4.2)、根据四旋翼无人机的姿态,对此飞行高度h进行修正:h=cos(Pitch)*cos(Roll)*h',Pitch为四旋翼无人机的俯仰角,Roll为四旋翼无人机的横滚角;
(5)、使用卡尔曼滤波进行视觉惯导定位;
(6)、基于图优化方法融合视觉惯导定位及GPS和超声波定位,得到四旋翼无人机的最终定位;
其中,所述视觉惯导定位的具体步骤为:
(2.1)、设置卡尔曼滤波器的系统状态量X=[p,q,v,ba,bω],其中,p,q,v,ba,bω分别为四旋翼无人机的位置、姿态、速度、IMU的偏置、IMU的随机游走噪声;设置卡尔曼滤波器中相机传感器的测量噪声R,R为六维单位矩阵;设置状态协方差矩阵P,初始值为15维的单位阵;设置过程噪声误差矩阵其中,σp为位置误差因子,为速度误差因子,σq为姿态误差因子,σba为IMU偏置误差;
(2.2)、计算机载IMU传感器在t1时刻及t1-1时刻采集数据的时间差Δt,建立四旋翼无人机的运动学预测矩阵F;
(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);
记视觉传感器的测量值为Z=[p2,q2],然后更新卡尔曼滤波器的系统状态量X'=X+K(Z-HX);更新状态协方差矩阵P'=(I-KH)P;
(2.7)、输出更新后的系统状态量X',然后提取X'中的p,q,作为融合后的视觉惯导定位数据;
其中,所述的基于图优化方法融合视觉惯导定位及GPS和超声波定位的具体过程为:
(3.3)、在L个视觉惯导定位数据中,分别计算相邻两组数据的位姿变换量Tij,i≠j,j∈[1,L];计算图优化方法的状态节点集合S的局部约束因子f(S)1=∑Tij;
(3.4)、读入GPS定位数据p3以及对应时间t3,在图优化状态节点集合S中寻找与时间t3匹配对应的状态节点,如果找到匹配对应的状态节点Mi,则先取Mi中的位置数据再计算状态节点集合S的GPS约束因子若无法找到匹配对应的状态节点,则记f2(S)=0;
(3.5)、读入超声波定位数据h以及对应时间t4,在图优化状态节点集合S中寻找与时间t4匹配对应的状态节点,如果找到匹配对应的状态节点Mi,则先取Mi中的位置数据再提取中z轴值作为高度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个时刻融合后的位姿。
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 CN113465596A (zh) | 2021-10-01 |
CN113465596B true 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) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114419109B (zh) * | 2022-03-29 | 2022-06-24 | 中航金城无人系统有限公司 | 一种基于视觉和气压信息融合的飞行器定位方法 |
CN114755693B (zh) * | 2022-06-15 | 2022-09-16 | 天津大学四川创新研究院 | 基于多旋翼无人机的基建设施测量系统和方法 |
Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109376785A (zh) * | 2018-10-31 | 2019-02-22 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
Family Cites Families (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US11199414B2 (en) * | 2016-09-14 | 2021-12-14 | Zhejiang University | Method for simultaneous localization and mapping |
CN106679648B (zh) * | 2016-12-08 | 2019-12-10 | 东南大学 | 一种基于遗传算法的视觉惯性组合的slam方法 |
CN107869989B (zh) * | 2017-11-06 | 2020-02-07 | 东北大学 | 一种基于视觉惯导信息融合的定位方法及系统 |
CN109147058B (zh) * | 2018-08-31 | 2022-09-20 | 腾讯科技(深圳)有限公司 | 用于视觉惯导信息融合的初始化方法和装置以及存储介质 |
CN111121767B (zh) * | 2019-12-18 | 2023-06-30 | 南京理工大学 | 一种融合gps的机器人视觉惯导组合定位方法 |
CN111880573B (zh) * | 2020-07-31 | 2022-09-06 | 电子科技大学 | 一种基于视觉惯导融合的四旋翼自主导航方法 |
-
2021
- 2021-06-25 CN CN202110712202.9A patent/CN113465596B/zh active Active
Patent Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109376785A (zh) * | 2018-10-31 | 2019-02-22 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
Also Published As
Publication number | Publication date |
---|---|
CN113465596A (zh) | 2021-10-01 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110243358B (zh) | 多源融合的无人车室内外定位方法及系统 | |
CN109887057B (zh) | 生成高精度地图的方法和装置 | |
CN109991636B (zh) | 基于gps、imu以及双目视觉的地图构建方法及系统 | |
CN108731670B (zh) | 基于量测模型优化的惯性/视觉里程计组合导航定位方法 | |
CN110030994B (zh) | 一种基于单目的鲁棒性视觉惯性紧耦合定位方法 | |
CN108036785A (zh) | 一种基于直接法与惯导融合的飞行器位姿估计方法 | |
CN106767785B (zh) | 一种双回路无人机的导航方法及装置 | |
CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
CN110296702A (zh) | 视觉传感器与惯导紧耦合的位姿估计方法及装置 | |
CN113465596B (zh) | 一种基于多传感器融合的四旋翼无人机定位方法 | |
CN108253963A (zh) | 一种基于多传感器融合的机器人自抗扰定位方法以及定位系统 | |
CN109238277B (zh) | 视觉惯性数据深度融合的定位方法及装置 | |
CN112577493B (zh) | 一种基于遥感地图辅助的无人机自主定位方法及系统 | |
CN112556719B (zh) | 一种基于cnn-ekf的视觉惯性里程计实现方法 | |
Mercado et al. | Gps/ins/optic flow data fusion for position and velocity estimation | |
CN115574816B (zh) | 仿生视觉多源信息智能感知无人平台 | |
CN110081878A (zh) | 一种多旋翼无人机的姿态及位置确定方法 | |
CN109506660A (zh) | 一种用于仿生导航的姿态最优化解算方法 | |
CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
CN112444245B (zh) | 一种基于偏振光、光流矢量、双目视觉传感器的仿昆虫视觉组合导航方法 | |
CN108444468B (zh) | 一种融合下视视觉与惯导信息的定向罗盘 | |
CN109521785A (zh) | 一种随身拍智能旋翼飞行器系统 | |
CN117739972B (zh) | 一种无全球卫星定位系统的无人机进近阶段定位方法 | |
CN113110556B (zh) | 一种基于视觉传感器的无人机位置估计系统及估计方法 | |
Li et al. | Combined RGBD-inertial based state estimation for MAV in GPS-denied indoor environments |
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 |