一种环卫车辆轨迹跟踪控制系统及方法与流程

1.本发明公开了一种环卫车辆轨迹跟踪控制系统及方法，属于环卫车辆控制技术领域。


背景技术：

2.近年来，园区的道路清扫和维护工作量不断加重，环卫设备的机械化、自动化、智能化运行一直是国内外发展趋势。常用的清扫机械主要是由专用汽车底盘改装，具有道路清扫功能的道路清扫车。该汽车由人工驾驶用于各种环境下道路清扫、洒水、收集垃圾等工作，大大提高了清扫效率，但也存在人工成本高的问题，随着无人驾驶车辆技术逐渐成熟，在园区环卫中的应用成本也逐渐下降，环卫车可以不间断连续作业，车辆利用率和清扫效率成倍提升。由于无人驾驶环卫车系统是一个复杂的非线性系统，运行在不同的路况和气候条件下，车辆轮胎与地面的摩擦系数等参数时变，这些变化因素都会影响车辆的控制运行。同时，随着环卫车清扫垃圾的重量不断变化，车载重量也是实时变化、无法预测的。目前现有轨迹跟踪控制算法对车辆纵向控制研究较少，大多以匀速处理，但车辆实际作业时速度是变化的。亟需一种先进的轨迹跟踪控制算法用于高精度的轨迹跟踪控制，提高跟踪精度和工作效率。


技术实现要素：

3.本发明的目的在于解决现有的现有轨迹跟踪控制算法对车辆纵向控制研究较少，大多以匀速处理，无法根据车辆实际作业时速度变化的问题，提出一种环卫车辆轨迹跟踪控制系统及方法。
4.本发明所要解决的问题是由以下技术方案实现的：
5.根据本发明实施例的第一方面，提供一种环卫车辆轨迹跟踪控制系统，包括设置在环卫车辆上的车辆动力系统、多种传感器信息采集设备以及车辆导航系统，所述车辆动力系统包括：行进电机及与其电性连接的行进控制器和转向电机及与其电性连接的转向控制器，还包括设置在环卫车辆上的横向控制器和车速跟随器，所述多种传感器信息采集设备以及车辆导航系统分别与横向控制器和车速跟随器电性连接，所述横向控制器和车速跟随器分别与转向控制器和行进控制器电性连接。
6.优选的是，所述多种传感器信息采集设备包括分别与横向控制器和车速跟随器的惯性测量单元以及与横向控制器连接的全球导航卫星系统和前轮转向角角度传感器。
7.根据本发明实施例的第二方面，提供一种环卫车辆轨迹跟踪控制方法，应用于第一方面所述的环卫车辆轨迹跟踪控制系统，包括：
8.通过所述惯性测量单元获取车辆实时速度分别发送给横向控制器和车速跟随器；
9.通过所述全球导航卫星系统获取车辆在全局坐标系的位置信息和车辆横摆角发送给横向控制器；
10.通过所述前轮转向角角度传感器获取车辆前轮转角发送给横向控制器；
11.通过所述车辆导航系统获取车辆预定行驶轨迹分别发送给横向控制器和车速跟随器；
12.所述车速跟随器分别获取车辆实时速度和车辆预定行驶轨迹得到车辆纵向车速所需的模拟量电压发送给行进控制器对车辆进行纵向控制；
13.所述横向控制器分别获取车辆横摆角、车辆实时速度、车辆前轮转角、车辆在全局坐标系的位置信息以及车辆预定行驶轨迹得到车辆前轮转角发送给转向控制器用于对车辆进行横向控制。
14.优选的是，所述车辆预定行驶轨迹至少包括：车辆参考轨迹横向位置、车辆参考轨迹航偏角以及车辆参考轨迹速度。
15.优选的是，所述车速跟随器分别获取车辆实时速度和车辆预定行驶轨迹得到车辆纵向车速所需的模拟量电压发送给行进控制器对车辆进行纵向控制，包括：
16.所述车速跟随器获取车辆实时速度进行处理得到处理后的车辆实时速度；
17.所述车速跟随器根据处理后的车辆实时速度和车辆参考轨迹速度通过公式(1)得到车辆纵向车速所需的模拟量电压发送给行进控制器对对车辆进行纵向控制：
[0018][0019]
其中，δv为车速跟随器输出的模拟量电压增量；k
p
,ki,kd分别为pid控制器的比例环节、积分环节、微分环节的系数；δv为车辆实时速度和车辆参考轨迹速度的差值。
[0020]
优选的是，所述横向控制器分别获取车辆横摆角、车辆实时速度、车辆前轮转角、车辆在全局坐标系的位置信息以及车辆预定行驶轨迹得到车辆前轮转角发送给转向控制器用于对车辆进行横向控制，包括：
[0021]
所述横向控制器分别获取车辆横摆角、车辆实时速度、车辆前轮转角和车辆在全局坐标系的位置信息进行处理分别得到处理后的车辆横摆角和处理后的车辆在全局坐标系的位置信息；
[0022]
所述横向控制器分别根据处理后的车辆横摆角和车辆参考轨迹航偏角的得到航偏角误差以及根据处理后的车辆在全局坐标系的位置信息和车辆参考轨迹横向位置得到横向位置误差；
[0023]
所述横向控制器通过航偏角误差和横向位置误差得到车辆前轮转角发送给转向控制器用于对车辆进行横向控制。
[0024]
本发明的有益效果在于：
[0025]
本专利提供一种环卫车辆轨迹跟踪控制系统及方法，通过车速跟随器分别获取车辆实时速度和车辆预定行驶轨迹得到车辆纵向车速所需的模拟量电压发送给行进控制器对车辆进行纵向控制，通过横向控制器分别获取车辆横摆角、车辆实时速度、车辆前轮转角、车辆在全局坐标系的位置信息以及车辆预定行驶轨迹得到车辆前轮转角发送给转向控制器用于对车辆进行横向控制，实现对车辆状态参数动态精确估计，有效提升车辆动力学控制性能，相比现有的基于车辆运动学的方法来进行轨迹跟踪的方法，能够快速、高效的完成车辆的横向和纵向协调控制。
[0026]
应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本发明。
附图说明
[0027]
图1为本发明一种环卫车辆轨迹跟踪控制系统的结构图。
具体实施方式
[0028]
下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0029]
在本发明的描述中，需要说明的是，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
[0030]
在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。
[0031]
实施例一
[0032]
图1是根据一示例性实施例示出的一种环卫车辆轨迹跟踪控制系统，包括设置在环卫车辆上的车辆动力系统、多种传感器信息采集设备、车辆导航系统、横向控制器和车速跟随器，下面将详细介绍下上述各组件的具体连接关系。
[0033]
其中，车辆动力系统包括：行进电机及与其电性连接的行进控制器和转向电机及与其电性连接的转向控制器，多种传感器信息采集设备以及车辆导航系统分别与横向控制器和车速跟随器电性连接，具体的多种传感器信息采集设备包括分别与横向控制器和车速跟随器的惯性测量单元以及与横向控制器连接的全球导航卫星系统和前轮转向角角度传感器，横向控制器和车速跟随器分别与转向控制器和行进控制器电性连接。
[0034]
实施例二
[0035]
本示例性实施例示出的一种环卫车辆轨迹跟踪控制方法，具体步骤包括：
[0036]
步骤s10，通过惯性测量单元获取车辆实时速度分别发送给横向控制器和车速跟随器，通过全球导航卫星系统获取车辆在全局坐标系的位置信息和车辆横摆角发送给横向控制器，通过前轮转向角角度传感器获取车辆前轮转角发送给横向控制器，通过车辆导航系统获取车辆预定行驶轨迹作为参考轨迹分别发送给横向控制器和车速跟随器。
[0037]
其中，上述车辆实时速度具体包括车辆的横摆角速度、车辆纵向速度和车辆横向速度。上述惯性测量单元、全球导航卫星系统和前轮转向角角度传感器是周期性获取的每隔t时间对数据进行一次采集更新。车辆行驶轨迹为园区固定道路，通过全局坐标系规划出车辆行进的路径以及速度信息。车辆预定行驶轨迹至少包括：车辆参考轨迹横向位置y
des
、车辆参考轨迹航偏角以及车辆参考轨迹速度v
des
。
[0038]
上述惯性测量单元、全球导航卫星系统、前轮转向角角度传感器以及车辆导航系统获取的数据且没有以时间为维度进行描述本实施例的附图仅为其中一种情况但不限于其他获取数据顺序。
[0039]
步骤s20，车速跟随器分别获取车辆实时速度和车辆预定行驶轨迹得到车辆纵向车速所需的模拟量电压发送给行进控制器对车辆进行纵向控制，具体内容如下：
[0040]
车速跟随器获取车辆实时速度进行基于扩展卡尔曼滤波法的多源信息融合，去除传感器参数的干扰和累计误差，得到处理后的车辆实时速度v
x
；
[0041]
为实现环卫车辆工作模式下对目标车速的保持，车速跟随器基于pid控制算法将根据处理后的车辆实时速度和车辆参考轨迹速度通过公式(1)得到车辆纵向车速所需的模拟量电压发送给行进控制器对车辆进行纵向控制，控制车辆的纵向速度，用以弥补因转向过程中产生侧向速度而导致的纵向车速降低的现象：
[0042][0043]
其中，δv为车速跟随器输出的模拟量电压增量；k
p
,ki,kd分别为pid控制器的比例环节、积分环节、微分环节的系数；δv为车辆实时速度v
x
和车辆参考轨迹速度v
des
的差值。
[0044]
步骤s30，横向控制器分别获取车辆横摆角、车辆实时速度、车辆前轮转角、车辆在全局坐标系的位置信息以及车辆预定行驶轨迹得到车辆前轮转角发送给转向控制器用于对车辆进行横向控制，具体内容如下：
[0045]
横向控制器分别获取车辆横摆角、车辆实时速度、车辆前轮转角和车辆在全局坐标系的位置信息进行处理分别得到处理后的车辆横摆角和处理后的车辆在全局坐标系的位置信息y；
[0046]
横向控制器分别根据处理后的车辆横摆角和车辆参考轨迹航偏角的得到航偏角误差以及根据处理后的车辆在全局坐标系的位置信息y和车辆参考轨迹横向位置y
des
得到横向位置误差δy；
[0047]
横向控制器通过航偏角误差和横向位置误差δy得到车辆前轮转角发送给转向控制器用于对车辆进行横向控制，具体内容如下：
[0048]
车辆任意时刻的状态和前轮转角的关系为：
[0049][0050]
在任意点(ξr,ur)处进行泰勒展开，保留一阶项，忽略高阶项得到：
[0051][0052]
也可以写成：
[0053][0054]
式中，jf(ξ)为f相对于ξ的雅克比矩阵，jf(u)为f相对于u的雅克比矩阵。
[0055]
转换成连续的状态方程：
[0056][0057]
式中，ξ2＝ξ-ξr,u2＝u-ur,a(t)＝jf(ξ),b(t)＝jf(u)
[0058]
采用向前欧拉法近似离散线性化模型：
[0059]
ξ0(k+1)＝a
k,t
ξ0(k)+b
k,t
u0(k)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
[0060]
式中，a
k,t
＝i+ta(t),b
k,t
＝tb(t)，t是采样周期。
[0061]
为防止前轮转角发生突变，影响车辆稳定性，选取前轮转角增量为控制量，设定：
[0062][0063]
得到一个新的状态空间表达式：
[0064][0065][0066]
式中，状态量为控制量为δu＝δδf。各矩阵定义为：
[0067][0068]
由于预测时域时间较短，所以认为在每个采样时刻的预测时域内矩阵不发生变化，即：
[0069][0070][0071]
某一车速下对应的预测时域为n
p
，控制时域为nc，并且n
p
＞nc，预测时域内的状态量表示为：
[0072][0073]
同样的，输出方程表示为：
[0074][0075]
为了使整个关系更加明确，将系统未来时刻的输出以矩阵的形式表达：
[0076]
y(t)＝ψ
t
ξ(t|t)+θ
t
δu(t)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(12)
[0077]
式中，
[0078]
[0079][0080]
把前轮转角增量作为目标函数的状态量，并加入松弛因子，优化目标为：
[0081][0082]
式中，为航偏角误差和横向位置误差δy的平方和，为前轮转角增量δδf的平方，ρ为权重系数，ε为松弛因子。
[0083]
经过相应的矩阵计算，可以将优化目标调整为：
[0084]
j(ξ(t),u(t-1),δu(t))＝[δu(t)
t
,ε]
tht
[δu(t)
t
,ε]+g
t
[δu(t)
t
,ε]+p
t
ꢀꢀ
(14)
[0085]
式中：g
t
＝[2e(t)
t
qeθ
t 0] p
t
＝e(t)
t
qee(t)。
[0086]
在每一步的带约束优化求解问题都等价于求解如下的二次规划问题：
[0087][0088]
s.t.δu
min
≤δu(k)≤δu
max
,k＝t,t+h
e-1
[0089][0090]ymin-ε≤ψ
t
ξ(t|t)+θ
t
δu(t)≤y
max
+ε
ꢀꢀꢀ
(15)
[0091]
在每个控制周期内，获得整车状态信息和定位信息，通过模型预测控制算法，使目标函数各项之和最小，得到控制时域内的一系列前轮转角增量：
[0092][0093]
将该序列中第一个元素作为实际的前轮转角增量作用于转向电机，控制车辆前轮转向，进而实现对轨迹的横向跟踪控制。
[0094]
尽管本发明的实施方案已公开如上，但其并不仅仅限于说明书和实施方式中所列运用。它完全可以被适用于各种适合本发明的领域。对于熟悉本领域的人员而言，可容易地实现另外的修改。因此在不背离权利要求及等同范围所限定的一般概念下，本发明并不限于特定的细节和这里示出与描述的图例。

技术特征：
1.一种环卫车辆轨迹跟踪控制系统，包括设置在环卫车辆上的车辆动力系统、多种传感器信息采集设备以及车辆导航系统，所述车辆动力系统包括：行进电机及与其电性连接的行进控制器和转向电机及与其电性连接的转向控制器，其特征在于，还包括设置在环卫车辆上的横向控制器和车速跟随器，所述多种传感器信息采集设备以及车辆导航系统分别与横向控制器和车速跟随器电性连接，所述横向控制器和车速跟随器分别与转向控制器和行进控制器电性连接。2.根据权利要求1所述的一种环卫车辆轨迹跟踪控制系统，其特征在于，所述多种传感器信息采集设备包括分别与横向控制器和车速跟随器的惯性测量单元以及与横向控制器连接的全球导航卫星系统和前轮转向角角度传感器。3.一种环卫车辆轨迹跟踪控制方法，应用于权利要求1或2所述的环卫车辆轨迹跟踪控制系统，其特征在于，包括：通过所述惯性测量单元获取车辆实时速度分别发送给横向控制器和车速跟随器；通过所述全球导航卫星系统获取车辆在全局坐标系的位置信息和车辆横摆角发送给横向控制器；通过所述前轮转向角角度传感器获取车辆前轮转角发送给横向控制器；通过所述车辆导航系统获取车辆预定行驶轨迹分别发送给横向控制器和车速跟随器；所述车速跟随器分别获取车辆实时速度和车辆预定行驶轨迹得到车辆纵向车速所需的模拟量电压发送给行进控制器对对车辆进行纵向控制；所述横向控制器分别获取车辆横摆角、车辆实时速度、车辆前轮转角、车辆在全局坐标系的位置信息以及车辆预定行驶轨迹得到车辆前轮转角发送给转向控制器用于对车辆进行横向控制。4.根据权利要求3所述的一种环卫车辆轨迹跟踪控制方法，其特征在于，所述车辆预定行驶轨迹至少包括：车辆参考轨迹横向位置、车辆参考轨迹航偏角以及车辆参考轨迹速度。5.根据权利要求3所述的一种环卫车辆轨迹跟踪控制方法，其特征在于，所述车速跟随器分别获取车辆实时速度和车辆预定行驶轨迹得到车辆纵向车速所需的模拟量电压发送给行进控制器对车辆进行纵向控制，包括：所述车速跟随器获取车辆实时速度进行处理得到处理后的车辆实时速度；所述车速跟随器根据处理后的车辆实时速度和车辆参考轨迹速度通过公式(1)得到车辆纵向车速所需的模拟量电压发送给行进控制器对车辆进行纵向控制：其中，δv为车速跟随器输出的模拟量电压增量；k
p
,k
i
,k
d
分别为pid控制器的比例环节、积分环节、微分环节的系数；δv为车辆实时速度和车辆参考轨迹速度的差值。6.根据权利要求3所述的一种环卫车辆轨迹跟踪控制方法，其特征在于，所述横向控制器分别获取车辆横摆角、车辆实时速度、车辆前轮转角、车辆在全局坐标系的位置信息以及车辆预定行驶轨迹得到车辆前轮转角发送给转向控制器用于对车辆进行横向控制，包括：所述横向控制器分别获取车辆横摆角、车辆实时速度、车辆前轮转角和车辆在全局坐标系的位置信息进行处理分别得到处理后的车辆横摆角和处理后的车辆在全局坐标系的位置信息；
所述横向控制器分别根据处理后的车辆横摆角和车辆参考轨迹航偏角的得到航偏角误差以及根据处理后的车辆在全局坐标系的位置信息和车辆参考轨迹横向位置得到横向位置误差；所述横向控制器通过航偏角误差和横向位置误差得到车辆前轮转角发送给转向控制器用于对车辆进行横向控制。

技术总结
本发明公开了一种环卫车辆轨迹跟踪控制系统及方法，属于环卫车辆控制技术领域，包括设置在环卫车辆上的车辆动力系统、多种传感器信息采集设备以及车辆导航系统，所述车辆动力系统包括：行进电机及与其电性连接的行进控制器和转向电机及与其电性连接的转向控制器，还包括设置在环卫车辆上的横向控制器和车速跟随器。本专利通过车速跟随器和横向控制器分别获取车辆纵向车速所需的模拟量电压和车辆前轮转角对车辆进行纵向控制和横向控制，实现对车辆状态参数动态精确估计，有效提升车辆动力学控制性能，相比现有的基于车辆运动学的方法来进行轨迹跟踪的方法，能够快速、高效的完成车辆的横向和纵向协调控制。车辆的横向和纵向协调控制。车辆的横向和纵向协调控制。


技术研发人员：李天宇 李少奇 杨庭毅 黄思成 刘惠影
受保护的技术使用者：长沙汽车创新研究院
技术研发日：2023.03.27
技术公布日：2023/6/27