CN115334448A - 一种基于蓝牙和惯性传感器的无人自跟随装置的精确动态定位方法 - Google Patents

Abstract

一种基于蓝牙和惯性传感器的无人自跟随装置的精确动态定位方法，定位步骤如下：1)四个蓝牙基站分别获得与蓝牙信标的距离，采用四点定位算法解算观测坐标数据；利用惯性传感器推算出先验坐标数据；2)将先验坐标数据和观测坐标数据通过卡尔曼滤波算法融合得到后验坐标数据，对无人自跟随装置进行精确定位；3)当出现断连现象时，以断连前最后一次定位到蓝牙信标的位置坐标为目标点，使用惯性传感器进行惯性导航，控制无人自跟随装置移动到目标点等待重新连接，重新连上蓝牙信号或在移动到目标点的过程中重新连上蓝牙信号，则继续利用蓝牙和惯性传感器的信号进行精确定位。

Description

一种基于蓝牙和惯性传感器的无人自跟随装置的精确动态定 位方法

技术领域

本发明属于无人自跟随装置控制领域，特别涉及一种基于蓝牙和惯性传感器的无人自跟随装置的精确动态定位方法。

背景技术

近年来，随着计算机技术和无线通信技术的发展，智能化的设备在人们生活中的出现越来越频繁，尤其是无人自动跟随装置。以无人机为例，无人机在军事和民用领域都得到了广泛的应用，其中自动跟随技术作为无人机的核心技术之一，得到了学术界和工业界的广泛研究，自动跟随技术的实现主要有两种技术方案：(1)基于GPS定位的自动跟随，这种技术方案需要在无人机和移动设备(比如遥控器)内部分别安装GPS模块，通过GPS的精确定位来确定无人机和被拍摄者的相对位置，飞控***根据两者的相对位置控制无人机实现自动跟随功能，该方案必须保证GPS信号的准确性，在信号较弱的山区或者室内无法使用；(2)基于视觉的跟随，该技术要求跟随目标相对周边环境辨识度较高，同时跟随目标需要一直保持在摄像头的捕捉范围内，“找不到目标物”将导致视觉跟随被迫暂时停止。

发明内容

本发明的目的是针对现有技术的不足，提供一种基于蓝牙和惯性传感器的无人自跟随装置的精确动态定位方法。本发明所述方法通过利用惯性传感器和蓝牙RSSI测距定位进行数据融合，实现了自动跟随装置相对于移动端的精确动态定位，既没有GPS自跟随无法在信号较差的山区或室内工作的缺点，对环境辨识度的要求也没有视觉自跟随那么高，同时蓝牙信号在跟惯性传感器的数据进行了基于卡尔曼滤波的数据融合处理后还解决了信号不稳定和误差较大的问题。

本发明的技术方案是：

一种基于蓝牙和惯性传感器的无人自跟随装置的精确动态定位方法，

包括由设有惯性传感器和四个蓝牙基站的无人自跟随装置与手持式蓝牙信标组成的动态定位***，该定位***按以下步骤定位：

1)四个蓝牙基站分别通过RSSI测距法获得各自与蓝牙信标的距离，采用四点定位算法解算出蓝牙信标的位置坐标作为观测坐标数据；同时利用惯性传感器解算出的无人自跟随装置速度推算出蓝牙信标位置坐标作为先验坐标数据；

2)将先验坐标数据和观测坐标数据通过卡尔曼滤波算法融合得到后验坐标数据，对无人自跟随装置进行精确定位；

3)当蓝牙基站与蓝牙信标出现断连现象时，以断连前最后一次定位到蓝牙信标的位置坐标为目标点，使用惯性传感器进行惯性导航，控制无人自跟随装置移动到目标点停滞等待重新连接蓝牙信号，一旦重新连上蓝牙信号，则继续利用蓝牙和惯性传感器的信号进行精确定位；如果无人自跟随装置在移动到目标点的过程中重新连上蓝牙信号，则继续利用蓝牙和惯性传感器的信号进行精确定位。

进一步地，所述蓝牙信标集成在遥控器中，或采用智能手机蓝牙作为蓝牙信标。

进一步地，所述无人自跟随装置为无人机或物料运输车。

进一步地，所述蓝牙信标为蓝牙5.0信标，所述蓝牙基站为蓝牙5.0基站或信号发射器。

进一步地，步骤1)所述RSSI测距方法为，利用接收端与发射端之间的距离，构建对数信号强度-距离衰减模型，将每个蓝牙基站测得的信号强度数据，换算成蓝牙基站与蓝牙信标之间的相对距离，蓝牙信号在发射端和接收端的信号强度关系如下式：

L_p(dB)＝L₀(dB)+10αlg d+ω(dB)

其中，L₀表示1米距离处的信号能量损耗；L_p表示在距离d处的信号能量损耗；α表示路径损耗指数；ω表示误差；d表示发射端和接收端之间的直线距离。

进一步地，步骤1)中，四点定位算法解算蓝牙信标的观测位置坐标的具体过程如下：

(x-x₁)²+(y-y₁)²+(z-z₁)²＝R₁ ²

(x-x₂)²+(y-y₂)²+(z-z₂)²＝R₂ ²

(x-x₃)²+(y-y₃)²+(z-z₃)²＝R₃ ²

(x-x₄)²+(y-y₄)²+(z-z₄)²＝R₄ ²

其中(x₁，y₁，z₁)，(x₂，y₂，z₂)，(x₃，y₃，z₃)，(x₄，y₄，z₄)分别为四个蓝牙基站的坐标，R₁，R₂，R₃，R₄分别为四个蓝牙基站测得的与蓝牙信标的相对距离，则(x，y，z)为蓝牙信标的位置坐标；

采用最小二乘法进行估计，对上式进行整理，得到方程解的矩阵形式：

AX＝B

式中，A表示系数矩阵，X表示蓝牙信标在无人自跟随装置坐标系下的三维坐标，B表示线性方程组整理后的三维向量；

其误差向量为：ε＝AX-B，则有，

F(x)＝||ε||²＝(AX-B)^T(AX-B)

式中，F(x)表示误差向量的二范数的平方；

若要误差最小，则要使得F(x)最小，利用上式对X求导，使其导数为0，计算公式如下：

求解可得X＝(A^TA)^-1A^TB，此时误差F(x)最小化，由此可计算得到最终的X坐标。

进一步地，步骤1)采用蓝牙信号和惯性传感器的组合动态定位算法，具体为：

(1)保持无人自跟随装置的z轴高度不变，输入蓝牙信标位置坐标的初始值(x₀，y₀)，该初始值为第一次通过蓝牙RBSI值获得的蓝牙信标坐标位置；

(2)惯性传感器分别测量无人自跟随装置沿x轴的加速度a_x和沿y轴的加速度a_y，通过对加速度a_x和a_y积分得到无人自跟随装置沿x轴的速度v_x和沿y轴的速度v_y；

(3)通过惯性传感器输出的沿z轴的角速度ω_z乘以采样时间dt，得到惯性传感器相邻两次采样的航向角变化dθ，公式如下：

dθ＝dt·w_z；

(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⁺A^T+Q

S＝HP^-(K+1)H^T+R

K＝P^-(K+1)H^TS^-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测距方法为，利用接收端与发射端之间的距离，构建对数信号强度-距离衰减模型，将每个蓝牙基站测得的信号强度数据，换算成蓝牙基站与蓝牙信标之间的相对距离，蓝牙信号在发射端和接收端的信号强度关系如下式：

L_p(dB)＝L₀(dB)+10αlg d+ω(dB)

其中，L₀表示1米距离处的信号能量损耗；L_p表示在距离d处的信号能量损耗；a表示路径损耗指数；ω表示误差，通常看作是零均值的高斯噪声，其方差与环境有关；d表示发射端和接收端之间的直线距离。

3-2)四点定位算法解算蓝牙信标的观测位置坐标的具体过程如下：

(x-x₁)²+(y-y₁)²+(z-z₁)²＝R₁ ²

(x-x₂)²+(y-y₂)²+(z-z₂)²＝R₂ ²

(x-x₃)²+(y-y₃)²+(z-z₃)²＝R₃ ²

(x-x₄)²+(y-y₄)²+(z-z₄)²＝R₄ ²

其中(x₁，y₁，z₁)，(x₂，y₂，z₂)，(x₃，y₃，z₃)，(x₄，y₄，z₄)分别表示四个蓝牙基站的坐标，R₁，R₂，R₃，R₄分别为四个蓝牙基站测得的与蓝牙信标的相对距离，则(x，y，z)为蓝牙信标的位置坐标；

在测得的蓝牙基站与蓝牙信标之间的距离没有误差的情况下，以蓝牙基站为圆心，测得的距离为半径的定位圆球将相交于一点，蓝牙信标即位于该交点上，而蓝牙信号在传播测量过程中，往往会遇到多径及非视距误差，该误差通常为正值，因此会出现多个定位圆两两相交，则具有多个交点。

因此，采用最小二乘法进行估计，对上式进行整理，得到方程解的矩阵形式：

AX＝B

式中：A表示系数矩阵，X表示蓝牙信标在无人自跟随装置坐标系下的三维坐标，B表示线性方程组整理后的三维向量；

其误差向量为：ε＝AX-B，则有：

F(x)＝||ε||²＝(AX-B)^T(AX-B)

式中，F(x)表示误差向量的二范数的平方；

若要误差最小，则要使得F(x)最小，利用上式对X求导，使其导数为0，计算公式如下：

求解可得X＝(A^TA)^-1A^TB，此时误差F(x)最小化，由此可计算得到最终的X坐标，即得到蓝牙信标的位置坐标作为观测坐标数据。

4)利用惯性传感器解算出的无人自跟随装置速度推算出蓝牙信标位置坐标作为先验坐标数据，具体算法为：

由于无人机是定高度跟随，所以z值保持不变，在做飞控算法时将无人机控制在一定飞行高度不变，所以在跟随蓝牙信标的移动过程中可以将三维空间坐标转换为二维坐标来讨论，即仅关心x轴和y轴的值即可；

4-1)保持无人自跟随装置的z轴高度不变，输入蓝牙信标位置坐标的初始值(x₀，y₀)，该初始值为第一次通过蓝牙RSSI值获得的蓝牙信标坐标位置；

4-2)惯性传感器分别测量无人自跟随装置沿x轴的加速度a_x和沿y轴的加速度a_y，通过对加速度a_x和a_y积分得到无人自跟随装置沿x轴的速度v_x和沿y轴的速度v_y；

4-3)通过惯性传感器输出的沿z轴的角速度ω_z乘以采样时间dt，得到惯性传感器相邻两次采样的航向角变化dθ，算法的迭代周期为20ms，因此在依次迭代过程中认为蓝牙信标保持精致，算法公式如下：

dθ＝dt·w_z；

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⁺A^T+Q

S＝HP^-(K+1)H^T+R

K＝P^-(K+1)H^TS^-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测距方法为，利用接收端与发射端之间的距离，构建对数信号强度-距离衰减模型，将每个蓝牙基站测得的信号强度数据，换算成蓝牙基站与蓝牙信标之间的相对距离，蓝牙信号在发射端和接收端的信号强度关系如下式：

L_p(dB)＝L₀(dB)+10αlg d+ω(dB)

其中，L₀表示1米距离处的信号能量损耗；L_p表示在距离d处的信号能量损耗；a表示路径损耗指数；ω表示误差；d表示发射端和接收端之间的直线距离。

6.根据权利要l所述的定位方法，其特征在于，步骤1)中，四点定位算法解算蓝牙信标的观测位置坐标的具体过程如下：

(x-x₁)²+(y-y₁)²+(z-z₁)²＝R₁ ²

(x-x₂)²+(y-y₂)²+(z-z₂)²＝R₂ ²

(x-x₃)²+(y-y₃)²+(z-z₃)²＝R₃ ²

(x-x₄)²+(y-y₄)²+(z-z₄)²＝R₄ ²

其中(x₁，y₁，z₁)，(x₂，y₂，z₂)，(x₃，y₃，z₃)，(x₄，y₄，z₄)分别为四个蓝牙基站的坐标，R₁，R₂，R₃，R₄分别为四个蓝牙基站测得的与蓝牙信标的相对距离，则(x，y，z)为蓝牙信标的位置坐标；

采用最小二乘法进行估计，对上式进行整理，得到方程解的矩阵形式：

AX＝B

式中：A表示系数矩阵，X表示蓝牙信标在无人自跟随装置坐标系下的三维坐标，B表示线性方程组整理后的三维向量；

其误差向量为：ε＝AX-B，则有：

F(x)＝||ε||²＝(AX-B)^T(AX-B)

式中，F(x)表示误差向量的二范数的平方；

若要误差最小，即使得F(x)最小，利用上式对X求导，使其导数为0，计算公式如下：

求解可得X＝(A^TA)^-1A^TB，这使得误差F(x)最小化，由此可计算得到最终的X坐标。

7.根据权利要1所述的定位方法，其特征在于，步骤1)采用蓝牙信号和惯性传感器的组合动态定位算法，具体为：

(1)保持无人自跟随装置的z轴高度不变，输入蓝牙信标位置坐标的初始值(x₀，y₀)，该初始值为第一次通过蓝牙RSSI值获得的蓝牙信标坐标位置；

(2)惯性传感器分别测量无人自跟随装置沿x轴的加速度a_x和沿y轴的加速度a_y，通过对加速度a_x和a_y积分得到无人自跟随装置沿x轴的速度v_x和沿y轴的速度v_y；

(3)通过惯性传感器输出的沿z轴的角速度ω_z乘以采样时间dt，得到惯性传感器相邻两次采样的航向角变化dθ，公式如下：

dθ＝dt·w_z；

(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⁺A^T+Q

S＝HP^-(K+1)H^T+R

K＝P^-(K+1)H^TS^-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)表示蓝牙信标的观测坐标数据。

Images