CN108871338B - 一种拖车系统位姿预测方法、装置及存储介质 - Google Patents

Abstract

本申请涉及一种拖车系统位姿预测方法、装置及存储介质，所述拖车系统包括多节拖车，所述方法，包括：获取所述拖车系统的目标路径，以及，获取当前周期内当前节拖车的速度和位姿；预测当前周期内下一节拖车的速度和位姿；基于所述预测的位姿更新所述下一节拖车的位姿。该方法在对拖车系统中拖车的位置进行预测时，可基于前一节拖车的信息，对下一节拖车的位姿及速度进行预测，从而使得只需要在第一节拖车上设置检测传感器，而其它节拖车均无需再单独设置传感器，就可以实现整个拖车系统进行预测，复杂度较低，使得预测效率大大提高，可以满足实时性要求。

Description

一种拖车系统位姿预测方法、装置及存储介质

技术领域

本申请涉及车辆自动控制技术领域，尤其涉及一种拖车系统位姿预测方法、装置及存储介质。

背景技术

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

拖车系统无人驾驶技术的实现需要车辆的规划控制系统能够对拖车的位姿进行预测。但目前现有的拖车位姿预测技术应用对象主要带有一到两节拖车的重型货车，这种重型货车所带拖车一方面往往具有复杂的机械结构，另一方面仍需要驾驶员操控，其采用的拖车位姿的预测主要为驾驶员提供基础的辅助驾驶功能，对预测算法的实时性要求不高。

但随着港口、机场的货物吞吐量提高，需要对拖车结构的运行轨迹进行精准预测，目前为了提高预测精度，通常需要在每节拖车上安装位置传感器及姿态传感器，但这样一方面使得成本大大提高，另一方面每节拖车上都安装一套传感器，在后续对数据处理时，复杂度较高，使得预测效率降低，无法满足实时性要求。

发明内容

为了解决上述技术问题或者至少部分地解决上述技术问题，本申请提供了一种拖车系统位姿预测方法、装置及存储介质。

有鉴于此，第一方面，本申请提供了一种拖车系统位姿预测方法，所述拖车系统包括多节拖车，所述方法，包括：

获取所述拖车系统的目标路径，以及，获取当前周期内当前节拖车的速度和位姿；

预测当前周期内下一节拖车的速度和位姿；所述下一节拖车与当前节拖车相邻，所述下一节拖车的速度和位姿的预测基于所述目标路径、当前周期内当前节拖车的速度和位姿；

基于所述预测的位姿更新所述下一节拖车的位姿。

第二方面，本申请提供了一种拖车系统位姿预测装置，所述拖车系统包括多节拖车，所述装置包括：

路径获取模块，用于获取所述拖车系统的目标路径；

信息获取模块，用于获取当前周期内当前节拖车的速度和位姿；

预测模块，用于预测当前周期内下一节拖车的速度和位姿；所述下一节拖车与当前节拖车相邻，所述下一节拖车的速度和位姿的预测基于所述目标路径、当前周期内当前节拖车的速度和位姿；

更新模块，用于基于所述预测的位姿更新所述下一节拖车的位姿。

第三方面，本申请还提供一种拖车系统位姿预测设备，包括：

处理器、存储器、网络接口和用户接口；

所述处理器、存储器、网络接口和用户接口通过总线系统耦合在一起；

所述处理器通过调用所述存储器存储的程序或指令，用于执行如上述第一方面任意一项所述拖车系统位姿预测方法的步骤。

第四方面，本申请还提供一种非暂态计算机可读存储介质，其特征在于，所述非暂态计算机可读存储介质存储计算机指令，所述计算机指令使所述计算机执行如上述第一方面任意一项拖车系统位姿预测方法的步骤。

本申请实施例提供的上述技术方案与现有技术相比具有如下优点：

本申请实施例提供的该方法，在对拖车系统中拖车的位置进行预测时，可以基于前一节拖车的信息，对下一节拖车的位姿及速度进行预测，从而使得只需要在第一节拖车上设置检测传感器，而其它节拖车均无需再单独设置传感器，就可以实现整个拖车系统进行预测。

因此，在后续对数据处理时，由于只需要一套传感器的信息即可完成对拖车系统的整体预测，所以系统复杂度较低，使得预测效率大大提高，可以满足实时性要求。

附图说明

此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本申请的实施例，并与说明书一起用于解释本申请的原理。

为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，对于本领域普通技术人员而言，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本申请实施例提供的一种拖车的结构示意图；

图2是本申请实施例提供的拖车系统位姿预测设备的结构示意图。

图3为本申请实施例提供的一种拖车位姿预测方法的流程示意图；

图4为本申请实施例提供的一种拖车位姿预测时的示意图；

图5为本申请实施例提供的另一种拖车位姿预测方法的流程示意图；

图6为本申请实施例提供的又一种拖车位姿预测方法的流程示意图；

图7为本申请实施例提供的一种拖车系统预测装置的结构示意图。

具体实施方式

为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请的一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。

本申请实施例提供了的一种拖车位姿预测方法，应用于拖车系统中，其中，拖车系统可以包括多节拖车，拖车的节数大于等于三。拖车系统中的拖车包括但不局限于：单轴拖车、双轴拖车。

若为双轴拖车，每节拖车上设置有四个车轮，若为单轴拖车，每节拖车上设置有两个车轮。相邻拖车之间相连接，在本身一个实施例中，在相邻两节拖车之间均通过拖钩和拖杆相配合连接。

图1为本申请实施例提供的一种拖车的结构示意图。下面以双轴拖车为例进行说明，如图1所示，图中包括：前一节拖车10，后一节拖车20，前一节拖车10的纵向车轴101，前一节拖车10的拖钩102，后一节拖车的拖杆103，后一节拖车20的纵向车轴104以及后一节拖车的拖钩105。

从图中可以看到，每节拖车的拖钩均固定在该节拖车的尾部，拖杆安装在该节拖车的头部。拖杆与拖钩之间采用转动连接，包括但不局限于：轴杆与轴孔配合方式、轴杆与轴套配合方式、钩环配合方式。通过转动连接，前一节拖车通过拖钩带动拖杆运动以及转向，拖杆带动后一节拖车运动及转向，实现拖车系统的运行。

对于拖车系统而言，第一节拖车可以为牵引车，牵引车之后的其余节拖车为装载拖车，在牵引车上设置有包括但不局限于：显示系统、动力系统、导航定位系统、速度控制系统、方向控制系统、速度检测系统、方向检测系统、无线通信系统。在本申请一些实施例中，牵引车上可以安装有包括但不局限于：计算机、微处理器、数据处理平台，作为数据控制中心，用于处理牵引车上各个系统之间的数据处理以及数据传输。

图2是本申请实施例提供的拖车系统位姿预测设备的结构示意图。图2所示的设备200可以安装在牵引车中，也可以安装在任意一节拖车中。

图2所示的设备200可以的包括：至少一个处理器201、至少一个存储器202、至少一个网络接口204和其他用户接口203。电子设备中的各个组件通过总线系统205耦合在一起。可理解，总线系统205用于实现这些组件之间的连接通信。总线系统205除包括数据总线之外，还包括电源总线、控制总线和状态信号总线。但是为了清楚说明起见，在图2中将各种总线都标为总线系统205。

其中，用户接口203可以包括显示器、键盘或者点击设备(例如，鼠标，轨迹球(trackball)或者触感板等。

可以理解，本实施例中的存储器202可以是易失性存储器或非易失性存储器，或可包括易失性和非易失性存储器两者。其中，非易失性存储器可以是只读存储器(Read-OnlyMemory，ROM)、可编程只读存储器(Programmable ROM，PROM)、可擦除可编程只读存储器(Erasable PROM，EPROM)、电可擦除可编程只读存储器(Electrically EPROM，EEPROM)或闪存。易失性存储器可以是随机存取存储器(Random Access Memory，RAM)，其用作外部高速缓存。通过示例性但不是限制性说明，许多形式的RAM可用，例如静态随机存取存储器(Static RAM，SRAM)、动态随机存取存储器(Dynamic RAM，DRAM)、同步动态随机存取存储器(Synchronous DRAM，SDRAM)、双倍数据速率同步动态随机存取存储器(Double Data RateSDRAM，DDRSDRAM)、增强型同步动态随机存取存储器(Enhanced SDRAM，ESDRAM)、同步连接动态随机存取存储器(Synch link DRAM，SLDRAM)和直接内存总线随机存取存储器(DirectRambus RAM，DRRAM)。本文描述的存储器202旨在包括但不限于这些和任意其它适合类型的存储器。

在一些实施方式中，存储器202存储了如下的元素，可执行单元或者数据结构，或者他们的子集，或者他们的扩展集：操作系统2021和应用程序2022。

其中，操作系统2021，包含各种系统程序，例如框架层、核心库层、驱动层等，用于实现各种基础业务以及处理基于硬件的任务。应用程序2022，包含各种应用程序，例如媒体播放器(Media Player)、浏览器(Browser)等，用于实现各种应用业务。实现本申请实施例方法的程序可以包含在应用程序2022中。

在本申请实施例中，处理器201通过调用存储器202存储的程序或指令，具体的，可以是应用程序2022中存储的程序或指令，处理器201用于执行如下方法步骤，例如包括：

获取所述拖车系统的目标路径，以及当前周期内当前节拖车的速度和位姿；预测当前周期内与当前节拖车相邻的下一节拖车的速度和位姿；所述下一节拖车的速度和位姿的预测基于所述目标路径、当前周期内当前节拖车的速度和位姿；基于所述预测的位姿对所述下一节拖车的位姿进行更新。

上述本申请实施例揭示的方法可以应用于处理器201中，或者由处理器201实现。处理器201可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器201中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器201可以是通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(FieldProgrammable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本申请实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本申请实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件单元组合执行完成。软件单元可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器202，处理器201读取存储器202中的信息，结合其硬件完成上述方法的步骤。

图3为本申请实施例提供的一种拖车位姿预测方法的流程示意图。

图3所示的该方法可以应用于图2所示的电子设备中，如图3所示，该拖车位姿预测方法可以包括以下步骤。

S101，获取所述拖车系统的目标路径，以及，获取当前周期内当前节拖车的速度和位姿。

拖车系统的目标路径可以通过多种方式获取，包括但不局限于：读取路径文件获得、从路径生成模块直接获得。

其中，路径文件中可以系统预存的或操作人员通过无线通信方式发送给系统的。

若系统中设置有路径生成模块时，操作人员可以预先在路径生成模块中手动设置出发地和目的地，进而系统自动根据目的地以及出发地生成导航路线作为目标路径。

当前节拖车的速度的获取方式包括但不局限于：通过传感器采集得到、利用前一节拖车的速度计算得到。

通常，在拖车系统中，除牵引车外，其它拖车不设置传感器，所以在确定速度获取方式时，需要首先确定当前节拖车在整个拖车系统中的位置，根据位置分布的不同，而采用不同的获取方式。

其中，若确定当前节拖车为牵引车，则直接基于牵引车上设置的速度传感器检测得到。

若确定当前节拖车为牵引车之后的其它节拖车时，可根据前一节拖车和后一节拖车之间的连接关系，由前一节拖车的速度计算得到后一节拖车的速度。

当前节拖车的位姿的获取方式八廓但不局限于：通过传感器采集得到、利用位姿和速度计算得到。

通常，在拖车系统中，除牵引车外，其它拖车不设置传感器，所以在确定位姿获取方式时，需要首先确定当前节拖车在整个拖车系统中的位置，根据位置分布的不同，而采用不同的获取方式。

其中，若确定当前节拖车为位于第一节的牵引车，可直接通过牵引车上设置的传感器检测得到位姿，所述传感器包括但不局限于：GPS、激光雷达SLAM等，在本申请一些实施例中，GPS还可以与激光雷达配合来检测位姿。

若确定当前节拖车为第一节牵引车之后的其它节拖车，则直接基于当前周期拖车速度、以及，上一个周期当前节拖车的位姿，对当前节拖车在当前周期的位姿进行预测。

在本申请一些实施例中，该步骤S101可以包括以下步骤：

S1，判断当前节拖车是否为第一节的牵引车。

S2，若当前节拖车是第一节的牵引车，基于所述牵引车上速度传感器检测所述牵引车的速度，以及，基于所述牵引车上位姿传感器检测所述牵引车的位姿。

S3，若当前节拖车是第一节的牵引车之后的拖车，基于所述当前节拖车的前一节拖车的速度及位置，计算当前周期当前节拖车的速度及位姿。

S102，预测当前周期内下一节拖车的速度和位姿。

所述下一节拖车与当前节拖车相邻，参见图1所示实施例中，下一节拖车的拖杆与当前节拖车的拖钩相连接。

两节拖车之间的牵引角度、牵引速度等运动情况与拖车之间的连接关系相对应。为此，在该步骤中，首先基于车辆运动学模型，对当前节拖车和下一节拖车之间的连接关系进行解耦合，建立当前节拖车和下一节拖车之间的速度和位姿更新模型。

在建立的速度和位置更新模型中，通过输入所述目标路径、当前周期内当前节拖车的速度和位姿，可计算得到下一节拖车的速度以及位姿。

其中，车辆运动学模型，可预先将车辆的各项参数输入到预设模型中进行学习，最终得到与车辆相匹配的车辆运动学模型。

S103，基于所述预测的位姿更新所述下一节拖车的位姿。

由于车辆的运动情况时刻都在变化，所以在进行位姿预测时，需要利用后一次的预测结果，对前一次的预测结果进行更新，以便技术人员能够及时掌握最新的位姿。

在本申请一些实施例中，对拖车系统进行预测采用周期预测方式，为此，在预测得到当前周期下一节拖车的位置后，需要对上一周期该节拖车对应的位姿进行更新。

在本申请一些实施例中，以图1所示的双轴拖车为例，设计了一套速度和位姿的预测方程，可以依据第一节牵引车的速度和位姿信息逐节推得下节拖车的速度和位姿数据，递推过程如下：

在图1所示示图的基础上，参见图4所示，第一节牵引车(Car0)牵引n节拖车(Car1～n)的原理图中，以车辆后轴中心点为车辆几何中心点。

牵引车位置为(x₀,y₀)，航向为θ₀，前轮偏角为δ₀，速度为V，轴距为l₀，拖钩长度为c₀。第一节拖车位置为(x₁,y₁)，航向为θ₁，前轮偏角为δ₁，拖杆长度为a₁，轴距为l₁。

由图4拖车系统运动情况可知，在牵引车牵引拖车运动时，拖车的拖杆与前车纵向车轴会产生一个夹角α₀，其方向由拖杆指向前车纵向车轴，由上图中位置关系可知α₀可以表示为：

α₀＝θ₀-β₁ (1)

其中β₁为拖车拖杆的航向，可以表示为：

β₁＝θ₁+δ₁ (2)

拖车的拖杆在B点的速度有两部分组成，一个速度沿前车车轴方向向前，图中表示为V；另一个速度垂直于前车车轴，方向向右，该速度由前车航向角变化在车尾处生成，图中表示为V_{0_c}，数值大小表示为：

为了预测对下一节拖车的运动情况，需要将车辆连接点B出的速度分解到沿拖杆纵向和横向的分速度，由车辆运动学原理可知，沿前车车轴方向的速度在拖杆纵向的分量可以表示为：

V_{1_al1}＝V*cos(α₀) (4)

沿前车车轴方向的速度在拖杆横向的分量可以表示为：

V_{1_ac1}＝V*sin(α₀) (5)

设由前一节拖车车航向角变化生成的横向速度与拖杆航向之间的夹角为

方向由横向速度到拖杆航向方向，其数值可以表示为：

则前车航向角速度产生的横向速度V_{0_c}在拖杆纵向的速度分量可以表示为：

前车航向角速度产生的横向速度V_{0_c}在拖杆横向的速度分量可以表示为：

则前车在B点处产生的沿拖杆纵向的合速度可以表示为：

V_{1_al}＝V_{1_al1}+V_{1_al2} (9)

前车在B点处产生的沿拖杆横向的合速度可以表示为：

V_{1_ac}＝V_{1_ac1}+V_{1_ac2} (10)

而由拖杆上B点的速度可以得到拖车上C点沿拖车车轴方向的速度可以表示为：

V_{1_l}＝V_{1_al}*cos(δ₁) (11)

对应，C点沿垂直拖车车轴方向的速度可以表示为：

V_{1_c}＝V_{1_al}*sin(δ₁) (12)

由运动学原理可知，拖车航向角变化率可以表示为：

而拖车的拖杆航向角变化率可以表示为：

拖车的前轮偏角变化率可以表示为：

由车辆运动学模型可以得到拖车坐标在全局坐标系下X轴方向的速度可以表示为：

拖车坐标在全局坐标系下Y轴方向的速度可以表示为：

综合上式可得第一节拖车的速度可以表示为：

第一节拖车的位姿和前轮偏角微分量可以表示为公式(19)：

同理可以得到第i+1节拖车在t时刻的速度可以表示为公式(20)：第i+1节拖车在t时刻的位姿和前轮偏角微分量可以表示为公式(21)：

最终，基于前述公式预测得到当前周期t第i+1节拖车的位姿和前轮偏角可以表示为：

本申请实施例提供的该方法，在对拖车系统中拖车的位置进行预测时，可以基于前一节拖车的信息，对下一节拖车的位姿及速度进行预测，从而使得只需要在第一节拖车上设置检测传感器，而其它节拖车均无需再单独设置传感器，就可以实现整个拖车系统进行预测。

因此，在后续对数据处理时，由于只需要一套传感器的信息即可完成对拖车系统的整体预测，所以系统复杂度较低，使得预测效率大大提高，可以满足实时性要求。

在本申请一个实施例中，针对拖车系统整车进行轨迹预测时，需要呈现给技术人员的是整个拖车系统的轨迹，，为此，本申请实施例还提供一种拖车位姿预测方法，如图5所示，该方法可以包括以下步骤。

S104，预测所述拖车系统中的每节拖车的速度和位姿并迭代更新，直至得到当前周期内所述拖车系统的运行轨迹；

在本申请一些实施例中，迭代可以采用以下方式：

判断所述下一节拖车是否为最后一节拖车；

当所述下一节拖车不是最后一节拖车时，基于所述下一节拖车的速度及位姿计算位于所述下一节拖车的后一节拖车的速度和位姿；

当所述下一节拖车是最后一节拖车时，结束迭代，并组合所述拖车系统中每节拖车的位姿，得到当前周期所述拖车系统的运行轨迹。

S105，输出当前周期内所述拖车系统的运行轨迹。

输出运动轨迹的方式，包括但不局限于：发送给后台控制中心、在显示屏上显示、绘制图形、记录到存储器中等方式，便于用户现场查看或远程查看预测结果，为了进一步的调度以及控制提供帮助。

本申请一些实施例中，在对拖车系统该进行预测时，为此，可以按照周期进行预测。但由于拖车系统运动的实时性，拖车系统中的拖车的位置也会随时发生变化，为例便于对拖车的运行轨迹进行预测，以便于操作人员对拖车系统控制进行帮助，可以设置一个预测时域，一个预测时域包括多个预测周期。

拖车系统的预测周期包括但不局限于：传感器采集周期、人为设置周期等。在本本申请一些实施例中，以一个周期为100ms为例，相应地，预测时域可以为1min。

为此，本申请实施例提供的该方法可以进一步包括：

S106，判断所述当前周期是否超出预测时域。

由于预测时域内有多个周期，所以，步骤S106可以在完成任意周期内预测后进行。在具体判断时，可以将当前周期的结束时刻t0与预测时域的最大时刻t1进行大小比较，若t0大于t1，则当前周期超过预测时域，否则，当前周期未超出预测时域。

S107，当所述当前周期超出预测时域时，结束预测，输出所述预测时域内所有周期内所述拖车系统中运动轨迹；

而若所述当前周期未超出预测时域，则返回拖车系统的第一节拖车第一节的牵引车，再对拖车系统中的每节拖车进行一次预测，直至最终超出预测时域后结束预测。

下面以拖车系统以第一节为牵引车为例，对该拖车位姿预测方法进行说明，本申请实施例还提供了一种拖车系统位姿预测方法，如图6所示，该方法可以包括以下步骤。

S201，获取拖车系统的目标路径；

S202，在第一周期T1内采集第一节拖车的速度和位姿；

S203，基于第一节拖车的速度和位姿，预测下一节拖车的速度和位姿；

S204，判断下一节拖车是否为最后一节拖车；

当所述下一节拖车不是最后一节拖车时，返回步骤S203，基于所述下一节拖车的速度及位姿向后进行迭代，计算位于所述下一节拖车的后一节拖车的速度和位姿；

S205，当所述下一节拖车是最后一节拖车时，结束迭代，并组合所述拖车系统中每节拖车的位姿，得到第一周期T1内所述拖车系统的运行轨迹。

此外，当对未来一段时间内的拖车进行预测时，还会获取预先设定的预测时域，通常为一个时间段，例如：当前时间为15：00，预测未来10分钟内的运行轨迹，那么预测时域为[15:00，15:10]。

为此，该方法可以进一步包括以下步骤：

当所述下一节拖车是最后一节拖车时，一种情况下，可以直接开始第二周期T2的预测，并且在第二周期T2内，对所述拖车系统从头到尾依次进行预测。

另一种情况下，在开始第二周期T2的预测之前，该方法可以进一步包括以下步骤：

S206，判断当前的周期是否超过预测时域；

S207，当所述当前周期超出预测时域时，结束本次预测，并且输出所述预测时域内所有周期内所述拖车系统中运动轨迹。

当所述当前周期未超出预测时域时，开始第二周期T2的预测，并且在第二周期T2内，对所述拖车系统从头到尾依次进行预测。

为了描述的方便，描述以上装置时以功能分为各种单元分别描述。当然，在实施本申请时可以把各单元的功能在同一个或多个软件和/或硬件中实现。

本申请实施例还提出一种非暂态计算机可读存储介质，所述非暂态计算机可读存储介质存储计算机指令，所述计算机指令使所述计算机执行图3、图6实施例所提供的方法步骤，包括：获取所述拖车系统的目标路径，以及，获取当前周期内当前节拖车的速度和位姿；预测当前周期内下一节拖车的速度和位姿；所述下一节拖车与当前节拖车相邻，所述下一节拖车的速度和位姿的预测基于所述目标路径、当前周期内当前节拖车的速度和位姿；基于所述预测的位姿更新所述下一节拖车的位姿。

图6为本申请实施例提供的一种拖车系统预测装置的结构示意图。

图6所示的拖车系统预测装置可以设置在图2所示的电子设备中，如图6所示，该拖车系统预测装置可以包括：

路径获取模块61，用于获取所述拖车系统的目标路径；

信息获取模块62，用于获取当前周期内当前节拖车的速度和位姿；

预测模块63，用于预测当前周期内下一节拖车的速度和位姿。

所述下一节拖车与当前节拖车相邻，所述下一节拖车的速度和位姿的预测基于所述目标路径、当前周期内当前节拖车的速度和位姿。

更新模块64，用于基于所述预测的位姿更新所述下一节拖车的位姿。

在本申请一个实施例中，图6所示的装置可以进一步包括：

迭代更新模块，用于预测所述拖车系统中的每节拖车的速度和位姿并迭代更新，直至得到当前周期内所述拖车系统的运行轨迹；

输出模块，用于输出当前周期内所述拖车系统的运行轨迹。

在本申请一个实施例中，图6所示的装置中，所述迭代更新模块，包括：

车尾判断子模块，用于判断所述下一节拖车是否为最后一节拖车；

计算子模块，用于当所述下一节拖车不是最后一节拖车时，基于所述下一节拖车的速度及位姿计算位于所述下一节拖车之后一节拖车的速度和位姿；

组合子模块，用于当所述下一节拖车是最后一节拖车时，结束迭代，并组合所述拖车系统中每节拖车的位姿，得到当前周期所述拖车系统的运行轨迹。

在本申请一个实施例中，图6所示的装置可以进一步包括：

时域判断模块，用于判断所述当前周期是否超出预测时域；

所述输出模块还用于，当所述当前周期超出预测时域时，输出所述预测时域内所有周期内所述拖车系统中运动轨迹。

在本申请一个实施例中，图6所示的装置中，所述信息获取模块，包括：

牵引车判断子模块，用于判断当前节拖车是否为第一节牵引车；

采集模块，用于当当前节拖车为第一节牵引车时，采集所述牵引车上速度传感器检测到所述牵引车的速度，采集所述牵引车上位姿传感器检测到所述牵引车的位姿；

计算模块，用于当当前节拖车为第一节牵引车之后的拖车时，基于所述当前节拖车的前一节拖车的速度及位置，计算当前周期当前节拖车的速度及位姿。

本申请公开了一种拖车系统位姿预测方法、装置及存储介质，包括：

A1：一种拖车系统位姿预测方法，所述拖车系统包括多节拖车，所述方法，包括：获取所述拖车系统的目标路径，以及，获取当前周期内当前节拖车的速度和位姿；预测当前周期内下一节拖车的速度和位姿；所述下一节拖车与当前节拖车相邻，所述下一节拖车的速度和位姿的预测基于所述目标路径、当前周期内当前节拖车的速度和位姿；基于所述预测的位姿更新所述下一节拖车的位姿。

A2：根据A1所述的方法，基于车辆运动学模型，对所述下一节拖车的速度和位姿进行预测。

A3：根据A1所述的方法，所述方法进一步包括：预测所述拖车系统中的每节拖车的速度和位姿并迭代更新，直至得到当前周期内所述拖车系统的运行轨迹；输出当前周期内所述拖车系统的运行轨迹。

A4：根据A3所述的方法，所述预测所述拖车系统中的每节拖车的速度和位姿并迭代更新，包括：判断所述下一节拖车是否为最后一节拖车；当所述下一节拖车不是最后一节拖车时，基于所述下一节拖车的速度及位姿计算位于所述下一节拖车的后一节拖车的速度和位姿；当所述下一节拖车是最后一节拖车时，结束迭代，并组合所述拖车系统中每节拖车的位姿，得到当前周期所述拖车系统的运行轨迹。

A5：根据A2所述的方法，所述方法进一步包括：若所述当前周期超出预测时域，输出所述预测时域内所有周期内所述拖车系统中运动轨迹。

A6：根据A1所述的方法，所述获取当前周期内当前节拖车的速度和位姿，包括：判断当前节拖车是否为第一节的牵引车；若当前节拖车是第一节的牵引车，基于所述牵引车上速度传感器检测所述牵引车的速度，基于所述牵引车上位姿传感器检测所述牵引车的位姿；若当前节拖车不是第一节的牵引车，基于所述当前节拖车的前一节拖车的速度及位置，计算当前周期当前节拖车的速度及位姿。

B1：一种拖车系统位姿预测装置，所述拖车系统包括多节拖车，所述装置包括：路径获取模块，用于获取所述拖车系统的目标路径；信息获取模块，用于获取当前周期内当前节拖车的速度和位姿；预测模块，用于预测当前周期内下一节拖车的速度和位姿；所述下一节拖车与当前节拖车相邻，所述下一节拖车的速度和位姿的预测基于所述目标路径、当前周期内当前节拖车的速度和位姿；更新模块，用于基于所述预测的位姿更新所述下一节拖车的位姿。

B2：根据B1所述的装置，所述装置进一步包括：迭代更新模块，用于预测所述拖车系统中的每节拖车的速度和位姿并迭代更新，直至得到当前周期内所述拖车系统的运行轨迹；输出模块，用于输出当前周期内所述拖车系统的运行轨迹。

B3：根据B2所述的装置，所述迭代更新模块，包括：车尾判断模块，用于判断所述下一节拖车是否为最后一节拖车；计算模块，用于当所述下一节拖车不是最后一节拖车时，基于所述下一节拖车的速度及位姿计算位于所述下一节拖车的后一节拖车的速度和位姿；轨迹组合模块，用于当所述下一节拖车是最后一节拖车时，结束迭代，并组合所述拖车系统中每节拖车的位姿，得到当前周期所述拖车系统的运行轨迹。

B4：根据B2所述的装置，所述装置进一步包括：时域判断模块，用于判断所述当前周期是否超出预测时域；所述输出模块还用于，当所述当前周期超出预测时域时，输出所述预测时域内所有周期内所述拖车系统中运动轨迹。

B5：根据B1所述的装置，所述信息获取模块，包括：所述信息获取模块，包括：牵引车判断模块，用于判断当前节拖车是否为第一节的牵引车；检测模块，用于若当前节拖车是第一节的牵引车，基于所述牵引车上速度传感器检测所述牵引车的速度，基于所述牵引车上位姿传感器检测所述牵引车的位姿；计算模块，用于若当前节拖车不是第一节的牵引车，基于所述当前节拖车的前一节拖车的速度及位置，计算当前周期当前节拖车的速度及位姿。

本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于装置或系统实施例而言，由于其基本相似于方法实施例，所以描述得比较简单，相关之处参见方法实施例的部分说明即可。以上所描述的装置及系统实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。

可以理解的是，本申请可用于众多通用或专用的计算系统环境或配置中。例如：个人计算机、服务器计算机、手持设备或便携式设备、平板型设备、多处理器系统、基于微处理器的系统、置顶盒、可编程的消费电子设备、网络PC、小型计算机、大型计算机、包括以上任何系统或设备的分布式计算环境等等。

本申请可以在由计算机执行的计算机可执行指令的一般上下文中描述，例如程序模块。一般地，程序模块包括执行特定任务或实现特定抽象数据类型的例程、程序、对象、组件、数据结构等等。也可以在分布式计算环境中实践本申请，在这些分布式计算环境中，由通过通信网络而被连接的远程处理设备来执行任务。在分布式计算环境中，程序模块可以位于包括存储设备在内的本地和远程计算机存储介质中。

需要说明的是，在本文中，诸如“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

以上所述仅是本申请的具体实施方式，使本领域技术人员能够理解或实现本申请。对这些实施例的多种修改对本领域的技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本申请的精神或范围的情况下，在其它实施例中实现。因此，本申请将不会被限制于本文所示的这些实施例，而是要符合与本文所申请的原理和新颖特点相一致的最宽的范围。

Claims (12)

1.一种拖车系统位姿预测方法，其特征在于，所述拖车系统包括多节拖车，拖车的节数大于等于三，所述方法，包括：

获取所述拖车系统的目标路径，以及，获取当前周期内当前节拖车的速度和位姿；

预测当前周期内下一节拖车的速度和位姿；所述下一节拖车与当前节拖车相邻，所述下一节拖车的速度和位姿的预测基于所述目标路径、当前周期内当前节拖车的速度和位姿；

基于所述预测的位姿更新所述下一节拖车的位姿；

若当前节拖车为第i节拖车，下一节拖车为第i+1节拖车，则所述下一节拖车的速度和位姿的预测结果，基于下述公式得到：

其中，当前节拖车轴距为l_i，拖钩长度为c_i；在t时刻，当前节拖车航向为θ_(i，t)，前轮偏角为δ_(i，t)，速度为V_(i，t)；

下一节拖车轴距为l_i+1，拖杆长度为a_i+1；在t时刻，下一节拖车位置为(x_(i+1，t)，y_(i+1，t))，航向为θ_(i+1，t)，前轮偏角为δ_(i+1，t)，速度为V_(i+1，t)；在t-1时刻，下一节拖车位置为(x_(i+1，t-1)，y_(i+1，t-1))，航向为θ_(i+1，t-1)，前轮偏角为δ_(i+1，t-1)。

2.根据权利要求1所述的方法，其特征在于，所述方法进一步包括：

预测所述拖车系统中的每节拖车的速度和位姿并迭代更新，直至得到当前周期内所述拖车系统的运行轨迹；

输出当前周期内所述拖车系统的运行轨迹。

3.根据权利要求2所述的方法，其特征在于，所述预测所述拖车系统中的每节拖车的速度和位姿并迭代更新，包括：

判断所述下一节拖车是否为最后一节拖车；

当所述下一节拖车不是最后一节拖车时，基于所述下一节拖车的速度及位姿计算位于所述下一节拖车的后一节拖车的速度和位姿；

当所述下一节拖车是最后一节拖车时，结束迭代，并组合所述拖车系统中每节拖车的位姿，得到当前周期所述拖车系统的运行轨迹。

4.根据权利要求2所述的方法，其特征在于，所述方法进一步包括：

若所述当前周期超出预测时域，输出所述预测时域内所有周期内所述拖车系统中运动轨迹。

5.根据权利要求1所述的方法，其特征在于，所述获取当前周期内当前节拖车的速度和位姿，包括：

判断当前节拖车是否为第一节的牵引车；

若当前节拖车是第一节的牵引车，基于所述牵引车上速度传感器检测所述牵引车的速度，基于所述牵引车上位姿传感器检测所述牵引车的位姿；

若当前节拖车不是第一节的牵引车，基于所述当前节拖车的前一节拖车的速度及位置，计算当前周期当前节拖车的速度及位姿。

6.一种拖车系统位姿预测装置，其特征在于，所述拖车系统包括多节拖车，拖车的节数大于等于三，所述装置包括：

路径获取模块，用于获取所述拖车系统的目标路径；

信息获取模块，用于获取当前周期内当前节拖车的速度和位姿；

预测模块，用于预测当前周期内下一节拖车的速度和位姿；所述下一节拖车与当前节拖车相邻，所述下一节拖车的速度和位姿的预测基于所述目标路径、当前周期内当前节拖车的速度和位姿；

更新模块，用于基于所述预测的位姿更新所述下一节拖车的位姿；

若当前节拖车为第i节拖车，下一节拖车为第i+1节拖车，则所述下一节拖车的速度和位姿的预测结果，基于下述公式得到：

其中，当前节拖车轴距为l_i，拖钩长度为c_i；在t时刻，当前节拖车航向为θ_(i，t)，前轮偏角为δ_(i，t)，速度为V_(i，t)；

下一节拖车轴距为l_i+1，拖杆长度为a_i+1；在t时刻，下一节拖车位置为(x_(i+1，t)，y_(i+1，t))，航向为θ_(i+1，t)，前轮偏角为δ_(i+1，t)，速度为V_(i+1，t)；在t-1时刻，下一节拖车位置为(x_(i+1，t-1)，y_(i+1，t-1))，航向为θ_(i+1，t-1)，前轮偏角为δ_(i+1，t-1)。

7.根据权利要求6所述的装置，其特征在于，所述装置进一步包括：

迭代更新模块，用于预测所述拖车系统中的每节拖车的速度和位姿并迭代更新，直至得到当前周期内所述拖车系统的运行轨迹；

输出模块，用于输出当前周期内所述拖车系统的运行轨迹。

8.根据权利要求7所述的装置，其特征在于，所述迭代更新模块，包括：

车尾判断模块，用于判断所述下一节拖车是否为最后一节拖车；

计算模块，用于当所述下一节拖车不是最后一节拖车时，基于所述下一节拖车的速度及位姿计算位于所述下一节拖车的后一节拖车的速度和位姿；

轨迹组合模块，用于当所述下一节拖车是最后一节拖车时，结束迭代，并组合所述拖车系统中每节拖车的位姿，得到当前周期所述拖车系统的运行轨迹。

9.根据权利要求7所述的装置，其特征在于，所述装置进一步包括：

时域判断模块，用于判断所述当前周期是否超出预测时域；

所述输出模块还用于，当所述当前周期超出预测时域时，输出所述预测时域内所有周期内所述拖车系统中运动轨迹。

10.根据权利要求6所述的装置，其特征在于，所述信息获取模块，包括：

牵引车判断模块，用于判断当前节拖车是否为第一节的牵引车；

检测模块，用于若当前节拖车是第一节的牵引车，基于所述牵引车上速度传感器检测所述牵引车的速度，基于所述牵引车上位姿传感器检测所述牵引车的位姿；

计算模块，用于若当前节拖车不是第一节的牵引车，基于所述当前节拖车的前一节拖车的速度及位置，计算当前周期当前节拖车的速度及位姿。

11.一种拖车系统位姿预测设备，其特征在于，包括：

处理器、存储器、网络接口和用户接口；

所述处理器、存储器、网络接口和用户接口通过总线系统耦合在一起；

所述处理器通过调用所述存储器存储的程序或指令，用于执行如权利要求1-5任一项所述拖车系统位姿预测方法的步骤。

12.一种非暂态计算机可读存储介质，其特征在于，所述非暂态计算机可读存储介质存储计算机指令，所述计算机指令使所述计算机执行如权利要求1至5所述拖车系统位姿预测方法的步骤。

Images