轮式挖掘机自动循迹行驶控制系统研究

马厚雪，王 龙

（江苏徐工工程机械研究院有限公司，江苏 徐州 221004）

轮式工程机械机动性高，与履带式挖掘机相比，更加适合于快速移动。在某些环境异常恶劣（垃圾清理、地震灾害抢险救灾、防爆作业、放射性场合）作业人员操作困难，因此如何实现工程机械的无人化控制，实现轮式车辆自动循迹行驶已经成为现代机械研究的一个重点方向。

1 硬件系统设计

本文以轮式挖掘机为载体，采用多种传感器融合的方案，实现多重特定的功能。这些设备包括：安装在挖掘机的驾驶室顶部的激光雷达，安装在挖掘机前后左右的米波雷达，安装在后配重的GPS 定位定向天线、惯性导航接收机等。系统还配置有工控机和可编程控制器（图1）。

图1 硬件系统

GPS 定位定向天线与惯性导航接收机组成定位系统，用于确定挖掘机的精确位置坐标。激光和毫米波雷达用于避障。工控机用于处理传感器信号、规划路径，发送控制指令到可编程控制器。可编程控制器用于接受上层工控机发出的油门、制动、转向等控制信号，并且向底层转向控制器以及驱动系统控制器发送控制指令以实现上层的控制指令。其中惯导、雷达、工控机等均连接到交换机上，通过以太网进行通信，可编程控制器与工控机、底层控制器等通过CAN 线通信。

2 软件系统设计

自动循迹软件系统包括定位数据采集模块、曲线拟合模块和轨迹跟踪控制模块组成（图2）。这3 个模块均部署在工控机上。

图2 自动循迹软件系统

1）定位数据采集模块 主要完成挖掘机的轨迹路径点的坐标信息以及挖掘机自身位置坐标信息的采集，并对这些信息进行坐标转换，输出转换后的路径数据以及定位数据。

2）曲线拟合模块 主要接收定位数据采集模块中转换后的路径数据，对这些离散的路径数据进行曲线拟合，输出挖掘机的行驶轨迹曲线，即目标轨迹。

3）轨迹跟踪控制模块 又包括横向控制模块和纵向控制模块，其主要接收定位数据采集模块输出的实时定位数据，曲线拟合模块输出的行驶轨迹曲线，以及可编程控制器输出的挖掘机位姿数据；通过横纵向控制模块的计算；输出油门踏板深度、制动踏板深度以及转向角等控制信息给可编程控制器。

2.1 定位数据采集模块

由于通过GPS 天线和惯导读到的GPS 数据是地理坐标系（WGS84），需要进行解析与坐标变换，转换成X轴指向正东，Y轴指向正北，原点位于后轴中心的直角坐标系数据。使用高斯正算公式进行坐标变换与解析，将大地坐标经度(L)和纬度(B)坐标通过高斯投影得到高斯平面直角坐标(x,y)，然后再将平面直角坐标，转换成当地的水平坐标，其转换公式如式(1)所示

可将式(1)展开为l的幂级数，如式(2)所示

其中，X，Y是坐标值，其中Y轴向东为正，X轴向北为正，l是无限小的数，m，n是待定系数，是等量维度q的函数。

经过上述过程得到的坐标系转化为当地坐标系，其中：正东向为y轴正方向，正北向为x轴正方向，与无人驾驶通用的坐标系定义不一致，使用旋转矩阵变换，如式(3)所示

可得到无人驾驶通用坐标系，其中，X2、Y2是坐标值，其中X2轴向东为正，Y2轴向北为正。

2.2 曲线拟合模块

由于GPS 数据点是离散的，可以通过最小二乘法进行分段连续拟合，得到挖机的平滑运行轨迹。最小二乘法通常作为一元线性回归模型,用残差平方和达到最小，作为拟合方法构建损失函数。从GPS 采集的全体数据点中，分段提取n组（X1，Y1），（X2，Y2），…，（Xn，Yn）。对这n组数据，采用最小化误差的平方和寻找数据的最佳函数匹配。拟合后获得挖掘机循迹行驶连续轨迹。作为循迹行驶的目标值发送的轨迹控制模块。

2.3 轨迹跟踪控制模块

横向控制模块，主要基于当前车辆坐标、航向角和行驶轨迹曲线，采用预瞄PID 控制算法，计算转向角。纵向控制模块，主要采用PID 算法，将期望行驶速度与实际车速进行对比，将速度误差作为PID 的输入，计算油门踏板深度和制动踏板深度。

2.3.1 横向控制模块

图3 为横向控制模块模型。横向控制采用的是预瞄PID 控制算法，首先通过定位数据采集模块获得当前车辆坐标和航向角，然后结合曲线拟合模块中获取的轨迹曲线，进行最近路径点的搜索，求出离当前位置最近的路径点，根据得到的路径点和下两个路径点，可以求出车辆横向运动的曲率，并通过式(4)求得转向前馈角度。

式中：a为挖掘机质心到前轴的长度；b为挖掘机质心到后轴的长度。

之后根据图3 中的横向控制模型，确定预瞄距离L后得到预瞄点，根据式(5)可以获得预瞄点的横向偏差e1a。

式中：e为车辆质心到轨迹曲线的横向偏差，为航向角偏差。

图3 横向控制模块模型

以预瞄点的横向偏差e1a作为PID 输入，PID的输出为转向反馈角度q2。则通过式（6）可以求出最终的转向角q

式中，k1为前馈角度权重，k2为反馈角度权重。

2.3.2 纵向控制模块

纵向控制模块（图4），主要通过速度闭环，控制挖掘机按照给定的速度行驶。其控制加速控制器实现加速和降速；控制制动控制器实现减速制动；控制切换器实现加速和制动的切换。根据挖掘机行驶速度传感器或者惯性导航系统反馈的实际车速，进行行驶度闭环。闭环控制算法采用PID 算法。纵向控制模块将期望行驶速度与实际车速进行对比，将速度误差作为PID 的输入，输出为油门/制动开度的控制信息。

图4 纵向控制模块

3 系统测试

在某型号轮式挖掘机上进行测试。安装响应的硬件设备。设计循迹行走轨迹分别按直线和U 型曲线行驶。轮式挖掘机由起点处，分别按加载的路径轨迹进行自动循迹行驶，循迹过程中过弯时自动减速。对循迹行驶过程的横向误差进行统计，误差与照轨迹点一一对应，正向循迹行走速度为8km/h。经测试直线循迹行驶横向误差<0.2m，U 型曲线循迹行驶横向误差<0.7m，具体效果如图5～图8 所示。

图5 直线循迹行驶轨迹

图6 直线循迹行驶横向位置误差

图7 U型循迹行驶轨迹

图8 U型循迹横向位置误差

4 结语

本文构建的轮式挖掘机自动循迹行驶控制系统，可以实现对给定的路径轨迹的自动循迹行驶。在自动循迹行驶过程中，直线循迹效果较好，曲线循迹在拐弯处误差较大。考虑车辆非线性、不确定性和时变特性地对车辆的影响，在以后的研究中，采用智能控制和其他控制相结合的方法，对横向控制器预瞄算法进行优化，进而减小横向控制偏差。