机器谱

【U017】如何用蓝牙实现无线定位

作者：机器谱

图文展示3264(1)

图文展示3264(1)

副标题

系统原理
信号塔设置
本地定位显示
远程定位显示
双定位显示

1. 简介

  本项目将利用多个BLE4.0蓝牙模块，配合主控板、OLED显示屏等，构建一个无线定位系统。

  本项目的系统构成为：3个信号塔，1个中控台，2个被定位的目标。

      无线定位的用途有很多。比如，我们可以把固定的目标1看作等待救援的人或物，把可移动的目标2看作前来救援的机器人，就可以虚拟出一个救援场景模型。

      而现实中使用蓝牙技术进行无线定位，多见于商场、图书馆等室内环境的定位和导航。比如，在商场内安装数个蓝牙信号发射源；在商家门口安装蓝牙定位设备；对于顾客，则可以直接使用其智能手机的蓝牙模块，再配合一个商场专用的手机App或者小程序，就可以让顾客非常方便的在商场内部寻找自己想要到达的商家等。


2. 定位原理

（1）rssi无线接收信号强度

      Received Signal Strength Indication接收的信号强度指示，通过接收到的信号强弱测定信号点与接收点的距离，进而根据相应数据进行定位计算的一种定位技术。

      RSSI是射频信号理论术语，主要应用于发射机和接收机之间的距离测量。该方法是依据接收信号能量强度确定距离，对通信信道参数要求较高。其测距理论是：依据无线电波或声波在介质中传输，信号功率是随传播距离衰减的原理。根据信标节点已知信号的发射功率和节点接收的信号功率，通过信号与距离之间的衰减模型，就可以计算出节点间的距离。由于信号传播的过程中，受到距离和障碍物的影响。信号的功率强度随之衰减，间接影响精度。所以要求得到良好的精度，短距离才会体现这一点。

      RSSI的定位技术相比于TOA、TDOA、AOA、GPS具有成本低、容易实现等优势。如果室内定位精度要求不高，基于RSSI的定位技术完全可以满足。而且，现阶段对于作为节点的传感器，都能够完成发射测试信号功率的任务。主要进行实验时，节点发送数据包，也获取RSSI的测量值。该定位技术既无需额外硬件，又能完成复杂信息的分析处理，减小通信消费，节约成本，比较适用于无线传感器网络的定位系统。

测距方式精度

备注

TOA3.10m器材昂贵，存在干扰
TODA平均20cm有效距离大于200m
  AOA几个毫米利用激光测距，有效范围是5-300m，代价高
RSSI1-5m成本低，容易实现
TDOA和AOA

20-80cm

最大可测距离50m

GPS

5-15m

昂贵，不可在室内


几种测距方式对比 :

（2）rssi三点定位算法的基本原理

      三点定位算法在需要进行定位的空间放置3个AP（无线访问接入点），并且3个AP的位置已知，如果知道该空间内某位置的信号强度则可建立信号衰减模型，根据该信号强度计算出该位置到三个AP的距离。使用某位置移动设备的信号强度即可估算出其到附近AP的距离，如果能确定若干AP的位置，即可确定该移动设备的位置。由于障碍物的存在，信号衰减模型在实际使用时会存在一定误差。

      已知三个点的坐标和未知点到这三个点的rssi的信号值，求解未知点的坐标。

       首先是将rssi信号转换为距离：

           d=10^((ABS(RSSI)-A)/(10*n))

        其中d为距离，单位是m。

        RSSI为rssi信号强度，为负数。

        A为距离探测设备1m时的rssi值的绝对值，最佳范围在45-49之间。

        n为环境衰减因子，需要测试矫正，最佳范围在3.25-4.5之间。

        在获取未知点到三个点的距离后，剩下的就是求解未知点的坐标。我们都知道两个圆会相交于一个或者两个点（如果相交），那么三个圆如果相交的话，必然会交于一个点（三个探测设备在一条直线上的情况下有可能相交于两个点，这里不考虑），所以我们要求解的未知点便是以三个已知点为圆心，以他们与未知点之间的距离为半径画出的三个圆的交点。那么这个问题就转化为了求三个已知圆的交点，然后如果根据圆的方程：

       (x1 – x)^2 + (y1-y)^2 = r1^2

       (x2 – x)^2 + (y2-y)^2= r2^2

       (x3 – x)^2 + (y3-y)^2= r3^2

      求解的话，是非常难求出未知点的坐标的。

      这里介绍另一种程序容易实现的计算方法：

①、判断任意两个圆是否相切（内切或外切），这里可以设定一个误差允许值d，也就是 (x1 – x2)^2 + (y1-y2)^2= (r1+r2+d)^2

满足上述公式时就认为两个圆相切，其中d为误差值，可以是正数或者负数。如果两个圆相切的话，那么交点就比较好求解了：

   x = x1+ (x2 - x1)*(r1/(r1 + r2));

   y = y1 + (y2- y1)*(r1/(r1 + r2));

      求解到x和y的坐标后，只需要用第三个圆进行验证，即求出这个点到第三个圆的圆心的距离，再和第三个圆的半径做比较，如果在误差允许范围内，那么就可以认为求得的x，y是三个圆的交点，也就是未知点的坐标。

  ②、没有任意两个圆相切，那么就先用两个圆求解两个交点，如下图:

其中A,B是两个圆心，坐标分别为（xa，ya）和（xb，yb），C,D是两个圆的交点，E为AB与CD的交点。

        其中

   AB^2 = (xa – xb)^2 +(ya-yb)^2

        其中

    AC^2 =AE^2 + CE^2         ……………………1

    BC^2 = BE^2+ CE^2         ……………………2  

        AC = ra

        BC= rb

         AE + BE = AB = (xa – xb)^2+ (ya – yb)^2

   等式2转换为

        BC^2 = (AB - AE)^2 + CE^2

        BC^2 = AB^2 + AE^2 – 2*AB*AE + CE^2   ……………………3

       等式3减去等式1:

       BC^2 - AC^2 = AB^2 – 2*AB*AE

       AE = (rb ^2 - ra ^2 -AB^2)/( – 2*AB)

       于是可以根据以下公式求得CE

       CE^2 = AC^2 – AE^2

我们还可以获取E点的坐标(xE, yE)

       xE = xa + ( (xb- xa)*AE )/AB

       yE = ya + ( (yb- ya)*AE )/AB

然后我需要求得AB和CD的斜率KAB和KCD

      kAB = (yb - ya)/(xb - xa)

      kCD = (-1)/kAB //这里要注意kAB为0的情况

然后求得CD和x轴的夹角

      ∠CDX = atan(kCD)

这时候就可以求得C (xc, yc)和D(xd, yd)的坐标

      xc = xE + CE*cos(∠CDXs)

      yc = yE + CE*sin (∠CDXs)

      xd = xE - CE*cos(∠CDXs)

      yd = yE - CE*sin (∠CDXs) //这里也要注意sin (∠CDXs)和cos(∠CDXs)为nan的情况。

至此，我们就求得了两个圆的两个交点坐标，然后只需要用这两个点去第三个圆做验证，就可以获得三个圆的交点，也就是我们要求的未知点。

当我们基于rssi的三点定位算法实现蓝牙无线定位时，可按如下图所示位置放置蓝牙信号发射塔。

      在理想情况下，以三个定节点为圆心，各自与盲节点的距离为半径的圆会交于一点，但是实际测量中，因为有误差因素，导致三组方程出现6个解(即三圆两两相交，产生6个交点，如上图。选取靠内的三个交点(即图中的红色区域)，取它们的坐标的平均值作为最终定位的盲节点坐标，即可完成定位。

系统原理
信号塔设置

1. 配置BLE4.0模块

      根据三点定位原理，本项目需要使用3个信号塔。3个信号塔的主体均为BLE4.0模块，需要把BLE4.0模块的AT指令设置为“从设备”。

方法为：

    （1）给控制板刷一套空的程序。初始打开arduino IDE或新建，都是空程序。

    （2）按下图所示连接电路，错误的连接会导致模块损坏。将蓝牙模块使用杜邦线连接到Bigfish扩展板上，并将扩展板插到控制板上。

  （3）打开Arduino的串口监视器，输入AT指令。


测试查询、设置主从模式

指令

应答

参数

查询：AT+ROLE?

OK+ Get:[para]

Para1: 0 ~ 1

1: 主设备

0: 从设备

Default: 0

设置：AT+ROLE[para]

OK+ Set:[Para]


注：该指令执行后，会导致模块延时500ms重启。

视频教程：

BLE4.0模块的完整AT指令集可以参考  如何使用探索者通信模块


（3）信号塔的三个BLE4.0a蓝牙模块都设为从设备后，使用以下命令直接进行配置。在查询模块的MAC地址时，可以将查询到底地址统一记录在一个文本文件中，以便后续编写使用。（可参考《蓝牙配置说明.txt》）

AT+RENEW      //恢复默认设置

AT -- OK             //测试模块正常

AT+ADDR? -- MAC    //查询模块MAC地址

AT+BAUD0 -- 9600   //设置波特率为9600

AT+CLEAR -- OK     //清除设备配对信息

AT+IMME0 -- OK    //设置模块工作类型：上电立即工作

AT+ROLE0 -- OK    //设置主从模式：从设备

AT+MODE1 -- OK   //设置模块工作模式：远控模式

0：D8A98B788750 （从）

1：D8A98B788732 （从）

2：380B3CFFC5B0 （从）

信号塔设备MAC地址：

以上地址可根据自己的BLE4.0a蓝牙模块进行修改，详细参考《蓝牙配置说明.txt》


2. 信号塔的安装

     将配置好的BLE4.0a蓝牙模块按照下图所示与电池盒进行连接，用于之后安装在信号塔中（注：红色正极线接蓝牙模块的3.3V，黑色负极线接蓝牙模块的GND，错误的连接会导致模块损坏）。

用零件将其固定。

这样的信号塔需要做三个。

3. 资料清单

序号

内容
1

蓝牙配置说明.txt


文件下载
【整体打包】-【U017】如何用蓝牙实现无线定位-2.信号塔设置-资料附件.zip
1.01KB下载4次下载
上一页 1 下一页

本地定位显示

1. 被定位目标

      本项目设计有两个定位装置，一个用于固定目标，一个用于可移动设备。在定位系统的帮助下，我们可以操作可移动设备向固定目标移动。假设这是一个救援场景的话，我们就可以把固定的目标看作等待救援的人或物，把可移动的设备看作前来救援的机器人。


2. 构建待救援者的定位设备

     定位设备主要由1个Arduino Mega2560控制板、1个Bigfish扩展板、2个BLE4.0模块构成。在本实验中，我们为目标安装1个OLED显示屏，以便在调试时查看坐标值。

      两个BLE4.0模块中的一个用于接收3个信号塔的信号（主），在控制板中计算后生成坐标信息。坐标信息可以在OLED显示屏上显示出来，并可以通过另一个BLE4.0模块发送出去（从）。从BLE4.0模块在本功能中暂时用不上，后续的远程定位功能将会用到。

按照上面的针脚使用杜邦线将待救援设备的主蓝牙和从蓝牙连接到主控板上，注意错误的连接会导致模块损坏。


参考视频如下：

3. 目标的BLE4.0a蓝牙模块配置。

     目标的主蓝牙需要接收三个信号发射塔的信号，从而通过三点定位的算法计算出自己当前的位置信息，并发送显示。

使用下面的命令，配置目标的主蓝牙。

AT+RENEW    //恢复默认设置

AT -- OK//测试模块正常

AT+ADDR? -- MAC//查询模块MAC地址

AT+BAUD0 -- 9600//设置波特率为9600

AT+CLEAR -- OK//清除设备配对信息

AT+IMME1 -- OK    //设置模块工作类型：上电等待触发

AT+ROLE1 -- OK        //设置主从模式：主设备

AT+MODE1 -- OK//设置模块工作模式：远控模式


0：D8A98B788750 （从）

1：D8A98B788732 （从）

2：380B3CFFC5B0 （从）


B：F83002253178 （从）

主：D8A98B788758 （主）


      各设备主从蓝牙的MAC地址（以下地址可根据自己的BLE4.0a蓝牙模块的实际MAC地址进行修改，详细参考：蓝牙配置说明.txt）


信号塔MAC地址：

目标1设备地址：

4. 坐标位置的本地显示

    将OLED模块按如图所示的方法连接在Bigfish扩展板上

     按照上面的针脚使用杜邦线将OLED显示屏连接到主控板上，注意错误的连接会导致模块损坏。


参考视频如下：

5. 位置的获取与发送

烧录以下程序（human_rssi.ino），移动目标的位置，可观察到OLED模块显示坐标值的变化。

注意：该例程仅作调试使用，后期构建完整系统时会使用新的程序。

/*------------------------------------------------------------------------------------

  版权说明：Copyright 2022 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2022-5-30 https://www.robotway.com/

  -------------------------------------------------------------------------------------*/

//蓝牙4.0无线连接定位实验：待救援主控接收例程

//配置模块：(模块在配置时与正常工作时TX、RX线序不同，请注意)

//第一步：使用AT指令将所使用的四个模块其中一个设置为主模式，其余三个为从模式；所有模块波特率全部为默认的9600，并记录三个从模块的地址(AT+ADDR?);

//第二步：将主模块在未连接时清除之前的配对信息(AT+CLEAR),再设置其工作类型为类型1(AT+IMME即上后处于等待状态，收到AT+START,AT+DISC,AT+CONNL等指令后开始工作）；

//开始工作：

//第三步：将所有模块上电(主模块使用杜邦线接在MEGA2560上，从模块只给两个电源引脚提供3.3或5V电源即可);

//第四步：将本程序下载进MEGA2560中，将从模块分别摆开，观察显示屏数据；

//按救援机器人的场地在信号塔位置放置3个从模块，主模块放置在中间区域；

//本实验使用的3个从模块地址分别为：

/*********从设备地址********/

// "D8A98B788750",

// "D8A98B788732",

// "380B3CFFC5B0"

/*********从设备地址********/


/**********头文件***************/


#include <Arduino.h>

#include <Wire.h>

#include <MultiLCD.h>

#include <RssiPositionComputer.h>


/***********宏定义**************/

#define DEBUG_SERIAL Serial //打印信息串口

#define CON_SERIAL Serial1 //蓝牙通信串口

#define SEND_SERIAL Serial2 //数据发送串口


#define CMD_CON "AT+CON"

#define CMD_DIS_CON "AT"

#define CMD_GET_RSSI "AT+RSSI?"


LCD_SSD1306 lcd;


RssiPositionComputer myPositionComputer;

Point2D master_point;


//基站数量

#define SLAVENUMBER 3

//基站地址


String BLUETOOTHADDRESS[3] = {

"D8A98B788750",

"D8A98B788732",

"380B3CFFC5B0"

};


//位置发送蓝牙地址

// D8A98B788758


String search_result_string[SLAVENUMBER] = {""};

String rssi[SLAVENUMBER] = {""};

float distance[SLAVENUMBER] = {};


void setup()

{

#if defined(DEBUG_SERIAL)

DEBUG_SERIAL.begin(9600);

#endif

CON_SERIAL.begin(9600);

SEND_SERIAL.begin(9600);


delay(1000);


lcd.begin();

lcd.clear();

lcd.setCursor(25, 3);

lcd.print("Hello World!");

delay(1000);


init_ble();

}


void loop()

{

read_ble(BLUETOOTHADDRESS);

to_axis(distance, &master_point);

}

//读取串口

String serial_read(int _len)

{

String data = "";

int len = 0;

unsigned long t = millis() + 500;


while(1)

{

while(CON_SERIAL.available())

{

char c = CON_SERIAL.read();

data += c; len++;

}

if(len == _len)

{

Serial.println("the len");

Serial.println(data);

break;

}

if(millis() > t)

{

Serial.println("the t");

Serial.println(data);

break;

}

}


#if defined(DEBUG_SERIAL)

//DEBUG_SERIAL.println(data);

#endif


return data;

}


//初始化

void init_ble()

{

CON_SERIAL.print(CMD_DIS_CON);

delay(100);

serial_read(2);

}


//获取设备 RSSI

void read_ble(String * address)

{

for(int i=0;i<SLAVENUMBER;i++)

{

CON_SERIAL.print(CMD_DIS_CON);

delay(100);

serial_read(2);

CON_SERIAL.print(CMD_CON + address[i]);

serial_read(8);

delay(500);

CON_SERIAL.print(CMD_GET_RSSI);

String rssi_str = serial_read(10);

String _rssi = rssi_str.substring(7, rssi_str.length());

//rssi

rssi[i] = _rssi;

//distance

distance[i] = rssiToDistance(rssi[i].toFloat());


#if defined(DEBUG_SERIAL)

DEBUG_SERIAL.println("BLE_" + String(i) + ": " + rssi[i]);

//DEBUG_SERIAL.println("BLE_" + String(i) + ": " + distance[i]);

#endif

//delay(800);

}

}

//计算距离

float rssiToDistance(float rssi)

{

float dis = 0;

//dis = pow(10.0,((abs(rssi)-56)/10.0/1.05));

dis = pow(10.0,((abs(rssi)-56)/5.0/1.65));


return dis;

}


//转换为2d坐标x,y

void to_axis(float * dis, Point2D* actual_master_point)

{

//myPositionComputer.distanceToPoint(*dis,*(dis+1),*(dis+2),actual_master_point);   

myPositionComputer.distanceToPoint(*dis,*(dis+1),random(0,77),actual_master_point);


int x = master_point.x*100;

int y = master_point.y*100;


char point[100];

sprintf(point, "[ax:%3d,ay:%3d]\n",abs(x),abs(y));


#if defined(DEBUG_SERIAL)

DEBUG_SERIAL.println(point);

lcd.clear();

lcd.setCursor(10, 1);

lcd.print("a: human");

lcd.setCursor(25, 5);

lcd.print(point);

#endif


SEND_SERIAL.print(point);

}

6. 待救援定位装置的摆放

     使用下图场地进行构建，在信号塔的位置分别放置三个信号发射塔，将待救援定位装置尽量放置在三个信号塔连线构成的三角形内。依次开启3个信号塔、目标的电源，观察目标的OLED屏幕上显示的坐标位置值（注：目标的电源必须最后打开）。

定位效果如下图所示：

7. 资料清单

序号

内容
1

函数库

2
目标定位设备调试例程
3
蓝牙配置说明.txt


文件下载
【整体打包】-【U017】如何用蓝牙实现无线定位-3.本地定位显示-资料附件.zip
54.8KB下载7次下载
上一页 1 下一页

远程定位显示

1. 待救援定位设备

      按照下面的针脚使用杜邦线将待救援定位设备的主蓝牙、从蓝牙连接到主控板上，和本地显示时的连接针脚是一样的，但是由于不需要连接OLED，因此不需要堆叠Bigfish。

注意：错误的连接会导致模块损坏。

参考视频如下：

烧录程序如下（human.ino）：

/*------------------------------------------------------------------------------------

  版权说明：Copyright 2022 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2022-5-30 https://www.robotway.com/

  -------------------------------------------------------------------------------------*/

//待救援主控例程

//配置模块：(模块在配置时与正常工作时TX、RX线序不同，请注意)

//第一步：使用AT指令将所使用的两个模块其中一个设置为主模式，另外一个为从模式；所有模块波特率全部为默认的9600，并记录主从模块的地址(AT+ADDR?);

//第二步：将主模块在未连接时清除之前的配对信息(AT+CLEAR),再设置其工作类型为类型1(AT+IMME即上后处于等待状态，收到AT+START,AT+DISC,AT+CONNL等指令后开始工作）；

//开始工作：

//第三步：将所有模块上电(主从模块按照上面介绍的连接方式连接即可);

//第四步：将本程序下载进MEGA2560中，将从模块分别摆开，观察显示屏数据；



/*********从设备地址********/

//   "D8A98B788750",

//   "D8A98B788732",

//   "380B3CFFC5B0"

/*********从设备地址********/


/**********头文件***************/

#include <Arduino.h>

#include <Wire.h>

#include <MultiLCD.h>

#include <RssiPositionComputer.h>


/***********宏定义**************/

//#define DEBUG_SERIAL Serial   //打印信息串口

#define CON_SERIAL Serial1    //蓝牙通信串口

#define SEND_SERIAL Serial2   //数据发送串口


#define CMD_CON "AT+CON"

#define CMD_DIS_CON "AT"

#define CMD_GET_RSSI "AT+RSSI?"



RssiPositionComputer myPositionComputer;

Point2D master_point;


//基站数量

#define SLAVENUMBER 3

//基站地址

String BLUETOOTHADDRESS[3] = {

  "D8A98B788750",

  "D8A98B788732",

  "380B3CFFC5B0"

};


//位置发送蓝牙地址

// F83002253650


String search_result_string[SLAVENUMBER] = {""};

String rssi[SLAVENUMBER] = {""};

float distance[SLAVENUMBER] = {};



void setup() {

  #if defined(DEBUG_SERIAL)

  DEBUG_SERIAL.begin(9600);

  #endif

  CON_SERIAL.begin(9600);

  SEND_SERIAL.begin(9600);

  delay(1000);

  init_ble();


}


void loop() {

  read_ble(BLUETOOTHADDRESS);

  to_axis(distance, &master_point);


}


//读取串口

String serial_read(int _len){

  String data = "";

  int len = 0;

  unsigned long t = millis() + 500;

  while(1)

  {

    while(CON_SERIAL.available()){

      char c = CON_SERIAL.read();

      data += c;

      len++;

    }  

    if(len == _len){

       break;

    }

    if(millis() > t)

      break;

  }  


  #if defined(DEBUG_SERIAL)

  //DEBUG_SERIAL.println(data);

  #endif


  return data;

}


//初始化

void init_ble(){

  CON_SERIAL.print(CMD_DIS_CON);delay(100);

  serial_read(2);

}


//获取设备 RSSI

void read_ble(String * address){

  for(int i=0;i<SLAVENUMBER;i++){

    CON_SERIAL.print(CMD_DIS_CON);delay(100);

    serial_read(2);

    CON_SERIAL.print(CMD_CON + address[i]);

    serial_read(8);

    delay(500);

    CON_SERIAL.print(CMD_GET_RSSI);

 

    String rssi_str = serial_read(10);

    String _rssi = rssi_str.substring(7, rssi_str.length());


    //rssi

    rssi[i] = _rssi;

    //distance

    distance[i] = rssiToDistance(rssi[i].toFloat());


    #if defined(DEBUG_SERIAL)

      DEBUG_SERIAL.println("BLE_" + String(i) + ": " + rssi[i]);

      //DEBUG_SERIAL.println("BLE_" + String(i) + ": " + distance[i]);

    #endif


    //delay(800);

  }

 

}


//计算距离

float rssiToDistance(float rssi){

  float dis = 0;

  //dis = pow(10.0,((abs(rssi)-56)/10.0/1.05));

  dis = pow(10.0,((abs(rssi)-56)/5.0/1.65));

  return dis;  

}


//转换为2d坐标x,y

void to_axis(float * dis, Point2D* actual_master_point){

   //myPositionComputer.distanceToPoint(*dis,*(dis+1),*(dis+2),actual_master_point);

   myPositionComputer.distanceToPoint(*dis,*(dis+1),random(0,77),actual_master_point);


   int x = master_point.x*100;

   int y = master_point.y*100;


   char point[100];


   sprintf(point, "[ax:%3d,ay:%3d]\n",abs(x),abs(y));


   #if defined(DEBUG_SERIAL)

      DEBUG_SERIAL.println(point);

   #endif


   SEND_SERIAL.print(point);

}

你也可以按照“本地定位显示”实验的方法，为该定位装置安装OLED模块，这样的话，可以实现本地和远程的同时显示。

2. 构建远程中控台

      远程中控台用于远程、无线、实时显示被定位目标的坐标值，主要由1个Arduino Mega2560控制板、2个BLE4.0模块、1个OLED显示屏、1块锂电池构成。其中两个BLE4.0模块都要设置成主设备模式。

（1）蓝牙模块的配置

主蓝牙设备配置命令：

AT+RENEW    //恢复默认设置

AT -- OK//测试模块正常

AT+ADDR? -- MAC//查询模块MAC地址

AT+BAUD0 -- 9600//设置波特率为9600

AT+CLEAR -- OK//清除设备配对信息

AT+IMME1 -- OK    //设置模块工作类型：上电等待触发

AT+ROLE1 -- OK        //设置主从模式：主设备

AT+MODE1 -- OK//设置模块工作模式：远控模式

0：D8A98B788750 （从）

1：D8A98B788732 （从）

2：380B3CFFC5B0 （从）

B：F83002253178 （从）

主：D8A98B788758 （主）

主1：F8300225362D （主）

主2：F830022538C4 （主）

      各设备主从蓝牙的MAC地址（以下地址可根据自己的BLE4.0a蓝牙模块进行修改，详细参考：蓝牙配置说明.txt）

信号塔设备MAC地址：

待救援定位设备MAC地址：

中控台设备MAC地址：

（2）远程中控台的电路连接

OLED显示屏

GND5VSDA

SCL

主控板引脚

GND5VSDASCL


主蓝牙1

3.3V

GND

RX

TX

主控板引脚

3.3V

GND

TX1

RX1


主蓝牙1

3.3V

GND

RX

TX

主控板引脚

5V

GND

TX2

RX2


/*------------------------------------------------------------------------------------

  版权说明：Copyright 2022 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2022-5-30 https://www.robotway.com/

  -------------------------------------------------------------------------------------*/

//救援指挥中心例程

//配置模块：(模块在配置时与正常工作时TX、RX线序不同，请注意)

//第一步：使用AT指令将所使用的两个模块设置为主模式；所有模块波特率全部为默认的9600，并记录待救援设备从模块的地址(AT+ADDR?);

//第二步：将主模块在未连接时清除之前的配对信息(AT+CLEAR),再设置其工作类型为类型1(AT+IMME即上后处于等待状态，收到AT+START,AT+DISC,AT+CONNL等指令后开始工作）；

//开始工作：

//第三步：将所有模块上电(主模块参考控制中心的连接进行接线，待救援设备从模块参考相应的连接);

//第四步：将本程序下载进MEGA2560中，将从模块分别摆开，观察显示屏数据；

#include <Arduino.h>

#include <Wire.h>

#include <MultiLCD.h>

#define DATA_SERIAL Serial //信息打印串口

#define HUMAN_SERIAL Serial1 //待救援数据

#define ROBOT_SERIAL Serial2 //救援机器人数据

#define CMD_CON "AT+CON"

#define CMD_DIS_CON "AT"

#define CMD_START "AT+START"

LCD_SSD1306 lcd;

String human_address = "F83002253650"; //待救援设备地址

String robot_address = "F83002253178"; //救援机器人设备地址

String human_point = "[ax: 0 , ay: 0]";

String robot_point = "[bx: 0 , by: 0]";

void setup()

{

DATA_SERIAL.begin(9600);

HUMAN_SERIAL.begin(9600);

ROBOT_SERIAL.begin(9600);

lcd.begin();

master_init();

}

void loop()

{

read_human_point();

read_robot_point();

point_display();

}

void connect_ble()

{

//连接待救援设备地址

// HUMAN_SERIAL.print(CMD_START);delay(1000);

HUMAN_SERIAL.print(CMD_DIS_CON);delay(200);

HUMAN_SERIAL.print(CMD_CON + human_address);

//连接救援机器人设备地址

ROBOT_SERIAL.print(CMD_DIS_CON);delay(200);

ROBOT_SERIAL.print(CMD_CON + robot_address);

}

//读取待救援位置

void read_human_point()

{

while(1)

{

String str;

while(HUMAN_SERIAL.available() > 0)

{

str = HUMAN_SERIAL.readStringUntil('\n');

str.trim();

}

if(!str.equals(""))

{

Serial.println(str);

human_point = str; break;

}

}

}

//读取救援机器人位置

void read_robot_point()

{

while(1)

{

String str;

while(ROBOT_SERIAL.available() > 0)

{

str = ROBOT_SERIAL.readStringUntil('\n');

str.trim();

}

if(!str.equals(""))

{

Serial.println(str);

robot_point = str;

break;

}

}

}

void point_display()

{

lcd.clear();

lcd.setCursor(10, 1);

lcd.print("a: human b: robot");

lcd.setCursor(20, 3);

lcd.print(human_point);

lcd.setCursor(20, 5);

lcd.print(robot_point);

}

void master_init()

{

lcd.clear();

lcd.setCursor(25, 3);

lcd.print("Hello World!");

delay(1000);

point_display();

connect_ble();

while(true)

{

if(HUMAN_SERIAL.available() > 0 || ROBOT_SERIAL.available() > 0)

break;

}

}

参考视频如下：

3. 中控台接收待救援者的坐标位置

      在中控台的控制板中烧录以下程序(master.ino)，并在待救援定位装置的主控板中烧录相应的程序，移动待救援定位装置的位置，可以观察到中控台OLED模块显示坐标的变化。

      烧录了例程后，依次开启信号塔、待救援定位装置、中控台的电源，中控台的OLED屏幕上即可显示待救援者的坐标值

（注：中控台的电源必须最后打开）。


定位效果如下图所示:

4. 资料清单

序号

内容
1

远程定位显示-例程

2蓝牙配置说明.txt


文件下载
【整体打包】-【U017】如何用蓝牙实现无线定位-远程定位显示-资料附件.zip
60.89KB下载2次下载
上一页 1 下一页

双定位显示

1. 机器人定位装置的构建

      按照上面的针脚使用杜邦线将救援机器人定位装置的主从蓝牙连接到主控板上，注意错误的连接会导致模块损坏。


参考视频如下：

2. 机器人位置的获取与发送

(1)在机器人定位装置的控制板中烧录以下程序（robot.ino）

/*------------------------------------------------------------------------------------

  版权说明：Copyright 2022 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2022-5-30 https://www.robotway.com/

  -------------------------------------------------------------------------------------*/

//救援机器人定位装置例程

//配置模块：(模块在配置时与正常工作时TX、RX线序不同，请注意)

//第一步：使用AT指令将所使用的两个模块其中一个设置为主模式，另外一个为从模式；所有模块波特率全部为默认的9600，并记录主从模块的地址(AT+ADDR?);

//第二步：将主模块在未连接时清除之前的配对信息(AT+CLEAR),再设置其工作类型为类型1(AT+IMME即上后处于等待状态，收到AT+START,AT+DISC,AT+CONNL等指令后开始工作）；

//开始工作：

//第三步：将所有模块上电(主从模块按照上面介绍的连接方式连接即可);

//第四步：将本程序下载进MEGA2560中，将从模块分别摆开，观察显示屏数据；

/*********从设备地址********/

// "D8A98B788750",

// "D8A98B788732",

// "380B3CFFC5B0"

/*********从设备地址********/

/**********头文件***************/

#include <Arduino.h>

#include <Wire.h>

#include <MultiLCD.h>

#include <RssiPositionComputer.h>

/***********宏定义**************/

#define DEBUG_SERIAL Serial //打印信息串口

#define CON_SERIAL Serial1 //蓝牙通信串口

#define SEND_SERIAL Serial2 //数据发送串口

#define CMD_CON "AT+CON"

#define CMD_DIS_CON "AT"

#define CMD_GET_RSSI "AT+RSSI?"

RssiPositionComputer myPositionComputer;

Point2D master_point; //基站数量

#define SLAVENUMBER 3 //基站地址

String BLUETOOTHADDRESS[3] = {

"D8A98B788750",

"D8A98B788732",

"380B3CFFC5B0"

};

//位置发送蓝牙地址

// F83002253178

String search_result_string[SLAVENUMBER] = {""};

String rssi[SLAVENUMBER] = {""};

float distance[SLAVENUMBER] = {};


void setup()

{

#if defined(DEBUG_SERIAL)

DEBUG_SERIAL.begin(9600);

#endif

CON_SERIAL.begin(9600);

SEND_SERIAL.begin(9600);

delay(1000);

init_ble();

}


void loop()

{

read_ble(BLUETOOTHADDRESS);

to_axis(distance, &master_point);

} //读取串口


String serial_read(int _len)

{

String data = "";

int len = 0;

unsigned long t = millis() + 1000;

while(1)

{

while(CON_SERIAL.available())

{

char c = CON_SERIAL.read();

data += c; len++;

}

if(len == _len)

{

break;

}

if(millis() > t)

break;

}

#if defined(DEBUG_SERIAL)

DEBUG_SERIAL.print("Serialread_data=");

DEBUG_SERIAL.println(data);

#endif

return data;

}


//初始化

void init_ble()

{

CON_SERIAL.print(CMD_DIS_CON);

delay(100);

serial_read(2);

}


//获取设备1 RSSI

void read_ble(String * address)

{

DEBUG_SERIAL.println("-----------------Start------------------");

for(int i=0;i<SLAVENUMBER;i++)

{

CON_SERIAL.print(CMD_DIS_CON);

delay(500);

serial_read(2);

CON_SERIAL.print(CMD_CON + address[i]);

serial_read(8);

delay(800);

CON_SERIAL.print(CMD_GET_RSSI);

String rssi_str = serial_read(10);

String _rssi = rssi_str.substring(7, rssi_str.length());

//rssi

rssi[i] = _rssi;

//distance

distance[i] = rssiToDistance(rssi[i].toFloat());

#if defined(DEBUG_SERIAL)

DEBUG_SERIAL.println("BLE_" + String(i) + ": " + rssi[i]);

DEBUG_SERIAL.println("BLE_" + String(i) + ": " + distance[i]);

#endif

//delay(800);

}

DEBUG_SERIAL.println("------------------End------------------");

DEBUG_SERIAL.println();

}


//计算距离

float rssiToDistance(float rssi)

{

float dis = 0;

//dis = pow(10.0,((abs(rssi)-56)/10.0/1.05));

dis = pow(10.0,((abs(rssi)-56)/5.0/1.65));

return dis;

}

//转换为2d坐标x,y

void to_axis(float * dis, Point2D* actual_master_point)

{

//myPositionComputer.distanceToPoint(*dis,*(dis+1),*(dis+2),actual_master_point);

myPositionComputer.distanceToPoint(*dis,*(dis+1),random(0,77),actual_master_point);

int x = master_point.x*100;

int y = master_point.y*100;

char point[100];

sprintf(point, "[bx:%3d,by:%3d]\n",abs(x),abs(y));

#if defined(DEBUG_SERIAL)

DEBUG_SERIAL.println(point);

#endif

SEND_SERIAL.print(point);

}

（2）在中控台的主控板中烧录以下程序（master.ino）

/*------------------------------------------------------------------------------------

  版权说明：Copyright 2022 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2022-5-30 https://www.robotway.com/

  -------------------------------------------------------------------------------------*/

//救援指挥中心例程

//配置模块：(模块在配置时与正常工作时TX、RX线序不同，请注意)

//第一步：使用AT指令将所使用的两个模块设置为主模式；所有模块波特率全部为默认的9600，并记录待救援设备从模块的地址(AT+ADDR?);

//第二步：将主模块在未连接时清除之前的配对信息(AT+CLEAR),再设置其工作类型为类型1(AT+IMME即上后处于等待状态，收到AT+START,AT+DISC,AT+CONNL等指令后开始工作）；

//开始工作：

//第三步：将所有模块上电(主模块参考控制中心的连接进行接线，待救援设备从模块参考相应的连接);

//第四步：将本程序下载进MEGA2560中，将从模块分别摆开，观察显示屏数据；


#include <Arduino.h>

#include <Wire.h>

#include <MultiLCD.h>


#define DATA_SERIAL Serial   //信息打印串口

#define HUMAN_SERIAL Serial1 //待救援数据

#define ROBOT_SERIAL Serial2 //救援机器人数据


#define CMD_CON "AT+CON"

#define CMD_DIS_CON "AT"

#define CMD_START "AT+START"


LCD_SSD1306 lcd;


String human_address = "F83002253650";   //待救援设备地址

String robot_address = "F83002253178";   //救援机器人设备地址


String human_point = "[ax: 0 , ay: 0]";

String robot_point = "[bx: 0 , by: 0]";


void setup() {

  DATA_SERIAL.begin(9600);

  HUMAN_SERIAL.begin(9600);

  ROBOT_SERIAL.begin(9600);

  lcd.begin();

  master_init();

}


void loop() {

  read_human_point();

  read_robot_point();

  point_display();

}


void connect_ble(){

 

  //连接待救援设备地址

//   HUMAN_SERIAL.print(CMD_START);delay(1000);

  HUMAN_SERIAL.print(CMD_DIS_CON);delay(200);

  HUMAN_SERIAL.print(CMD_CON + human_address);

  //连接救援机器人设备地址

  ROBOT_SERIAL.print(CMD_DIS_CON);delay(200);

  ROBOT_SERIAL.print(CMD_CON + robot_address);

 

}


//读取待救援位置

void read_human_point(){

  while(1){

    String str;

    while(HUMAN_SERIAL.available() > 0){

      str = HUMAN_SERIAL.readStringUntil('\n');

      str.trim();  

    }

    if(!str.equals("")){

      Serial.println(str);

      human_point = str;

      break;

    }

  }

}


//读取救援机器人位置

void read_robot_point(){

  while(1){

    String str;

    while(ROBOT_SERIAL.available() > 0){

      str = ROBOT_SERIAL.readStringUntil('\n');

      str.trim();  

    }

    if(!str.equals("")){

      Serial.println(str);

      robot_point = str;

      break;

    }

  }

}


void point_display(){

  lcd.clear();

  lcd.setCursor(10, 1);

  lcd.print("a: human   b: robot");

  lcd.setCursor(20, 3);

  lcd.print(human_point);

  lcd.setCursor(20, 5);

  lcd.print(robot_point);

 

}


void master_init(){

  lcd.clear();

  lcd.setCursor(25, 3);

  lcd.print("Hello World!");

  delay(1000);

  point_display();

  connect_ble();

  while(true){

    if(HUMAN_SERIAL.available() > 0 || ROBOT_SERIAL.available() > 0)

      break;

  }

}

（3）在待救援定位装置的控制板中烧录以下程序（human.ino）

/*------------------------------------------------------------------------------------

  版权说明：Copyright 2022 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2022-5-30 https://www.robotway.com/

  -------------------------------------------------------------------------------------*/

//待救援主控例程

//配置模块：(模块在配置时与正常工作时TX、RX线序不同，请注意)

//第一步：使用AT指令将所使用的两个模块其中一个设置为主模式，另外一个为从模式；所有模块波特率全部为默认的9600，并记录主从模块的地址(AT+ADDR?);

//第二步：将主模块在未连接时清除之前的配对信息(AT+CLEAR),再设置其工作类型为类型1(AT+IMME即上后处于等待状态，收到AT+START,AT+DISC,AT+CONNL等指令后开始工作）；

//开始工作：

//第三步：将所有模块上电(主从模块按照上面介绍的连接方式连接即可);

//第四步：将本程序下载进MEGA2560中，将从模块分别摆开，观察显示屏数据；


/*********从设备地址********/

//   "7CEC79613E1B",

//   "20C38FF2BAA9",

//   "7C669D9B281B"

/*********从设备地址********/


/**********头文件***************/

#include <Arduino.h>

#include <Wire.h>

#include <MultiLCD.h>

#include <RssiPositionComputer.h>


/***********宏定义**************/

//#define DEBUG_SERIAL Serial //打印信息串口

#define CON_SERIAL Serial1 //蓝牙通信串口

#define SEND_SERIAL Serial2 //数据发送串口


#define CMD_CON "AT+CON"

#define CMD_DIS_CON "AT"

#define CMD_GET_RSSI "AT+RSSI?"



RssiPositionComputer myPositionComputer;

Point2D master_point;


//基站数量

#define SLAVENUMBER 3

//基站地址

String BLUETOOTHADDRESS[3] = {

  "D8A98B788750",

  "D8A98B788732",

  "380B3CFFC5B0"

};


//位置发送蓝牙地址

// 20CD397F05B7


String search_result_string[SLAVENUMBER] = {""};

String rssi[SLAVENUMBER] = {""};

float distance[SLAVENUMBER] = {};



void setup() {

  #if defined(DEBUG_SERIAL)

  DEBUG_SERIAL.begin(9600);

  #endif

  CON_SERIAL.begin(9600);

  SEND_SERIAL.begin(9600);

  delay(1000);

  init_ble();


}


void loop() {

  read_ble(BLUETOOTHADDRESS);

  to_axis(distance, &master_point);


}


//读取串口

String serial_read(int _len){

  String data = "";

  int len = 0;

  unsigned long t = millis() + 500;

  while(1)

  {

    while(CON_SERIAL.available()){

      char c = CON_SERIAL.read();

      data += c;

      len++;

    }  

    if(len == _len){

       break;

    }

    if(millis() > t)

      break;

  }  


  #if defined(DEBUG_SERIAL)

  //DEBUG_SERIAL.println(data);

  #endif


  return data;

}


//初始化

void init_ble(){

  CON_SERIAL.print(CMD_DIS_CON);delay(100);

  serial_read(2);

}


//获取设备 RSSI

void read_ble(String * address){

  for(int i=0;i<SLAVENUMBER;i++){

    CON_SERIAL.print(CMD_DIS_CON);delay(100);

    serial_read(2);

    CON_SERIAL.print(CMD_CON + address[i]);

    serial_read(8);

    delay(500);

    CON_SERIAL.print(CMD_GET_RSSI);

 

    String rssi_str = serial_read(10);

    String _rssi = rssi_str.substring(7, rssi_str.length());


    //rssi

    rssi[i] = _rssi;

    //distance

    distance[i] = rssiToDistance(rssi[i].toFloat());


    #if defined(DEBUG_SERIAL)

      DEBUG_SERIAL.println("BLE_" + String(i) + ": " + rssi[i]);

      //DEBUG_SERIAL.println("BLE_" + String(i) + ": " + distance[i]);

    #endif


    //delay(800);

  }

 

}


//计算距离

float rssiToDistance(float rssi){

  float dis = 0;

  //dis = pow(10.0,((abs(rssi)-56)/10.0/1.05));

  dis = pow(10.0,((abs(rssi)-56)/5.0/1.65));

  return dis;  

}


//转换为2d坐标x,y

void to_axis(float * dis, Point2D* actual_master_point){

   //myPositionComputer.distanceToPoint(*dis,*(dis+1),*(dis+2),actual_master_point);

   myPositionComputer.distanceToPoint(*dis,*(dis+1),random(0,77),actual_master_point);


   int x = master_point.x*100;

   int y = master_point.y*100;


   char point[100];


   sprintf(point, "[ax:%3d,ay:%3d]\n",abs(x),abs(y));


   #if defined(DEBUG_SERIAL)

      DEBUG_SERIAL.println(point);

   #endif


   SEND_SERIAL.print(point);

}

（4）把定位装置安装在救援机器人身上。

（5）依次开启信号塔、待救援定位装置、机器人定位装置、中控台的电源，中控台的OLED屏幕上即可同时显示待救援者和机器人的坐标值（注：中控台的电源必须最后打开）。


（6）控制救援机器人移动，可看到机器人的定位坐标随着它的移动而变化。

     接下来就可以控制机器人向待救援目标前进了。如果再给救援机器人增加一套WiFi视频遥控的功能（请参考【R231】斜三角履带机械臂小车），就可以构建出一套完整的机器人救援场景模型了。

3. 资料清单

序号

内容
1

双目标显示-例程

2蓝牙配置说明.txt


文件下载
【整体打包】-【U017】如何用蓝牙实现无线定位-5.双目标显示-资料附件.zip
65.58KB下载3次下载
上一页 1 下一页
© 2022 机器时代（北京）科技有限公司  版权所有
机器谱——机器人共享方案网站
学习  开发  设计  应用