CN115525054A - 大型工业园区无人清扫车沿边路径跟踪控制方法及系统 - Google Patents

Abstract

本发明公开了一种大型工业园区无人清扫车沿边路径跟踪控制方法及系统。获取无人清扫车在工业园区内沿边行驶轨迹作为后期无人清扫车的目标路径；依据纯追踪算法计算出当前无人清扫车前轮转角；激光雷达高频拾取无人清扫车与路沿距离并将该距离作为无人清扫车是否采用MPC算法对无人清扫车进行转角补偿的参考；利用MPC算法及时对无人清扫车进行转角补偿，根据无人清扫车稳定性状态及时调整控制量；通过udp协议将纯追踪算法和MPC路径跟踪控制器得到的期望前轮转角控制量输出到车辆底盘，完成无人清扫车的路径跟踪任务。本发明能够确保无人清扫车沿目标路径精确行驶、高效安全工作，最大程度上降低清扫车碰撞路沿的概率。

Description

大型工业园区无人清扫车沿边路径跟踪控制方法及系统

技术领域

本领域涉及运动及动力学研究技术领域，尤其是涉及一种大型工业园区无人清扫车的沿边路径跟踪控制方法。

背景技术

随着传感器技术和深度学习的发展，自动驾驶相关研究取得了重大进步。路径跟踪是自动驾驶车辆需要实现的基本功能之一。其基本目的是通过控制车辆的横向和纵向运动来跟随目标路径轨迹；基本方法是是根据前轮转角、驱动力、制动力等控制量的输入，使无人车能够依照目标路径行驶；基本要求是使无人车有效的跟踪目标路径并保证车辆稳定性，目前应用较多的控制算法有PID控制算法、LQR算法和模型预测控制算法(即MPC算法)等。

不同于一般的路径跟踪任务，工业园区中清扫车沿边清扫具有鲜明特点。清扫车的车速不需要很快，而且沿边路径的结构易导致清扫车碰撞路沿，加之自身清扫时的抖动会影响路径跟踪安全，因此可以采用纯追踪算法进行路径跟踪，当激光雷达检测到无人清扫车路径偏离超过一定阈值，就启动MPC算法控制器，以车辆二自由度模型为预测模型，及时对无人清扫车的前轮转角进行补偿，使其能够按照目标路径执行清扫任务。

发明内容

本发明针对现有技术问题，设计了一个具备高精度的无人清扫车沿边路径跟踪控制系统，确保无人清扫车沿目标路径精确行驶、高效安全工作，最大程度上降低清扫车碰撞路沿的概率。

为解决上述技术问题，本发明采用如下技术方案：

一种大型工业园区无人清扫车的沿边路径跟踪控制方法，其特征在于，包括以下步骤：

S1：利用GPS提前录入无人清扫车在工业园区内沿边行驶轨迹，并将该沿边行驶轨迹作为后期无人清扫车的目标路径；

S2：建立阿克曼转向模型、无人清扫车与目标路径的几何关系图，依据纯追踪算法计算出当前无人清扫车前轮转角；

S3：激光雷达高频拾取无人清扫车与路沿距离，并将该距离与设定阈值比较以判断是否采用MPC算法对无人清扫车进行转角补偿；

S4：利用MPC算法及时对无人清扫车进行转角补偿：建立车辆二自由度模型，构建用于MPC算法的无人清扫车线性状态方程，再利用欧拉法将无人清扫车线性状态方程离散化并用于模型预测控制方法中，搭建MPC路径跟踪控制器，根据无人清扫车稳定性状态及时调整控制量；

S5：通过udp协议将纯追踪算法和MPC路径跟踪控制器得到的期望前轮转角控制量输出到车辆底盘，完成无人清扫车的路径跟踪任务。

上述技术方案中，所述步骤S2中依据纯追踪算法计算出当前无人清扫车前轮转角，包括以下步骤：

步骤S21：根据阿克曼转向几何关系，得到前轮转角δ与转弯半径R、车轴长度L之间的关系：

步骤S22：建立前轮转角δ与预瞄点距离l_d、车轴长度L、目标点方向与当前航向角度α(t)之间的关系：

上述技术方案中，所述步骤S3利用激光雷达高频拾取无人清扫车与路沿距离，包括以下步骤：

步骤S31：建立并训练激光雷达道路感知网络；

步骤S32：确定输入激光雷达道路感知网络的点云数据空间范围；

步骤S33：获取测距采样点；

步骤S34：基于测距采样点计算车辆与路沿距离；

步骤S35：选取4个相邻时刻的车辆与路沿距离的平均值作为某一时刻无人清扫车与路沿的最终预测距离。

上述技术方案中，步骤S31中采取pointnet++架构作为道路感知网络的基本结构；采用Semantic KITTI数据集中ground大类中road小类的点云数据为训练集；并简化逐点语义分割为逐点二分类任务；训练、验证结束后，将best_epoch权重参数加载至工控机以执行后续的实时测距任务。

上述技术方案中，步骤S32中确定输入激光雷达道路感知网络的点云数据空间范围时，首先设定激光雷达坐标系：以激光雷达发射激光束的位置为坐标原点；以车辆前进的方向为y轴；以车辆前进的左侧方向为x轴；z轴的方向利用右手定则得出；然后依据无人清扫车在过去时域内的循迹效果并考虑到激光雷达的安装位置和车辆自身大小，将设定范围内的点云数据输入特征提取网络进行感知。

上述技术方案中，步骤S32将坐标原点[0～2m,0～-4m,0～-1.6m]范围内的点云数据输入特征提取网络进行感知；输入频率定为0.5hz。

上述技术方案中，步骤S33获取测距采样点时，根据Pointnet++输出的所有属于道路的点云数据的id和坐标值，将输入点云数据沿y轴均匀分割成8组；每一组内选取x轴坐标最大的点作为样本点；分别验证8组点云数据是否能较好地形成道路轨迹：计算样本点集合x轴坐标的均值和方差，若均值和方差处于阈值内，则将其作为测距采样点集合；反之则去除某一组内的样本点，进一步搜索该组内x轴坐标最大的点替代之；之后重新计算样本点集合的均值和方差。

上述技术方案中，步骤S34中基于测距采样点，按权重机制计算车辆与路沿距离：

设P＝{p₁,p₂,p₃,p₄,p₅,p₆,p₇,p₈}为测距采样点集合，X＝{x₁,x₂,x₃,x₄,x₅,x₆,x₇,x₈}为x轴坐标集合；Y＝{y₁,y₂,y₃,y₄,y₅,y₆,y₇,y₈}为y轴坐标集合；W＝{w₁,w₂,w₃,w₄,w₅,w₆,w₇,w₈}为权重集合；其中：

w_i＝softmax(tem_i)；

tem_i表示第i个点与x轴的靠近程度，该靠近程度用该点y轴坐标绝对值的倒数与所有测距采样点y轴坐标绝对值之和的比值来表示；

softmax函数又称归一化指数函数，将任一k维向量转化成同一维度的新向量，该向量的每一个元素均分布在0～1之间，且所有元素的和为1；

W_i表示第i个测距采样点对最终估算车辆与路沿距离的贡献值大小，该贡献值处于0～1之间；

第i时刻车辆与路沿的距离表示为：

D_i＝XW^T。

上述技术方案中，步骤S4利用MPC算法及时对无人清扫车进行转角补偿，包括以下调整步骤：

步骤S41:建立车辆二自由度动力学模型：

式中β为质心侧偏角、ω为横摆角速度、v_x为纵向车速、δ_f为车辆前轮转角、m为整车的质量、I_z为车绕铅垂方向的转动惯量、C_f为前轮胎的等效侧偏刚度、C_r为后轮胎的等效侧偏刚度、l_f为质心到前轴的距离、l_r为质心到后轴的距离；

步骤S42：对于车辆二自由度运动学方程进行离散化处理，建立用于MPC算法的离散线性化方程：

式中x_mpc(k)、u_mpc(k)、y(k)分别为k时刻的无人清扫车的状态量、控制量、输出量；

步骤S43：搭建MPC路径跟踪控制器，将无人清扫车当前状态参数输入到MPC路径跟踪控制器，通过对无人清扫车信息的进一步处理，带入如下优化目标方程：

式中Q、R分别为输出量和控制量的权重函数；

步骤S44：根据道路实际情况加入横摆角速度限制、质心侧偏角限制、前轮转角限制；

步骤S45：利用MATLAB自带的Quadprog算法得到最优前轮转角，并通过udp协议将MPC路径跟踪控制器得到的控制量发送给无人清扫车。

一种大型工业园区无人清扫车的沿边路径跟踪控制系统，其特征在于在处理器中存储有计算机程序，当所述计算机程序被执行时执行上述步骤。

由此，本发明采用以下技术方案：一种工业园区无人清扫车的路径控制调整方法和系统，无人清扫车采用电机驱动，主控计算机结合GPS执行定位，将控制器输出的控制指令按照udp协议传输，使无人清扫车进行前轮转向，并获得无人清扫车当前状态参数。

本发明方法首先录入园区沿边道路的路径轨迹并存储；之后建立清扫车跟踪动力学模型，利用纯追踪算法实现路径跟踪，考虑到对目标路沿轨迹跟踪作业难度较大，因此当激光雷达检测无人清扫车偏离目标路径超过一定阈值，启动MPC控制器，利用车辆二自由度模型作为模型预测控制算法的预测模型，根据设定的损失函数和约束条件求解得到最优转角补偿角，实现基于车辆稳定性状态的运动调节；在底层控制中,通过udp协议将纯跟踪算法和MPC算法得到的前轮转角映射到无人清扫车上。

相对于现有视乎，本发明能够对工业园区内道路边沿轨迹实现准确跟踪，最大程度上降低清扫车碰撞路沿的概率，对于提高工业园区无人清扫车的路径跟踪准确度和清扫安全性具有重大意义。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1是本发明的无人清扫车整车信息传输和框架结构示意图。

图2是本发明实施的大型工业园区无人清扫车沿边路径跟踪控制方法中纯算法路径跟踪控制框架图。

图3是本发明无人清扫车沿边路径跟踪控制方法中激光雷达测距流程框图。

图4是本发明的无人清扫车MPC转角补偿算法流程框图。

图5是本发明无人清扫车的整体算法流程框图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

如图1-5，为根据本发明实施的大型工业园区无人清扫车沿边路径跟踪控制方法及系统。

如图1所述，作为优选，本发明实施的大型工业园区无人清扫车沿边路径跟踪控制系统包括GPS模块、若干个激光雷达、若干个摄像头、主控制器。

本发明应用的无人清扫车采用电机驱动，主控计算机结合清扫车上的GPS模块执行定位，将控制器输出的控制指令按照udp协议传输，并获得无人清扫车当前状态参数,无人清扫车进行前轮转向，如图1-5所示，包括以下步骤：

步骤S1：利用无人清扫车上的GPS模块，精确地将工业园区的沿边清扫路径录入，并将其作为无人清扫车清扫任务的标准。

步骤S2：建立阿克曼转向模型、无人清扫车与目标路径的几何关系图，依据纯追踪算法计算出当前无人清扫车前轮转角。

所述纯算法跟踪包括以下调整步骤：

步骤S21：确定无人清扫车的当前位置。

步骤S22:在目标路径中找到离无人清扫车最近的点，即为目标点。

步骤S23:换算目标点坐标系为车辆坐标系。

步骤S24:计算无人清扫车前轮转角]δ：

式中：L为车轴长度、α(t)为目标点方向与当前航向角度、l_d为预瞄点距离。

步骤S3：利用激光雷达高频拾取无人清扫车与路沿的距离D作为无人清扫车是否采用MPC算法纠偏的参考。

拾取无人清扫车与路沿的距离D所涉及的测距方法包括以下调整步骤：

步骤S31：建立并训练激光雷达道路感知网络。

建立并训练激光雷达道路感知网络。考虑到点云数据的无序性和旋转不变性以及语义分割任务的特点；本发明决定采取pointnet++架构作为道路检测网络的基本结构。为了提高网络提取道路特征的能力和训练效率，本发明拟采用Semantic KITTI数据集中ground大类中road小类的点云数据为训练集；并简化逐点语义分割为逐点二分类任务。训练、验证结束后，需将best_epoch权重参数加载至工控机以便执行后续的实时测距任务。

步骤S32：确定输入激光雷达道路感知网络的点云数据空间范围。

设定激光雷达坐标系：以激光雷达发射激光束的位置为坐标原点；以车辆前进的方向为y轴；以车辆前进的左侧方向为x轴；z轴的方向可以利用右手定则得出。

确定清扫车在过去时域内的循迹效果并考虑到激光雷达的安装位置和车辆自身大小。本发明拟将[0～2m,0～-4m,0～-1.6m]范围内的点云数据输入特征提取网络进行感知。输入频率定为0.5hz。

步骤S33：获取测距采样点：

Pointnet++输出所有属于道路的点云数据的id和坐标值。为了更好地描绘路沿特征，将输入点云数据沿y轴均匀分割成8组；每一组内选取x轴坐标最大的点作为样本点。为验证这8个点是否能较好地形成道路轨迹，本发明拟计算样本点集合x轴坐标的均值和方差。若均值和方差处于阈值内，则将其作为测距采样点集合；反之则去除某一组内的样本点，进一步搜索该组内x轴坐标最大的点替代之；之后重新计算样本点集合的均值和方差。为适应弯道路型，均值和方差的阈值可以适当放大。

步骤S34：基于测距采样点计算车辆与路沿距离。

考虑到在y轴方向离车辆越近的点更能表示邻近时刻的循迹效果，本发明设计了一种权重机制。设P＝{p₁,p₂,p₃,p₄,p₅,p₆,p₇,p₈}为测距采样点集合。X＝{x₁,x₂,x₃,x₄,x₅,x₆,x₇,x₈}为x轴坐标集合；Y＝{y₁,y₂,y₃,y₄,y₅,y₆,y₇,y₈}为y轴坐标集合；W＝{w₁,w₂,w₃,w₄,w₅,w₆,w₇,w₈}为权重集合。

w_i＝softmax(tem_i)；

tem_i表示第i个点与x轴的靠近程度，该靠近程度用该点y轴坐标绝对值的倒数与所有测距采样点y轴坐标绝对值之和的比值来表示。

softmax函数又称归一化指数函数，可以将任一k维向量转化成同一维度的新向量，该向量的每一个元素均分布在0～1之间，且所有元素的和为1。

W_i表示第i个测距采样点对最终估算车辆与路沿距离的贡献值大小，该贡献值处于0～1之间。

第i时刻的距离可以这样表示：

D_i＝XW^T；

为了提高预测准确性，本发明拟选取4个相邻时刻的D₁、D₂、D₃、D₄的平均值作为某一时刻无人清扫车与路沿的最终预测距离D：

阈值判断，如图5所示，当然，这个距离阈值根据清扫车的规格高度和大小可以变动。

本实施例中，距离阈值大小设置为：

若激光雷达探测到的距离1.5米≤D≤2米，无人清扫车在目标路径上行驶良好，清扫车保持此刻航向。

若激光雷达探测到的距离D≥2米或者D≤1.5米，则主控计算机结合路径信息，执行MPC控制器，对无人清扫车进行跟踪控制。

步骤S4，按照MPC算法纠偏，利用MPC算法及时对无人清扫车进行转角补偿。

MPC控制器对无人清扫车进行转角补偿包括以下调整步骤：

步骤S41:建立车辆二自由度运动学方程：

式中:β为质心侧偏角、ω为横摆角速度、v_x为纵向车速、δ_f为车辆前轮转角、m为整车的质量、I_z为车绕铅垂方向的转动惯量、C_f为前轮胎的等效侧偏刚度、C_r为后轮胎的等效侧偏刚度、l_f为质心到前轴的距离、l_r为质心到后轴的距离。

步骤S42：对于车辆二自由度运动学方程进行离散化处理，建立用于MPC算法的离散线性化方程：

式中：x_mpc(k)、u_mpc(k)、y(k)分别为k时刻的无人清扫车的状态量、控制量、输出量；得到MPC的预测方程：

Y(k)＝φ·x_mpc(k)+Θ·U_mpc(k)

式中：U_mpc(k)为预测时域的控制量；

步骤S43:搭建MPC路径跟踪控制器，将得到的状态量输入到MPC路径跟踪控制器，通过对无人清扫车信息的进一步处理，带入如下优化目标方程：

式中：Q、R分别为输出量和控制量的权重函数；

步骤S44：根据道路实际情况加入横摆角速度限制、质心侧偏角限制、车辆前轮转角限制；

ω_min≤ω≤ω_max；

β_min≤β≤β_max；

δ_min≤δ≤δ_max；

步骤S45：利用MATLAB自带的Quadprog算法得到最优车辆前轮转角，并通过udp协议中将MPC控制器计算出的控制量发送给无人清扫车。

作为优选，所述无人清扫车模型基于Carsim/Simulink仿真平台建立；所述模型包括由Simulink创建的电机驱动系统模型、转向系统模型、制动系统模型。

作为优选，在步骤S352中，在一个工作时域[t₀，t₀+T]中，MPC控制器计算出t₀时刻的最优解u(t)后只选择控制序列的第一个控制量作为实际输出，然后再根据下一个采样时间的车辆状态重新计算最优解u(t+1)，实现滚动优化，根据实际的车辆稳定性状态及时调整控制量。

作为优选，所述的udp通信包括命令的发送与接收两个工作流程,依据udp协议，编辑对应的控制信息。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (10)

1.一种大型工业园区无人清扫车的沿边路径跟踪控制方法，其特征在于，包括以下步骤：

S1：利用GPS提前录入无人清扫车在工业园区内沿边行驶轨迹，并将该沿边行驶轨迹作为后期无人清扫车的目标路径；

S2：建立阿克曼转向模型、无人清扫车与目标路径的几何关系图，依据纯追踪算法计算出当前无人清扫车前轮转角；

S3：激光雷达高频拾取无人清扫车与路沿距离，并将该距离与设定阈值比较以判断是否采用MPC算法对无人清扫车进行转角补偿；

S4：利用MPC算法及时对无人清扫车进行转角补偿：建立车辆二自由度模型，构建用于MPC算法的无人清扫车线性状态方程，再利用欧拉法将无人清扫车线性状态方程离散化并用于模型预测控制方法中，搭建MPC路径跟踪控制器，根据无人清扫车稳定性状态及时调整控制量；

S5：通过udp协议将纯追踪算法和MPC路径跟踪控制器得到的期望前轮转角控制量输出到车辆底盘，完成无人清扫车的路径跟踪任务。

2.根据权利要求1所述的大型工业园区无人清扫车的沿边路径跟踪控制方法，其特征在于所述步骤S2中依据纯追踪算法计算出当前无人清扫车前轮转角，包括以下步骤：

步骤S21：根据阿克曼转向几何关系，得到前轮转角δ与转弯半径R、车轴长度L之间的关系：

步骤S22：建立前轮转角δ与预瞄点距离l_d、车轴长度L、目标点方向与当前航向角度α(t)之间的关系：

3.根据权利要求1所述的大型工业园区无人清扫车的沿边路径跟踪控制方法，其特征在于所述步骤S3利用激光雷达高频拾取无人清扫车与路沿距离，包括以下步骤：

步骤S31：建立并训练激光雷达道路感知网络；

步骤S32：确定输入激光雷达道路感知网络的点云数据空间范围；

步骤S33：获取测距采样点；

步骤S34：基于测距采样点计算车辆与路沿距离；

步骤S35：选取4个相邻时刻的车辆与路沿距离的平均值作为某一时刻无人清扫车与路沿的最终预测距离。

4.根据权利要求3所述的大型工业园区无人清扫车的沿边路径跟踪控制方法，其特征在于步骤S31中采取pointnet++架构作为道路感知网络的基本结构；采用Semantic KITTI数据集中ground大类中road小类的点云数据为训练集；并简化逐点语义分割为逐点二分类任务；训练、验证结束后，将best_epoch权重参数加载至工控机以执行后续的实时测距任务。

5.根据权利要求3所述的大型工业园区无人清扫车的沿边路径跟踪控制方法，其特征在于步骤S32中确定输入激光雷达道路感知网络的点云数据空间范围时，首先设定激光雷达坐标系：以激光雷达发射激光束的位置为坐标原点；以车辆前进的方向为y轴；以车辆前进的左侧方向为x轴；z轴的方向利用右手定则得出；然后依据无人清扫车在过去时域内的循迹效果并考虑到激光雷达的安装位置和车辆自身大小，将设定范围内的点云数据输入特征提取网络进行感知。

6.根据权利要求3所述的大型工业园区无人清扫车的沿边路径跟踪控制方法，其特征在于步骤S32将坐标原点[0～2m,0～-4m,0～-1.6m]范围内的点云数据输入特征提取网络进行感知；输入频率定为0.5hz。

7.根据权利要求3所述的大型工业园区无人清扫车的沿边路径跟踪控制方法，其特征在于步骤S33获取测距采样点时，根据Pointnet++输出的所有属于道路的点云数据的id和坐标值，将输入点云数据沿y轴均匀分割成8组；每一组内选取x轴坐标最大的点作为样本点；分别验证8组点云数据是否能较好地形成道路轨迹：计算样本点集合x轴坐标的均值和方差，若均值和方差处于阈值内，则将其作为测距采样点集合；反之则去除某一组内的样本点，进一步搜索该组内x轴坐标最大的点替代之；之后重新计算样本点集合的均值和方差。

8.根据权利要求3所述的大型工业园区无人清扫车的沿边路径跟踪控制方法，其特征在于步骤S34中基于测距采样点，按权重机制计算车辆与路沿距离：

设P＝{p₁,p₂,p₃,p₄,p₅,p₆,p₇,p₈}为测距采样点集合，X＝{x₁,x₂,x₃,x₄,x₅,x₆,x₇,x₈}为x轴坐标集合；Y＝{y₁,y₂,y₃,y₄,y₅,y₆,y₇,y₈}为y轴坐标集合；W＝{w₁,w₂,w₃,w₄,w₅,w₆,w₇,w₈}为权重集合；其中：

w_i＝softmax(tem_i)；

tem_i表示第i个点与x轴的靠近程度，该靠近程度用该点y轴坐标绝对值的倒数与所有测距采样点y轴坐标绝对值之和的比值来表示；

softmax函数为归一化指数函数，将任一k维向量转化成同一维度的新向量，该向量的每一个元素均分布在0～1之间，且所有元素的和为1；

W_i表示第i个测距采样点对最终估算车辆与路沿距离的贡献值大小，该贡献值处于0～1之间；

第i时刻车辆与路沿的距离表示为：

D_i＝XW^T。

9.根据权利要求1所述的大型工业园区无人清扫车的沿边路径跟踪控制方法，其特征在于步骤S4利用MPC算法及时对无人清扫车进行转角补偿，包括以下调整步骤：

步骤S41:建立车辆二自由度动力学模型：

式中β为质心侧偏角、ω为横摆角速度、v_x为纵向车速、δ_f为车辆前轮转角、m为整车的质量、I_z为车绕铅垂方向的转动惯量、C_f为前轮胎的等效侧偏刚度、C_r为后轮胎的等效侧偏刚度、l_f为质心到前轴的距离、l_r为质心到后轴的距离；

步骤S42：对于车辆二自由度运动学方程进行离散化处理，建立用于MPC算法的离散线性化方程：

式中x_mpc(k)、u_mpc(k)、y(k)分别为k时刻的无人清扫车的状态量、控制量、输出量；

步骤S43：搭建MPC路径跟踪控制器，将无人清扫车当前状态参数输入到MPC路径跟踪控制器，通过对无人清扫车信息的进一步处理，带入如下优化目标方程：

式中Q、R分别为输出量和控制量的权重函数；

步骤S44：根据道路实际情况加入横摆角速度限制、质心侧偏角限制、前轮转角限制；

步骤S45：利用MATLAB自带的Quadprog算法得到最优前轮转角，并通过udp协议将MPC路径跟踪控制器得到的控制量发送给无人清扫车。

10.一种大型工业园区无人清扫车的沿边路径跟踪控制系统，其特征在于在处理器中存储有计算机程序，当所述计算机程序被执行时执行上述权利要求1-10任一项所述的步骤。

Images