柔性连接挂载物跟踪轨迹的补偿方法、装置和相关设备

技术领域

本发明涉及车辆控制技术领域，具体涉及一种柔性连接挂载物跟踪轨迹的补偿方法、装置和相关设备。

背景技术

无论是在工程作业还是农业作业中，经常可以见到以一个牵引车辆拉载一个较大的挂载物进行作业，挂载物的控制是通过车头转向达到的，牵引车辆和挂载物之间柔性连接。该种连接方式需要根据牵引车辆的状况设计路径使得挂载物达到预期路径。如在农业应用上通过大马力拖拉机柔性连接拉载农具，在转向控制时不需要提升农具直接牵引进行作业。在具体使用中通常会设计许多平行等距的作业路径，理想上期望每条作业路径都是拉载的农具中心所形成的路径。然而实际操作中无论是自动驾驶，还是辅助驾驶更多都是以牵引车辆的位置进行控制，这样就导致相邻作业过程中，可能存在重复作业及遗漏作业的情况。

发明内容

为了解决现有技术存在的重复作业、容易遗漏的问题，本发明提供了一种柔性连接挂载物跟踪轨迹的补偿方法、装置和相关设备，其具有作业更加精准、不易遗漏、重复作业等特点。

根据本发明具体实施方式提供的一种柔性连接挂载物跟踪轨迹的补偿方法，包括：

获取牵引车辆的原始运行轨迹中的采集点集；

基于所述采集点集中的每个采集点的经纬度坐标和航向角以及挂载物的长度，确定所述挂载物的原始运行轨迹；

基于作业宽度生成所述挂载物的等距轨迹，所述等距轨迹与所述原始运行轨迹的间隔距离为所述作业宽度；

基于所述等距轨迹和所述挂载物的长度确定所述牵引车辆的控制轨迹。

进一步地，所述挂载物跟踪轨迹的补偿方法还包括：在基于所述等距轨迹和所述挂载物的长度确定所述牵引车辆的控制轨迹之前，对所述等距轨迹进行优化，以减小所述等距轨迹中相邻点的角度跃变。

进一步地，所述获取牵引车辆的原始运行轨迹中的采集点集，包括：

从所述牵引车辆的原始运行轨迹的离散曲线点集中，分别剔除相邻间隔距离小于预设距离值以及速度小于预设速度值的点得到所述采集点集。

进一步地，所述基于所述采集点集中的每个采集点的经纬度坐标和航向角以及挂载物的长度，确定所述挂载物的原始运行轨迹，包括：

基于

得到所述挂载物的原始运行轨迹，x(Tool

进一步地，所述基于作业宽度生成所述挂载物的等距轨迹，包括：

基于所述挂载物的原始运行轨迹的一侧的法矢量和所述挂载物的作业宽度，得到所述等距轨迹。

进一步地，所述对所述等距轨迹进行优化，以减小所述等距轨迹中相邻点的角度跃变，包括：

基于二次规划对所述等距轨迹中和所述采集点相对应的点进行优化，得最优拟合后的处理点集，以减小所述等距轨迹中的角度跃变。

进一步地，所述基于所述等距轨迹和所述挂载物的长度确定所述牵引车辆的控制轨迹，包括：

基于

θ＝atan(k

β＝atan(carLen*k

得到所述牵引车辆的控制轨迹，θ为所述牵引车辆的车头航向角，ToolLen为所述挂载物的长度，α为所述等距轨迹中挂载物的航向角，

β为所述牵引车辆的航向角，carLen为所述牵引车辆的长度，

d

xTool为所述等距轨迹中坐标点的横坐标，yTool为所述等距轨迹中坐标点的纵坐标，x为所述牵引车辆的控制轨迹中坐标点的横坐标，y为所述牵引车辆的控制轨迹中坐标点的纵坐标。

根据本发明具体实施方式提供的一种柔性连接挂载物跟踪轨迹的补偿装置，包括：

点集获取模块，用于获取牵引车辆的原始运行轨迹中的采集点集；

轨迹确定模块，用于基于所述采集点集中的每个采集点的经纬度坐标和航向角以及挂载物的长度，确定所述挂载物的原始运行轨迹；

轨迹平移模块，用于基于作业宽度生成所述挂载物的等距轨迹，所述等距轨迹与所述原始运行轨迹的间隔距离为所述作业宽度；以及

轨迹合成模块，用于基于所述等距轨迹和所述挂载物的长度确定所述牵引车辆的控制轨迹。

根据本发明具体实施方式提供的一种设备，包括：存储器和处理器；

所述存储器，用于存储程序；

所述处理器，用于执行所述程序，实现如上所述的柔性连接挂载物跟踪轨迹的补偿方法的各个步骤。

根据本发明具体实施方式提供的一种存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时，实现如上所述的柔性连接挂载物跟踪轨迹的补偿方法的各个步骤。

本发明所提供的柔性连接挂载物跟踪轨迹的补偿方法，可以获取牵引车辆的原始运行轨迹中的采集点集。然后基于得到的采集点集中的每个采集点的经纬度坐标和航向角以及挂载物的长度，确定挂载物的原始运行轨迹。再基于作业需求生成与挂载物的原始运行轨迹相平行的等距轨迹，其中等距轨迹与挂载物的原始运行轨迹的间隔距离为挂载物的作业宽度。基于等距轨迹和挂载物的长度可反推出牵引车辆的控制轨迹。该补偿方法能够在保证挂载物作业宽度的前提下确定出牵引车辆的控制轨迹，能够有效避免重复作业、遗漏作业等情况的发生，提高作业的效率。

附图说明

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

图1是根据一示例性实施例提供的柔性连接挂载物跟踪轨迹的补偿方法的流程图；

图2是根据一示例性实施例提供的牵引车辆和挂载物的连接图；

图3是根据一示例性实施例提供的等距轨迹的轨迹图；

图4是根据一示例性实施例提供的牵引车辆的控制轨迹图；

图5是根据一示例性实施例提供的轨迹优化的效果图；

图6是根据一示例性实施例提供的距离差异图；

图7是根据一示例性实施例提供的柔性连接挂载物跟踪轨迹的补偿装置的结构图；

图8是根据一示例性实施例提供的设备的结构图。

具体实施方式

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

参照图1所示，本发明的实施例提供了一种柔性连接挂载物跟踪轨迹的补偿方法，该方法可以包括以下步骤：

101、获取牵引车辆的原始运行轨迹中的采集点集。

参照图2所示，牵引车辆2和挂载物1如农具之间为柔性连接的方式，可从牵引车辆的原始运行轨迹的离散曲线点集中，分别剔除相邻间隔距离小于预设距离值以及速度小于预设速度值的点得到采集点集。由于一般地图存在一定的投影偏差，且函数式设计的轨迹线不能反映作业实际的状况。故在工程上通常以牵引车辆实际行驶的离散点集路径作为牵引车辆作业的原始运行轨迹。通常涵盖经纬度坐标和航向角值。而在记录该离散数据时，受到的影响因素较多，故需要先对原始运行轨迹进行优化：以采集到的原始运行轨迹的轨迹集P为例

P＝{P

根据点集中采集点的速度和位置的变化进行优化：

dis＝hypot(diff(yp),diff(xp))，delete dis＜0.1m

其中diff(yp)为计算相邻两点中y坐标差值，diff(xp)为计算相邻两点中x坐标差值，hypot为标准平方和的平方根距离函数，dis为得到的两点间的距离，当距离小于0.1m则会剔除掉相应的点，V

可以理解的是，本领域技术人员可根据实际应用需要对上述轨迹集的剔除方式进行修改和限定，本发明在此不做限制。

102、基于采集点集中的每个采集点的经纬度坐标和航向角以及挂载物的长度，确定挂载物的原始运行轨迹。

基于得到的采集点集，可进一步的推算所跟踪的挂载物的原始运行轨迹：

可通过

得到挂载物的原始运行轨迹，其中x(Tool

103、基于作业宽度生成挂载物的等距轨迹，等距轨迹与挂载物的原始运行轨迹的间隔距离为作业宽度。

在得到挂载物的原始运行轨迹后，将该轨迹作为跟踪的基轨迹，后续的相关作业都是以该轨迹作为基础进行跟踪，在实际作业中需要根据实际作业需求指定跟踪轨迹位于基轨迹的左侧还是右侧。其中可基于挂载物的原始运行轨迹的一侧的法矢量和挂载物的作业宽度，得到和基轨迹的间隔距离为挂载物作业宽度的等距轨迹：

其中width为挂载物的作业宽度，η为法向量，x(P′)为等距轨迹上P′点的横坐标，y(P′)为等距轨迹上P′点的纵坐标，x(P)原始运行轨迹中P点的横坐标，y(P)原始运行轨迹中P点的纵坐标。得到的等距轨迹如图3所示，根据牵引车辆的原始运行轨迹5得到挂载物的原始运行轨迹4，然后得到位于挂载物的原始运行轨迹左侧的等距轨迹3。

104、基于等距轨迹和挂载物的长度确定牵引车辆的控制轨迹。

在得到等距轨迹后，需要根据等距的轨迹推算以车头为控制中心的牵引车辆的控制轨迹，其计算过程如下：

基于

θ＝atan(k

β＝atan(carLen*k

得到牵引车辆的控制轨迹，θ为牵引车辆的车头航向角，ToolLen为挂载物的长度，α为等距轨迹中挂载物的航向角，

β为牵引车辆的航向角，carLen为牵引车辆的长度，

d

xTool为等距轨迹中坐标点的横坐标，yTool为等距轨迹中坐标点的纵坐标，x为牵引车辆的控制轨迹中坐标点的横坐标，y为牵引车辆的控制轨迹中坐标点的纵坐标。

其中通过计算轨迹弧长，然后根据轨迹弧长可得到上述各个参数，轨迹弧长的计算为：

其中s

为进一步优化该技术方案，在本发明的另一具体实施例中，在基于等距轨迹和挂载物的长度确定牵引车辆的控制轨迹之前，对等距轨迹进行优化，以减小等距轨迹中相邻点的角度跃变。其具体可包括：首先基于二次规划对等距轨迹中和采集点相对应的点进行优化，得到最优拟合后的处理点集，以减小等距轨迹中相邻点的角度跃变。

具体的，由于得到的等距轨迹不具有和原始运行轨迹相同的准确航向数据，需要通过位置数据进行推导，而由于采集的点集为离散点集，直接利用点集中的位置数据(航向角、经纬度坐标)得到的角度值跳跃变化程度大，故需要进行一定的优化处理，可将整个轨迹转换为二次规划问题进行最优化目标函数的求解，并在最小拟合方差等约束条件下完成整个路径轨迹的优化处理。其中二次规划(QP)是求解一种特殊的数学优化问题的过程，其中一个有n个变数与m个限制的二次规划问题可以用以下的形式描述。首先给定：一个n维的向量N，一个n×n维的对称矩阵M，一个m×n维的矩阵A，一个m维的向量b。二次规划问题的目标即是在限制条件为：

AX＝b的条件下找到一个n维的向量X，使得

X＝{coef

coef

a

y＝a

需要说明的是，上述向量N、矩阵M和矩阵A的具体构成本领域技术人员可根据实际需要进行设定，本发明在此不再赘述。

参照图5所示的经优化后的角度变化较为平滑，图6所示优化后的点集之间的距离差异较小误差在毫米级。

基于同样的设计思路，参照图7所示，本发明的实施例还提供了一种柔性连接挂载物跟踪轨迹的补偿装置，该装置在运行时可实现上述柔性连接挂载物跟踪轨迹的补偿方法的各个步骤，该装置可以包括：

点集获取模块701，用于获取牵引车辆的原始运行轨迹中的采集点集.

轨迹确定模块702，用于基于采集点集中的每个采集点的经纬度坐标和航向角以及挂载物的长度，确定挂载物的原始运行轨迹。

轨迹平移模块703，用于基于作业宽度生成挂载物的等距轨迹，等距轨迹与原始运行轨迹的间隔距离为作业宽度。以及

轨迹合成模块704，用于基于优化后的等距轨迹和挂载物的长度确定牵引车辆的控制轨迹。

该柔性连接挂载物跟踪轨迹的补偿装置具有和上述柔性连接挂载物跟踪轨迹的补偿方法相同的有益效果，其具体实现方式可参照上述挂载物跟踪轨迹的补偿方法的实施例，本发明在此不再赘述。

参照图8所示，本发明的实施例还提供了一种设备，该设备可以包括：存储器和处理器。

存储器801，用于存储程序.

处理器802，用于执行该程序，实现如上实施例所述的柔性连接挂载物跟踪轨迹的补偿方法的各个步骤。

本发明的实施例还提供了一种存储介质，其上存储有计算机程序，该计算机程序被处理器执行时，实现如上实施例所述的柔性连接挂载物跟踪轨迹的补偿方法的各个步骤。

对于前述的各方法实施例，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明并不受所描述的动作顺序的限制，因为依据本发明，某些步骤可以采用其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作和模块并不一定是本发明所必须的。

需要说明的是，本说明书中的各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似的部分互相参见即可。对于装置类实施例而言，由于其与方法实施例基本相似，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。

本发明各实施例方法中的步骤可以根据实际需要进行顺序调整、合并和删减，各实施例中记载的技术特征可以进行替换或者组合。

本发明各实施例种装置及终端中的模块和子模块可以根据实际需要进行合并、划分和删减。

本发明所提供的几个实施例中，应该理解到，所揭露的终端，装置和方法，可以通过其它的方式实现。例如，以上所描述的终端实施例仅仅是示意性的，例如，模块或子模块的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个子模块或模块可以结合或者可以集成到另一个模块，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或模块的间接耦合或通信连接，可以是电性，机械或其它的形式。

作为分离部件说明的模块或子模块可以是或者也可以不是物理上分开的，作为模块或子模块的部件可以是或者也可以不是物理模块或子模块，即可以位于一个地方，或者也可以分布到多个网络模块或子模块上。可以根据实际的需要选择其中的部分或者全部模块或子模块来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能模块或子模块可以集成在一个处理模块中，也可以是各个模块或子模块单独物理存在，也可以两个或两个以上模块或子模块集成在一个模块中。上述集成的模块或子模块既可以采用硬件的形式实现，也可以采用软件功能模块或子模块的形式实现。

专业人员还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

结合本文中所公开的实施例描述的方法或算法的步骤可以直接用硬件、处理器执行的软件单元，或者二者的结合来实施。软件单元可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。

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

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