基于图形建模的未知环境定位系统

文/卢箫扬 谢阳腾 胡剑忠 郭凡靖

针对移动机器人研究领域中

的地图构建问题，传统方案往往

会出现机器人不能准确定位，创

建地图时间长等问题。为了解决

该问题，本文提出了一种较为可

靠的方案。通过激光传感器回传

的数据传输回控制室绘图并显示，

以此即使在不可见情况让探测人

员实时了解周遭环境，可以应用

在矿区探测，深海探测以及外星

探测中。

摘 要

据，该传感器一次共回传三个数据quality 信

号质量distance 距离，angle 角度三个数据。

二是远程控制部分，采用stm32 最为MCU, 以

及L298N 模块作为电机驱动驱动履带电机，

MCU 主要经由控制台传输的指令进行相应的

移动操作，三是无线通讯模块，在平台制作中，

首先基于成本考虑，采用了蓝牙模块HC-05

进行无线通讯，接收控制端数据至MCU，以

及传输由Rplidar 激光传感器输出的数据至控

制端口进行解算。移动平台部分代码详解:

USART1_Config(); 初始化串口1 设置

波特率9600 GPIO_Config(); 设置控制电机

的4 个GPIO 口初始化电平通过USART_

ReceiveData(USART1) 函数接收串口数据根

据相应数据与设定值进行分析对比，最后从

中得出相应的命令控制移动平台前后左右停

止。

3 控制端搭建

控制端分为两部分，一是控制移动平台

的移动控制端，采用手机APP 的形式，通过

蓝牙通信给移动平台通信发送指令。二是绘图

部分，将无线传输的数据回传到控制端后，解

算出相应的角度与距离数据，按比例缩放后调

用图形界面，绘制出二维平面图。

绘图软件部分详解：

使用CDC 与CBitmap 分别定义一个显示

设备对象和一个位图对象，接下来建立一个与

屏幕显示兼容的位图，将位图选入到内存的显

示设备中以及设置窗口大小，然后创建画笔，

之后就开始画图。借着建立一个循环次数为传

输回来的数据包的大小，每次循环都将保存数

据包的数据取出来，包括角度数据以及距离数

据，将距离数据按比例系数缩小，接着将角度

数据取出来经过换算得到正确角度，距离与

角度换算如下：

f l o a t d i s t P i x e l = _ s c a n _ d a t a [ p o s ] .

dist*distScale;

float rad = (float)(_scan_data[pos].

angle*PI/180.0);

接下来将以上数据换算成二维平面图的

相应坐标，公式如下centerPt.x，centerPt.y 为

中心坐标的值。

float X = sin(rad)*(distPixel) + centerPt.x;

float Y = centerPt.y - cos(rad)*(distPixel);

然后使用quality 来对亮度信息来反应该

点的信号质量信息。接着使用FillSolidRect 方

法将计算出的相应数值作为参数填入，该方法

输入参数有三个，分别为X ,Y 坐标，以及亮

度信息，该方法能够根据以上参数在窗口的相

应的X,Y 坐标标出点，点的亮度由第三个参

数决定，每次循环都能完成一个点的绘制，扫

描一圈的数据绘制将会循环结束后完成，便可

以得到周遭环境的二维模型。基本功能如上，

为了实用性的考虑还需要能够知道传感器的旋

转起始角度，因此在回传回的数据存储将旋转

角度先存储，如果开始的数据不是起始角度，

将从下一圈开始作为起始圈重新开始存储以及

绘图。在此之外，还要进行噪点的判断以增强

程序的健壮性，激光在测量的范围内返回的

quality 是有一定范围的，如果超过了范围就可

以判断为噪点，在绘图时去除；去除后为了保

证图像的连续性，还需要在绘制过程中将图像

补充完整，可以通过对相邻点的数据求导，取

极限来估计噪点的近似距离值，最后以将此数

值代替原有的距离数值填入原来的数组中，从

而在绘制中更加真实的还原环境信息。

4 小结

基于图形建模的未知环境定位系统主要

是通过搭建移动平台同时，搭载上激光雷达传

感器，通过无线通讯技术将扫描得到的数实时

回传，控制端通过解析回传的数据包得到环境

中相应的数据，并以此进行在上位机进行二维

制图以尽可能的还原所探测的环境信息，控制

端再以此并对移动平台做出相应的控制，不断

循环，对未知环境进行探测。

参考文献

[1] 刘博. 基于激光雷达的室内外巡检移动机

器人原型设计与实现[D]. 西南交通大学,

计算机技术,2015.

[2] 项志宇, 刘济林.PC 机和激光测距雷达双

路高速数据通信接口卡[J]. 电子技术应

用,2000(10).

[3] 陆祖康, 臧侃, 李培勇, 洪治, 吴碧珍. 激

光雷达三维成像系统的研究[J]. 浙江大

学学报( 工学版),1999(04).

[4] 王宏, 艾海舟, 何克忠, 张钹. 移动

机器人体系结构与系统设计[J]. 机器