CN106153050A - 一种基于信标的室内定位系统和方法 - Google Patents

Abstract

本发明公开了一种基于信标的室内定位系统和方法，系统包括机器人、天花板和若干信标；所述机器人顶部设有图像采集模块，机器人内部设有图像处理模块、建库模块和定位模块；所述信标分散设置于所述天花板上；每一个信标具有独特的ID信息、方向信息以及实际位置信息，所述图像采集模块拍摄、采集天花板及信标的图像；所述定位模块将通过图像处理模块处理得到的机器人的实时位置对应的天花板及信标的图像信息，与建库模块中存储的信标信息进行对比，结合所述建库模块得到的世界坐标系和图像坐标系的比例关系，获得机器人实时位置信息。本发明能实现室内实时绝对定位，且处理速度快，抗干扰能力强。

Description

一种基于信标的室内定位系统和方法

技术领域

本发明属于室内定位技术领域，具体涉及一种基于信标的室内定位系统和方法。

背景技术

室内定位是室内机器人领域的研究热点。目前主要靠GPS、有源信标等方法进行定位。在室内GPS因受到房屋的屏蔽，导致定位精度大大降低；有源信标，如蓝牙，WLAN等，安装较为不方便，而且定位精度在人多或环境变化的时候精度会降低。图像处理也应用到该领域，通过摄像头观察周围信息，进行帧间特征点匹配，进而进行室内定位，不过其计算量较大，实时性差，同时也易受到环境变化的干扰。

专利一种室内视觉定位系统及方法(申请号为201410504875.5)，公开了一种室内视觉定位系统及方法，包括电源模块、传感器组、信息处理模块，还包括前置面板、无线通信模块，并在前置面板上设置了定位灯组。室内视觉定位装置的两个摄像机可以灵活部署于墙壁和移动载体上，具有双目视觉和单目视觉两种工作模式。基于该装置的定位方法，首先，利用摄像机的前置面板信息，标定双目视觉系统外参数；其次，利用双目视觉测量准确信息构建地图，为后续单目视觉定位模式提供先验知识；最后，运用地图中蕴含的点、线、面、距离等知识，启动单目视觉定位模式，实现高效快速定位。本发明解决了传统视觉定位方法不能兼顾精度、速度的问题，提高了定位系统的灵活性和自动

但是仍然存在计算量较大，实时性差，容易受到环境变化的干扰的问题。

发明内容

本发明的目的是提供一种基于信标的室内定位系统和方法，通过自动学习，将天花板信标的信标库建立出来，并根据信标库对机器人进行定位，处理速度快，抗干扰能力强。

本发明提供了如下的技术方案：

一种基于信标的室内定位系统，包括：机器人、天花板、和若干信标；

所述机器人顶部设有图像采集模块，机器人内部设有图像处理模块、建库模块和定位模块；

所述信标分散设置于所述天花板上；每一个信标具有独特的ID信息、方向信息以及实际位置信息，所述ID信息由信标的形状、内部孔洞位置和数量决定，所述方向信息由信标上的缺口方向决定，所述实际位置信息由信标的特征点在天花板中的坐标决定；

所述图像采集模块随着所述机器人的运动而运动，拍摄、采集天花板及信标的图像；

所述图像处理模块，采集所述位置信号获取装置得到的天花板及信标的图像并进行处理，得到信标的图像信息，包括信标的ID信息，信标特征点在图像坐标系中的坐标，以及信标在图像坐标系中的方向信息；

所述建库模块，在第一次检测到信标时确立世界坐标系、图像坐标系，使其坐标原点重合，坐标方向与信标的方向一致，坐标原点为信标的特征点，然后将该信标以(0,0)坐标信息和0°方向信息存入信标库中；之后检测计算出世界坐标系、图像坐标系的比例关系，通过比较新检测到的信标和在同一图像中的库中信标在该图像中的相对位置关系，推断出该信标在图像坐标系中的位置和方向，并结合其ID信息将其存储到信标库中,为定位模块提供定位所需的信标库，相同的环境只建库一次；

所述定位模块，通过对图像处理模块处理得到的机器人的实时位置对应的天花板及信标的图像信息，与建库模块中存储的信标信息进行对比，结合所述建库模块得到的世界坐标系和图像坐标系的比例关系，获得机器人实时位置信息。

优选的，所述图像采集模块为摄像头，所述摄像头竖直向上设于所述机器人顶部；

优选的，所述信标为带有一个缺角的正方形，所述ID信息由在其上布置的孔洞位置和数量表示；所述特征点为信标的中心点r_c，所述实际位置信息是由所述信标的中心点r_c在天花板中的坐标表示；所述缺角使信标具有方向性，所述方向信息是由中心点r_c指向正方形缺角的方向v表示。

一种基于信标的室内定位方法，其步骤包括：

一、设置信标：在天花板上设置若干信标，并分别赋予信标不同的ID信息(由信标的形状、内部孔洞位置和数量决定)、方向信息(由信标上的缺角方向决定，使得信标能且只能表征唯一的方向)及实际位置信息(由信标的特征点在天花板中的实际坐标决定)；

信标布置的位置没有精度要求，间隔距离合适即可，但要保证机器人在学习过程中，在一个新信标建库时要至少已有另一个库中的信标存在于拍摄的图像中。

二、获取天花板及信标图像；机器人启动，开始在地面移动，所述图像采集模块拍摄、采集天花板及信标图像。

三、图像处理：

所述图像处理模块对所述位置信号获取装置拍摄得到的天花板及信标图像进行处理，得到信标的图像信息，包括所述信标的ID信息，信标特征点在图像坐标系中的坐标，以及信标在图像坐标系中的方向信息。

四、信标库建库：

首先确定世界坐标系和图像坐标系，使得世界坐标系和图像坐标系原点重合，方向相同；其次把拍摄到的第一个信标的图像信息(包括所述信标的ID信息，信标特征点在图像坐标系中的坐标，以及信标在图像坐标系中的方向信息)进行存库；然后检测该信标在图像中的面积，与实际面积对比，获得世界坐标系和图像坐标系的比例关系，用于后期定位过程中；在下一个新的信标出现在拍摄的图像中时，要有已存库的信标存在于相同的图像中，这样通过两个信标在图像中的相对位置关系，推断出在图像坐标系中的相对位置关系并存储到库中；最终处理、存储所有信标的图像信息，得到完整的信标库。

优选的，信标特征点为其中心点，经图像处理模块处理后得到信标的图像信息为信标的ID信息、信标中心点在图像坐标系中的坐标(x_c,y_c)以及信标在图像坐标系中的方向θ；

在第一次识别到一个信标时，将该信标的实际中心点r_c作为世界坐标系的原点，该信标的方向(中心点r_c指向正方形缺角的方向v)作为世界坐标系的方向，其坐标信息为(0,0)，角度为0，连同其ID信息存入至信标库；以同样的原点和方向建立图像坐标系，这样世界坐标系和全局图像坐标系原点重合，方向相同，但尺度有一定的比例关系，其比例值k通过识别到的第一个信标的面积p_area与实际的面积r_area计算得到,k＝p_area/r_area。

五、实时定位：

机器人任意在具有信标的位置移动，机器人本身的坐标系与图像坐标系一致，在运动的过程中拍摄天花板及信标的图像；定位模块通过对图像处理模块获取的机器人的实时位置对应的天花板及信标的图像信息，与建库模块中存储的信标库进行对比，结合所述建库模块中确定的世界坐标系和图像坐标系的比例关系，判断该图像在图像坐标系的位置和方向，从而获得机器人在世界坐标系中的实时位置和方向，完成定位。

所述定位模块的具体定位方法为：

信标在图像坐标系中的坐标与在世界坐标系中的坐标转换关系如下：

$\left[\begin{array}{c}ku\\ kv\\ 1\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\φ}& -\sin \mathrm{\φ}& P_x\\ \sin \mathrm{\φ}& \cos \mathrm{\φ}& P_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ 1\end{array}\right]$

其中，(u，v)表示信标在图像坐标系中的坐标，（P_x，P_y)表示世界坐标系相对于图像坐标系的坐标,(x_w，y_w)表示天花板上的信标的特征点P_i在世界坐标系中的坐标，表示机器人的方向角

信标库中存储的信标图像信息关系如下：

$\left[\begin{array}{c}{\mathrm{ku}}_i\\ {\mathrm{kv}}_i\\ 1\end{array}\right]=\left[\begin{array}{ccc}{\mathrm{cos\φ}}_i& -{\mathrm{sin\φ}}_i& P_{xi}\\ {\mathrm{sin\φ}}_i& {\mathrm{cos\φ}}_i& P_{yi}\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{wi}\\ y_{wi}\\ 1\end{array}\right]---\left(1\right)$

实际定位时，位置信号获取装置所得到的图像信标信息和信标实际位置坐标的关系如下

$\left[\begin{array}{c}{\mathrm{ku}}_{i+1}\\ {\mathrm{kv}}_{i+1}\\ 1\end{array}\right]=\left[\begin{array}{ccc}{\mathrm{cos\φ}}_{i+1}& -{\mathrm{sin\φ}}_{i+1}& P_{xi+1}\\ {\mathrm{sin\φ}}_{i+1}& {\mathrm{cos\φ}}_{i+1}& P_{yi+1}\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{wi+1}\\ y_{wi+1}\\ 1\end{array}\right]$

$\left(2\right)$

其中x_wi＝x_wi+1，y_wi＝y_wi+1，(1)(2)两式结合消去天花板上信标特征点Pi在世界坐标系坐标(x_wi，y_wi)，(x_wi+1，y_wi+1)，得：

ΔP_x＝P_xi+1-P_xi＝c₁cosφ_i+c₂sinφ_i

ΔP_y＝P_yi+1-P_yi＝-c₁sinφ_i+c₂cosφ_i

式中，

c₁＝k(u_i+1-u_icosΔφ_i+v_isinΔφ_i)+P_xicosΔφ_i-P_yisinΔφ_i-P_xi

c₂＝k(v_i+1-u_isinΔφ_i-v_icosΔφ_i)+P_xisinΔφ_i-P_yicosΔφ_i-P_yi

其中，ΔP_x和ΔP_y表示图像坐标系中，世界坐标系相对于两组不同的图像坐标系的位置关系，通过坐标变换可以得到机器人待定位位置与相应的信标库中的位置的关系，从而计算出机器人待定位位置。

本发明的有益效果是：通过竖直向上的图像采集模块拍摄天花板及信标图像，图像处理模块、建库模块、定位模块分析处理获得机器人的当前位置，实现室内实时绝对定位，该方法处理速度快，抗干扰能力强。

附图说明

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。在附图中：

图1是本发明实施例结构示意图；

图2是本发明信标实施例示意图；

图3是本发明实施例工作流程图。

具体实施方式

实施例如下，如图1所示，一种基于信标的室内定位系统，包括机器人1和天花板3，机器人1的顶部设有竖直向上的摄像头2，天花板3上布置若干信标4。

所述信标如图2所示，信标形状为带有一个缺角的正方形，缺角使信标具有方向性。通过在其上布置的孔洞位置和数量的不同表示信标中不同的ID信息。为了表示方便，我们用中心点r_c和一个向量v的方向来表征信标的位置和方向。r_c是正方形信标的中心，v是由中心点r_c指向正方形缺口的方向。

信标布置的位置没有精度要求，间隔距离合适即可，但要保证机器人在学习过程中，在新信标入库时要至少有另一个库中的信标存在于拍摄的图像中。

工作流程图如图3所示，机器人启动，开始在地面移动，摄像头竖直向上，拍摄采集天花板及信标图像，通过图像处理模块获取信标的ID信息、信标中心点在当前图像坐标系中的坐标(x_c,y_c)，以及信标在当前图像坐标系中的方向信息(θ)。在第一次识别到一个信标时，将该信标的实际中心点作为世界坐标系的原点，该信标向量v的方向作为世界坐标系的方向，建立图像坐标系，以第一个识别到的信标的中心点作为图像坐标系原点，该信标的方向作为图像坐标系的方向，这样，世界坐标系和图像坐标系原点重合，方向相同，但尺度有一定的比例关系值k，表示图像中1个像素代表的实际长度，该值通过识别到的第一个信标的面积p_area与实际的面积r_area计算得到,k＝p_area/r_area。

在学习过程中，信标库被建立，信标库存储了信标的ID信息，信标在图像坐标系中的坐标，以及在图像坐标系中的方向信息。识别到第一个信标后，其坐标信息为(0,0)，角度为0，连同其ID信息存入至信标库。在识别到新的信标时，首先获取在当前图像中其与已经存库的信标的相对位置关系，结合库中所存的信息，获得该新信标在图像坐标系中的位置坐标和方向，并将其存入库中，用这种方法将所有的信标都存入信标库中，并完成图像坐标系的建立。为了保证上述工作的顺利完成，要求信标的布置要满足在一张图像中同时可以看到至少两个信标的要求。

机器人本身的坐标系与图像坐标系一致，在运动的过程中拍摄图像，通过库中信标在当前图像中的位置和方向，判断该图像在图像坐标系的位置，从而通过比例关系k获得机器人在世界坐标系中的位置和方向，完成实时定位的目的。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种基于信标的室内定位系统，其特征在于，包括：机器人、天花板和若干信标；

所述机器人顶部设有图像采集模块，机器人内部设有图像处理模块、建库模块和定位模块；

所述信标分散设置于所述天花板上；每一个信标具有独特的ID信息、方向信息以及实际位置信息，所述ID信息由信标的形状、内部孔洞位置和数量决定，所述方向信息由信标上的缺口方向决定，所述实际位置信息由信标的特征点在天花板中的坐标决定；

所述图像采集模块随着所述机器人的运动而运动，拍摄、采集天花板及信标的图像；

所述图像处理模块，对所述位置信号获取装置得到的天花板及信标的图像进行处理，得到信标的图像信息，包括信标的ID信息，信标特征点在图像坐标系中的坐标，以及信标在图像坐标系中的方向信息；

所述建库模块，在第一次检测到信标时确立世界坐标系、图像坐标系，使其坐标原点重合，坐标方向与信标的方向一致，坐标原点为信标的特征点，然后将该信标以(0,0)坐标信息和0°方向信息存入信标库中；之后检测计算出世界坐标系、图像坐标系的比例关系，通过比较新检测到的信标和在同一图像中的库中信标在该图像中的相对位置关系，推断出该信标在图像坐标系中的位置和方向，并结合其ID信息将其存储到信标库中,为定位模块提供定位所需的信标库，相同的环境只建库一次；

所述定位模块，通过对图像处理模块处理得到的机器人的实时位置对应的天花板及信标的图像信息，与建库模块中存储的信标信息进行对比，结合所述建库模块得到的世界坐标系和图像坐标系的比例关系，获得机器人实时位置信息。

2.根据权利要求1所述的基于信标的室内定位系统，其特征在于，所述图像采集模块为摄像头，所述摄像头竖直向上设于所述机器人顶部。

3.根据权利要求1所述的基于信标的室内定位系统，其特征在于，所述信标为带有一个缺角的正方形，所述ID信息由在其上布置的孔洞位置和数量表示；所述特征点为信标的中心点r_c，所述实际位置信息是由所述信标的中心点r_c在天花板中的坐标表示；所述缺角使信标具有方向性，所述方向信息是由中心点r_c指向正方形缺角的方向v表示。

4.一种基于信标的室内定位方法，其特征在于，其步骤包括：

(一)设置信标：在天花板上设置若干信标，并分别赋予信标不同的ID信息、方向信息及实际位置信息；

(二)获取天花板及信标图像；机器人启动，开始在地面移动，所述图像采集模块拍摄、采集天花板及信标图像；

(三)图像处理：所述图像处理模块对所述位置信号获取装置拍摄得到的天花板及信标图像进行处理，得到信标的图像信息，包括所述信标的ID信息，信标特征点在图像坐标系中的坐标，以及信标在图像坐标系中的方向信息；

(四)信标库建库：

首先确定世界坐标系和图像坐标系，使得世界坐标系和图像坐标系原点重合，方向相同；其次把拍摄到的第一个信标的图像信息进行存库；然后检测该信标在图像中的面积，与实际面积对比，获得世界坐标系和图像坐标系的比例关系，用于后期定位过程中；在下一个新的信标出现在拍摄的图像中时，要有已存库的信标存在于相同的图像中，这样通过两个信标在图像中的相对位置关系，推断出在图像坐标系中的相对位置关系并存储到库中；最终处理、存储所有信标的图像信息，得到完整的信标库；

(五)实时定位：

机器人任意在具有信标的位置移动，机器人本身的坐标系与图像坐标系一致，在运动的过程中拍摄天花板及信标的图像；定位模块通过对图像处理模块获取的机器人的实时位置对应的天花板及信标的图像信息，与建库模块中存储的信标库进行对比，结合所述建库模块中确定的世界坐标系和图像坐标系的比例关系，判断该图像在图像坐标系的位置和方向，从而获得机器人在世界坐标系中的实时位置和方向，完成定位。

5.根据权利要求4所述的基于信标的室内定位方法，其特征在于，信标特征点为其中心点，经图像处理模块处理后得到信标的图像信息为信标的ID信息、信标中心点在图像坐标系中的坐标(x_c,y_c)以及信标在图像坐标系中的方向θ；

在第一次识别到一个信标时，将该信标的实际中心点r_c作为世界坐标系的原点，该信标的方向即中心点r_c指向正方形缺角的方向v作为世界坐标系的方向，其坐标信息为(0,0)，角度为0，连同其ID信息存入至信标库；以同样的原点和方向建立图像坐标系，这样世界坐标系和全局图像坐标系原点重合，方向相同，但尺度有一定的比例关系，其比例值k通过识别到的第一个信标的面积p_area与实际的面积r_area计算得到,k＝p_area/r_area。

6.根据权利要求4所述的基于信标的室内定位方法，其特征在于，

所述定位模块的具体定位方法为：

信标在图像坐标系中的坐标与在世界坐标系中的坐标转换关系如下：

$\left[\begin{array}{c}ku\\ kv\\ 1\end{array}\right]=\left[\begin{array}{ccc}cos\mathrm{\φ}& -sin\mathrm{\φ}& P_x\\ sin\mathrm{\φ}& \cos \mathrm{\φ}& P_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ 1\end{array}\right]$

其中，(u，v)表示信标在图像坐标系中的坐标，(P_x，P_y)表示世界坐标系相对于图像坐标系的坐标,(x_w，y_w)表示天花板上的信标的特征点P_i在世界坐标系中的坐标，Φ表示机器人的方向角；

信标库中存储的信标图像信息关系如下：

$\left[\begin{array}{c}k{u}_i\\ k{v}_i\\ 1\end{array}\right]=\left[\begin{array}{ccc}{\mathrm{cos\φ}}_i& -{\mathrm{sin\φ}}_i& P_{xi}\\ {\mathrm{sin\φ}}_i& {\mathrm{cos\φ}}_i& P_{yi}\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{wi}\\ y_{wi}\\ 1\end{array}\right]---\left(1\right)$

实际定位时，图像采集装置所得到的图像信标信息和信标实际位置坐标的关系如下

$\left[\begin{array}{c}k{u}_{i+1}\\ k{v}_{i+1}\\ 1\end{array}\right]=\left[\begin{array}{ccc}{\mathrm{cos\φ}}_{i+1}& -{\mathrm{sin\φ}}_{i+1}& P_{xi+1}\\ {\mathrm{sin\φ}}_{i+1}& {\mathrm{cos\φ}}_{i+1}& P_{yi+1}\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{wi+1}\\ y_{wi+1}\\ 1\end{array}\right]---\left(2\right)$

其中x_wi＝x_wi+1，y_wi＝y_wi+1，(1)(2)两式结合消去天花板上信标特征点Pi在世界坐标系坐标(x_wi，y_wi)，(x_wi+1，y_wi+1),得：

ΔP_x＝P_xi+1-P_xi＝c₁cosφ_i+c₂sinφ_i

ΔP_y＝P_yi+1-P_yi＝-c₁sinφ_i+c₂cosφ_i

式中，

c₁＝k(u_i+1-u_icosΔφ_i+v_isinΔφ_i)+P_xicosΔφ_i-P_yisinΔφ_i-P_xi

c₂＝k(v_i+1-u_isinΔφ_i-v_icosΔφ_i)+P_xisinΔφ_i-P_yicosΔφ_i-P_yi

其中，ΔP_x和ΔP_y表示图像坐标系中，世界坐标系相对于两组不同的图像坐标系的位置关系，通过坐标变换可以得到机器人待定位位置与相应的信标库中的位置的关系，从而计算出机器人待定位位置。