c语言小车路径规划算法,基于路径规划的室内小车定位系统

李佳佳，李智，管四海

(西安电子科技大学 机电工程学院，陕西 西安 710071)

摘要：针对现有室内定位技术模型复杂、成本高等问题，设计了一种基于路径规划的室内小车定位系统。该系统的主控芯片选用STM32F103ZET6，偏角测量模块基于HMC5883L，障碍处理模块选用HCSR04；另外，采用极坐标的表示方法，给定室内初始位置和目标点，测出各目标点相对于初始位置的偏角和距离，依据相对偏角和相对距离规划小车的行走路径。实际环境测试表明，该系统不仅能使小车准确到达目标点，而且还能有效避障。

关键词：室内定位；路径规划；极坐标

中图分类号：TN96文献标识码：ADOI： 10.19358/j.issn.1674-7720.2017.07.025

引用格式：李佳佳，李智，管四海.基于路径规划的室内小车定位系统［J］.微型机与应用，2017,36(7)：84-87.

0引言

*基金项目：国家自然科学基金(61673310)GPS定位系统由于其快速、高效、全球覆盖等特点，被广泛地应用于汽车导航、工程测量、飞机导航等领域。而在室内，由于建筑物的遮挡，GPS的信号会减弱，并且精度满足不了室内定位的要求。现有的室内定位技术主要有RFID、视频、ZibBee［1］、蓝牙［2］、WLAN［3］等，但由于室内物体间距离较短，信号存在反射、衍射和绕射等非直线传播的情况，再加之现有设备分辨率不足，使得室内定位误差较大［2］。

在现有的室内路径规划定位系统中，大多使用栅格构图法和路标法［4］进行室内定位，其中栅格构图法是在机器人行走过程中，实时构建地图并规划路径，但在定位过程中，需要搜索的空间范围很大，若没有很好的简化算法，实时性很难保证。路标法的定位中，若有一个路标出现故障，则系统的整体稳定性就会受到很大的影响。本文采用二维极坐标的方式确定位置，根据各目标点的位置，事先为智能小车规划好行走路线［5］，避免了复杂算法的优化和参照物的使用，还能有效避障。本系统具有设计简单、易操作、成本低的特点，可以应用于餐厅服务机器人的设计，以机器人代替人工送餐，给人们的生活带来便利。

1系统整体设计

系统整体结构设计如图1，流程如下：

图1基于路径规划的室内定位系统结构框图首先需要事先测量小车的起点与各目标点的距离ρ1,ρ2…ρn和角度θ1,θ2…θn，并将坐标表示为(ρ1,θ1),(ρ2,θ2)…(ρn,θn)；然后利用码盘和光电对管测出固定占空比下的车轮转速，并通过LCD1602实时显示，从而通过控制时间来达到控制小车前进指定距离的目的。利用HMC5883L电子罗盘测量角度，将电子罗盘的测量数据通过I2C总线传输到主控芯片STM32F103ZET6，主控芯片将传感器采集的数据进行分析处理，进而控制电机转动使车前进。在此期间，通过舵机带动HCSR04超声波模块不断地转动测距，当测得距离小于某一设定值时，控制小车停止，并且继续不断检测距离。当撤去障碍物，再按事先规划的路径继续前进，到达目标点。

2硬件设计

2.1主控芯片

该系统的主控芯片选用STM32F103ZET6，此芯片作为32位ARM微控制器，有着比16位、8位处理器更强的数据处理能力、更丰富的外设接口。在该系统设计中，由于STM32系列微控制器的I2C接口稳定性较差，因此采用IO口模拟I2C总线的方式实现稳定的信息传输。选用PB6和PB7模拟I2C总线的SCL和SDA，实现了与HMC5883L模块的稳定通信。

2.2HMC5883L模块

霍尼韦尔HMC5883L是一种高集成模块，选其为偏角测量模块，是因为其具有高达1°~2°的测量精度；同时采用I2C数字接口通信，高效、稳定，程序设计简单。需要特别注意的是：

(1)为防止干扰，使用时应避免该模块直接与金属物体接触；

(2)在电磁干扰较大的室内环境，要对该模块进行校正［6］，以确保测量的准确；

(3)由于其测量灵敏，在设置转角时，可以用一个小范围的值代替特定值。

该系统在设计时，向小车后方加装木板，将HMC5883L模块固定于木板上，以降低干扰，模块上的x和y轴即为在水平面上建立的二维坐标系。下面给出电子罗盘返回角度θ的计算，其中x和y表示读出HMC5883L寄存器中的值。

经过此运算后，将电子罗盘测得的数据范围转化为0°~360°。即在0°时，x轴所指向的方位为南。

2.3电机驱动设计

电机驱动模块电路设计如图2，驱动芯片采用L293，一个芯片可以驱动两个直流电机，简化了系统电路的设计，同时，在引脚电平低于一定水平时，芯片停止工作，对电源起到了保护作用。电机调速方式为PWM方式，即在周期固定的前提下，通过控制输出脉宽的不同来进行速度的调节。

2.4测速模块

测速模块电路设计如图3。其中，光电对管选用ITR9608，在系统设计时，将码盘固定在电机上，并置于光电对管的凹槽中，车轮带动码盘转动，将PWM的信号传输至主控芯片，从而计算车轮的转速，并通过LCD1602实时显示。

设车轮速度为v，直径为d，光电码盘的齿数是n，即车轮转动一周的计数次数为2n，单位时间高低电平的变化次数为t，则车轮速度：

2.5超声波避障模块

该系统将超声波和舵机通过云台结合起来，放在车体的最前端，通过转动舵机获取小车各个方向的距离值，进行障碍处理。在对舵机的控制中，用不同宽度的脉冲信号，对应舵机转动的不同角度。选用的HCSR04超声波模块测量范围可达2 cm~400 cm，满足该系统对避障精度的要求，并通过高电平的持续时间计算障碍物的距离，即：

距离=(高电平时间×声速)/2(3)

3软件设计

软件设计流程如图4所示，其主要部分包括系统初始化、采集角度数据、角度的判断、避障等。系统选用Keil uVision5开发工具，选用C语言编程。其中，为了使智能小车在行走的同时检测障碍，设计了10 ms计数法，同时定义num变量，保存测距函数执行的次数，从而达到记录距离的目的。在避障函数中，控制舵机左转、居中、右转，不断地循环，达到多方位检测距离的目的。具体程序设计如下：

(1)先对系统的外设资源初始化，然后判断当jiaodu≠预设角时，执行右转函数Turn_Right()，直到jiaodu=预设角，执行停止函数Stop()，并使得标志位set=1。

jiaodu=read_HMC5883L();

if(jiaodu>=5&&jiaodu<=355)//预设角度为0

{ while(jiaodu>=5&&jiaodu<=355 )

{ Turn_Right();

jiaodu=read_HMC5883L();

}

Stop();

set=1;

Delayms(2000);

}

(2)当set=1时，表明已找到指定方向，执行直行函数Go()，并延迟适当时间，该时间由目标距离和车速决定。

if(set==1)

{set=0;

just_car();//前进中避障

Stop();

}

(3)接着获取角度值，判断是否为下一目标点所设角度，不断循环，直到最后一个目标点停止。

(4)在直行过程中，用TIM2定时器产生1 ms时基信号，每隔10 ms，执行一次距离检测函数Get_distance()和障碍处理函数Handle_bar()。并定义一个num变量用来计数，每过10 ms，num++。

void just_car(void)

{int num=0;

while(num<300)//延迟3 s

{if(count_1ms>10)

{ count_1ms=0;

Get_distance();

Handle_bar();

num++;

}

}

}

(5)当Get_distance()返回值小于10 cm时，则认为前方存在障碍，执行障碍处理函数Handle_bar()。

void Handle_bar()

{if(juli_cm < 10)

{ while(1)

{

障碍处理

……

}

}

else

Go();

break;

}

(6)当距离检测函数Get_distance()返回值大于10 cm时，则认为障碍已经撤走，此时程序通过num中保存的值可以计算出剩余要走的路程。

(7)返回(3)。

4实际测试

为了测试该系统能准确定位且能有效避障，设计实验如下：

在室内环境，给定初始位置和各目标点，并假定预设角为0°，即正南方向，对小车进行直行和转弯定位测试。为了更易观察实验效果，设定直行距离为50 cm，转角为90°，避障距离为10 cm。设置预设角时，由于电子罗盘的灵敏度较高，因此用一定角度范围代替具体的角度值，且在此实验中角度范围为10°。测试中的实际目标点分布如图5示：O表示起点，A、B、C分别为目标点。

首先，将小车置于O点寻找预设角；然后前进50 cm，右转90°，到达点A；接着前进50 cm，再右转90°，到达点B；最后前进50 cm停止于点C。在小车前进过程中，将手挡在小车的前方作为障碍物，当手与小车距离小于10 cm时，小车停止前进，同时舵机带动超声波模块左右转动测距，若障碍一直存在，则会循环检测距离，直到将障碍物撤去，此后小车继续前进剩余距离。实验中测量其运行时距离与角度的对应关系，并将实际测得的轨迹与理想轨迹进行仿真，结果如图6。从图6中可知，基于路径规划的室内小车定位系统，能够在有效误差范围内到达各目标点，完成室内定位任务。

参考文献

［1］ 何海平，郭杭，方爽.基于模糊聚类的ZigBee室内定位系统设计［J］.电子技术应用，2016,42(5)：71-77.

［2］ 陈国平，马耀辉，张百珂.基于指纹技术的蓝牙室内定位系统［J］. 电子技术应用，2013,39(3)：104-107.

［3］ 刘洺辛, 孙建利. 基于能效的WLAN室内定位系统模型设计与实现［J］. 仪器仪表学报, 2014, 35(5):11691178.［4］ 宋婷.室内移动机器人路径规划与定位技术研究［D］.西安：西安电子科技大学，2011.

［5］ JARVIS R. Distance transform based path planning for robot navigation［M］. Singapore：World Scientific, 1993.

［6］ 刘诗斌，冯小毅，李宏.基于椭圆假设的电子罗盘误差补偿方法［J］.传感器技术，2002,21(10)：28-33.