CN115334448A - 一种基于蓝牙和惯性传感器的无人自跟随装置的精确动态定位方法 - Google Patents
一种基于蓝牙和惯性传感器的无人自跟随装置的精确动态定位方法 Download PDFInfo
- Publication number
- CN115334448A CN115334448A CN202210974622.9A CN202210974622A CN115334448A CN 115334448 A CN115334448 A CN 115334448A CN 202210974622 A CN202210974622 A CN 202210974622A CN 115334448 A CN115334448 A CN 115334448A
- Authority
- CN
- China
- Prior art keywords
- bluetooth
- following device
- inertial sensor
- positioning
- coordinate
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 41
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 27
- 238000001914 filtration Methods 0.000 claims abstract description 11
- 239000011159 matrix material Substances 0.000 claims description 21
- 230000001133 acceleration Effects 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 8
- 238000005070 sampling Methods 0.000 claims description 6
- 238000009795 derivation Methods 0.000 claims description 3
- 230000010354 integration Effects 0.000 claims description 3
- 239000000463 material Substances 0.000 claims description 3
- LFQSCWFLJHTTHZ-UHFFFAOYSA-N Ethanol Chemical compound CCO LFQSCWFLJHTTHZ-UHFFFAOYSA-N 0.000 claims description 2
- 238000005259 measurement Methods 0.000 claims description 2
- 238000005516 engineering process Methods 0.000 description 7
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000007499 fusion processing Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Images
Classifications
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/02—Services making use of location information
- H04W4/025—Services making use of location information using location based information parameters
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/02—Services making use of location information
- H04W4/029—Location-based management or tracking services
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/80—Services using short range communication, e.g. near-field communication [NFC], radio-frequency identification [RFID] or low energy communication
-
- 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
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D30/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
Landscapes
- Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Signal Processing (AREA)
- Navigation (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
一种基于蓝牙和惯性传感器的无人自跟随装置的精确动态定位方法,定位步骤如下:1)四个蓝牙基站分别获得与蓝牙信标的距离,采用四点定位算法解算观测坐标数据;利用惯性传感器推算出先验坐标数据;2)将先验坐标数据和观测坐标数据通过卡尔曼滤波算法融合得到后验坐标数据,对无人自跟随装置进行精确定位;3)当出现断连现象时,以断连前最后一次定位到蓝牙信标的位置坐标为目标点,使用惯性传感器进行惯性导航,控制无人自跟随装置移动到目标点等待重新连接,重新连上蓝牙信号或在移动到目标点的过程中重新连上蓝牙信号,则继续利用蓝牙和惯性传感器的信号进行精确定位。
Description
技术领域
本发明属于无人自跟随装置控制领域,特别涉及一种基于蓝牙和惯性传感器的无人自跟随装置的精确动态定位方法。
背景技术
近年来,随着计算机技术和无线通信技术的发展,智能化的设备在人们生活中的出现越来越频繁,尤其是无人自动跟随装置。以无人机为例,无人机在军事和民用领域都得到了广泛的应用,其中自动跟随技术作为无人机的核心技术之一,得到了学术界和工业界的广泛研究,自动跟随技术的实现主要有两种技术方案:(1)基于GPS定位的自动跟随,这种技术方案需要在无人机和移动设备(比如遥控器)内部分别安装GPS模块,通过GPS的精确定位来确定无人机和被拍摄者的相对位置,飞控***根据两者的相对位置控制无人机实现自动跟随功能,该方案必须保证GPS信号的准确性,在信号较弱的山区或者室内无法使用;(2)基于视觉的跟随,该技术要求跟随目标相对周边环境辨识度较高,同时跟随目标需要一直保持在摄像头的捕捉范围内,“找不到目标物”将导致视觉跟随被迫暂时停止。
发明内容
本发明的目的是针对现有技术的不足,提供一种基于蓝牙和惯性传感器的无人自跟随装置的精确动态定位方法。本发明所述方法通过利用惯性传感器和蓝牙RSSI测距定位进行数据融合,实现了自动跟随装置相对于移动端的精确动态定位,既没有GPS自跟随无法在信号较差的山区或室内工作的缺点,对环境辨识度的要求也没有视觉自跟随那么高,同时蓝牙信号在跟惯性传感器的数据进行了基于卡尔曼滤波的数据融合处理后还解决了信号不稳定和误差较大的问题。
本发明的技术方案是:
一种基于蓝牙和惯性传感器的无人自跟随装置的精确动态定位方法,
包括由设有惯性传感器和四个蓝牙基站的无人自跟随装置与手持式蓝牙信标组成的动态定位***,该定位***按以下步骤定位:
1)四个蓝牙基站分别通过RSSI测距法获得各自与蓝牙信标的距离,采用四点定位算法解算出蓝牙信标的位置坐标作为观测坐标数据;同时利用惯性传感器解算出的无人自跟随装置速度推算出蓝牙信标位置坐标作为先验坐标数据;
2)将先验坐标数据和观测坐标数据通过卡尔曼滤波算法融合得到后验坐标数据,对无人自跟随装置进行精确定位;
3)当蓝牙基站与蓝牙信标出现断连现象时,以断连前最后一次定位到蓝牙信标的位置坐标为目标点,使用惯性传感器进行惯性导航,控制无人自跟随装置移动到目标点停滞等待重新连接蓝牙信号,一旦重新连上蓝牙信号,则继续利用蓝牙和惯性传感器的信号进行精确定位;如果无人自跟随装置在移动到目标点的过程中重新连上蓝牙信号,则继续利用蓝牙和惯性传感器的信号进行精确定位。
进一步地,所述蓝牙信标集成在遥控器中,或采用智能手机蓝牙作为蓝牙信标。
进一步地,所述无人自跟随装置为无人机或物料运输车。
进一步地,所述蓝牙信标为蓝牙5.0信标,所述蓝牙基站为蓝牙5.0基站或信号发射器。
进一步地,步骤1)所述RSSI测距方法为,利用接收端与发射端之间的距离,构建对数信号强度-距离衰减模型,将每个蓝牙基站测得的信号强度数据,换算成蓝牙基站与蓝牙信标之间的相对距离,蓝牙信号在发射端和接收端的信号强度关系如下式:
Lp(dB)=L0(dB)+10αlg d+ω(dB)
其中,L0表示1米距离处的信号能量损耗;Lp表示在距离d处的信号能量损耗;α表示路径损耗指数;ω表示误差;d表示发射端和接收端之间的直线距离。
进一步地,步骤1)中,四点定位算法解算蓝牙信标的观测位置坐标的具体过程如下:
(x-x1)2+(y-y1)2+(z-z1)2=R1 2
(x-x2)2+(y-y2)2+(z-z2)2=R2 2
(x-x3)2+(y-y3)2+(z-z3)2=R3 2
(x-x4)2+(y-y4)2+(z-z4)2=R4 2
其中(x1,y1,z1),(x2,y2,z2),(x3,y3,z3),(x4,y4,z4)分别为四个蓝牙基站的坐标,R1,R2,R3,R4分别为四个蓝牙基站测得的与蓝牙信标的相对距离,则(x,y,z)为蓝牙信标的位置坐标;
采用最小二乘法进行估计,对上式进行整理,得到方程解的矩阵形式:
AX=B
式中,A表示系数矩阵,X表示蓝牙信标在无人自跟随装置坐标系下的三维坐标,B表示线性方程组整理后的三维向量;
其误差向量为:ε=AX-B,则有,
F(x)=||ε||2=(AX-B)T(AX-B)
式中,F(x)表示误差向量的二范数的平方;
若要误差最小,则要使得F(x)最小,利用上式对X求导,使其导数为0,计算公式如下:
求解可得X=(ATA)-1ATB,此时误差F(x)最小化,由此可计算得到最终的X坐标。
进一步地,步骤1)采用蓝牙信号和惯性传感器的组合动态定位算法,具体为:
(1)保持无人自跟随装置的z轴高度不变,输入蓝牙信标位置坐标的初始值(x0,y0),该初始值为第一次通过蓝牙RBSI值获得的蓝牙信标坐标位置;
(2)惯性传感器分别测量无人自跟随装置沿x轴的加速度ax和沿y轴的加速度ay,通过对加速度ax和ay积分得到无人自跟随装置沿x轴的速度vx和沿y轴的速度vy;
(3)通过惯性传感器输出的沿z轴的角速度ωz乘以采样时间dt,得到惯性传感器相邻两次采样的航向角变化dθ,公式如下:
dθ=dt·wz;
(4)根据步骤(2)、(3)得到的惯性传感器数据和上一时刻的后验坐标数据(x(k),y(k)),计算下一时刻的先验坐标数据(x(k+1),y(k+1)),计算公式如下:
将上式写成矩阵形式:
(5)通过蓝牙的RSSI测距并结合四点定位算法计算蓝牙信标位置坐标作为观测数据,观测坐标数据方程为:
式中,(x′(k+1),y′(k+1))为k+1时刻的观测坐标数据。
进一步地,步骤2)所述卡尔曼滤波算法为:
(1)初始化参数:
式中,x-表示无人自跟随装置蓝牙信标的x轴先验坐标值,y-表示无人自跟随装置蓝牙信标的y轴先验坐标值,x+表示无人自跟随装置蓝牙信标的x轴后验坐标值,y+表示无人自跟随装置蓝牙信标的y轴后验坐标值,X(0)表示无人自跟随装置蓝牙信标的初始观测坐标数据;
(2)迭代更新最优后验坐标数据:
X-(K+1)=AX+(k)+Bu(k)
P-(K+1)=AP+AT+Q
S=HP-(K+1)HT+R
K=P-(K+1)HTS-1
X+(k+1)=X-(k+1)+K(Z(k+1)-HX-(k+1))
P+(k+1)=(I-KH)P-(k+1)
将第k+1次迭代后的后验状态量X+(K+1)作为最终的定位位置坐标,用以控制无人机实现自动跟随功能;
式中,带上标的数据为先验数据,带上标+的数据为后验数据,P为状态协方差矩阵,是一个迭代更新的量,设其初始值为二阶单位矩阵,Q、R矩阵的参数根据实际工程调试情况进行设置,S表示用于公式推导的中间量,K表示计算得到的卡尔曼增益,Z(k+1)表示蓝牙信标的观测坐标数据。
采用上述技术方案的有益效果:本发明所述方法,定位精度高,定位性能稳定,适用于自动跟随装置对移动目标进行动态精确定位的技术,实现难度低,所需设备的成本低,易于部署。本发明有效降低了因蓝牙信号传输过程中易受到环境影响而产生的定位误差,并提供了信号失联后的解决办法:仅依靠惯性导航控制自动跟随装置寻找蓝牙信号并请求重连信号。本发明为自动跟随装置的自跟随功能实现提供了一种低成本,效果好,难度低的技术方案。
附图说明
图1为蓝牙5.0基站和惯性传感器安装位置示意图;
图2为利用惯性传感器计算信标先验坐标数据的示意图;
图3为无人机自跟随精确动态定位流程图。
具体实施方式
一种基于蓝牙和惯性传感器的无人自跟随装置的精确动态定位方法,
包括由设有惯性传感器和四个蓝牙基站的无人自跟随装置与手持式蓝牙信标组成的动态定位***,
其中,蓝牙基站为蓝牙5.0基站或信号发射器,无人自跟随装置为无人机或无人物料运输车,本实例以无人机为例,手持式蓝牙信标采用蓝牙5.0信标,可集成在无人机的遥控器中,或是采用智能手机蓝牙作为蓝牙信标。
该定位***按以下步骤定位:
1)通过四个蓝牙基站检测手机或手持式蓝牙信标的蓝牙信号强度。
2)对蓝牙信号强度进行平均值滤波处理;
3)四个蓝牙基站分别通过RSSI测距法获得各自与蓝牙信标的距离,然后采用四点定位算法解算出蓝牙信标的位置坐标作为观测坐标数据,具体算法如下:
3-1)所述RSSI测距方法为,利用接收端与发射端之间的距离,构建对数信号强度-距离衰减模型,将每个蓝牙基站测得的信号强度数据,换算成蓝牙基站与蓝牙信标之间的相对距离,蓝牙信号在发射端和接收端的信号强度关系如下式:
Lp(dB)=L0(dB)+10αlg d+ω(dB)
其中,L0表示1米距离处的信号能量损耗;Lp表示在距离d处的信号能量损耗;a表示路径损耗指数;ω表示误差,通常看作是零均值的高斯噪声,其方差与环境有关;d表示发射端和接收端之间的直线距离。
3-2)四点定位算法解算蓝牙信标的观测位置坐标的具体过程如下:
(x-x1)2+(y-y1)2+(z-z1)2=R1 2
(x-x2)2+(y-y2)2+(z-z2)2=R2 2
(x-x3)2+(y-y3)2+(z-z3)2=R3 2
(x-x4)2+(y-y4)2+(z-z4)2=R4 2
其中(x1,y1,z1),(x2,y2,z2),(x3,y3,z3),(x4,y4,z4)分别表示四个蓝牙基站的坐标,R1,R2,R3,R4分别为四个蓝牙基站测得的与蓝牙信标的相对距离,则(x,y,z)为蓝牙信标的位置坐标;
在测得的蓝牙基站与蓝牙信标之间的距离没有误差的情况下,以蓝牙基站为圆心,测得的距离为半径的定位圆球将相交于一点,蓝牙信标即位于该交点上,而蓝牙信号在传播测量过程中,往往会遇到多径及非视距误差,该误差通常为正值,因此会出现多个定位圆两两相交,则具有多个交点。
因此,采用最小二乘法进行估计,对上式进行整理,得到方程解的矩阵形式:
AX=B
式中:A表示系数矩阵,X表示蓝牙信标在无人自跟随装置坐标系下的三维坐标,B表示线性方程组整理后的三维向量;
其误差向量为:ε=AX-B,则有:
F(x)=||ε||2=(AX-B)T(AX-B)
式中,F(x)表示误差向量的二范数的平方;
若要误差最小,则要使得F(x)最小,利用上式对X求导,使其导数为0,计算公式如下:
求解可得X=(ATA)-1ATB,此时误差F(x)最小化,由此可计算得到最终的X坐标,即得到蓝牙信标的位置坐标作为观测坐标数据。
4)利用惯性传感器解算出的无人自跟随装置速度推算出蓝牙信标位置坐标作为先验坐标数据,具体算法为:
由于无人机是定高度跟随,所以z值保持不变,在做飞控算法时将无人机控制在一定飞行高度不变,所以在跟随蓝牙信标的移动过程中可以将三维空间坐标转换为二维坐标来讨论,即仅关心x轴和y轴的值即可;
4-1)保持无人自跟随装置的z轴高度不变,输入蓝牙信标位置坐标的初始值(x0,y0),该初始值为第一次通过蓝牙RSSI值获得的蓝牙信标坐标位置;
4-2)惯性传感器分别测量无人自跟随装置沿x轴的加速度ax和沿y轴的加速度ay,通过对加速度ax和ay积分得到无人自跟随装置沿x轴的速度vx和沿y轴的速度vy;
4-3)通过惯性传感器输出的沿z轴的角速度ωz乘以采样时间dt,得到惯性传感器相邻两次采样的航向角变化dθ,算法的迭代周期为20ms,因此在依次迭代过程中认为蓝牙信标保持精致,算法公式如下:
dθ=dt·wz;
4-4)根据步骤(2)、(3)得到的惯性传感器数据和上一时刻的后验坐标数据(x(k),y(k)),推算下一时刻的先验坐标数据(x(k+1),y(k+1)),计算公式如下:
将上式写成矩阵形式:
4-5)通过蓝牙的RSSI测距并结合四点定位算法计算蓝牙信标位置坐标作为观测数据,观测坐标数据方程为:
式中,(x′(k+1),y′(k+1))为k+1时刻的观测坐标数据,观测坐标数据的具体算法参见步骤3)。
5)将先验坐标数据和观测坐标数据通过卡尔曼滤波算法融合得到后验坐标数据,对无人自跟随装置进行精确定位;
所述卡尔曼滤波算法为:
5-1)初始化参数:
式中,x-表示无人自跟随装置蓝牙信标的x轴先验坐标值,y-表示无人自跟随装置蓝牙信标的y轴先验坐标值,x+表示无人自跟随装置蓝牙信标的x轴后验坐标值,y+表示无人自跟随装置蓝牙信标的y轴后验坐标值,X(0)表示无人自跟随装置蓝牙信标的初始观测坐标数据;
5-2)迭代更新最优后验坐标数据:
X-(K+1)=AX+(k)+Bu(k)
P-(K+1)=AP+AT+Q
S=HP-(K+1)HT+R
K=P-(K+1)HTS-1
X+(k+1)=X-(k+1)+K(Z(k+1)-HX-(k+1))
P+(k+1)=(I-KH)P-(k+1)
将第k+1次迭代后的后验状态量X+(K+1)作为最终的定位位置坐标,用以控制无人机实现自动跟随功能;
式中,带上标的数据为先验数据,带上标+的数据为后验数据,P为状态协方差矩阵,是一个迭代更新的量,设其初始值为二阶单位矩阵,Q、R矩阵的参数根据实际工程调试情况进行设置,S表示用于公式推导的中间量,K表示计算得到的卡尔曼增益,Z(k+1)表示蓝牙信标的观测坐标数据。
6)当蓝牙基站与蓝牙信标出现断连现象时,以断连前最后一次定位到蓝牙信标的位置坐标为目标点,使用惯性传感器进行惯性导航,控制无人自跟随装置移动到目标点停滞等待重新连接蓝牙信号,一旦重新连上蓝牙信号,则继续将蓝牙和惯性传感器的信号通过卡尔曼滤波算法融合得到后验坐标数据,由后验坐标数据对无人机进行精确定位;如果无人自跟随装置在移动到目标点的过程中重新连上蓝牙信号,也将利用蓝牙和惯性传感器的信号通过卡尔曼滤波算法融合得到后验坐标数据,由后验坐标数据进行精确定位。
7)重新连接后,循环上述步骤对无人机进行精确动态定位。
Claims (8)
1.一种基于蓝牙和惯性传感器的无人自跟随装置的精确动态定位方法,其特征在于,
包括由设有惯性传感器和四个蓝牙基站的无人自跟随装置与手持式蓝牙信标组成的动态定位***,该定位***按以下步骤定位:
1)四个蓝牙基站分别通过RSSI测距法获得各自与蓝牙信标的距离,采用四点定位算法解算出蓝牙信标的位置坐标作为观测坐标数据;同时利用惯性传感器解算出的无人自跟随装置速度推算出蓝牙信标位置坐标作为先验坐标数据;
2)将先验坐标数据和观测坐标数据通过卡尔曼滤波算法融合得到后验坐标数据,对无人自跟随装置进行精确定位;
3)当蓝牙基站与蓝牙信标出现断连现象时,以断连前最后一次定位到蓝牙信标的位置坐标为目标点,使用惯性传感器进行惯性导航,控制无人自跟随装置移动到目标点停滞等待重新连接蓝牙信号,一旦重新连上蓝牙信号,则继续利用蓝牙和惯性传感器的信号进行精确定位;如果无人自跟随装置在移动到目标点的过程中重新连上蓝牙信号,则继续利用蓝牙和惯性传感器的信号进行精确定位。
2.根据权利要1所述的定位方法,其特征在于:所述蓝牙信标集成在遥控器中,或采用智能手机蓝牙作为蓝牙信标。
3.根据权利要1所述的定位方法,其特征在于:所述无人自跟随装置为无人机或物料运输车。
4.根据权利要1所述的定位方法,其特征在于:所述蓝牙信标为蓝牙5.0信标,所述蓝牙基站为蓝牙5.0基站或信号发射器。
5.根据权利要1所述的定位方法,其特征在于,步骤1)所述RSSI测距方法为,利用接收端与发射端之间的距离,构建对数信号强度-距离衰减模型,将每个蓝牙基站测得的信号强度数据,换算成蓝牙基站与蓝牙信标之间的相对距离,蓝牙信号在发射端和接收端的信号强度关系如下式:
Lp(dB)=L0(dB)+10αlg d+ω(dB)
其中,L0表示1米距离处的信号能量损耗;Lp表示在距离d处的信号能量损耗;a表示路径损耗指数;ω表示误差;d表示发射端和接收端之间的直线距离。
6.根据权利要l所述的定位方法,其特征在于,步骤1)中,四点定位算法解算蓝牙信标的观测位置坐标的具体过程如下:
(x-x1)2+(y-y1)2+(z-z1)2=R1 2
(x-x2)2+(y-y2)2+(z-z2)2=R2 2
(x-x3)2+(y-y3)2+(z-z3)2=R3 2
(x-x4)2+(y-y4)2+(z-z4)2=R4 2
其中(x1,y1,z1),(x2,y2,z2),(x3,y3,z3),(x4,y4,z4)分别为四个蓝牙基站的坐标,R1,R2,R3,R4分别为四个蓝牙基站测得的与蓝牙信标的相对距离,则(x,y,z)为蓝牙信标的位置坐标;
采用最小二乘法进行估计,对上式进行整理,得到方程解的矩阵形式:
AX=B
式中:A表示系数矩阵,X表示蓝牙信标在无人自跟随装置坐标系下的三维坐标,B表示线性方程组整理后的三维向量;
其误差向量为:ε=AX-B,则有:
F(x)=||ε||2=(AX-B)T(AX-B)
式中,F(x)表示误差向量的二范数的平方;
若要误差最小,即使得F(x)最小,利用上式对X求导,使其导数为0,计算公式如下:
求解可得X=(ATA)-1ATB,这使得误差F(x)最小化,由此可计算得到最终的X坐标。
7.根据权利要1所述的定位方法,其特征在于,步骤1)采用蓝牙信号和惯性传感器的组合动态定位算法,具体为:
(1)保持无人自跟随装置的z轴高度不变,输入蓝牙信标位置坐标的初始值(x0,y0),该初始值为第一次通过蓝牙RSSI值获得的蓝牙信标坐标位置;
(2)惯性传感器分别测量无人自跟随装置沿x轴的加速度ax和沿y轴的加速度ay,通过对加速度ax和ay积分得到无人自跟随装置沿x轴的速度vx和沿y轴的速度vy;
(3)通过惯性传感器输出的沿z轴的角速度ωz乘以采样时间dt,得到惯性传感器相邻两次采样的航向角变化dθ,公式如下:
dθ=dt·wz;
(4)根据步骤(2)、(3)得到的惯性传感器数据和上一时刻的后验坐标数据(x(k),y(k)),计算下一时刻的先验坐标数据(x(k+1),y(k+1)),计算公式如下:
将上式写成矩阵形式:
(5)通过蓝牙的RSSI测距并结合四点定位算法计算蓝牙信标位置坐标作为观测数据,观测坐标数据方程为:
式中,(x′(k+1),y′(k+1))为k+1时刻的观测坐标数据。
8.根据权利要l所述的定位方法,其特征在于,步骤2)所述卡尔曼滤波算法为:
(1)初始化参数:
式中,x-表示无人自跟随装置蓝牙信标的x轴先验坐标值,y-表示无人自跟随装置蓝牙信标的y轴先验坐标值,x+表示无人自跟随装置蓝牙信标的x轴后验坐标值,y+表示无人自跟随装置蓝牙信标的y轴后验坐标值,X(0)表示无人自跟随装置蓝牙信标的初始观测坐标数据;
(2)迭代更新最优后验坐标数据:
X-(K+1)=AX+(k)+Bu(k)
P-(K+1)=AP+AT+Q
S=HP-(K+1)HT+R
K=P-(K+1)HTS-1
X+(k+1)=X-(k+1)+K(Z(k+1)-HX-(k+1))
P+(k+1)=(I-KH)P-(k+1)
将第k+1次迭代后的后验状态量X+(K+1)作为最终的定位位置坐标,用以控制无人机实现自动跟随功能;
式中,带上标-的数据为先验数据,带上标+的数据为后验数据,P为状态协方差矩阵,是一个迭代更新的量,设其初始值为二阶单位矩阵,Q、R矩阵的参数根据实际工程调试情况进行设置,S表示用于公式推导的中间量,K表示计算得到的卡尔曼增益,Z(k+1)表示蓝牙信标的观测坐标数据。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210974622.9A CN115334448B (zh) | 2022-08-15 | 2022-08-15 | 一种基于蓝牙和惯性传感器的无人自跟随装置的精确动态定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210974622.9A CN115334448B (zh) | 2022-08-15 | 2022-08-15 | 一种基于蓝牙和惯性传感器的无人自跟随装置的精确动态定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN115334448A true CN115334448A (zh) | 2022-11-11 |
CN115334448B CN115334448B (zh) | 2024-03-15 |
Family
ID=83924073
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210974622.9A Active CN115334448B (zh) | 2022-08-15 | 2022-08-15 | 一种基于蓝牙和惯性传感器的无人自跟随装置的精确动态定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115334448B (zh) |
Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107734457A (zh) * | 2017-09-29 | 2018-02-23 | 桂林电子科技大学 | 智慧停车场导航***及方法 |
CN109612466A (zh) * | 2018-11-30 | 2019-04-12 | 北斗天地股份有限公司山东分公司 | 一种井下车用多传感器组合导航方法及*** |
CN109782227A (zh) * | 2019-02-20 | 2019-05-21 | 核芯互联科技(青岛)有限公司 | 一种基于蓝牙信号rssi的室内定位方法 |
WO2019239365A1 (en) * | 2018-06-13 | 2019-12-19 | Purohit Ankit | System and method for position and orientation tracking of multiple mobile devices |
CN111024075A (zh) * | 2019-12-26 | 2020-04-17 | 北京航天控制仪器研究所 | 一种结合蓝牙信标和地图的行人导航误差修正滤波方法 |
CN111681452A (zh) * | 2020-01-19 | 2020-09-18 | 重庆大学 | 一种基于Frenet坐标系下的无人驾驶汽车动态换道轨迹规划方法 |
CN111726757A (zh) * | 2020-05-25 | 2020-09-29 | 南京理工大学 | 一种基于蓝牙的室内停车场定位与导航方法 |
WO2021139590A1 (zh) * | 2020-01-06 | 2021-07-15 | 三个机器人公司 | 基于蓝牙与slam的室内定位导航装置及其方法 |
CN114509069A (zh) * | 2022-01-25 | 2022-05-17 | 南昌大学 | 基于蓝牙aoa和imu融合的室内导航定位*** |
CN114894185A (zh) * | 2022-05-09 | 2022-08-12 | 南昌大学 | 基于蓝牙aoa和imu融合的载体姿态零速校正*** |
CN115103437A (zh) * | 2022-04-27 | 2022-09-23 | 电子科技大学 | 一种蓝牙和惯性测量单元的紧耦合室内定位方法 |
-
2022
- 2022-08-15 CN CN202210974622.9A patent/CN115334448B/zh active Active
Patent Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107734457A (zh) * | 2017-09-29 | 2018-02-23 | 桂林电子科技大学 | 智慧停车场导航***及方法 |
WO2019239365A1 (en) * | 2018-06-13 | 2019-12-19 | Purohit Ankit | System and method for position and orientation tracking of multiple mobile devices |
CN109612466A (zh) * | 2018-11-30 | 2019-04-12 | 北斗天地股份有限公司山东分公司 | 一种井下车用多传感器组合导航方法及*** |
CN109782227A (zh) * | 2019-02-20 | 2019-05-21 | 核芯互联科技(青岛)有限公司 | 一种基于蓝牙信号rssi的室内定位方法 |
CN111024075A (zh) * | 2019-12-26 | 2020-04-17 | 北京航天控制仪器研究所 | 一种结合蓝牙信标和地图的行人导航误差修正滤波方法 |
WO2021139590A1 (zh) * | 2020-01-06 | 2021-07-15 | 三个机器人公司 | 基于蓝牙与slam的室内定位导航装置及其方法 |
CN111681452A (zh) * | 2020-01-19 | 2020-09-18 | 重庆大学 | 一种基于Frenet坐标系下的无人驾驶汽车动态换道轨迹规划方法 |
CN111726757A (zh) * | 2020-05-25 | 2020-09-29 | 南京理工大学 | 一种基于蓝牙的室内停车场定位与导航方法 |
CN114509069A (zh) * | 2022-01-25 | 2022-05-17 | 南昌大学 | 基于蓝牙aoa和imu融合的室内导航定位*** |
CN115103437A (zh) * | 2022-04-27 | 2022-09-23 | 电子科技大学 | 一种蓝牙和惯性测量单元的紧耦合室内定位方法 |
CN114894185A (zh) * | 2022-05-09 | 2022-08-12 | 南昌大学 | 基于蓝牙aoa和imu融合的载体姿态零速校正*** |
Non-Patent Citations (6)
Title |
---|
KATARZYNA FILUS;SŁAWOMIR NOWAK;JOANNA DOMAŃSKA;JAKUB DUDA: "Cost-effective filtering of unreliable proximity detection results based on BLE RSSI and IMU readings using smartphones", Retrieved from the Internet <URL:https://www.nature.com/articles/s41598-022-06201-y> * |
MOHAMMADAMIN ATASHI: "Multiple Model-based Indoor Localization via Bluetooth Low Energy and Inertial Measurement Unit Sensors", Retrieved from the Internet <URL:《https://spectrum.library.concordia.ca/id/eprint/987720/7/Atashi_MSc_S2021.pdf》> * |
PAUL K. YOON; SHAGHAYEGH ZIHAJEHZADEH; BONG-SOO KANG; EDWARD J. PARK: "Adaptive Kalman filter for indoor localization using Bluetooth Low Energy and inertial measurement unit", 2015 37TH ANNUAL INTERNATIONAL CONFERENCE OF THE IEEE ENGINEERING IN MEDICINE AND BIOLOGY SOCIETY (EMBC), 5 November 2015 (2015-11-05) * |
佚名: "多传感器最优信息融合卡尔曼滤波器", Retrieved from the Internet <URL:《https://zhuanlan.zhihu.com/p/99041293》> * |
刘忠志: "基于空间定位模型的三维室内定位***研究", 中国优秀硕士学位论文全文数据库, 15 January 2021 (2021-01-15) * |
赖德朴: "基于蓝牙位置指纹与惯性传感器的室内定位技术研究", 中国优秀硕士学位论文全文数据库, 15 April 2018 (2018-04-15) * |
Also Published As
Publication number | Publication date |
---|---|
CN115334448B (zh) | 2024-03-15 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110375730B (zh) | 基于imu和uwb融合的室内定位导航*** | |
CN105157697B (zh) | 基于光电扫描的室内移动机器人位姿测量***及测量方法 | |
Fan et al. | Data fusion for indoor mobile robot positioning based on tightly coupled INS/UWB | |
CN105589064B (zh) | Wlan位置指纹数据库快速建立和动态更新***及方法 | |
CN206649345U (zh) | 一种基于超宽带通信的无人机导航装置 | |
CN107132542B (zh) | 一种基于光学和多普勒雷达的小天体软着陆自主导航方法 | |
CN110026993B (zh) | 一种基于uwb及热释电红外传感器的人体跟随机器人 | |
CN106950549B (zh) | 一种基于无线射频中继传输技术的雷达标定方法与*** | |
CN108489382A (zh) | 一种基于空间多点约束的agv动态位姿测量方法 | |
CN113382354B (zh) | 一种基于因子图的无线定位非视距信号判别方法 | |
CN111694001A (zh) | 一种用于无人机的实时测距定位*** | |
CN114509069B (zh) | 基于蓝牙aoa和imu融合的室内导航定位*** | |
CN110986952A (zh) | 一种针对行人的高精度抗干扰室内定位方法 | |
CN112269202A (zh) | 一种运动载体辅助的空间基准传递***及方法 | |
CN111305859A (zh) | 基于双目视觉的盾构机自动导向***和方法 | |
CN113324544A (zh) | 一种基于图优化的uwb/imu的室内移动机器人协同定位方法 | |
CN105180940A (zh) | 一种基于wMPS***的室内目标天文坐标的确定方法 | |
CN206876184U (zh) | 一种基于rssi和惯性导航的室内定位装置 | |
CN108445446A (zh) | 一种无源测速定位方法及装置 | |
CN111694006A (zh) | 一种用于室内无人***的导航传感*** | |
CN115334448A (zh) | 一种基于蓝牙和惯性传感器的无人自跟随装置的精确动态定位方法 | |
CN105979581A (zh) | 一种基于功率差的室内定位方法 | |
CN111913204B (zh) | 一种基于rtk定位的机械臂引导方法 | |
CN113156366B (zh) | 一种嘈杂电磁环境下集群无人机的空间定位方法 | |
CN214276792U (zh) | 基于激光雷达和室内gps***的飞机大尺寸测量*** |
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 |