基于RTK 的增量PID 差速小车循迹实现

朱海波，李毅

（四川大学计算机学院，成都610065）

0 引言

自主循迹是无人驾驶的基础，也是智能车辆研究的核心技术之一。定位则是第一个要解决的问题。在无人车驾驶系统中普遍采用GNSS 技术，通过一组卫星（至少四颗卫星）的伪距、星历及用户钟差等测量值，为地球表面或近地空间的地点提供三维坐标和速度以及时间信息。但常规的GNSS 由于存在误差，且无法消除，所以导致精度有限，一般最好在米级。利用RTK技术可以最大程度的减小电离层延迟和对流层延迟带来的误差，可以使精度到达厘米级[1]。但RTK 也存在距离的限制，由于基站校正数据的有效作用范围有限，当与基准站距离过大时，定位精度会下降，所以移动站与基准站的距离在单频下不能超过10 千米，双频不能超过30 千米。

实现车辆自主循迹的核心内容包括如下三个：RTK-BDS 信息采集与存储；将采集的RTK-BDS 信息形成目标轨迹；参照目标轨迹与当前定位信息，依据增量式PID 控制算法，将实测横向轨迹与预期轨迹的差作为PID 偏差度量，生成控制命令，实现智能车辆的自主行驶。

1 算法原理

1.1 RTK基本原理

RTK 系统硬件上主要由三部分组成，即基准站、移动站和两者的通信链路。RTK 定位原理[2]是基准站上的BDS 天线和流动站上的BDS 天线同时接收一组卫星信号，两个BDS 天线将接收到的卫星信号再分别传递给基准站BDS 接收机和流动站上的BDS 接收机。由于基准站被安置在某一固定位置，因此基准站的坐标作为固定值已知。根据基准站的坐标和已知卫星的距离，可以反算出卫星的坐标，通过比较解算出差分改正数。同时基准站将解算出的差分改正数通过基准站和移动站的数据链路发送给移动站。移动站通过差分改正数和其接收的GPS 数据解算出精确的用户坐标。之所以选用中国北斗导航系统（BDS）而不选用全球定位系统（GPS）是由于经过实地测量，测试地所处环境下，BDS 可见星数要大于GPS，精度因子（DOP）较小，定位误差更小。BDS-RTK 由于定位接收器在被建筑遮挡时，导致可见卫星数目大大减小，极易从非固定解状态，变成浮动解，使得定位出现偏差，所以无法在高大建筑物遮挡的环境下进行。

载波相位差分技术中，基准站接收机的相位为：

公式中c 表示光速，δti为接收机钟差，δtj为第j颗卫星钟差，为电离层误差，为大气层误差，δMi为多路径效应，Vi为GPS 接收机噪声。在基准站可求出伪距改正数：

用此改正数对用户站伪距观测值进行校正，有：

当基准站与用户站的间距小于30km，可认为

式中Δδρ=c∙( δtk-δti)+(δMk-δMi)+(Vk-Vi)。

将载波相位伪距观测值（2）代入上式，则可得：

上式中令Nj( t0)=( t0)-( t0)为起始整周数之差，在观测过程中若卫星跟踪不失锁，Nj( t0)即为常数，设载波相位测量差值：

式（7）可以表示为：

公式中的λNj( t0)、Xk、Yk、Zk，Δφ 均为常数，仅Δδρ 未知。但两接收机钟差之差、噪声之差及两站间多路径之差在相邻历元间的变换量均小于厘米级动态定位允许的误差，所以可将Δδρ 视作常数。

1.2 增量PID基本原理

比例积分微分控制，通过被控对象的输出来影响PID 控制器的输出，形成闭环，是一种经典的负反馈控制方法，其特点为算法较为简单、鲁棒性好并且可靠性较高[3]。

比例控制KP能加快系统的动态响应速度，快速地对误差进行反应，从而减少误差，但无法消除稳态误差。KP越大，控制器对于偏差的反应越快，但过大的增益会降低系统相对的稳定性；越小则反应越慢，会花费较多时间进入到稳定状态。一般而言需要使得KP尽可能的大。

积分控制KI，能够消除无法通过KP和KD解决的稳态误差（即系统进入了一个存在误差的稳态，但并非是预期的稳态）。积分作用将会不断积累误差，输出控制量来消除误差，直至偏差为零时积分停止，但也存在一个问题，其控制作用较慢。

微分控制KD，与变化率有关，可以减小超调量来克服震荡，使系统稳定性提高，同时加快响应速度，使系统更快有更好的动态性能。

增量式PID 控制，数字PID 控制算法的一种基本形式，是通过对控制量的增量（本次控制量和上次控制量的差值）进行PID 控制的一种控制算法，是递推式算法。表示第n 时刻的控制量，表示控制量的增量，e[n]表示n 时刻的误差。

公式：

通过位置式PID 公式推出[4]：：

1.3 循迹策略

通过RTK-BDS 获取对应的NEMA-0183 格式的定位数据，将经度与纬度通过高斯投影映射成高斯坐标系下的三维坐标。首先录入预想轨迹点，通过B 样条拟合出车辆行驶轨迹。通过计算车辆当前位置距轨迹的距离，作为PID 纠正偏差。通过调节PID 参数使得该偏差尽可能的减小，使车辆实现自主的循迹。

从轨迹起点P0起，对轨迹以K 米的距离取点（P0,…,Pn）。将问题让车辆拟合一段长且复杂的曲线变成拟合一段短且简单的曲线。设无人车所在位置为Q。

（1）启动时，先判断车辆在哪一段较短轨迹中。首先计算车辆当前位置Q 与P0～Pn中最靠近的点Pi，选定PiPi+1作为当前待拟合的曲线。

（2）过点Q 作PiPi+1连线的垂线，交Pi-1Pi图形点R，QR 的距离即为PID 需要消除的偏差。通过PID 获取车辆的输入量，将输入量转换成无人车控制命令，发送到小车。

（3）当Q 经过点Pi+1所在位置切线的垂线时，表示车辆已经来到下一条曲线Pi+1Pi+2。

（4）若未到达终点，则重复步骤（2-4）；若到达终点，则停止。

2 实验设计

2.1 实验准备

（1）实验小车

实验小车为四轮小车，长为595 毫米，宽为631 毫米，高为217 毫米，前轮作为驱动轮，后轮为万向轮，作为从动轮。两个主动轮连接PWM 直流电机。通过增减PWM 电机占空比实现车速控制；通过小车主动轮左右轮差速来进行转弯控制。通过串口与PC 进行通信，波特率115200bps，数据位为8，停止位1，无校验。控制命令格式如表1。

表1 车辆串口控制命令格式

第0 位控制A 电机，也就是右轮电机速度大小，第1 位控制B 电机，也就是左轮电机速度大小，第0 位与第1 位范围为0～0xFF。第2 位与第3 位分别表示右轮电机转向与左轮电机转向，范围为0～1，0 表示正向，1 表示负向。

（2）基准站与移动站

将NEO-M8T 作为基准站和基准站的定位信息接受模块，通过局域网建立基准站与移动站的数据链路，解算之后，以数据格式NEMA0183 通过Socket 通信传到PC。

2.2 实验流程

首先规划轨迹，将规划好的轨迹录入小车PC 中。将小车置于起点处，小车上的PC 接受RTK-RDB 传送的定位信息，根据循迹策略进行循迹。

实验设计为两组，第一组为循迹直线，将小车移动至偏离预计轨迹2m 位置处，第二组实验为循迹弯道，两组实验观察实际轨迹与预期轨迹的差距。

2.3 实验结果

（1）循迹直线

直线循迹结果图见图1。

图1 循迹直线定位结果图

（2）循迹弯道

弯道循迹结果图见图2。

图2 循迹弯道定位结果图（起点为右上角）

2.4 结果分析

在实际环境中，通过试凑法调整PID 参数，获取了效果较好的参数，实现了循迹直线与弯道。在循迹弯道的实验中，在转弯过程中存在一定的偏差，分析得KP和KD参数仍然过小，导致无法及时消除误差。其中原因也与PID 参数没有有效的调整方法有关，无法得到最优的PID 参数。

3 结语

本文首先介绍了RTK 定位与增量式PID 控制的原理，经过实验，以RTK-RDB 获取厘米级精度的定位信息，通过增量式PID 控制实现了车辆的循迹。实验表明该策略是可行的，但实验过程中，无法高效地获取合适的PID 参数。在下一步研究中，将针对PID 参数调整方法进行研究，针对高效的PID 调节方法进行研究。