CN109407517B - 一种双轴拖车系统路径跟踪方法、装置、系统及存储介质 - Google Patents

一种双轴拖车系统路径跟踪方法、装置、系统及存储介质 Download PDF

Info

Publication number
CN109407517B
CN109407517B CN201811557457.7A CN201811557457A CN109407517B CN 109407517 B CN109407517 B CN 109407517B CN 201811557457 A CN201811557457 A CN 201811557457A CN 109407517 B CN109407517 B CN 109407517B
Authority
CN
China
Prior art keywords
trailer system
trailer
state quantity
double
shaft
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201811557457.7A
Other languages
English (en)
Other versions
CN109407517A (zh
Inventor
赵世杰
田晓生
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Uisee Technologies Beijing Co Ltd
Original Assignee
Uisee Technologies Beijing Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Uisee Technologies Beijing Co Ltd filed Critical Uisee Technologies Beijing Co Ltd
Priority to CN201811557457.7A priority Critical patent/CN109407517B/zh
Publication of CN109407517A publication Critical patent/CN109407517A/zh
Application granted granted Critical
Publication of CN109407517B publication Critical patent/CN109407517B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/048—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators using a predictor
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0219—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory ensuring the processing of the whole working surface

Abstract

本发明提供了一种双轴拖车系统路径跟踪方法、装置、系统及存储介质。该方法包括：获取目标路径点的期望位姿；基于双轴拖车系统的双轴拖车系统模型根据所述期望位姿确定所述双轴拖车系统在目标路径点的参考状态量和前馈控制量；在每个控制周期，获取所述双轴拖车系统的实时状态量，并利用所述实时状态量基于所述参考状态量和所述前馈控制量控制所述双轴拖车系统的牵引车的前轮偏转角度。上述方案可以适应不同款式的拖车，从而极大提高了双轴拖车系统无人驾驶技术的普适性和实用性。

Description

一种双轴拖车系统路径跟踪方法、装置、系统及存储介质
技术领域
本发明涉及车辆自动控制领域，更具体地涉及一种双轴拖车系统路径跟踪方法、装置、系统及存储介质。
背景技术
随着车辆智能化技术的发展，无人驾驶载物技术逐渐成为车辆自动控制领域的一个热点。带有若干节拖车的货物运输车由于具有拖车数量可任意改变的特点，在港口、仓库和机场等场合被大量应用。基于无人驾驶技术的双轴拖车系统不仅能够降低机场、港口的人员消耗，更能够保证双轴拖车系统运行过程中的安全。
双轴拖车系统无人驾驶技术需要拖车能够对目标路径进行路径跟踪。当前基于最优控制路径跟踪的技术一般应用于单轴、单节拖车，适用范围较窄。
因此，迫切需要一种新的双轴拖车系统路径跟踪技术，以解决上述问题。
发明内容
考虑到上述问题而提出了本发明。本发明提供了一种双轴拖车系统路径跟踪方法、装置、系统及存储介质。
本发明提供了一种双轴拖车系统路径跟踪方法，包括：
获取目标路径点的期望位姿；
基于双轴拖车系统的双轴拖车系统模型根据所述期望位姿确定所述双轴拖车系统在目标路径点的参考状态量和前馈控制量；
在每个控制周期，获取所述双轴拖车系统的实时状态量，并利用所述实时状态量基于所述参考状态量和所述前馈控制量控制所述双轴拖车系统的牵引车的前轮偏转角度。
示例性地，所述双轴拖车系统模型通过以下方式构建：
根据所述双轴拖车系统的第i节拖车和第i+1节拖车之间的连接关系，基于所述第i节拖车的运动状态确定所述第i+1节拖车的运动学模型，其中i＝0，1，2，…，m-1，m为拖车节数，第0节拖车为牵引车；
对所述运动学模型进行线性化和离散化处理，以得到所述双轴拖车系统模型。
示例性地，所述基于所述第i节拖车的运动状态确定所述第i+1节拖车的运动学模型包括：
根据如下公式(1)确定所述第i+1节拖车的运动学模型：
Figure GDA0003214556680000021
其中，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)构造所述目标函数，
Figure GDA0003214556680000031
其中，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获取的期望位姿利用纯跟踪(Pure Pursuit)的方式确定参考状态量和前馈控制量。
纯跟踪是一种广泛使用的基于几何跟踪的技术。基于上述双轴拖车系统模型，让双轴拖车系统按照纯跟踪的方式跟踪目标路径的期望位姿，记录双轴拖车系统在滑动窗口中每个目标路径点的状态量和得到的控制量，作为对应时刻该双轴拖车系统的参考状态量和前馈控制量。参考状态量表示基于纯跟踪预测得到的该双轴拖车系统到达滑动窗口中每个目标路径点的状态。具体地，参考状态量可以包括牵引车的航向θ0和每节拖车的位置(xiyi)、拖车航向θi以及拖杆航向αi，其中，i＝1,2,…,m，m为拖车节数，i为拖车序号。对于滑动窗口中的第k个目标路径点，参考状态量可以表示为Rk＝[xmk ymk θmk αmk … x1k y1kθ1k α1k θ0k]T。其中，k＝0表示滑动窗口中的第0个目标路径点，也即当前目标路径点。
由此，利用纯跟踪技术进行路径跟踪仿真预测，给双轴拖车系统路径跟踪提供了仿真结果作为路径跟踪参考，提高了双轴拖车系统路径跟踪的准确性。
步骤S1300，在每个控制周期，获取双轴拖车系统的实时状态量，并利用该实时状态量基于步骤S1200确定的参考状态量和前馈控制量控制该双轴拖车系统的牵引车的前轮偏转角度。
控制周期是对双轴拖车系统施加控制的时间周期。双轴拖车系统的实时状态量，用以表示该双轴拖车系统当前所处位置以及航向等信息。具体地，双轴拖车系统的实时状态量可以包括牵引车的航向和每节拖车的位置、拖车航向以及拖杆航向等。可以利用任何现有的或未来开发的技术获取双轴拖车系统的实时状态量。
示例性地，可以利用传感器定位获取双轴拖车系统的实时状态量。例如，可以利用全球定位系统(Global Positioning System，简称GPS)传感器获取双轴拖车系统的实时状态量。又例如，可以利用惯性导航系统(Inertial Navigation System，简称INS)传感器获取双轴拖车系统的实时状态量。也可以利用GPS传感器与INS传感器组合导航系统获取双轴拖车系统的实时状态量。在双轴拖车系统的牵引车以及每节拖车安装传感器，由此，可以获得双轴拖车系统精确的实时状态，从而提高了双轴拖车系统路径跟踪的准确性。
示例性地，可以利用同时定位与建图(Simultaneously Localization andMapping，简称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为牵引车的轴距，00为牵引车的航向，x1为第1节拖车的全局横坐标，y1为第1节拖车的全局纵坐标，θ1为第1节拖车的航向，α1为第1节拖车的拖杆航向，L1为第1节拖车的轴距，c1为第1节拖车的拖杆长度。
示例性地，可以根据如下公式(1)确定第i+1节拖车的运动学模型：
Figure GDA0003214556680000081
其中，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节拖车的各项运动参数的变化率
Figure GDA0003214556680000091
Figure GDA0003214556680000092
根据第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＝[x1 y1 θ1 α1 θ0]T表示。x1为第1节拖车的全局横坐标，y1为第1节拖车的全局纵坐标，θ1为第1节拖车的航向，α1第1节拖车的拖杆航向，θ0为牵引车的航向。单节双轴双轴拖车系统的运动学模型如下公式(3)：
Figure GDA0003214556680000101
对公式(3)进行线性化和离散化处理，得到如上述公式(2)所示的双轴拖车系统模型。系统矩阵A和控制矩阵B如下公式(4)和公式(5)所示。其中，公式(5)中的T为控制周期。
Figure GDA0003214556680000102
Figure GDA0003214556680000103
上文以只有一节拖车的单节双轴双轴拖车系统为例进行了描述，其双轴拖车系统状态量X由第1节拖车的4个状态参数和牵引车的航向共5个元素组成，系统矩阵A为5×5的矩阵，控制矩阵B为5×1的矩阵。对于具有多节拖车的双轴拖车系统，每增加一节拖车，双轴拖车系统状态量X相应的增加该节拖车的4个状态参数。例如，具有2节拖车的双轴拖车系统，其双轴拖车系统状态量X增加第2节拖车的4个状态参数[x2 y2 θ2 α2]，即X＝[x2 y2 θ2α2 x1 y1 θ1 α1 θ0]T，相应的系统矩阵A为9×9的矩阵，控制矩阵B为9×1的矩阵。具有m节拖车的双轴拖车系统，其双轴拖车系统状态量为Xk＝[xmk ymk θmk αmk … x1k y1k θ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以及步骤S 1314。
步骤S1311，利用上述实时状态量更新该双轴拖车系统的系统矩阵A和控制矩阵B。
如前所述，在每个控制周期，获取当前控制周期的实时状态量。并把实时状态量的值代入系统矩阵A和控制矩阵B进行更新计算，从而得到当前控制周期的系统矩阵A和控制矩阵B。由此，保证了双轴拖车系统模型的准确性，从而提高了双轴拖车系统路径跟踪的精确性。
步骤S1312，利用步骤S1311更新的系统矩阵A和控制矩阵B并基于步骤S1200确定的参考状态量和前馈控制量构造目标函数。
根据本发明一个实施例，可以定义如以下公式(6)所示的评价函数。评估函数是用来评估研究对象整体以及局部性能的一种数学函数模型。公式(6)所示的评价函数用来评估双轴拖车系统路径跟踪的准确性。评价函数J的值越小，表示双轴拖车系统的路径跟踪越准确。
Figure GDA0003214556680000121
其中，Q0为第一误差权重矩阵，Q1为第二误差权重矩阵，R为控制量权重矩阵，XN为双轴拖车系统在第N个目标路径点的预测状态量，RN为双轴拖车系统在第N个目标路径点的参考状态量，Xk为双轴拖车系统在第k个目标路径点的预测状态量，Rk为双轴拖车系统在第k个目标路径点的参考状态量，uk为双轴拖车系统在第k个目标路径点的前馈控制量，Δμk为双轴拖车系统在第k个目标路径点的反馈控制量。
引入拉格朗日乘子，基于公式(6)构造如以下公式(7)所示的无约束的最优化目标函数。
Figure GDA0003214556680000131
其中，λk+1为第k+1个目标路径点对应的拉格朗日乘子系数矩阵。目标函数H取得最小值时的Δuk为反馈控制的最优控制序列。ΔXk和ΔXk+1分别为所述双轴拖车系统在第k个和第k+1个目标路径点的预测误差。
该目标函数充分考虑了滑动窗口中各目标路径点的预测状态量与参考状态量之间的差距、前馈控制量与需要补充增加的反馈控制量以及预测误差等因素对双轴拖车系统性能的影响，由此实现对双轴拖车系统路径跟踪的最优控制，提高双轴拖车系统路径跟踪的准确性。
步骤S1313，利用步骤S1312构造的目标函数优化求解反馈控制量。
示例性地，可以基于步骤S1312构造的目标函数根据第N个目标路径点的预测状态量通过线性二次型调节器(Linear Quadratic Regulation，简称LQR)方法迭代，以得到反馈控制量。
为使目标函数H取得最小值，可以对其求偏导，
Figure GDA0003214556680000132
Figure GDA0003214556680000133
根据终端状态向前迭代，也即假设滑动窗口的终端(第N个目标路径点)的预测状态量XN是准确的，预测误差ΔXN为零，由此通过LQR方法向前迭代，可以得到如下公式(8)所示的反馈控制的最优控制序列Δμk。
Δuk＝-KΔXk-Kuuk-Kvvk+1 (8)
其中，
K＝(BTPk+1B+R)-1BTPk+1A；
Ku＝(BTPk+1B+R)-1R；
Kv＝(BTPk+1B+R)-1BT；
Pk＝ATPk+1(A-BK)+Q1；
vk＝(A-BK)vk+1-KTRkuk。
当前控制周期的反馈控制量即为第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)来实现根据本发明实施例的视觉定位地图加载装置中的一些模块的一些或者全部功能。本发明还可以实现为用于执行这里所描述的方法的一部分或者全部的装置程序(例如，计算机程序和计算机程序产品)。这样的实现本发明的程序可以存储在计算机可读介质上，或者可以具有一个或者多个信号的形式。这样的信号可以从因特网网站上下载得到，或者在载体信号上提供，或者以任何其他形式提供。
应该注意的是上述实施例对本发明进行说明而不是对本发明进行限制，并且本领域技术人员在不脱离所附权利要求的范围的情况下可设计出替换实施例。在权利要求中，不应将位于括号之间的任何参考符号构造成对权利要求的限制。单词“包含”不排除存在未列在权利要求中的元件或步骤。位于元件之前的单词“一”或“一个”不排除存在多个这样的元件。本发明可以借助于包括有若干不同元件的硬件以及借助于适当编程的计算机来实现。在列举了若干装置的单元权利要求中，这些装置中的若干个可以是通过同一个硬件项来具体体现。单词第一、第二、以及第三等的使用不表示任何顺序。可将这些单词解释为名称。
以上所述，仅为本发明的具体实施方式或对具体实施方式的说明，本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。本发明的保护范围应以权利要求的保护范围为准。

Claims (13)

1.一种双轴拖车系统路径跟踪方法，包括：
获取目标路径点的期望位姿；
基于双轴拖车系统的双轴拖车系统模型根据所述期望位姿确定所述双轴拖车系统在目标路径点的参考状态量和前馈控制量；
在每个控制周期，获取所述双轴拖车系统的实时状态量，并利用所述实时状态量基于所述参考状态量和所述前馈控制量控制所述双轴拖车系统的牵引车的前轮偏转角度，包括利用所述实时状态量基于所述参考状态量和所述前馈控制量确定前轮期望偏角，根据所述前轮期望偏角控制所述双轴拖车系统的牵引车的前轮偏转角度；其中，所述利用所述实时状态量基于所述参考状态量和所述前馈控制量确定前轮期望偏角包括：
利用所述实时状态量更新所述系统矩阵A和所述控制矩阵B；
利用所述系统矩阵A和所述控制矩阵B并基于所述参考状态量和所述前馈控制量构造目标函数，根据公式(7)所述目标函数为，
Figure FDA0003314040210000011
其中，Q0为第一误差权重矩阵，Q1为第二误差权重矩阵，R为控制量权重矩阵，XN为所述拖车系统在第N个目标路径点的预测状态量，RN为所述拖车系统在第N个目标路径点的参考状态量，Xk为所述拖车系统在第k个目标路径点的预测状态量，Rk为所述拖车系统在第k个目标路径点的参考状态量，uk为所述拖车系统在第k个目标路径点的前馈控制量，λk为第k个目标路径点对应的拉格朗日乘子系数矩阵。
2.如权利要求1所述的方法，其中，所述双轴拖车系统模型通过以下方式构建：
根据所述双轴拖车系统的第i节拖车和第i+1节拖车之间的连接关系，基于所述第i节拖车的运动状态确定所述第i+1节拖车的运动学模型，其中i＝0，1，2，…，m-1，m为拖车节数，第0节拖车为牵引车；
对所述运动学模型进行线性化和离散化处理，以得到所述双轴拖车系统模型。
3.如权利要求2所述的方法，其中，所述基于所述第i节拖车的运动状态确定所述第i+1节拖车的运动学模型包括：
根据如下公式(1)确定所述第i+1节拖车的运动学模型：
Figure FDA0003314040210000021
其中，
Figure FDA0003314040210000022
其中，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为牵引车的轴距。
4.如权利要求2或3所述的方法，其中，所述对所述运动学模型进行线性化和离散化处理，以得到所述双轴拖车系统模型包括：
对所述运动学模型进行线性化和离散化处理，得到如公式(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为控制矩阵。
5.如权利要求1所述的方法，其中，所述利用所述实时状态量基于所述参考状态量和所述前馈控制量确定前轮期望偏角还包括：
利用所述目标函数优化求解反馈控制量；
根据所述前馈控制量和所述反馈控制量确定所述前轮期望偏角。
6.如权利要求5所述的方法，其中，所述利用所述目标函数优化求解反馈控制量包括：
基于所述目标函数根据第N个目标路径点的预测状态量通过线性二次型调节器方法迭代，以得到所述反馈控制量。
7.如权利要求5或6所述的方法，其中，所述根据所述前馈控制量和所述反馈控制量确定所述前轮期望偏角包括：
根据公式(10)确定所述双轴拖车系统在第0个目标路径点的预测误差ΔX0，
ΔX0＝X0-R0 (10)
其中，X0为所述实时状态量，用作所述双轴拖车系统在第0个目标路径点的预测状态量，R0为所述双轴拖车系统在第0个目标路径点的参考状态量；
利用所述双轴拖车系统在第0个目标路径点的预测误差ΔX0确定所述双轴拖车系统在第0个目标路径点的反馈控制量Δu0；
根据所述双轴拖车系统在第0个目标路径点的前馈控制量u0和反馈控制量Δu0确定所述前轮期望偏角。
8.如权利要求1至3任一项所述的方法，其中，所述方法还包括：
在每个控制周期，获取所述双轴拖车系统的实时状态量后，对所述实时状态量中的速度值进行取负操作，以得到反向实时状态量；
利用所述反向实时状态量基于所述参考状态量和所述前馈控制量控制所述双轴拖车系统的牵引车的前轮偏转角度，以进行反向路径跟踪。
9.如权利要求1至3任一项所述的方法，其中，所述基于双轴拖车系统的双轴拖车系统模型根据所述期望位姿确定所述双轴拖车系统在目标路径点的参考状态量和前馈控制量包括：
基于所述双轴拖车系统模型根据所述期望位姿利用纯跟踪的方式确定所述参考状态量和所述前馈控制量。
10.如权利要求1至3任一项所述的方法，其中，所述获取所述双轴拖车系统的实时状态量包括：
利用传感器定位获取所述双轴拖车系统的实时状态量；或
利用同时定位与建图技术获取所述双轴拖车系统的实时状态量。
11.一种用于双轴拖车系统路径跟踪的装置，包括：
获取模块，用于获取目标路径点的期望位姿；
参考模块，用于基于双轴拖车系统的双轴拖车系统模型根据所述期望位姿确定所述双轴拖车系统在目标路径点的参考状态量和前馈控制量；
跟踪模块，用于在每个控制周期，获取所述双轴拖车系统的实时状态量，并利用所述实时状态量基于所述参考状态量和所述前馈控制量控制所述双轴拖车系统的牵引车的前轮偏转角度包括利用所述实时状态量基于所述参考状态量和所述前馈控制量确定前轮期望偏角，根据所述前轮期望偏角控制所述双轴拖车系统的牵引车的前轮偏转角度；其中，所述利用所述实时状态量基于所述参考状态量和所述前馈控制量确定前轮期望偏角包括：
利用所述实时状态量更新所述系统矩阵A和所述控制矩阵B；
利用所述系统矩阵A和所述控制矩阵B并基于所述参考状态量和所述前馈控制量构造目标函数，根据公式(7)所述目标函数为，
Figure FDA0003314040210000041
其中，Q0为第一误差权重矩阵，Q1为第二误差权重矩阵，R为控制量权重矩阵，XN为所述拖车系统在第N个目标路径点的预测状态量，RN为所述拖车系统在第N个目标路径点的参考状态量，Xk为所述拖车系统在第k个目标路径点的预测状态量，Rk为所述拖车系统在第k个目标路径点的参考状态量，uk为所述拖车系统在第k个目标路径点的前馈控制量，λk为第k个目标路径点对应的拉格朗日乘子系数矩阵。
12.一种用于双轴拖车系统路径跟踪的系统，包括处理器和存储器，其中，所述存储器中存储有计算机程序指令，所述计算机程序指令被所述处理器运行时用于执行如权利要求1至10任一项所述的用于双轴拖车系统路径跟踪的方法。
13.一种存储介质，在所述存储介质上存储了程序指令，所述程序指令在运行时用于执行如权利要求1至10任一项所述的用于双轴拖车系统路径跟踪的方法。
CN201811557457.7A 2018-12-19 2018-12-19 一种双轴拖车系统路径跟踪方法、装置、系统及存储介质 Active CN109407517B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811557457.7A CN109407517B (zh) 2018-12-19 2018-12-19 一种双轴拖车系统路径跟踪方法、装置、系统及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811557457.7A CN109407517B (zh) 2018-12-19 2018-12-19 一种双轴拖车系统路径跟踪方法、装置、系统及存储介质

Publications (2)

Publication Number Publication Date
CN109407517A CN109407517A (zh) 2019-03-01
CN109407517B true CN109407517B (zh) 2021-12-14

Family

ID=65460171

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811557457.7A Active CN109407517B (zh) 2018-12-19 2018-12-19 一种双轴拖车系统路径跟踪方法、装置、系统及存储介质

Country Status (1)

Country Link
CN (1) CN109407517B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110008514A (zh) * 2019-03-06 2019-07-12 深兰科技（上海）有限公司 一种进行线性化处理的方法和设备
CN111038477B (zh) * 2019-11-29 2021-07-02 苏州智加科技有限公司 车辆控制方法、装置和设备
CN114590333B (zh) * 2020-12-03 2023-07-14 宇通客车股份有限公司 一种自动驾驶多节拖车及其位姿确定方法
CN113641159B (zh) * 2021-08-16 2024-04-12 佛山市毕佳索智能科技有限公司 一种基于agv的拖车控制方法
CN114707103B (zh) * 2022-06-06 2022-10-18 民航成都物流技术有限公司 一种基于单一感知数据的多拖车行驶轨迹计算方法

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101093396A (zh) * 2007-07-04 2007-12-26 华南农业大学 一种农业机械的导航控制方法
CN103941737A (zh) * 2014-05-09 2014-07-23 济南大学 一种复杂环境下拖挂式移动机器人的运动规划与控制方法
CN107561942A (zh) * 2017-09-12 2018-01-09 重庆邮电大学 基于模型补偿的智能车辆轨迹跟踪模型预测控制方法
CN108334086A (zh) * 2018-01-25 2018-07-27 江苏大学 一种基于软约束二次规划mpc的无人驾驶车辆路径跟踪控制方法
CN108710294A (zh) * 2018-04-13 2018-10-26 四川木牛流马智能科技有限公司 一种无人车路径跟踪前馈控制方法
CN108871338A (zh) * 2018-06-25 2018-11-23 驭势科技（北京）有限公司 一种拖车系统位姿预测方法、装置及存储介质
CN108958246A (zh) * 2018-06-29 2018-12-07 长安大学 无人车在U-Turn场景的轨迹跟踪控制方法
CN108973769A (zh) * 2018-06-15 2018-12-11 吉林大学 一种全线控电动汽车路径跟踪控制方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101093396A (zh) * 2007-07-04 2007-12-26 华南农业大学 一种农业机械的导航控制方法
CN103941737A (zh) * 2014-05-09 2014-07-23 济南大学 一种复杂环境下拖挂式移动机器人的运动规划与控制方法
CN107561942A (zh) * 2017-09-12 2018-01-09 重庆邮电大学 基于模型补偿的智能车辆轨迹跟踪模型预测控制方法
CN108334086A (zh) * 2018-01-25 2018-07-27 江苏大学 一种基于软约束二次规划mpc的无人驾驶车辆路径跟踪控制方法
CN108710294A (zh) * 2018-04-13 2018-10-26 四川木牛流马智能科技有限公司 一种无人车路径跟踪前馈控制方法
CN108973769A (zh) * 2018-06-15 2018-12-11 吉林大学 一种全线控电动汽车路径跟踪控制方法
CN108871338A (zh) * 2018-06-25 2018-11-23 驭势科技（北京）有限公司 一种拖车系统位姿预测方法、装置及存储介质
CN108958246A (zh) * 2018-06-29 2018-12-07 长安大学 无人车在U-Turn场景的轨迹跟踪控制方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
带两节拖车拖挂式移动机器人控制策略研究;秦超;《中国优秀硕士学位论文全文数据库 信息科技辑》;20130415;第1-2章 *

Also Published As

Publication number Publication date
CN109407517A (zh) 2019-03-01

Similar Documents

Publication Publication Date Title
CN109407517B (zh) 一种双轴拖车系统路径跟踪方法、装置、系统及存储介质
CN110155031A (zh) 使用神经网络的用于车辆横向控制的轨迹跟踪
CN110816528A (zh) 用于自主系统中嵌入式实时受约束最优控制问题的求解器
CN108871338B (zh) 一种拖车系统位姿预测方法、装置及存储介质
Caporale et al. Towards the design of robotic drivers for full-scale self-driving racing cars
US11279372B2 (en) System and method for controlling a vehicle having an autonomous mode and a semi-autonomous mode
Hu et al. Path planning of UGV based on Bézier curves
Hartono et al. Motion and Navigation Control System of a Mobile Robot as A Prototype of An Autonomous Vehicle
CN211427151U (zh) 一种应用于封闭场地无人驾驶货运车辆上的自动引导系统
Helmick et al. Terrain adaptive navigation for Mars rovers
Ventura Safe and flexible hybrid control architecture for the navigation in formation of a group of vehicles
US20210158059A1 (en) Systems and methods for determining the direction of an object in an image
Sun A Comparative Study on the Monte Carlo Localization and the Odometry Localization
Wang et al. Accurate Perception for Autonomous Driving: Application of Kalman Filter for Sensor Fusion
Gelbal et al. Design and HIL setup of an autonomous vehicle for crowded environments
Duprey et al. Simulation of the performance based standards (PBS) low-speed 90 turn test in TruckSim by jumping back in time
Szͩcs et al. Experimental verification of a control system for autonomous navigation
Wang et al. Hinged Sweeper Kinematic Modeling and Path Tracking Control
US20240101153A1 (en) Systems and methods for online monitoring using a neural model by an automated vehicle
Lu Modeling and control for vision based rear wheel drive robot and solving indoor SLAM problem using LIDAR
US20240157973A1 (en) System and method for determining a trajectory for a vehicle
Garigipati Evaluation of simultaneous localization and mapping (slam) algorithms
Beckman et al. Monitored Neural Networks for Autonomous Articulated Machines
Karasalo et al. Multi-robot terrain servoing with proximity sensors
Elshall et al. Design and Development of Path Planning and Obstacle Avoidance System for Driverless Pod Using Artificial Neural Network

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
REG Reference to a national code

Ref country code: HK

Ref legal event code: DE

Ref document number: 1261870

Country of ref document: HK

GR01 Patent grant
GR01 Patent grant