一种拖车系统路径跟踪方法、装置、系统及存储介质与流程

本发明涉及车辆自动控制领域，更具体地涉及一种拖车系统路径跟踪方法、装置、系统及存储介质。

背景技术：

随着车辆智能化技术的发展，无人驾驶载物技术逐渐成为车辆自动控制领域的一个热点。带有若干节拖车的货物运输车由于具有拖车数量可任意改变的特点，在港口、仓库和机场等场合被大量应用。基于无人驾驶技术的拖车系统不仅能够降低机场、港口的人员消耗，更能够保证拖车系统运行过程中的安全。

拖车系统无人驾驶技术需要拖车能够对目标路径进行路径跟踪。当前基于最优控制路径跟踪的技术一般应用于单轴、单节拖车，适用范围较窄。

因此，迫切需要一种新的拖车系统路径跟踪技术，以解决上述问题。

技术实现要素：

考虑到上述问题而提出了本发明。本发明提供了一种拖车系统路径跟踪方法、装置、系统及存储介质。

本发明提供了一种拖车系统路径跟踪方法，包括：

获取目标路径点的期望位姿；

基于拖车系统的拖车系统模型根据所述期望位姿确定所述拖车系统在目标路径点的参考状态量和前馈控制量；

在每个控制周期，获取所述拖车系统的实时状态量，并利用所述实时状态量基于所述参考状态量和所述前馈控制量控制所述拖车系统的牵引车的前轮偏转角度。

示例性地，所述拖车系统模型通过以下方式构建：

根据所述拖车系统的第i节拖车和第i+1节拖车之间的连接关系，基于所述第i节拖车的运动状态确定所述第i+1节拖车的运动学模型，其中i＝0，1，2，…，m-1，m为拖车节数，第0节拖车为牵引车；

对所述运动学模型进行线性化和离散化处理，以得到所述拖车系统模型。

示例性地，所述基于所述第i节拖车的运动状态确定所述第i+1节拖车的运动学模型包括：

根据如下公式(1)确定所述第i+1节拖车的运动学模型：

其中，

其中，xi+1为第i+1节拖车的全局横坐标，yi+1为第i+1节拖车的全局纵坐标，θi+1为第i+1节拖车的航向，αi+1为第i+1节拖车的拖杆航向，vi+1为第i+1节拖车的速度，li+1为第i+1节拖车的轴距，ci+1为第i+1节拖车的拖杆长度，θ0为牵引车的航向，δ0为牵引车的前轮偏角，v0为牵引车的速度，l0为牵引车的轴距。

示例性地，所述对所述运动学模型进行线性化和离散化处理，以得到所述拖车系统模型包括：

对所述运动学模型进行线性化和离散化处理，得到如公式(2)所示的所述拖车系统模型，

δxk+1＝aδxk+bδuk(2)

其中，k＝0，1，2，…，n，n为滑动窗口中目标路径点的个数，δxk＝[δxmkδymkδθmkδαmk…δx1kδy1kδθ1kδα1kδθ0k]^t为所述拖车系统在第k个目标路径点的预测误差，δθ0k为第k个目标路径点的牵引车的航向的预测误差，δxjk为第k个目标路径点的第j节拖车的全局横坐标的预测误差，δyjk为第k个目标路径点的第j节拖车的全局纵坐标的预测误差，δθjk为第k个目标路径点的第j节拖车的航向的预测误差，δαjk为第k个目标路径点的第j节拖车的拖杆航向的预测误差，j＝1,2,…,m，m为拖车节数，δμk为所述拖车系统在第k个目标路径点的反馈控制量，a为系统矩阵，b为控制矩阵。

示例性地，所述利用所述实时状态量基于所述参考状态量和所述前馈控制量控制所述拖车系统的牵引车的前轮偏转角度包括：

利用所述实时状态量基于所述参考状态量和所述前馈控制量确定前轮期望偏角；

根据所述前轮期望偏角控制所述拖车系统的牵引车的前轮偏转角度。

示例性地，所述利用所述实时状态量基于所述参考状态量和所述前馈控制量确定前轮期望偏角包括：

利用所述实时状态量更新所述系统矩阵a和所述控制矩阵b；

利用所述系统矩阵a和所述控制矩阵b并基于所述参考状态量和所述前馈控制量构造目标函数；

利用所述目标函数优化求解反馈控制量；

根据所述前馈控制量和所述反馈控制量确定所述前轮期望偏角。

示例性地，所述利用所述系统矩阵a和所述控制矩阵b并基于所述参考状态量和所述前馈控制量构造目标函数包括：

根据公式(7)构造所述目标函数，

其中，q0为第一误差权重矩阵，q1为第二误差权重矩阵，r为控制量权重矩阵，xn为所述拖车系统在第n个目标路径点的预测状态量，rn为所述拖车系统在第n个目标路径点的参考状态量，xk为所述拖车系统在第k个目标路径点的预测状态量，rk为所述拖车系统在第k个目标路径点的参考状态量，uk为所述拖车系统在第k个目标路径点的前馈控制量，λk为第k个目标路径点对应的拉格朗日乘子系数矩阵。

示例性地，所述利用所述目标函数优化求解反馈控制量包括：

基于所述目标函数根据第n个目标路径点的预测状态量通过线性二次型调节器方法迭代，以得到所述反馈控制量。

示例性地，所述根据所述前馈控制量和所述反馈控制量确定所述前轮期望偏角包括：

根据公式(10)确定所述拖车系统在第0个目标路径点的预测误差δx0，

δx0＝x0-r0(10)

其中，x0为所述实时状态量，用作所述拖车系统在第0个目标路径点的预测状态量，r0为所述拖车系统在第0个目标路径点的参考状态量；

利用所述拖车系统在第0个目标路径点的预测误差δx0确定所述拖车系统在第0个目标路径点的反馈控制量δu0；

根据所述拖车系统在第0个目标路径点的前馈控制量u0和反馈控制量δu0确定所述前轮期望偏角。

示例性地，所述方法还包括：

在每个控制周期，获取所述拖车系统的实时状态量后，对所述实时状态量中的速度值进行取负操作，以得到反向实时状态量；

利用所述反向实时状态量基于所述参考状态量和所述前馈控制量控制所述拖车系统的牵引车的前轮偏转角度，以进行反向路径跟踪。

示例性地，所述基于拖车系统的拖车系统模型根据所述期望位姿确定所述拖车系统在目标路径点的参考状态量和前馈控制量包括：

基于所述拖车系统模型根据所述期望位姿利用纯跟踪的方式确定所述参考状态量和所述前馈控制量。

示例性地，所述获取所述拖车系统的实时状态量包括：

利用传感器定位获取所述拖车系统的实时状态量；或

利用同时定位与建图技术获取所述拖车系统的实时状态量。

根据本发明另一方面，还提供了一种用于拖车系统路径跟踪的装置，包括：

获取模块，用于获取目标路径点的期望位姿；

参考模块，用于基于拖车系统的拖车系统模型根据所述期望位姿确定所述拖车系统在目标路径点的参考状态量和前馈控制量；

跟踪模块，用于在每个控制周期，获取所述拖车系统的实时状态量，并利用所述实时状态量基于所述参考状态量和所述前馈控制量控制所述拖车系统的牵引车的前轮偏转角度。

根据本发明又一方面，还提供了一种用于拖车系统路径跟踪的系统，包括处理器和存储器，其中，所述存储器中存储有计算机程序指令，所述计算机程序指令被所述处理器运行时用于执行上述拖车系统路径跟踪方法。

根据本发明再一方面，还提供了一种存储介质，在所述存储介质上存储了程序指令，所述程序指令在运行时用于执行上述拖车系统路径跟踪方法。

根据本发明实施例的拖车系统路径跟踪方法、装置、系统及存储介质，可以适应不同款式的拖车，从而极大提高了拖车系统无人驾驶技术的普适性和实用性。

上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其它目的、特征和优点能够更明显易懂，以下特举本发明的具体实施方式。

附图说明

通过结合附图对本发明实施例进行更详细的描述，本发明的上述以及其它目的、特征和优势将变得更加明显。附图用来提供对本发明实施例的进一步理解，并且构成说明书的一部分，与本发明实施例一起用于解释本发明，并不构成对本发明的限制。在附图中，相同的参考标号通常代表相同部件或步骤。

图1示出了根据本发明一个实施例的拖车系统路径跟踪方法的示意性流程图；

图2示出了根据本发明一个实施例的拖车系统模型的示意图；

图3示出了根据本发明一个实施例的利用实时状态量基于参考状态量和前馈控制量控制拖车系统的牵引车的前轮偏转角度的示意性流程图；

图4示出了根据本发明一个实施例的利用实时状态量基于参考状态量和前馈控制量确定前轮期望偏角的示意性流程图；

图5示出了根据本发明一个实施例的根据前馈控制量和反馈控制量确定前轮期望偏角的示意性流程图；

图6a示出了根据本发明另一个实施例的拖车系统路径跟踪方法的示意性流程图；

图6b示出了根据本发明又一个实施例的拖车系统路径跟踪方法的示意性流程图；以及

图7示出了根据本发明一个实施例的用于拖车系统路径跟踪的装置的示意性框图。

具体实施方式

为了使得本发明的目的、技术方案和优点更为明显，下面将参照附图详细描述根据本发明的示例实施例。显然，所描述的实施例仅仅是本发明的一部分实施例，而不是本发明的全部实施例，应理解，本发明不受这里描述的示例实施例的限制。基于本发明中描述的本发明实施例，本领域技术人员在没有付出创造性劳动的情况下所得到的所有其它实施例都应落入本发明的保护范围之内。

为了解决现有的拖车系统无人驾驶技术没有良好的适应性和实用性的问题，本发明实施例提供了一种新的拖车系统路径跟踪方法。该拖车系统路径跟踪方法可以实时控制各种拖车系统的牵引车的前轮偏转角度，从而能够控制拖车系统准确地跟踪路径。该拖车系统路径跟踪方法可以应用于各种使用拖车系统的场景，例如港口、机场、物流中心、仓库和工厂等场景。

下面，将参考图1描述根据本发明实施例的拖车系统路径跟踪方法。图1示出了根据本发明一个实施例的拖车系统路径跟踪方法1000的示意性流程图。如图1所示，方法1000包括步骤s1100、步骤s1200以及步骤s1300。

步骤s1100，获取目标路径点的期望位姿。

目标路径是预先规划好的路径，由一组目标路径点组成。每个目标路径点的期望位姿包含拖车系统在该路径点的空间位置信息，例如可以表示为全局横坐标、全局纵坐标和航向角。目标路径是拖车系统将要行驶的路线。可以利用任何现有的或未来开发的技术规划目标路径，例如可以把规划好的目标路径存为路径文件。从而可以通过读取路径文件获取目标路径点的期望位姿。

步骤s1200，基于拖车系统的拖车系统模型根据步骤s1100获取的期望位姿确定该拖车系统在目标路径点的参考状态量和前馈控制量。

拖车系统模型是关于拖车系统的数学抽象模型。可以根据牵引车与拖车之间的连接关系、前节拖车与后节拖车之间的连接关系和/或拖车的轴距、拖杆长度、拖车节数等参数构建拖车系统模型。

利用步骤s1100获取的期望位姿，结合拖车系统模型进行数学仿真，可以得到拖车系统到达每个目标路径点时的状态量和对拖车系统施加的控制量。可以把该状态量作为参考状态量、把该控制量作为前馈控制量，用于拖车系统的路径跟踪，以提高拖车系统路径跟踪的准确性。

示例性地，上述基于拖车系统的拖车系统模型根据步骤s1100获取的期望位姿确定该拖车系统在目标路径点的参考状态量和前馈控制量包括：基于上述拖车系统模型根据步骤s1100获取的期望位姿利用纯跟踪(purepursuit)的方式确定参考状态量和前馈控制量。

纯跟踪是一种广泛使用的基于几何跟踪的技术。基于上述拖车系统模型，让拖车系统按照纯跟踪的方式跟踪目标路径的期望位姿，记录拖车系统在滑动窗口中每个目标路径点的状态量和得到的控制量，作为对应时刻该拖车系统的参考状态量和前馈控制量。参考状态量表示基于纯跟踪预测得到的该拖车系统到达滑动窗口中每个目标路径点的状态。具体地，参考状态量可以包括牵引车的航向θ0和每节拖车的位置(xiyi)、拖车航向θi以及拖杆航向αi，其中，i＝1,2,…,m，m为拖车节数，i为拖车序号。对于滑动窗口中的第k个目标路径点，参考状态量可以表示为rk＝[xmkymkθmkαmk…x1ky1kθ1kα1kθ0k]^t。其中，k＝0表示滑动窗口中的第0个目标路径点，也即当前目标路径点。

由此，利用纯跟踪技术进行路径跟踪仿真预测，给拖车系统路径跟踪提供了仿真结果作为路径跟踪参考，提高了拖车系统路径跟踪的准确性。

步骤s1300，在每个控制周期，获取拖车系统的实时状态量，并利用该实时状态量基于步骤s1200确定的参考状态量和前馈控制量控制该拖车系统的牵引车的前轮偏转角度。

控制周期是对拖车系统施加控制的时间周期。拖车系统的实时状态量，用以表示该拖车系统当前所处位置以及航向等信息。具体地，拖车系统的实时状态量可以包括牵引车的航向和每节拖车的位置、拖车航向以及拖杆航向等。可以利用任何现有的或未来开发的技术获取拖车系统的实时状态量。

示例性地，可以利用传感器定位获取拖车系统的实时状态量。例如，可以利用全球定位系统(globalpositioningsystem，简称gps)传感器获取拖车系统的实时状态量。又例如，可以利用惯性导航系统(inertialnavigationsystem，简称ins)传感器获取拖车系统的实时状态量。也可以利用gps传感器与ins传感器组合导航系统获取拖车系统的实时状态量。在拖车系统的牵引车以及每节拖车安装传感器，由此，可以获得拖车系统精确的实时状态，从而提高了拖车系统路径跟踪的准确性。

示例性地，可以利用同时定位与建图(simultaneouslylocalizationandmapping，简称slam)技术获取拖车系统的实时状态量。slam技术通过在自身定位的基础上同时构建增量式地图。利用拖车系统车载slam系统，可以获得拖车系统精确的实时状态，从而提高了拖车系统路径跟踪的准确性。

上述技术方案利用拖车系统的实时状态量，结合通过数学仿真得到的参考状态量和前馈控制量，控制拖车系统的牵引车的前轮偏转角度，从而实现了拖车系统的路径跟踪。可以适应不同款式的拖车，从而极大提高了拖车系统无人驾驶技术的普适性和实用性。

示例性地，可以根据拖车系统的第i节拖车和第i+1节拖车之间的连接关系，基于第i节拖车的运动状态确定第i+1节拖车的运动学模型。其中i＝0,1,2,…,m-1，m为拖车节数，第0节拖车为牵引车。可选地，拖车系统的第i节拖车和第i+1节拖车之间的连接关系可以包括第i+1节拖车的轴距和第i+1节拖车的拖杆长度等信息。

图2示出了根据本发明一个实施例的拖车系统模型的示意图。如图2所示，该拖车系统进行反向行驶路径跟踪。201为反向行驶路径跟踪的起点，202为目标路径，203为牵引车纵向车轴，204为牵引车拖钩，205为第1节拖车的拖杆，206为第1节拖车的纵向车轴，207为第1节拖车的拖杆，208为反向行驶路径跟踪的终点。δ0为牵引车的前轮偏角，l0为牵引车的轴距，θ0为牵引车的航向，x1为第1节拖车的全局横坐标，y1为第1节拖车的全局纵坐标，θ1为第1节拖车的航向，α1为第1节拖车的拖杆航向，l1为第1节拖车的轴距，c1为第1节拖车的拖杆长度。

示例性地，可以根据如下公式(1)确定第i+1节拖车的运动学模型：

其中，

其中，xi+1为第i+1节拖车的全局横坐标。yi+1为第i+1节拖车的全局纵坐标。θi+1为第i+1节拖车的航向，θi为第i节拖车的航向。如上所述，第0节拖车是牵引车。αi+1为第i+1节拖车的拖杆航向。vi+1为第i+1节拖车的速度，vi为第i节拖车的速度。li+1为第i+1节拖车的轴距，ci+1为第i+1节拖车的拖杆长度。θ0为牵引车的航向，δ0为牵引车的前轮偏角，v0为牵引车的速度，l0为牵引车的轴距。

利用公式(1)，根据第i节拖车的速度vi、第i节拖车的航向θi以及第i+1节拖车的拖杆航向αi+1、第i+1节拖车的航向θi+1、第i+1节拖车的轴距li+1、第i+1节拖车的拖杆长度ci+1可以确定第i+1节拖车的各项运动参数的变化率根据第i+1节拖车的各项运动参数的变化率可以求取拖车系统行驶过程中第i+1节拖车的各项运动参数，由此，确定了拖车系统的运动学模型。

为了便于计算处理，可以对上述运动学模型进行线性化和离散化处理，以得到拖车系统模型。基于线性化和离散化处理后的拖车系统模型，提高了计算效率，从而提高了拖车系统路径跟踪的效率。

示例性地，对上述运动学模型进行线性化和离散化处理，得到如公式(2)所示的拖车系统模型，

δxk+1＝aδxk+bδuk(2)

其中，k＝0,1,2,…,n，n为滑动窗口中目标路径点的个数。δxk＝[δxmkδymkδθmkδαmk…δx1kδy1kδθ1kδα1kδθ0k]^t为所述拖车系统在第k个目标路径点的预测误差。δθ0k为第k个目标路径点的牵引车的航向的预测误差，δxjk为第k个目标路径点的第j节拖车的全局横坐标的预测误差，δyjk为第k个目标路径点的第j节拖车的全局纵坐标的预测误差，δθjk为第k个目标路径点的第j节拖车的航向的预测误差，δαjk为第k个目标路径点的第j节拖车的拖杆航向的预测误差，j＝1,2,…,m，m为拖车节数。δμk为所述拖车系统在第k个目标路径点的反馈控制量，a为系统矩阵，b为控制矩阵。

如前所述，控制周期是对拖车系统施加控制的时间周期。上述滑动窗口中的n个目标路径点包含了n个控制周期。以当前路径点为第0个目标路径点，第1个目标路径点与第0个目标路径点之间在时间轴上相隔一个控制周期，第k个目标路径点与第k+1个目标路径点之间在时间轴上相隔一个控制周期。第1个～第n个目标路径点对应于第1个～第n个控制周期的时刻点，也叫做预测时刻点。由此，向前取了第1个～第n个目标路径点作为参考目标路径点来参与当前控制周期对拖车系统的控制处理。对于正向行驶路径跟踪，第1个～第n个目标路径点分别对应于当前路径点的下1个路径点～下n个路径点；对于反向行驶路径跟踪，第1个～第n个目标路径点分别对应于当前路径点的前1个路径点～前n个路径点。滑动窗口中目标路径点的个数n可以使用根据工程经验设定的默认值，也可以根据系统的计算能力和目标路径的复杂程度设置n的大小。示例性地，n可以设置为1～100间的值，优选地，n可以设置为30～50间的值。例如，当前路径点是路径点100，n＝30，对于正向行驶路径跟踪，第1个～第n个目标路径点分别对应于路径点101～路径点130；对于反向行驶路径跟踪，第1个～第n个目标路径点分别对应于路径点99～路径点70。

本文中“第k个目标路径点”指的是滑动窗口中的第k个目标路径点，“第n个目标路径点”指的是滑动窗口中的第n个目标路径点。不再特别说明。

以只有一辆拖车的单节双轴拖车系统为例，拖车系统状态量可以用x＝[x1y1θ1α1θ0]^t表示。x1为第1节拖车的全局横坐标，y1为第1节拖车的全局纵坐标，θ1为第1节拖车的航向，α1第1节拖车的拖杆航向，θ0为牵引车的航向。单节双轴拖车系统的运动学模型如下公式(3)：

对公式(3)进行线性化和离散化处理，得到如上述公式(2)所示的拖车系统模型。系统矩阵a和控制矩阵b如下公式(4)和公式(5)所示。其中，公式(5)中的t为控制周期。

上文以只有一节拖车的单节双轴拖车系统为例进行了描述，其拖车系统状态量x由第1节拖车的4个状态参数和牵引车的航向共5个元素组成，系统矩阵a为5×5的矩阵，控制矩阵b为5×1的矩阵。对于具有多节拖车的拖车系统，每增加一节拖车，拖车系统状态量x相应的增加该节拖车的4个状态参数。例如，具有2节拖车的拖车系统，其拖车系统状态量x增加第2节拖车的4个状态参数[x2y2θ2α2]，即x＝[x2y2θ2α2x1y1θ1α1θ0]^t，相应的系统矩阵a为9×9的矩阵，控制矩阵b为9×1的矩阵。具有m节拖车的拖车系统，其拖车系统状态量为xk＝[xmkymkθmkαmk…x1ky1kθ1kα1kθ0k]^t，共

m×4+1个元素，系统矩阵a为(m×4+1)×(m×4+1)的矩阵，控制矩阵b为(m×4+1)×1的矩阵。具有不同节数拖车的拖车系统的系统矩阵a和控制矩阵b可根据上述公式(1)进行线性化和离散化处理得到公式(2)的过程中得到，具体不再一一例举。

上述拖车系统的运动学模型，基于双轴拖车的车辆轴距、拖杆长度等拖车间连接关系参数采用拖车间相对独立的建模方式构建，提高了拖车节数的拓展性，并且可以适应不同款式的拖车，从而极大提高了拖车系统无人驾驶技术的实用性。

图3示出了根据本发明一个实施例的步骤s1300利用实时状态量基于参考状态量和前馈控制量控制拖车系统的牵引车的前轮偏转角度的示意性流程图。如图3所示，步骤s1300包括步骤s1310和步骤s1320。

步骤s1310，利用上述实时状态量基于步骤s1200确定的参考状态量和前馈控制量确定前轮期望偏角。

前轮期望偏角是基于路径跟踪对牵引车的前轮进行控制的期望偏转角度。拖车系统无人驾驶技术通过控制牵引车的前轮偏转角度来实现对拖车系统的航向控制。

步骤s1320，根据步骤s1310确定的前轮期望偏角控制该拖车系统的牵引车的前轮偏转角度。

如图2所示，通过控制拖车系统的牵引车的前轮偏转角度δ0，从而控制牵引车的航向θ0，继而拉动(正向行驶)或推动(反向行驶)拖车按控制的方向前进，由此实现拖车系统对目标路径的跟踪。在一个实施例中，可以通过牵引车转向电机控制器模块根据上述步骤确定的前轮期望偏角控制该拖车系统的牵引车的前轮偏转角度，使牵引车的偏转角度到达前轮期望偏角，从而实现拖车系统对目标路径的跟踪。通过上述方案，实现了对拖车系统的精确控制，提高了拖车系统对目标路径的跟踪的准确性。

图4示出了根据本发明一个实施例的步骤s1310利用实时状态量基于参考状态量和前馈控制量确定前轮期望偏角的示意性流程图。如图4所示，步骤s1310包括步骤s1311、步骤s1312、步骤s1313以及步骤s1314。

步骤s1311，利用上述实时状态量更新该拖车系统的系统矩阵a和控制矩阵b。

如前所述，在每个控制周期，获取当前控制周期的实时状态量。并把实时状态量的值代入系统矩阵a和控制矩阵b进行更新计算，从而得到当前控制周期的系统矩阵a和控制矩阵b。由此，保证了拖车系统模型的准确性，从而提高了拖车系统路径跟踪的精确性。

步骤s1312，利用步骤s1311更新的系统矩阵a和控制矩阵b并基于步骤s1200确定的参考状态量和前馈控制量构造目标函数。

根据本发明一个实施例，可以定义如以下公式(6)所示的评价函数。评估函数是用来评估研究对象整体以及局部性能的一种数学函数模型。公式(6)所示的评价函数用来评估拖车系统路径跟踪的准确性。评价函数j的值越小，表示拖车系统的路径跟踪越准确。

其中，q0为第一误差权重矩阵，q1为第二误差权重矩阵，r为控制量权重矩阵，xn为拖车系统在第n个目标路径点的预测状态量，rn为拖车系统在第n个目标路径点的参考状态量，xk为拖车系统在第k个目标路径点的预测状态量，rk为拖车系统在第k个目标路径点的参考状态量，uk为拖车系统在第k个目标路径点的前馈控制量，δμk为拖车系统在第k个目标路径点的反馈控制量。

引入拉格朗日乘子，基于公式(6)构造如以下公式(7)所示的无约束的最优化目标函数。

其中，λk+1为第k+1个目标路径点对应的拉格朗日乘子系数矩阵。目标函数h取得最小值时的δuk为反馈控制的最优控制序列。δxk和δxk+1分别为所述拖车系统在第k个和第k+1个目标路径点的预测误差。

该目标函数充分考虑了滑动窗口中各目标路径点的预测状态量与参考状态量之间的差距、前馈控制量与需要补充增加的反馈控制量以及预测误差等因素对拖车系统性能的影响，由此实现对拖车系统路径跟踪的最优控制，提高拖车系统路径跟踪的准确性。

步骤s1313，利用步骤s1312构造的目标函数优化求解反馈控制量。

示例性地，可以基于步骤s1312构造的目标函数根据第n个目标路径点的预测状态量通过线性二次型调节器(linearquadraticregulation，简称lqr)方法迭代，以得到反馈控制量。

为使目标函数h取得最小值，可以对其求偏导，根据终端状态向前迭代，也即假设滑动窗口的终端(第n个目标路径点)的预测状态量xn是准确的，预测误差δxn为零，由此通过lqr方法向前迭代，可以得到如下公式(8)所示的反馈控制的最优控制序列δμk。

δuk＝-kδxk-kuuk-kvvk+1(8)

其中，

k＝(b^tpk+1b+r)^-1b^tpk+1a；

ku＝(b^tpk+1b+r)^-1r；

kv＝(b^tpk+1b+r)^-1b^t；

pk＝a^tpk+1(a-bk)+q1；

vk＝(a-bk)vk+1-k^trkuk。

当前控制周期的反馈控制量即为第0个目标路径点的反馈控制量δμ0，如以下公式(9)所示。

δu0＝-kδx0-kuu0-kvv1(9)

步骤s1314，根据步骤s1200确定的前馈控制量和步骤s1313求解的反馈控制量确定该拖车系统当前控制周期的前轮期望偏角。

上述技术方案在仿真解算得到的前馈控制量的基础上，通过构建目标函数优化求解反馈控制量，综合前馈控制量和反馈控制量精确确定前轮期望偏角，从而可以在每个控制周期精确控制拖车系统的航向，提高路径跟踪的准确性。

图5示出了根据本发明一个实施例的根据前馈控制量和反馈控制量确定前轮期望偏角的示意性流程图。如图5所示，步骤s1314包括步骤s1314a、步骤s1314b以及步骤s1314c。

步骤s1314a，根据公式(10)确定拖车系统在第0个目标路径点的预测误差δx0。

δx0＝x0-r0(10)

其中，x0为拖车系统在当前控制周期的实时状态量，用作该拖车系统在第0个目标路径点的预测状态量，r0为该拖车系统在第0个目标路径点的参考状态量。

步骤s1314b，利用步骤s1314a确定的拖车系统在第0个目标路径点的预测误差δx0确定该拖车系统在第0个目标路径点的反馈控制量δu0。

利用上述公式(9)，根据步骤s1314a确定的拖车系统在第0个目标路径点的预测误差δx0可以求得第0个目标路径点的反馈控制量δμ0。

步骤s1314c，根据拖车系统在第0个目标路径点的前馈控制量u0和反馈控制量δu0确定该拖车系统在当前控制周期的前轮期望偏角。

该拖车系统在当前控制周期的前轮期望偏角δ0为前馈控制量u0和反馈控制量δu0的和，如以下公式(11)所示。

δ0＝u0+δu0(11)

综合利用拖车系统在第0个目标路径点的前馈控制量u0和反馈控制量δu0对该拖车系统在当前控制周期的前轮期望偏角进行控制，在基于纯跟踪预测的基础上加以lqr迭代优化控制，提高了拖车系统路径跟踪的准确性。

上述实施例以正向路径跟踪为例描述了拖车系统路径跟踪的过程步骤。正向路径跟踪表示以牵引车为车头带动拖车基于目标路径从起点向终点正向行驶。相对的，反向路径跟踪表示以牵引车为车尾推动拖车基于目标路径从终点向起点反向行驶。

在本发明一个实施例中，在每个控制周期，获取了拖车系统的实时状态量后，对该实时状态量中的速度值进行取负操作，以得到反向实时状态量。再利用反向实时状态量基于参考状态量和前馈控制量控制该拖车系统的牵引车的前轮偏转角度，以进行反向路径跟踪。

速度值取负表示相反的行进方向。反向路径跟踪与正向路径跟踪唯一的区别在于：在每个控制周期，获取了拖车系统的实时状态量后，需要对该实时状态量中的速度值进行取负操作，以得到反向实时状态量。此后以反向实时状态量为上文中的实时状态量进行后续控制步骤即可。具体处理方法和过程都类似，在此不再赘述。

由此，通过增加简单的取负处理，即可实现拖车系统的反向路径跟踪，提高了拖车系统的适应性。

图6a示出了根据本发明另一个实施例的拖车系统路径跟踪方法600a的示意性流程图。如图6所示，方法600a包括如下步骤：

步骤s601a，拖车系统启动，准备开始路径跟踪。

步骤s602a，获取目标路径点的期望位姿。

步骤s603a，基于拖车系统的拖车系统模型根据步骤s602a获取的期望位姿仿真解算该拖车系统在目标路径点的状态量和需要施加的控制量，分别作为该拖车系统在该目标路径点的参考状态量和前馈控制量。

在每个控制周期，执行以下步骤s604a～步骤s611a，直至到达终点。

步骤s604a，获取该拖车系统实时状态量。

步骤s605a，判断该拖车系统是正向路径跟踪还是反向路径跟踪。对于正向路径跟踪，不需要任何处理。

步骤s606a，对于反向路径跟踪的情况，需要对该实时状态量中的速度值进行取负操作，以得到反向实时状态量。此后以反向实时状态量为实时状态量执行后续控制步骤。

步骤s607a，利用上述实时状态量更新系统矩阵a和控制矩阵b。

步骤s608a，利用目标函数优化求解反馈控制量。

步骤s609a，根据步骤s603a解算的前馈控制量和步骤s607a求解的反馈控制量确定该拖车系统的牵引车的前轮期望偏角。

步骤s610a，根据步骤s608a确定的前轮期望偏角控制该拖车系统的牵引车的前轮偏转角度。由此，通过牵引车拉动(正向行驶)或推动(反向行驶)拖车按控制的方向前进。

步骤s611a，判断是否到达终点？如果没有到达终点，在下一个控制周期，回到步骤s604a循环执行路径跟踪控制步骤。从而通过牵引车拉动(正向行驶)或推动(反向行驶)拖车按控制的方向持续前进，直至到达终点，从而实现拖车系统对目标路径的跟踪。

步骤s612a，到达终点，完成拖车系统对目标路径的跟踪。

图6b示出了根据本发明又一个实施例的拖车系统路径跟踪方法600b的示意性流程图。如图6所示，方法600b包括步骤s601b～步骤s612b。其中，步骤s601b～步骤s610b、步骤s612b与方法600a中的步骤s601a～步骤s610a、步骤s612a实现的功能、处理过程和方法类似，在此不再赘述。

步骤s611b，判断是否到达终点？如果没有到达终点，在下一个控制周期，回到步骤s602b循环执行路径跟踪控制步骤。从而通过牵引车拉动(正向行驶)或推动(反向行驶)拖车按控制的方向持续前进，直至到达终点，从而实现拖车系统对目标路径的跟踪。

上述方法600a中步骤s602a可以先获取完整的目标路径的每个目标路径点的期望位姿，步骤s603a可以针对所有目标路径点的期望位姿仿真解算拖车系统在每个目标路径点的参考状态量和前馈控制量。此后在每个控制周期，基于滑动窗口循环执行步骤s604a～步骤s611a控制拖车系统按预期方向持续前进，直至到达终点。例如，对于包括10000个目标路径点的目标路径，步骤s602a可以获取10000个目标路径点的期望位姿，步骤s603a可以仿真解算拖车系统在这10000个目标路径点的参考状态量和前馈控制量。由此，提前完成整个目标路径的仿真解算，可以减小此后的循环控制步骤的计算量，提高拖车系统路径跟踪的处理效率和性能。

上述方法600b中步骤s602b仅获取滑动窗口中的目标路径点的期望位姿，步骤s603b仅仿真解算拖车系统在滑动窗口中的目标路径点的参考状态量和前馈控制量。此后在每个控制周期，基于滑动窗口循环执行步骤s602a～步骤s611a控制拖车系统按预期方向持续前进，直至到达终点。例如，对于包括10000个目标路径点的目标路径，滑动窗口中目标路径点的个数n为40。初始控制周期，滑动窗口为目标路径点1～40，步骤s602b获取目标路径点1～40的期望位姿，步骤s603b仿真解算拖车系统在目标路径点1～40的参考状态量和前馈控制量。下一个控制周期，滑动窗口为目标路径点2～41，步骤s602b获取目标路径点2～41b的期望位姿，其中需要新获取目标路径点41的期望位姿；步骤s603b仿真解算拖车系统在目标路径点2～41的参考状态量和前馈控制量，其中需要新解算目标路径点41的参考状态量和前馈控制量。如此一步步往前推进，控制拖车系统跟踪目标路径。可以理解，方法600b不需要针对整个目标路径仿真解算拖车系统在每个目标路径点的参考状态量和前馈控制量，甚至不需要关心整个目标路径有多大，因此不需要由此带来的额外内存需求，同时提升了拖车系统路径跟踪的实用性。

根据本发明另一个实施例，还提供了一种用于拖车系统路径跟踪的装置。图7示出了根据本发明一个实施例的用于拖车系统路径跟踪的装置的示意性框图。如图7所示，用于视觉定位地图加载的装置7000包括获取模块7100、参考模块7200以及跟踪模块7300。

获取模块7100，用于获取目标路径点的期望位姿。

参考模块7200，用于基于拖车系统的拖车系统模型根据所述期望位姿确定所述拖车系统在目标路径点的参考状态量和前馈控制量。

跟踪模块7300，用于在每个控制周期，获取所述拖车系统的实时状态量，并利用所述实时状态量基于所述参考状态量和所述前馈控制量控制所述拖车系统的牵引车的前轮偏转角度。

总之，用于拖车系统路径跟踪的装置7000中的各个模块用于具体执行上述拖车系统路径跟踪方法中的相应步骤。通过阅读上述关于该方法的描述，本领域普通技术人员可以理解上述用于拖车系统路径跟踪的装置7000的具体实现和技术效果。

根据本发明又一方面，还提供了一种用于拖车系统路径跟踪的系统。包括处理器和存储器，其中，所述存储器中存储用于实现根据本发明实施例的拖车系统路径跟踪方法中的各个步骤的计算机程序指令。所述处理器用于运行所述存储器中存储的计算机程序指令，以执行根据本发明实施例的拖车系统路径跟踪方法的相应步骤，并且用于实现根据本发明实施例的用于拖车系统路径跟踪的装置中的获取模块7100、参考模块7200以及跟踪模块7300。

根据本发明再一方面，还提供了一种存储介质，在所述存储介质上存储了程序指令，在所述程序指令被计算机或处理器运行时使得所述计算机或处理器执行本发明实施例的拖车系统路径跟踪方法的相应步骤，并且用于实现根据本发明实施例的用于拖车系统路径跟踪的装置中的相应模块。所述存储介质例如可以包括平板电脑的存储部件、个人计算机的硬盘、只读存储器(rom)、可擦除可编程只读存储器(eprom)、便携式紧致盘只读存储器(cd-rom)、usb存储器、或者上述存储介质的任意组合。所述计算机可读存储介质可以是一个或多个计算机可读存储介质的任意组合。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

在本申请所提供的几个实施例中，应该理解到，所揭露的设备和方法，可以通过其它的方式实现。例如，以上所描述的设备实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个设备，或一些特征可以忽略，或不执行。

在此处所提供的说明书中，说明了大量具体细节。然而，能够理解，本发明的实施例可以在没有这些具体细节的情况下实践。在一些实例中，并未详细示出公知的方法、结构和技术，以便不模糊对本说明书的理解。

类似地，应当理解，为了精简本发明并帮助理解各个发明方面中的一个或多个，在对本发明的示例性实施例的描述中，本发明的各个特征有时被一起分组到单个实施例、图、或者对其的描述中。然而，并不应将该本发明的方法解释成反映如下意图：即所要求保护的本发明要求比在每个权利要求中所明确记载的特征更多的特征。更确切地说，如相应的权利要求书所反映的那样，其发明点在于可以用少于某个公开的单个实施例的所有特征的特征来解决相应的技术问题。因此，遵循具体实施方式的权利要求书由此明确地并入该具体实施方式，其中每个权利要求本身都作为本发明的单独实施例。

本领域的技术人员可以理解，除了特征之间相互排斥之外，可以采用任何组合对本说明书(包括伴随的权利要求、摘要和附图)中公开的所有特征以及如此公开的任何方法或者设备的所有过程或单元进行组合。除非另外明确陈述，本说明书(包括伴随的权利要求、摘要和附图)中公开的每个特征可以由提供相同、等同或相似目的的替代特征来代替。

此外，本领域的技术人员能够理解，尽管在此所述的一些实施例包括其它实施例中所包括的某些特征而不是其它特征，但是不同实施例的特征的组合意味着处于本发明的范围之内并且形成不同的实施例。例如，在权利要求书中，所要求保护的实施例的任意之一都可以以任意的组合方式来使用。

本发明的各个部件实施例可以以硬件实现，或者以在一个或者多个处理器上运行的软件模块实现，或者以它们的组合实现。本领域的技术人员应当理解，可以在实践中使用微处理器或者数字信号处理器(dsp)来实现根据本发明实施例的视觉定位地图加载装置中的一些模块的一些或者全部功能。本发明还可以实现为用于执行这里所描述的方法的一部分或者全部的装置程序(例如，计算机程序和计算机程序产品)。这样的实现本发明的程序可以存储在计算机可读介质上，或者可以具有一个或者多个信号的形式。这样的信号可以从因特网网站上下载得到，或者在载体信号上提供，或者以任何其他形式提供。

应该注意的是上述实施例对本发明进行说明而不是对本发明进行限制，并且本领域技术人员在不脱离所附权利要求的范围的情况下可设计出替换实施例。在权利要求中，不应将位于括号之间的任何参考符号构造成对权利要求的限制。单词“包含”不排除存在未列在权利要求中的元件或步骤。位于元件之前的单词“一”或“一个”不排除存在多个这样的元件。本发明可以借助于包括有若干不同元件的硬件以及借助于适当编程的计算机来实现。在列举了若干装置的单元权利要求中，这些装置中的若干个可以是通过同一个硬件项来具体体现。单词第一、第二、以及第三等的使用不表示任何顺序。可将这些单词解释为名称。

以上所述，仅为本发明的具体实施方式或对具体实施方式的说明，本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。本发明的保护范围应以权利要求的保护范围为准。