基于激光雷达自导航的AGV 系统设计

2021-05-18

南方农机 2021年9期

（重庆工业赋能创新中心有限公司，重庆 400023）

0 前言

当下社会自动化、智能化在各行业各领域都有很大的发展，自动导航车的应用范围也越来越广泛，现代化自动化的工厂车间，自动导航的AGV（Automated Guided Vehicle）小车随处可见；家庭中常见的扫地机器人也越来越多，但大多是传统导航方法。

激光雷达可以实现非接触式的空间感知，可以引导自动导航车进行路径规划和避障，针对农业温室环境之下的导航车，可根据激光雷达实现温室复杂环境内的运动路径规划。在激光雷达差动模型、里程计模型、激光传感器模型建模的研究为机器人自定位以及自动导航进行理论铺垫[1]，激光雷达技术已经在农用车辆、桥区船舶导航、道路车辆等领域已经开始有初步研究应用[2-11]，用激光雷达来做导航的AGV 偏少，因此，本文对基于激光雷达的AGV 进行路径规划与自主导航研究。

1 硬件系统设计

图1 硬件连接图

基于激光雷达的AGV 平台车主要由平台车体、激光雷达传感模块、点云数据采集、处理及平台车运动控制器组成。其中平台车是实现路径规划功能的承载主体，激光雷达传感器则负责采集外部环境数据以及自身状况并将其提供给树莓派控制器，控制器对数据进行处理与分析，计算获取平台车的运动路径，并控制车轮电机驱动器实现平台车自主导航[12]。硬件连接如图1 所示。

激光雷达传感器识别周围环境障碍，通过UART 通信协议将数据传送给树莓派，电机驱动器进行数据处理并经由某种算法规划完路径，然后树莓派通过GPIO 控制向电机驱动器发送命令，使其产生PWM 信号给小车的电机，最终能够实现小车在某些路况中的自动导航。采用的AGV 小车为应用于大棚、农场等结构化的场地作业平台车，该小车主要由两个伺服电机，以及分别驱动它们的两个伺服电机驱动器构成（DMS-055A 全数字交流伺服电机驱动器）。在结构化的场地中工作，就要求该作业平台车能够自动导航行驶于直线路段、简单转弯路段以及贴壁行驶路段。为了达到这些要求，作业平台车采用了两轮中置驱动的驱动模式，并且能够实现两轮的正反转向，同时利用差速原理实现直线行驶与转弯[13]。为了提高平台车的灵活性，在底盘下安装了四个万向轮，使得它们配合电机控制下的两个驱动轮实现原地转向的功能。而驱动电机采用了48V400W 的伺服直流电机，可以准确按照占空比的要求输出相应的转速。

1.1 RPLidar A2

RPLidar A2 是思岚科技开发的新一代低成本二维激光雷达，RPLidar A2 的参数如表1 所示。

表1 RPLidar A2 参数

RPLidar A2 采用的是激光三角测距技术，加上思岚科技开发的高速视觉采集处理机构，它可以以4 kHz 的高频率进行测距操作。在每次激光测距操作中，RPLidar A2先发射出经过调制的红外线，在碰到障碍物之后返回，并通过高速视觉采集处理机构再次接受，经过内置的DSP处理器进行实施解算，最后得到角度与距离数据并通过串口输出。

图2 RPLidar A2 的引脚定义与接线图

RPLidar A2 激光雷达与外接控制的接线方式如图2所示。其中UART 表示控制器上的通用异步收发传输器，在本文中指树莓派控制器。PWM(Pulse Width Modulation)Generator 指的是GPIO 引脚下的PWM 控制功能。

如图3 所示，激光雷达的发射端和接收端位于激光雷达的顶部，其顶部相对底部可进行旋转运动，从而采集360°一周的点云数据，激光雷达的正面为数据线的对应面，正面所对应的激光雷达转角为0°与360°，激光雷达按逆时针方向旋转，采集到的角度分别从0°到360°，采集到的距离为坐标轴原点到障碍物的直线距离d，通过极坐标的方式输出障碍物的位置参数。

图3 激光雷达坐标系

1.2 点云数据采集控制器

Raspberry Pi 3 Model B 的计算机体积小装载轻便，对AGV 小车的行驶影响较小；参数配置足够，能够承载激光雷达输出的数据并进行储存与处理。Raspberry Pi 3 Model B 参数表如表2 所示，其功能完全可以实现数据的采集存储以及处理。

表2 Raspberry Pi 3 Model B 参数表

树莓派实物如图4 所示。

图4 Raspberry Pi 3 Model B 实物图

1.3 电机驱动器模块

驱动伺服电机的驱动器采用的是DMS-055A 全数字交流伺服电机驱动器，该驱动器采用32 位高速DSP 芯片，支持速度闭环控制，速度控制模式下支持PWM 占空比信号，供电电压20V～50V，并且支持50W～500W 的交流伺服电机，符合上述采用的48V400W 的伺服电机。在本研究中所使用的激光雷达是思岚科技出品的RPLidar A2激光雷达，隶属于单线激光雷达。这款雷达是国内厂商思岚科技开发的一款低成本二维激光雷达，最主要的优点就是价格低廉、开发方便、精度较高。

1.4 基于激光雷达的AGV 导航系统框架连接

激光雷达通过USB 适配器将其通信接口与上位机Raspberry Pi 3 Model B 连接并进行数据通信；树莓派通过外接电源供电；GP IO 引脚与AGV 小车的两个伺服电机驱动器连接，下位机通过上位机处理完的数据信息分配PWM 波，设置好占空比后输出给伺服电机中的PWM 驱动器，实现对平台车运动控制。图5 为基于激光雷达的AGV 导航系统的硬件连接模拟图。

图5 硬件连接模拟图

2 基于RPLidar A2 的数据采集方法

为实时获取激光雷达采集到的点云数据并且进行实时处理，需编写实时数据采集程序，同时为了验证采集数据的准确性将数据保存为txt 文件。由于RPLidar A2 的头文件基于C++与C 语言的混编，故编程逻辑为先采用C++语言进行编程，调试成功后导入树莓派中，在Linux 系统python 2.7 的环境下进行编译和运行。

具体的数据采集编程思路为：先基于VS2010 的C++环境进行数据获取程序的编写，采用激光雷达所提供的SDK 包调用相关的已封装好的头文件和指令函数，实现指令通信和激光雷达作业控制，在获取了正确编译的C++源文件后，利用Xshell 软件将其导入进树莓派中进行基于python2 的编译，最后生成可执行文件，运行文件，树莓派便可控制激光雷达的工作以及数据的采集。本章在这些内容的基础上还添加了进行正式程序编写工作前的准备工作，包括PC 端的配置，树莓派的初始化设置以及二者的连接配置。如6 图所示。

图6 数据采集编程思路

2.1 基于VS2010 环境下的数据采集

图7 数据采集程序流程图

程序的编写在PC 端的VS2010 环境下完成，RPLidar A2 提供了能够自由开发的SDK（Software Development Kit，软件开发工具包），基于SDK 目录下的已存在的头文件进行数据采集程序的编写。头文件主要包括：rplidar.h（在程序编写时只需引用该头文件即可使用RPLidar SDK 的所有功能）、rplidar_driver.h（定义了核心驱动接口，RPLidarDriver 的类申明，包括了一些函数的接口）、rplidar_protocol.h（定义通讯协议的底层数据结构以及常量）、rplidar.h（定义通讯协议中各类请求命令、应答相关的数据结构以及常量定义）、rptypes.h（定义平台无关的结构和常量）。

图7 为RPLidar 数据采集流程图。程序运行时给激光雷达通电，通过GET_HEALTH 函数获取激光雷达的健康状况，检测是否发生了故障，没有发生故障则创建驱动实例（无论发生故障与否，都需要输出设备健康信息），接着检测硬件信息并输出，最后检测工作环境是否安全，若不安全则会中断程序[14]。在保障激光雷达健康工作后，设备正常运转并开始扫描同时获取数据。扫描过程处在一个while(1)的死循环中，只有当外部给出中断命令时，数据采集程序才会结束。

2.2 Raspberry Pi 在Linux 环境下的编译

由于Raspberry Pi 使用的是Linux 系统，在VS 中用C++语言编写的程序无法直接运行，因此，需要进行Linux中的编译。进行Linux 下的编译就需要将源文件导入进Raspberry Pi 中，需要使用Xshell 软件，该软件可以实现远程控制Raspberry Pi 的系统的功能。将树莓派与PC 连接，从而实现用Xshell 远程控制Linux 系统。

首先对树莓派进行初始化设置，本文选用Linux 发行的centOS 系统。使用W32DiskImager 将下载好的镜像系统文件烧写进SD 卡中，并与PC 连接使用SSH 远程登录进行文件的配置，且将点云数据采集程序导入树莓派。

3 实况模拟下的AGV 导航路径拟合方法研究

3.1 路径拟合算法

路径拟合的算法可以在图8 中体现，在障碍物两边分别取若干采集到的数据点，图中在障碍左和障碍右中分别取3 个点，两两相连求出其3 个中点坐标，根据最小二乘法[15]将这些中点坐标进行拟合，最后可以得到一条较为准确的方向导航线。

图8 路径拟合原理图

通过本文第2 章中的数据采集程序所获取到的数据实际上是极坐标下的数据（A，D），一个角度对应一个距离，在进行最小二乘法拟合之前，需要用公式（1）将其转化成直角坐标系的数据（X,Y）。通过公式（2）计算左障碍Ai与Bi中点Ci的坐标（其中i＝1,2,3），再通过公式（3）获得直线的斜率a 与截距值b。

3.2 特征点的提取

首先对实验场所进行数据采集，图9 为实验场所的环境实物图。获取数据后在MATLAB 软件下进行绘图，未进行数据过滤时的环境模拟图如图10 所示。

图9 环境实物图

图10 未过滤数据环境模拟图

观察图9、图10 可以发现，障碍物C、D 以及直角弯道与当前测量点距离过大，对当前点进行路径规划没有帮助，因此进行数据过滤，将距离值大于1 000mm 的数据点过滤。数据过滤环境模拟图如图11 所示。

图11 数据过滤后环境模拟图

如图11 所示，大致可以判断从30°～330°范围内的数据对于规划当前的路径是有效的，于是在数组中隔断选取6 个点，模拟中选择了数组中第30 个、50 个、70 个（这三者分布在路径的一边），第260 个、280 个、300 个（这三者分布在路径的另一边），这是因为数据在数组中的位置与其角度值相近似。

3.2.1 无偏差直线路径工况下的路径导航

在理想的直线路径工况中，保证AGV 可沿路径中线运动，采用最小二乘法直线路径拟合方法，图12 为试验路况图，可见该路况两侧障碍物平行，距离为200 mm，且障碍物均为平板状物，没有凸起点对导航路径拟合的影响。

将激光雷达采集到的数据导出，再导入Excel 中生成表格，对距离这一参数进行阈值滤波，将1 000mm 以上较远的无关数据点过滤，完成数据的导入之后进行MATLAB 的程序编写。

根据最小二乘法拟合方法拟合出来的方向导航线如图13 所示，6 个点被拟合成了一条几乎指向0°的直线，对于激光雷达来说也就是正前方。

3.2.2 存在导航偏距的直线路径工况下的路径导航

假设平台车仍然行驶在一条直线路段上，但是并不是沿着中线的理想路线上行走，而是产生了横向的导航偏距，与上述情况一样，将激光雷达采集到的数据导入进Excel 中，进行距离小于1 000mm 的滤波后导入MATLAB中，再次定义第一列为angle 变量，第二列为distance 变量。仍然选择30/50/70/260/280/300 这6 个点，进行最小二乘法直线拟合。

图13 无偏差时的模拟路径

通过最小二乘法拟合后得到的方向导航线如图14 所示，平台车处于该极坐标的中央位置，相对于上述情况来说，平台车向中线的左边产生了距离上的横向偏移量，而为了让平台车能够保持在中线上行驶，此刻通过最小二乘法拟合得到的路径指示平台车向右前方行驶，直至其可以正确行驶在中线上。

图14 有导航偏距时的模拟路径

3.2.3 存在航偏角的直线路径工况下的路径导航

图15 航偏角时的模拟路径

假设平台车行驶在直线路段上，虽然此刻仍处于直线路段的中线上，但是角度上产生了偏移，于是规划该路况下的路径。同样将激光雷达采集到的数据导入至Excel中，进行距离小于1 000mm 的滤波后导入MATLAB 中，再次定义angle 与distance 变量，通过选取关键点的方法绘制路径模拟图。

这种情况下通过最小二乘法拟合得到的方向导航线如图15 所示，在粗线所描出的环境地图上可以看出，地图相对于平台车向左产生了偏转，也就是说平台车相对于地图向右产生了偏转，于是需要进行符合细线行驶方向的操作才能回转到正确的正前方向[16]。

3.2.4 同时具有导航偏距和航偏角的直线路径工况下的路径导航

假设平台车行驶在直线路段，但是此刻既出现了导航偏距也出现了航偏角，于是对这种情况进行路径的模拟规划。同样将激光雷达采集到的数据导入到Excel 中，经过距离小于1 000mm 的滤波后导入进MATLAB 中，再次定义angle 与distance 变量，通过选取关键点的方法拟合路径。

在这种情况下通过最小二乘法拟合获得的方向导航线如图16 所示，可以明显看出，极坐标中心（也就是平台车的当前位置）与粗实线描出的地图两边的墙壁的距离不一致，同时整个地图也相对于平台车向左偏转了一定的角度。为了使平台车能够沿着中心路线行驶，需要根据图中细实线（也就是这种情况下的规划路径）所示的方向进行调整。

图16 导航偏距与航偏角都存在时的模拟路径

3.3 导航偏距值与航偏角的获取算法

以图16 为例，给出获取偏差值的算法。

3.3.1 导航偏距值的获取

取0°与270°两个点的数据，设距离分别为D(0)与D(270)，同时定义0°～180°内距离为正值，180°～360°范围内距离为负值，D(0)+D(270)就是导航偏距值。当D(0)+D(270)等于0 时，就表示不存在导航偏距值，也就是平台车处于当前路段的中心线上。

3.3.2 航偏角的获取

航偏角的获取更加便利，利用路径拟合算法中取得同侧障碍中的3 个点，同样是通过最小二乘法进行路径拟合，得到该障碍路线的斜率，对斜率求反正切即可得到当前的航偏角。同样，当斜率为0 时则表示不存在航偏角，当前平台车行驶的方向是沿着中心线方向的。

4 车辆自主导航算法研究

最小二乘拟合直线法是一种有效的寻找路径的方法，它只需要一些关键点就可以在不同的路况下拟合出满足AGV 小车行驶的路线；并且能够通过计算行驶路线的直线方程式得到此时角度的偏转量以及距离的偏移量，这对使用PID 算法实现车辆的自主导航将会起到非常大的作用。

4.1 车辆运动控制

用树莓派的GPIO 引脚与电机驱动器连接，通过PWM（脉冲宽度调试）进行驱动。用python2.7 编写一个程序通过接受拟合路径之后所得到的导航偏距以及航偏角来进行脉冲宽度调试，并输出给电机驱动器让其可以控制电机产生转速，实现平台车的前进运动，或是使得电机产生差速，实现平台车的转向运动。

总的来说，利用GPIO 引脚与电机驱动器连接，通过脉冲宽度调制，可以实现平台车的运动控制。

为了使用树莓派上的GPIO 引脚控制DMS-055A 电机驱动器，需要与DMS-055A 电机驱动器的Signal 模块进行连接，图17 所示即为DMS-055A 电机驱动器的Signal 模块功能区。

图17 DMS-055A 的信号模块

GPIO 与DMS-055A 接线如图18 所示，EN-、DR-、PU-都是接地，因此合并成一条线接在GPIO 引脚的GND上。连接完成后，进行引脚的初始化设置。

图18 GPIO 与DMS-055A 接线图

4.2 基于PID 路径导航控制方法与实现

4.2.1 变量初始化

PID 算法是比例、积分、微分的总称，以导航偏距值以及航偏角为输入量，电机的转速为输出量进行PID 控制，从而纠正平台车的行驶路径[17-19]。首先列出实现PID 控制程序所需要的初始化变量。

1）导航偏距值变量。通过获取导航偏距的算法可以得到这个偏差值distance_error。按PID 算法的思想，取其中的PD（分别是比例与微分）对导航偏距进行控制，纠正路径回归正确中线。这是因为如果只使用比例环节，平台车将会来回以Z 字形行驶，并不能达到沿中心线行驶的要求，而同时采取了微分环节后，将会使Z 字的幅度越来越小，直至趋于0，平台车得以沿中心线稳定行驶。使用P（比例）环节时，需要获取到导航偏距值，在上述过程中已经获得，即为distance_error 这个值；而在使用D（微分）环节时，则需要获取到“导航偏距值的微分”，也就是这一次的偏差值与上一次的偏差值的差值。在python 程序中实现时，将取得的两个点的距离值进行初始化定义，分别为direction_left 和direction_right，并且将distance_error＝direction_centre 同时将这个偏差值置为0。

2）航偏角变量。对航偏角的控制同样选取PD 两个环节，在P（比例）环节中，需要获取到航偏角；在D（微分）环节中，则需要获取到航偏角的微分。同样在python 程序中实现时，将取得的两个点的距离值进行初始化定义，分别为theta_initial 和theta，并且将theta_error＝theta_initial，同时将这个偏差值置为0。

4.2.2 结构路况下直线行驶控制

在这种路况下行驶，可能会出现导航偏距，可能会出现航偏角，更多的情况下是两者同时出现。这里的处理方法是分别计算出导航偏距以及航偏角的控制量，进行简单的权重耦合[15]，相当于两个控制量并联，各取50%的权重进行叠加。最后的到一个总的控制量输出给PWM 进行调制。下面的代码用于计算导航偏距值的控制量。

下面的代码用于计算航偏角的控制量。

获取两个控制量后，用权重耦合，最后的控制量为：

speed_control＝(speed_control_direction +speed_control_theta)/2

将这个控制量通过GPIO 引脚输出给电机驱动器对PWM 进行调制，从而纠正平台车的路线。

4.2.3 结构路况下转弯控制

此路况下与直线路况大致相同，但是要多一个数据的处理。在初始化的代码中添加一个变量“direction_behind”，用来表示平台车此刻与前方障碍物的距离。并为其设置一个阈值a，当距离小于这个值的时候进行控制量的调节并输出给电机驱动器。而这个偏差值控制量是施加在左轮的电机驱动器还是右轮的电机驱动器则取决于左右两端的距离大小，左端距离大则施加在右轮的电机驱动器上使PWM 占空比提高，右轮加速运转从而产生差速向左边转弯。这种情况下的控制调节一直持续到direction_behind 升到阈值a 之上。显然，在控制调节期间，direction_behind 在时刻变大，但平台车也是在时刻前进的，前进之后direction_behind 又再次减小降到阈值之下，又一次进行控制调节，在转弯完全进行完之前，控制调节的行为是以非常小的周期在断续进行的。

4.2.4 贴壁行驶路况下的控制

这种路况下，比沿中心线行驶更加容易实现，程序的运行只需要一个变量，加快了运算速度，提高了行驶的稳定性。仍然采用PD 两个环节来控制。主要控制左端距离即可，为其设置一个阈值b，当direction_left 不等于b 时则开始控制调节工作。此时的导航偏距控制量计算方法发生了变化，主要是distance_error 这个偏差值的定义将会改变。当偏差值小于0 时，将控制量加在左轮的电机驱动器上，当偏差值大于0 时，将控制量加在右轮的电机驱动器上。