基于可见光定位技术的智能车导航系统设计*

彭健坤，鲍军陶，胡沈阳，林作俊，于子涵，肖纯贤，2

（1.南开大学 电子信息与光学工程学院，天津 300350；2.天津市光电传感器与传感网络技术重点实验室，天津 300350）

0 引言

当今移动通信和无线网络技术发展迅速，无线定位技术成为当今热门技术。在室内及具有深度阴影效应的环境下，由于卫星和蜂窝信号中断，使全球定位系统（Global Positioning System，GPS）很难应用于室内环境[1]。根据前期的研究成果可知，传统的室内定位方法，如基于无线网络（Wireless Fidelity，Wi-Fi）的室内定位精度在1～5 m[2]，基于蓝牙的室内定位精度约为10 m[3]，使用可见光进行室内定位（Visible Light Positioning，VLP）在定位精度、对电磁波干扰和建设成本等方面具有一定的优势，将可见光定位应用于智能车导航，可有效提高导航精确度，可应用于酒店、饭店、隧道、商场等多种室内场景，有极大发展前景[4]。

1 系统介绍

基于VLP 技术的智能车导航系统由LED 灯发送端、光电二极管接收电路、单片机意法半导体32 位系列微控制器芯片（STMicroelectronics 32bit ARM Cortex MCU，STM32）、树莓配小车等部分组成，整体上可分为定位和导航两部分，如图1 所示。在定位部分，先对发送端LED 预处理使之发出不同频率的可见光信号，光电二极管接收端接收到光信号并还原成电信号，并将电信号传入STM32，单片机将电压信号用模/数转换（Analog-to-Digital Converter，ADC）采集存储，通过空间定位算法计算出当前空间坐标；在导航部分，使树莓派智能车采用Astar 寻路算法，通过启发式搜索方式寻找最优路径并生成节点坐标列表，传递给智能车。在智能车行进过程中，通过串口通信将STM32 获取的当前实际坐标传递给智能车，智能车通过比较当前实际坐标位置和规划好的节点坐标位置的直线距离，判断是否需要进行路径修正，距离过大时，智能车触发路径修正并向下一个理想坐标节点偏移，以回到规划好的路线上，从而达到高精度导航。定位部分涉及硬件电路设计和定位算法设计，导航部分主要是Astar 算法和路径修正算法设计，论文后续的描述从硬件和软件两方面展开介绍。

图1 基于VLP 的智能车导航系统

2 硬件部分

2.1 发送端电路

由图1 可知，需要对LED 进行预处理使之发出包含位置信息的光信号。通过实验可知，将方波信号加在LED 上，LED 会产生光照强度以方波形式变化的白光，接收端也能够正常地接收到方波信号。同时，LED 的位置坐标可用于定位，而闪烁不同频率的LED 则有不同的位置坐标，且闪烁频率与LED 的位置坐标具有一一映射关系，此关系可存储于单片机内部的存储程序中，方波发生电路如图2 所示。值得注意的是，此时调制灯具的闪烁频率应当大于人眼可以分辨的范围，以此满足照明需求，同时在设定不同LED 闪烁频率时要注意避免后续FFT 的频谱混叠问题。本实验使用了4 盏LED，闪烁频率分别为1.1 kHz、1.77 kHz、2.2 kHz、2.85 kHz，在给LED 足够大功率之后，方可满足照明需求。

图2 发送端电路

2.2 接收端电路

接收端电路如图3 所示，分为接收级、高通滤波级、放大级。接收级利用光电二极管（photoelectric diode，pd）的光电转化效应，将方波形式的光信号转化成方波形式的电压信号[5]；高通滤波级采用二阶有源滤波，滤除方波信号中的微小杂波；在放大级，通过对前级方波信号的放大使之变成单片机STM32 能够采集的电压信号[6]，范围为0～3.3 V。

图3 接收端电路

3 软件部分

3.1 单片机STM32 数据采集及FFT

单片机通过ADC 采集接收端电路输出的电压信号并转化成数字信号等待下一步操作。软件方面，通过控制采样频率将采集到的数字信号赋给特定数组，并进行FFT，将时域电压值转化为频域FFT 值[7]。理论上频域采集离散方波信号在信号频率处有最大值，在编程时，可以遍历所有频率数据，通过逐个比较从而取出最大的FFT 值和它对应的频率，以此区分空间中的4 盏LED。当所有LED 输出方波幅度相同时，如果建立单个LED灯在接收端对应的FFT 值和距离的关系，便可实现FFT和距离的转换，获取智能车和LED 灯之间的距离，用于后续定位算法。

本实验以1.77 kHz 的LED 灯为单一发送端，在考虑倾角的情况下[8]，收集距离d与FFT 对应频率点的数据，并通过MATLAB 拟合成曲线，如图4 所示。其中，曲线模型为：y=axb。曲线系数a为1.975×107，系数b为-2.106。

图4 距离与FFT 拟合结果图

3.2 定位算法

基于位置线（Line Of Position，LOP）的加权最小二乘算法，沿袭自三边定位算法[9-10]。假设共n个LED 节点所在坐标分别为（xi,yi,zi）（1≤i≤n），接收端与发送端实际距离分别为di（1≤i≤n），目标坐标为（x,y,z）,则所确定的圆方程组如下：

从信号传播模型方程组式（1）出发，将其第k个方程减去第k-1 个（k=2，3，4，…，n）方程将其变形为：

式（2）方程组的其中每个方程都是对应第k个圆和第k-1 个圆的根轴线，也称为位置线（LOP）[11-12]。

记：

方程组转化为矩阵形式：

定位系统对各LED 节点的误差叠加到接收端的测量距离di后，导致AX与B之间存在一定的误差[13-14]，记误差向量：

式（7）的平方和导数的零点可表示为：

由式（6）及方程理论，至少需要4 个LED 节点方能够给出解[15]。本次实验使用4 个LED 进行室内定位，并通过Taloy 迭代算法[16]进行求解获取空间坐标。

3.3 智能车导航及路线修正

在智能车运动方面，采用了以树莓派3B 搭建、机器人操作系统(Robot Operating System,ROS）为核心的阿克曼转向结构的小车[17]，同时配有接收可见光定位信息的光电二极管组件。

阿克曼结构的小车其运动包含两大核心部件: 转动结构，控制前轮转向；差速器，驱动后轮以及控制后轮的差速运动。阿克曼结构在直线行驶时，4 个车轮的轴线互相平行。转弯时，前轮转动的角度是不同的，要使阿克曼机器人实现纯滚动运动，必须保证机器人的4 个轮子运动方向的法线相交于一点，该点为机器人的转向中心点。

与通常的基于激光雷达的自建地图及导航（Simultaneous Localization and Mapping,SLAM）不同，因为本项目采用的是以可见光定位技术为基础的导航系统，所以在地图方面采用提前测量并画出实际场景的地图。由于小车为阿克曼结构并且自身有一定体积，为了避免导航规划路径的时候沿墙壁行进的小车撞到墙壁，预先将地图中的墙壁扩大一个小车的宽度（15 cm），如图5所示。然后通过开源计算机视觉库(Open Source Computer Vision Library,OpenCV)的地图处理模块，将该地图文件处理成可应用于导航算法的二维矩阵形式，如图6 所示。

图5 地图膨胀

图6 二维矩阵形式地图

由于适用场景不复杂的室内场景，因此在导航算法上采用了时间复杂度更小Astar 算法[18]。首先将处理过后的地图文件预置在智能车的存储中；当智能车工作时，接收需要到达的目的地坐标作为终点，通过可见光定位系统获得当前所在的位置坐标，并以此作为起点，再通过Astar 算法生成路线，智能车沿该路线行进到达终点。

在智能车运动的过程中，由于轮胎打滑和硬件反应速度等因素可能会带来误差，应对这类误差问题，采取以下方法：智能车在运动过程中通过可见光定位系统每5 s 获取一次当前的实际坐标位置信息并与开始时规划的理论坐标位置做对比，若实际坐标位置与理论坐标位置直线距离在12 cm 内，智能车便继续按照开始时规划的路线行进；若实际坐标位置与理论坐标位置直线距离超过12 cm，智能车则进入导航路径修正状态，以获取的当前实际位置为起点向下一目标节点进行路径修正，由于进行一次Astar 算法规划路线仅需要20 ms 左右，在系统中可以忽略不计。流程图如图7 所示，以此达到对导航路径的修正，提升导航精度。

图7 智能车导航及路径修正流程图

4 测量数据及分析

如图8 所示，实验采用4 盏LED，实验区域的范围为180 cm × 180 cm × 250 cm。图9 为VLP 空间模型俯视图，测试区域在xoy平面上，测试范围为150 cm × 150 cm，4 盏LED 灯的坐标分别为（-15，-15）、（-15，165）、（165，-15）、（165，165）。测试数据误差图如图10 所示，xy平面代表空间平面坐标，空间中每点代表实际坐标，z轴代表测量坐标与实际坐标直线距离差值并多次测量所取的平均值。由图10 可知，在测试区域中心处误差最小，为4.2 cm，从中心向四周方向误差呈现递增趋势，其中从中心向4 个正方形对角方向误差变化率最大，但都在12 cm 以内。

图8 VLP 空间模型

图9 VLP 空间模型俯视图

图10 实际坐标与测量坐标误差图

误差的来源可能来自以下三部分：调制LED 电路的输出电压和功率不稳定，造成 LED 产生的方波频率不稳定；从LED 到接收端电路的传输路径可能掺入环境的其他光源；接收端电路本身也存在噪声，不能完全地恢复出稳定的方波信号。同时由于接收角度问题以及模型误差也会对单片机采集的电压信号造成影响。以上几种情况造成FFT 之后的幅值与理论值有差异。

如图11 所示，智能车在可见光定位区域进行测试，其中虚线为Astar 算法规划路径，实线为智能车实际行进路径。测试中，规划起终点为（12.0，3.0）（3.0，10.0），实际起终点为（12.0，3.0）（2.8，9.7），智能车行进中共触发了两次路径修正，在（6.2，8.8）点，理想坐标为（7.0，8.0），两点间直线距离为12.45 cm，第一次触发路径修正；在（2.0，3.4）点，理想坐标为（3，4），两点间直线距离为12.83 cm，第二次触发路径修正，最终达到终点。该方案使导航误差控制在15 cm 以下，实现较高精度的室内导航。

图11 智能车实际路线与规划路线对比图（1 单位=11 cm）

5 结论

本文通过对LED 灯具的改造，使之满足照明和定位的双功能。文中提出了一种利用LED 可见光定位技术为智能车导航的方案，并且在实验中验证了它的可行性，实现了平面坐标定位、智能车导航及路线修正。本方案在室内导航方面采用基础的Astar 导航算法，后续可采用更先进的室内导航算法，以达到更高精度的室内导航；同时，在室内定位方面，可以对定位数据进一步拟合处理以获取更高精度的坐标信息。本方案不需要价钱昂贵的激光雷达，还具备低功耗和抗电磁干扰的优点。