CN109612462A - 多传感器数据融合定位方法及其安防机器人 - Google Patents

多传感器数据融合定位方法及其安防机器人 Download PDF

Info

Publication number
CN109612462A
CN109612462A CN201910105146.5A CN201910105146A CN109612462A CN 109612462 A CN109612462 A CN 109612462A CN 201910105146 A CN201910105146 A CN 201910105146A CN 109612462 A CN109612462 A CN 109612462A
Authority
CN
China
Prior art keywords
fusion
predicted
data
location data
uncertain probability
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.)
Pending
Application number
CN201910105146.5A
Other languages
English (en)
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.)
Changsha Wanwei Robot Co Ltd
Original Assignee
Changsha Wanwei Robot 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 Changsha Wanwei Robot Co Ltd filed Critical Changsha Wanwei Robot Co Ltd
Priority to CN201910105146.5A priority Critical patent/CN109612462A/zh
Publication of CN109612462A publication Critical patent/CN109612462A/zh
Pending legal-status Critical Current

Links

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; 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/16Navigation; 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/165Navigation; 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
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20Instruments for performing navigational calculations
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01SRADIO 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/00Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39Determining 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/42Determining position
    • G01S19/45Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47Determining 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

Landscapes

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

Abstract

本发明公开了一种多传感器数据融合定位方法,包括采用GPS传感器和惯性导航单元作为定位传感器;获取GPS传感器和惯性导航单元的定位数据;利用惯性导航单元的定位数据对位置进行预测,利用GPS传感器的定位数据作为观测数据对定位进行观测,并计算得到预测位置的不确定性概率;将预测数据和观测数据融合并计算得到融合位置的不确定性概率;选取最终的定位数据作为定位结果,完成多传感器数据融合的定位。本发明还公开了包括所述多传感器数据融合定位方法的安防机器人。本发明可靠性高,而且定位精度相对较好。

Description

多传感器数据融合定位方法及其安防机器人
技术领域
本发明具体涉及一种多传感器数据融合定位方法及其安防机器人。
背景技术
随着经济技术的发展,智能机器人已经逐步应用于人们的生产和生活,给人们的生产和生活带来了无尽的便利。
由于智能机器人一般都需要在工作环境中随意移动并完成相应的动作,因此智能机器人的定位就显得尤为重要。精确的对智能机器人进行定位,能够有效帮助智能机器人完成对应的动作,从而提高智能机器人的可靠性。
目前,现有的定位技术,一般情况下都是基于单一传感器的定位系统。如市面上用得较多的传感器有GPS(Global Positioning System,全球定位系统)、超声波、imu(Inertial Measurement Unit,惯性导航单元)及里程计。GPS精度高,其精度可以达到厘米级,适用于室外,但是其发布频率比较低,一般为单位级别的赫兹。超声波价格便宜,但是有一定的盲区。imu包括加速度计和陀螺仪,发布频率较快,但是有一定的累计误差。里程计价格低廉,发布频率快,也有较大的累计误差。所以,针对这些传感器的优缺点,市面上很多机器人都安装了这几类传感器,以达到互补。
比如,专利CN201310015632提出了一种分布式多传感器智能信息融合方法,该方法采用4个超声波数据,分别对其进行卡尔曼滤波处理,在此基础上,再进行时间对准、坐标转换和最优融合,拟合成一条新的轨迹。但是,该方法主要只是采用了多个一种传感器数据进行融合。或者专利CN201511006444.7,虽然提出了基于多传感器融合的单人定位导航器和定位导航方法,该方法融合了全球卫星定位系统、气压计、三轴陀螺仪、三轴加速度计和三轴地磁传感器数据,利用卡尔曼滤波算法进行多信息融合,以输出定位导航参数。但是该方法融合数据比较多,在融合计算的过程中,在计算姿态的过程中,会有非线性的情况,因为采用卡尔曼滤波有一定的风险性。
因此,现有的多传感器数据定位方法,均存在定位精度相对较差,可靠性不高的缺点。
发明内容
本发明的目的之一在于提供一种可靠性高且定位精度相对较好的多传感器数据融合定位方法。
本发明的目的之二在于提供一种安防机器人,该安防机器人采用所述的多传感器数据融合定位方法进行定位。
本发明提供的这种多传感器数据融合定位方法,包括如下步骤:
S1.采用GPS传感器和惯性导航单元(imu)作为定位传感器;
S2.获取GPS传感器的定位数据和惯性导航单元的定位数据;
S3.利用惯性导航单元的定位数据对位置进行预测,利用GPS传感器的定位数据作为观测数据对定位进行观测,并计算得到预测位置的不确定性概率;
S4.将步骤S3的预测数据和观测数据融合,并计算得到融合位置的不确定性概率;
S5.根据步骤S3得到的预测位置的不确定性概率和步骤S4得到的融合位置的不确定性概率,选取最终的定位数据作为定位结果,完成多传感器数据融合的定位。
步骤S3所述的利用惯性导航单元的定位数据对位置进行预测,具体为采用卡尔曼滤波器,利用惯性导航单元的定位数据对位置进行预测。
所述的卡尔曼滤波器,具体为采用如下算式建立卡尔曼滤波器:
x(t)-1=F*x(t-1)+B*u(t-1)+w(t)
z(t)=H*x(t)+v(t)
式中x(t)-1为系统在t时刻根据上一时刻(t-1)所预测的状态,x(t-1)为上一时刻(t-1)的状态,F为状态转移矩阵,B为控制矩阵,u(t-1)为上一时刻(t-1)的控制量,w(t)为服从高斯分布的过程噪声,z(t)为观测量,H为观测矩阵,x(t)为当前状态,v(t)为服从高斯分布的观测噪声。
步骤S3所述的计算得到预测位置的不确定性概率,具体为采用如下算式计算预测位置的不确定性概率:
P(t)-1=F*P(t-1)*FT+Q
式中P(t)-1为系统在t时刻根据上一时刻(t-1)所预测的预测位置的不确定性概率,F为状态转移矩阵,P(t-1)为状态协方差矩阵,Q为状态转移协方差矩阵。
步骤S4所述的将步骤S3的预测数据和观测数据融合,具体为采用如下算式进行融合:
K(t)=P(t)-1*HT*(H*P(t)-1*HT+R)-1
x(t)=x(t)-1+K(t)*(z(t)-H*x(t)-1)
式中K(t)为时刻t的卡尔曼增益,P(t)-1为系统在t时刻根据上一时刻(t-1)所预测的预测位置的不确定性概率,H为观测矩阵,R为观测噪声的方差,x(t)为预测数据和观测数据的融合结果,x(t)-1为系统在t时刻根据上一时刻(t-1)所预测的状态,z(t)为观测量。
步骤S4所述的计算得到融合位置的不确定性概率,具体为采用如下算式计算融合位置的不确定性概率:
P(t)=(I-K(t)*H)*P(t)-1
式中P(t)为融合位置的不确定性概率,I为单位矩阵,K(t)为时刻t的卡尔曼增益,H为观测矩阵,P(t)-1为系统在t时刻根据上一时刻(t-1)所预测的预测位置的不确定性概率。
步骤S5所述的选取最终的定位数据作为定位结果,具体为采用如下规则选取定位数据:
若融合位置的不确定性概率小于或等于预测位置的不确定性概率,则选取预测数据和观测数据的融合结果作为最终的定位结果;
若融合位置的不确定性概率大于预测位置的不确定性概率,则只选取GPS传感器的定位数据作为最终的定位数据。
本发明还提供了一种安防机器人,该安防机器人采用所述的多传感器数据融合定位方法进行定位。
本发明提供的这种多传感器数据融合定位方法及其安防机器人,结合GPS传感器数据,消除了惯性导航单元的累计误差,得到了误差值更小的定位值;同时结合imu传感器数据,补偿了在高速运行下GPS数据的发布频率慢的缺陷,从而可以更加精准的执行定位与导航;因此本发明的融合定位方法,可靠性高,而且定位精度相对较好。
附图说明
图1为本发明方法的方法流程图。
具体实施方式
如图1所示为本发明方法的方法流程图:本发明提供的这种多传感器数据融合定位方法,包括如下步骤:
S1.采用GPS传感器和惯性导航单元(imu)作为定位传感器;
S2.获取GPS传感器的定位数据和惯性导航单元的定位数据;
S3.利用惯性导航单元的定位数据对位置进行预测,利用GPS传感器的定位数据作为观测数据对定位进行观测,并计算得到预测位置的不确定性概率;具体为采用卡尔曼滤波器,利用惯性导航单元的定位数据对位置进行预测;
在具体实施时,采用如下算式建立卡尔曼滤波器:
x(t)-1=F*x(t-1)+B*u(t-1)+w(t)
z(t)=H*x(t)+v(t)
式中x(t)-1为系统在t时刻根据上一时刻(t-1)所预测的状态,x(t-1)为上一时刻(t-1)的状态,F为状态转移矩阵,B为控制矩阵,u(t-1)为上一时刻(t-1)的控制量,w(t)为服从高斯分布的过程噪声,z(t)为观测量,H为观测矩阵,x(t)为当前状态,v(t)为服从高斯分布的观测噪声;
当用在安防机器人上时,状态转移矩阵F、控制矩阵B和观测矩阵H可以根据具体的安防机器人的数据和现代控制理论等现有技术,由技术人员自行建立即可;
在具体定位时,采用惯性导航单元的定位数据对位置进行预测,即采用惯性导航单元的定位数据作为x(t)-1,采用GPS传感器的定位数据作为观测数据,即采用GPS传感器的定位数据作为z(t);
同时,采用如下算式计算预测位置的不确定性概率:
P(t)-1=F*P(t-1)*FT+Q
式中P(t)-1为系统在t时刻根据上一时刻(t-1)所预测的预测位置的不确定性概率,F为状态转移矩阵,P(t-1)为状态协方差矩阵,Q为状态转移协方差矩阵;
S4.将步骤S3的预测数据和观测数据融合,并计算得到融合位置的不确定性概率;
在具体实施时,采用如下算式进行融合:
K(t)=P(t)-1*HT*(H*P(t)-1*HT+R)-1
x(t)=x(t)-1+K(t)*(z(t)-H*x(t)-1)
式中K(t)为时刻t的卡尔曼增益,P(t)-1为系统在t时刻根据上一时刻(t-1)所预测的预测位置的不确定性概率,H为观测矩阵,R为观测噪声的方差,x(t)为预测数据和观测数据的融合结果,x(t)-1为系统在t时刻根据上一时刻(t-1)所预测的状态,z(t)为观测量;z(t)-H*x(t)-1为预测和观测间的残差,该残差的偏向跟卡尔曼增益K(t)相关,若置信预测模型,则K(t)就会小,若置信观测模型,则K(t)就会增大;
同时,采用如下算式计算融合位置的不确定性概率:
P(t)=(I-K(t)*H)*P(t)-1
式中P(t)为融合位置的不确定性概率,I为单位矩阵,K(t)为时刻t的卡尔曼增益,H为观测矩阵,P(t)-1为系统在t时刻根据上一时刻(t-1)所预测的预测位置的不确定性概率;
S5.根据步骤S3得到的预测位置的不确定性概率和步骤S4得到的融合位置的不确定性概率,选取最终的定位数据作为定位结果,完成多传感器数据融合的定位;具体为采用如下规则选取定位数据:
若融合位置的不确定性概率小于或等于预测位置的不确定性概率,则选取预测数据和观测数据的融合结果作为最终的定位结果;
若融合位置的不确定性概率大于预测位置的不确定性概率,则只选取GPS传感器的定位数据作为最终的定位数据。

Claims (8)

1.一种多传感器数据融合定位方法,包括如下步骤:
S1.采用GPS传感器和惯性导航单元作为定位传感器;
S2.获取GPS传感器的定位数据和惯性导航单元的定位数据;
S3.利用惯性导航单元的定位数据对位置进行预测,利用GPS传感器的定位数据作为观测数据对定位进行观测,并计算得到预测位置的不确定性概率;
S4.将步骤S3的预测数据和观测数据融合,并计算得到融合位置的不确定性概率;
S5.根据步骤S3得到的预测位置的不确定性概率和步骤S4得到的融合位置的不确定性概率,选取最终的定位数据作为定位结果,完成多传感器数据融合的定位。
2.根据权利要求1所述的多传感器数据融合定位方法,其特征在于步骤S3所述的利用惯性导航单元的定位数据对位置进行预测,具体为采用卡尔曼滤波器,利用惯性导航单元的定位数据对位置进行预测。
3.根据权利要求2所述的多传感器数据融合定位方法,其特征在于所述的卡尔曼滤波器,具体为采用如下算式建立卡尔曼滤波器:
x(t)-1=F*x(t-1)+B*u(t-1)+w(t)
z(t)=H*x(t)+v(t)
式中x(t)-1为系统在t时刻根据上一时刻(t-1)所预测的状态,x(t-1)为上一时刻(t-1)的状态,F为状态转移矩阵,B为控制矩阵,u(t-1)为上一时刻(t-1)的控制量,w(t)为服从高斯分布的过程噪声,z(t)为观测量,H为观测矩阵,x(t)为当前状态,v(t)为服从高斯分布的观测噪声。
4.根据权利要求3所述的多传感器数据融合定位方法,其特征在于步骤S3所述的计算得到预测位置的不确定性概率,具体为采用如下算式计算预测位置的不确定性概率:
P(t)-1=F*P(t-1)*FT+Q
式中P(t)-1为系统在t时刻根据上一时刻(t-1)所预测的预测位置的不确定性概率,F为状态转移矩阵,P(t-1)为状态协方差矩阵,Q为状态转移协方差矩阵。
5.根据权利要求4所述的多传感器数据融合定位方法,其特征在于步骤S4所述的将步骤S3的预测数据和观测数据融合,具体为采用如下算式进行融合:
K(t)=P(t)-1*HT*(H*P(t)-1*HT+R)-1
x(t)=x(t)-1+K(t)*(z(t)-H*x(t)-1)
式中K(t)为时刻t的卡尔曼增益,P(t)-1为系统在t时刻根据上一时刻(t-1)所预测的预测位置的不确定性概率,H为观测矩阵,R为观测噪声的方差,x(t)为预测数据和观测数据的融合结果,x(t)-1为系统在t时刻根据上一时刻(t-1)所预测的状态,z(t)为观测量。
6.根据权利要求5所述的多传感器数据融合定位方法,其特征在于步骤S4所述的计算得到融合位置的不确定性概率,具体为采用如下算式计算融合位置的不确定性概率:
P(t)=(I-K(t)*H)*P(t)-1
式中P(t)为融合位置的不确定性概率,I为单位矩阵,K(t)为时刻t的卡尔曼增益,H为观测矩阵,P(t)-1为系统在t时刻根据上一时刻(t-1)所预测的预测位置的不确定性概率。
7.根据权利要求6所述的多传感器数据融合定位方法,其特征在于步骤S5所述的选取最终的定位数据作为定位结果,具体为采用如下规则选取定位数据:
若融合位置的不确定性概率小于或等于预测位置的不确定性概率,则选取预测数据和观测数据的融合结果作为最终的定位结果;
若融合位置的不确定性概率大于预测位置的不确定性概率,则只选取GPS传感器的定位数据作为最终的定位数据。
8.一种安防机器人,其特征在于包括了权利要求1~7之一所述的多传感器数据融合定位方法。
CN201910105146.5A 2019-02-01 2019-02-01 多传感器数据融合定位方法及其安防机器人 Pending CN109612462A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910105146.5A CN109612462A (zh) 2019-02-01 2019-02-01 多传感器数据融合定位方法及其安防机器人

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910105146.5A CN109612462A (zh) 2019-02-01 2019-02-01 多传感器数据融合定位方法及其安防机器人

Publications (1)

Publication Number Publication Date
CN109612462A true CN109612462A (zh) 2019-04-12

Family

ID=66021162

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910105146.5A Pending CN109612462A (zh) 2019-02-01 2019-02-01 多传感器数据融合定位方法及其安防机器人

Country Status (1)

Country Link
CN (1) CN109612462A (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110936390A (zh) * 2019-12-18 2020-03-31 国网山东省电力公司信息通信公司 一种带机械臂的信息机房巡检机器人
CN112033412A (zh) * 2020-09-07 2020-12-04 中国南方电网有限责任公司超高压输电公司天生桥局 一种提高巡检机器人定位精度的方法及装置
CN112415558A (zh) * 2021-01-25 2021-02-26 腾讯科技(深圳)有限公司 行进轨迹的处理方法及相关设备
CN112985417A (zh) * 2021-04-19 2021-06-18 长沙万为机器人有限公司 移动机器人粒子滤波定位的位姿校正方法及移动机器人

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
余学祥等: "《GPS测量与数据处理》", 31 October 2013, 中国矿业大学出版社 *
孟维晓等: "《卫星定位导航原理》", 30 September 2013, 哈尔滨工业大学出版社 *
张光明: "《现代导航技术与方法》", 30 September 2017 *
胡小平: "《导航技术基础》", 31 July 2015, 国防工业出版社 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110936390A (zh) * 2019-12-18 2020-03-31 国网山东省电力公司信息通信公司 一种带机械臂的信息机房巡检机器人
CN112033412A (zh) * 2020-09-07 2020-12-04 中国南方电网有限责任公司超高压输电公司天生桥局 一种提高巡检机器人定位精度的方法及装置
CN112415558A (zh) * 2021-01-25 2021-02-26 腾讯科技(深圳)有限公司 行进轨迹的处理方法及相关设备
CN112985417A (zh) * 2021-04-19 2021-06-18 长沙万为机器人有限公司 移动机器人粒子滤波定位的位姿校正方法及移动机器人

Similar Documents

Publication Publication Date Title
CN109612462A (zh) 多传感器数据融合定位方法及其安防机器人
Shen et al. Observability analysis and adaptive information fusion for integrated navigation of unmanned ground vehicles
CN103472823B (zh) 一种智能机器人用的栅格地图创建方法
CN109752725A (zh) 一种低速商用机器人、定位导航方法及定位导航系统
CN109186610B (zh) 一种auv地形匹配导航的鲁棒bslam方法
CN107976193A (zh) 一种行人航迹推断方法、装置、航迹推断设备及存储介质
CN111947644B (zh) 一种室外移动机器人定位方法、系统及其电子设备
CN107561570B (zh) 一种拖挂车辆的位置和姿态测量方法及系统
CN108387236A (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN111487960A (zh) 一种基于定位能力估计的移动机器人路径规划方法
CN108036792A (zh) 一种用于移动机器人的里程计与测量位姿的数据融合方法
CN104316058B (zh) 一种采用交互多模型的移动机器人wsn/ins组合导航方法
Zong et al. Research on obstacle avoidance method for mobile robot based on multisensor information fusion
Bai et al. A novel plug-and-play factor graph method for asynchronous absolute/relative measurements fusion in multisensor positioning
CN114879660A (zh) 一种基于目标驱动的机器人环境感知方法
Magnago et al. Optimal landmark placement for indoor positioning using context information and multi-sensor data
Kang et al. Ultra-wideband aided UAV positioning using incremental smoothing with ranges and multilateration
CN117724059A (zh) 基于卡尔曼滤波算法的多源传感器融合航迹校正方法
CN206095257U (zh) 智能巡视机器人组合导航系统
CN113029173A (zh) 车辆导航方法及装置
CN116225029B (zh) 一种机器人路径规划方法
CN116202516B (zh) 一种轨道bim多维参数辅助imu的轨道三维重建方法
Cheng et al. Graph-based proprioceptive localization using a discrete heading-length feature sequence matching approach
CN116233755A (zh) 室内定位方法、装置、计算机设备及存储介质
Zhang et al. Indoor relative positioning method and experiment based on inertial measurement information/human motion model/UWB combined system

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
RJ01 Rejection of invention patent application after publication

Application publication date: 20190412

RJ01 Rejection of invention patent application after publication