CN109307857B - 一种基于多传感器的自主跟随定位装置及方法 - Google Patents

Abstract

本发明属于检测技术领域，公开了一种基于多传感器的自主跟随定位装置及方法，位于移动目标本体上的n个信号源以顺时针方向依次发送信号，位于跟随机器人本体上的两个检测节点依次接收n个信号源发送的信号，并根据接收到的信号获得信号源的坐标信息，根据任意两个信号源的坐标信息获得移动目标本体的中心位置的坐标信息及特征信息，通过多个数据融合获得移动目标本体的移动信息；其中，n个信号源均匀分布在以移动目标本体的中心位置为圆心，以R为半径的圆周上，两个检测节点与跟随机器人本体的原点O之间的距离相等。本发明装置可以准确地测量出移动目标的位置和姿态，满足跟随定位要求，简单紧凑，经济性高，便于推广。

Description

一种基于多传感器的自主跟随定位装置及方法

技术领域

本发明涉及检测技术领域，尤其涉及一种基于多传感器的自主跟随定位装置及方法。

背景技术

随着经济的不断发展，机器人已经慢慢开始进入人类社会的各种行业中，这其中就包括自主跟随机器人。自主跟随机器人被广泛的应用于军事、农业及日常生活等多个领域，在自由空间中对移动目标的精准定位是保证其跟随性能的关键，它的定位问题也是长期研究的重点。目前，主要的定位技术有GPS定位、视觉定位和声波定位等。其中，GPS定位受场地影响较大，GPS信号容易丢失，无法适用于自由空间；视觉定位技术通过识别特征标识实现定位，不仅算法复杂，而且易受环境影响从而丢失目标；声波定位技术主要采用超声波定位，对场地要求较低，且不会被光照等环境因素影响，因此它适合被应用于自主跟随机器人的定位，但是超声波传感器发射角度小，移动目标很容易进入检测盲区，导致定位失败，并且传统的超声波定位方法不能实时的测量出移动目标前进的方向，使得自主跟随机器人不能利用更多有效信息进行跟随。

发明内容

本申请实施例通过提供一种基于多传感器的自主跟随定位装置及方法，以克服现有技术中自主跟随定位的不足。

本申请实施例提供一种基于多传感器的自主跟随定位方法，包括以下步骤：

位于移动目标本体上的n个信号源M_i(i＝0,1,2......n-1)以顺时针方向依次发送信号；

位于跟随机器人本体上的两个检测节点依次接收n个信号源M_i发送的信号，并根据接收到的信号获得信号源的坐标信息；

根据任意两个信号源M_j和M_k的坐标信息获得所述移动目标本体的中心位置M_ojk的坐标信息及特征信息，所述特征信息包括中心距离信息d_ojk、方向偏角信息θ_ojk；

通过融合多个所述移动目标本体的中心位置M_ojk的坐标信息及特征信息，获得所述移动目标本体的移动信息；

其中，n个信号源M_i均匀分布在以移动目标本体的中心位置M_ojk为圆心，以R为半径的圆周上；两个所述检测节点与所述跟随机器人本体的原点O之间的距离相等。

优选的，两个所述检测节点A和B的坐标分别记为(x_a,y_a)、(x_b,y_b)，信号源M_i相对于两个所述检测节点A、B的距离分别为l_a、l_b，由几何关系可知：

设A、B的坐标分别为(-a,0)，(a,0)，带入公式(1)，得到：

(x_i,y_i)为信号源M_i的坐标。

优选的，所述根据任意两个信号源M_j和M_k的坐标信息获得所述移动目标本体的中心位置M_ojk的坐标信息的计算方法为：

设信号源M_j的坐标为(x_j,y_j)，信号源M_k的坐标为(x_k,y_k)，其中j>k，设移动目标本体的中心位置M_ojk的坐标为(x_ojk,y_ojk)，由几何关系可知：

用

表示由M_ojk到M_j的向量，用

表示由M_ojk到M_k的向量，则

的行列式为：

由于信号源是顺时针编号排布的，则：

即:

(x_k-x_ojk)(y_j-y_ojk)_-(x_j-x_ojk)(y_k-y_ojk)＜0 (7)

由公式(4)、公式(6)可计算得到移动目标本体的中心位置M_ojk的坐标(x_ojk,y_ojk)。

优选的，得到所述移动目标本体的中心位置M_ojk的坐标(x_ojk,y_ojk)后，根据公式(8)、公式(9)：

获得所述移动目标本体的中心位置M_ojk的中心距离信息d_ojk、方向偏角信息θ_ojk。

优选的，若所述检测节点接收到m个信号源的位置信息，则可以求出

个移动目标本体的中心位置M_ojk的特征信息；

采用最小二乘逼近对

个M_ojk的特征信息进行处理，使融合后的移动中心位置M的特征参数d、θ满足：

其中，d_x、θ_x分别表示

个M_ojk的距离d_ojk、方向偏角θ_ojk中的一个；

设M_min为m个信号源中距离原点O最近的一个信号源，d_min为M_min到原点O的距离，即d_min＝mind_i(i＝0、1、2...n-1)；

用向量表示M_min相对于移动中心M的方向为

设信号源M_i与信号源M₀相对于移动中心M的夹角为γ，其中

由罗德里格旋转公式计算得出:

表示所述移动目标本体的移动信息。

优选的，所述信号源为超声波传感器。

另一方面，本申请实施例提供一种基于多传感器的自主跟随定位装置，包括：移动目标、跟随机器人；

所述移动目标包括移动目标本体和设置在所述移动目标本体上的n个信号源，n个所述信号源均匀分布在以所述移动目标的中心位置M_ojk为圆心，以R为半径的圆周上；

所述跟随机器人包括跟随机器人本体和设置在所述跟随机器人上的两个检测节点，两个所述检测节点与所述随机器人本体的原点O之间的距离相等。

优选的，所述信号源采用超声波传感器。

优选的，所述信号源的个数n大于3。

本申请实施例中提供的一个或多个技术方案，至少具有如下技术效果或优点：

在本申请实施例中，在移动目标本体的一个近似圆形区域范围内，均匀分布置n个信号源，通过从第一个信号源开始，到第n个信号源为止，以顺时针的方向依次发送信号并进行信号源切换，跟随机器人本体上的检测节点获得n个信号源的坐标信息。然后根据任意两个信号源的坐标信息可以求出移动目标本体的中心位置及中心位置的特征信息，特征信息包括移动目标本体的中心位置与跟随机器人的原点之间的距离、移动目标本体的中心位置的方向偏角。通过对多个距离和方向偏角数据进行融合，从而计算出准确的移动目标本体的中心位置及实时移动姿态。本发明采用多传感器信息融合检测技术，抗干扰能力强，可以准确地测量出移动目标的位置和姿态，满足跟随定位要求。本发明的装置简单紧凑，经济性高，便于推广。

附图说明

为了更清楚地说明本实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一个实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的一种基于多传感器的自主跟随定位方法中多传感器信息融合定位的示意图；

图2为本发明实施例提供的一种基于多传感器的自主跟随定位方法中采用的三角定位法的示意图；

图3为本发明实施例提供的一种基于多传感器的自主跟随定位方法中计算中心位置M_ojk的示意图；

图4为本发明实施例提供的一种基于多传感器的自主跟随定位方法中计算移动姿态的示意图；

图5为本发明实施例提供的一种基于多传感器的自主跟随定位方法的仿真轨迹；

图6为图5中仿真轨迹的误差曲线。

其中，1-移动目标、2-跟随机器人。

具体实施方式

为了更好的理解上述技术方案，下面将结合说明书附图以及具体的实施方式对上述技术方案进行详细的说明。

本发明的技术方案包括：多信号源定位模型，多传感器信息融合的自主跟随定位方法的算法、多传感器信息融合的自主跟随定位方法模拟仿真结果。在被测对象(即移动目标)的一个近似圆形区域范围内，均匀分布置着n个信号源，通过从信号源M₀开始，到信号源M_n-1发送信号完成截止，以顺时针的方向依次进行信号源切换，获得信号源M_i(i＝0,1,2......n-1)的坐标和特征数据di和θi。对于任意两个信号源M_j和M_k可以求出对应的移动中心位置M_ojk的坐标信息及特征信息(包括距离d_ojk和方向偏角θ_ojk)，通过对多个信息数据的融合，从而计算出移动目标中心位置及实时移动姿态。通过MATLAB软件平台，进行仿真实验，从而验证本发明所提出的基于多传感器信息融合的自主跟随定位方法的可行性。

如图1所示，本发明提供的一种基于多传感器信息融合的自主跟随定位装置由两大部分组成：移动目标1和跟随机器人2组成。所述移动目标1包括移动目标本体，和在所述移动目标本体上圆形区域的范围内均匀分布着的n个信号源发射器，n个信号源发射器作为检测目标的可识别信号源。所述跟随机器人2包括跟随机器人主体和跟随机器人主体上的检测节点组成。跟随机器人本体上的检测节点接收到信号源发射器发出的信号后，由跟随机器人主体上的处理电路对所得到的信号进行处理，主要进行放大，滤波等处理，然后经过A\D转换，将模拟信号转为计算机可以处理的数字信号，通过算法对接收到的多个信号进行处理，最终得到准确的物体距离和方向信息。

本发明由多个最小的三角定位法组成，通过对多组三角定位法所计算结果的融合，使自主跟随机器人与移动目标之间建立冗余信道，从而计算出移动目标中心位置及实时移动姿态。通过对多组三角定位法所计算结果的融合，使自主跟随机器人与移动目标之间建立冗余信道，从而计算出移动目标中心位置及实时移动姿态。

参见图1，本发明所采用的技术方案是：在被测对象(即移动目标本体)的一个近似圆形区域范围内，均匀分布置着n个信号源，通过从第一个信号源M₀发送信号开始，到最后一个信号源M_n-1发送信号完成截止，以顺时针的方向依次进行信号源切换；然后通过位于跟随机器人本体上的两个检测节点获得信号源M_i(i＝0,1,2......n-1)的坐标和特征数据d_i和θ_i。

具体的，两个所述检测节点A和B的坐标分别记为(x_a,y_a)、(x_b,y_b)，跟随机器人主体上的处理电路对所得到的信号进行处理后获得信号源M_i相对于两个所述检测节点A、B的距离分别为l_a、l_b，参见图2，则由几何关系可知：

设A、B的坐标分别为(-a,0)，(a,0)，带入公式(1)，得到：

得到信号源M_i的坐标(x_i,y_i)。

参见图3和图4，本发明根据任意两个信号源M_j和M_k可以求出对应的移动目标本体的中心位置M_ojk的坐标信息及特征信息(包括距离信息d_ojk和方向偏角信息θ_ojk)。通过对信息和数据的融合，从而计算出准确的移动目标本体的中心位置及实时移动姿态。具体计算如下：

设一个信号源M_j的坐标为(x_j,y_j)、另一个信号源M_k的坐标为(x_k,y_k)，信号源M_j和信号源M_k的坐标采用上述公式可以得到。其中j>k。设移动目标本体的中心位置M_ojk的坐标为(x_ojk，y_ojk)，由几何关系可知：

由公式(4)算出的M_ojk的坐标(x_ojk，y_ojk)有两个，并不能确定移动目标本体的中心位置。本发明使用向量叉积的方法来确定移动中心位置。用

表示由M_ojk到M_j的向量，用

表示由M_ojk到M_k的向量，则

的行列式为：

由于信号源是顺时针编号排布的，则：

即:

(x_k-x_ojk)(y_j-y_ojk)-(x_j-x_ojk)(y_k-y_ojk)＜0 (7)

由式(4)、(6)便可以确定信号源M_j和M_k对应的移动目标本体的中心位置M_ojk的坐标(x_ojk，y_ojk)。

得到所述移动目标本体的中心位置M_ojk的坐标(x_ojk,y_ojk)后，根据公式(8)、公式(9)：

获得所述移动目标本体的中心位置M_ojk的中心距离信息d_ojk、方向偏角信息θ_ojk。

假设n个信号源中有m个信号源的坐标信息被有效的测量出，则可以求出

个移动目标本体的中心位置M_ojk的特征参数。

为了减小复杂因素对测量的干扰，减小误差，本发明采用最小二乘逼近对计算得到的移动中心位置的特征信息(即

个特征参数)进行处理，使融合后得到的移动中心位置M的特征参数d(M到O的距离)、θ(M相对于x轴的方向偏角)满足：

其中，d_x、θ_x分别表示

个移动目标本体的中心位置M_ojk的距离d_ojk、方向偏角θ_ojk的一个。

设M_min为m个有效的信号源中距离原点O最近的一个信号源，d_min为M_min到原点O的距离，即d_min＝mind_i(i＝0、1、2...n-1)。用向量表示M_min相对于移动中心M的方向为

设信号源M_i与信号源M₀相对于移动中心M的夹角为γ，其中

由罗德里格旋转公式计算得出:

表示移动物体面对的方向，代表了移动物体的实时移动姿态。

根据本发明提供的方法在MATLAB平台进行了仿真实验。实验模拟了一个移动目标在距离跟随机器人1m的位置沿直线运动，此移动目标由移动目标本体和设置在移动目标本体上的24个超声波传感器组成，24个超声波传感器以100mm为半径R均匀绕移动目标本体的圆心分布。其中，超声波传感器的信号发射角度为30°，测量距离为500mm-3000mm，测量误差为0-5mm。设置在跟随机器人上的两个信号接收节点距离为400mm，原点O设置在两个信号接收节点的中间位置。设置好系统所需要的参数，然后根据上面所阐述的算法步骤进行多次仿真实验，由本仿真结果(参见图5、图6)可知，计算结果轨迹与实际轨迹相近，x轴误差和y轴误差在±30mm以内，方向偏角在±15°以内，能够满足跟随的精度要求。本发明提出的方法及系统可以准确地测量出移动目标的位置和姿态(行进方向)，满足跟随定位要求。

最后所应说明的是，以上具体实施方式仅用以说明本发明的技术方案而非限制，尽管参照实例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (2)

1.一种基于多传感器的自主跟随定位方法，其特征在于，包括以下步骤：

位于移动目标本体上的n个信号源M_i以顺时针方向依次发送信号；其中，i＝0,1,2......n-1；

位于跟随机器人本体上的两个检测节点依次接收n个信号源M_i发送的信号，并根据接收到的信号获得信号源的坐标信息；

根据任意两个信号源M_j和M_k的坐标信息获得所述移动目标本体的中心位置M_ojk的坐标信息及特征信息，所述特征信息包括中心距离信息d_ojk、方向偏角信息θ_ojk；

通过融合多个所述移动目标本体的中心位置M_ojk的坐标信息及特征信息，获得所述移动目标本体的移动信息；

其中，n个信号源M_i均匀分布在以移动目标本体的中心位置M_ojk为圆心，以R为半径的圆周上；两个所述检测节点与所述跟随机器人本体的原点O之间的距离相等；

两个所述检测节点A和B的坐标分别记为(x_a,y_a)、(x_b,y_b)，信号源M_i相对于两个所述检测节点A、B的距离分别为l_a、l_b，由几何关系可知：

设A、B的坐标分别为(-a,0)，(a,0)，带入公式(1)，得到：

(x_i,y_i)为信号源M_i的坐标；

所述根据任意两个信号源M_j和M_k的坐标信息获得所述移动目标本体的中心位置M_ojk的坐标信息的计算方法为：

设信号源M_j的坐标为(x_j,y_j)，信号源M_k的坐标为(x_k,y_k)，其中j>k，设移动目标本体的中心位置M_ojk的坐标为(x_ojk,y_ojk)，由几何关系可知：

用

表示由M_ojk到M_j的向量，用

表示由M_ojk到M_k的向量，则

的行列式为：

由于信号源是顺时针编号排布的，则：

即:

(x_k-x_ojk)(y_j-y_ojk)-(x_j-x_ojk)(y_k-y_ojk)＜0 (7)

由公式(4)、公式(6)可计算得到移动目标本体的中心位置M_ojk的坐标(x_ojk,y_ojk)；

得到所述移动目标本体的中心位置M_ojk的坐标(x_ojk,y_ojk)后，根据公式(8)、公式(9)：

获得所述移动目标本体的中心位置M_ojk的中心距离信息d_ojk、方向偏角信息θ_ojk；

若所述检测节点接收到m个信号源的位置信息，则可以求出

个移动目标本体的中心位置M_ojk的特征信息；

采用最小二乘逼近对

个M_ojk的特征信息进行处理，使融合后的移动中心位置M的特征参数d、θ满足：

其中，d_x、θ_x分别表示

个M_ojk的距离d_ojk、方向偏角θ_ojk中的一个；

设M_min为m个信号源中距离原点O最近的一个信号源，d_min为M_min到原点O的距离；

用向量表示M_min相对于移动中心M的方向为

设信号源M_i与信号源M₀相对于移动中心M的夹角为γ，其中

由罗德里格旋转公式计算得出:

表示所述移动目标本体的移动信息。

2.根据权利要求1所述的基于多传感器的自主跟随定位方法，其特征在于，所述信号源为超声波传感器。

Images