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

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

【技术实现步骤摘要】
柔性连接挂载物跟踪轨迹的补偿方法、装置和相关设备


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

技术介绍

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

技术实现思路

[0003]为了解决现有技术存在的重复作业、容易遗漏的问题，本专利技术提供了一种柔性连接挂载物跟踪轨迹的补偿方法、装置和相关设备，其具有作业更加精准、不易遗漏、重复作业等特点。
[0004]根据本专利技术具体实施方式提供的一种柔性连接挂载物跟踪轨迹的补偿方法，包括：
[0005]获取牵引车辆的原始运行轨迹中的采集点集；
[0006]基于所述采集点集中的每个采集点的经纬度坐标和航向角以及挂载物的长度，确定所述挂载物的原始运行轨迹；
[0007]基于作业宽度生成所述挂载物的等距轨迹，所述等距轨迹与所述原始运行轨迹的间隔距离为所述作业宽度；
>[0008]基于所述等距轨迹和所述挂载物的长度确定所述牵引车辆的控制轨迹。
[0009]进一步地，所述挂载物跟踪轨迹的补偿方法还包括：在基于所述等距轨迹和所述挂载物的长度确定所述牵引车辆的控制轨迹之前，对所述等距轨迹进行优化，以减小所述等距轨迹中相邻点的角度跃变。
[0010]进一步地，所述获取牵引车辆的原始运行轨迹中的采集点集，包括：
[0011]从所述牵引车辆的原始运行轨迹的离散曲线点集中，分别剔除相邻间隔距离小于预设距离值以及速度小于预设速度值的点得到所述采集点集。
[0012]进一步地，所述基于所述采集点集中的每个采集点的经纬度坐标和航向角以及挂载物的长度，确定所述挂载物的原始运行轨迹，包括：
[0013]基于
[0014][0015]得到所述挂载物的原始运行轨迹，x(Tool
i
)为所述挂载物的原始运行轨迹上对应点的横坐标，x(P
i
)为第i个采集点的横坐标，ToolLen为所述挂载物的长度，θ
i
为第i个采集点处的航向角，y(Tool
i
)所述挂载物的原始运行轨迹上对应点的纵坐标，y(P
i
)为第i个采集点的纵坐标。
[0016]进一步地，所述基于作业宽度生成所述挂载物的等距轨迹，包括：
[0017]基于所述挂载物的原始运行轨迹的一侧的法矢量和所述挂载物的作业宽度，得到所述等距轨迹。
[0018]进一步地，所述对所述等距轨迹进行优化，以减小所述等距轨迹中相邻点的角度跃变，包括：
[0019]基于二次规划对所述等距轨迹中和所述采集点相对应的点进行优化，得最优拟合后的处理点集，以减小所述等距轨迹中的角度跃变。
[0020]进一步地，所述基于所述等距轨迹和所述挂载物的长度确定所述牵引车辆的控制轨迹，包括：
[0021]基于
[0022]θ＝atan(k1*ToolLen)+α
[0023]β＝atan(carLen*k0)
[0024][0025]得到所述牵引车辆的控制轨迹，θ为所述牵引车辆的车头航向角，ToolLen为所述挂载物的长度，α为所述等距轨迹中挂载物的航向角，
[0026][0027]β为所述牵引车辆的航向角，carLen为所述牵引车辆的长度，
[0028][0029]d1为预设常量，
[0030][0031]xTool为所述等距轨迹中坐标点的横坐标，yTool为所述等距轨迹中坐标点的纵坐标，x为所述牵引车辆的控制轨迹中坐标点的横坐标，y为所述牵引车辆的控制轨迹中坐标点的纵坐标。
[0032]根据本专利技术具体实施方式提供的一种柔性连接挂载物跟踪轨迹的补偿装置，包括：
[0033]点集获取模块，用于获取牵引车辆的原始运行轨迹中的采集点集；
[0034]轨迹确定模块，用于基于所述采集点集中的每个采集点的经纬度坐标和航向角以及挂载物的长度，确定所述挂载物的原始运行轨迹；
[0035]轨迹平移模块，用于基于作业宽度生成所述挂载物的等距轨迹，所述等距轨迹与所述原始运行轨迹的间隔距离为所述作业宽度；以及
[0036]轨迹合成模块，用于基于所述等距轨迹和所述挂载物的长度确定所述牵引车辆的控制轨迹。
[0037]根据本专利技术具体实施方式提供的一种设备，包括：存储器和处理器；
[0038]所述存储器，用于存储程序；
[0039]所述处理器，用于执行所述程序，实现如上所述的柔性连接挂载物跟踪轨迹的补偿方法的各个步骤。
[0040]根据本专利技术具体实施方式提供的一种存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时，实现如上所述的柔性连接挂载物跟踪轨迹的补偿方法的各个步骤。
[0041]本专利技术所提供的柔性连接挂载物跟踪轨迹的补偿方法，可以获取牵引车辆的原始运行轨迹中的采集点集。然后基于得到的采集点集中的每个采集点的经纬度坐标和航向角以及挂载物的长度，确定挂载物的原始运行轨迹。再基于作业需求生成与挂载物的原始运行轨迹相平行的等距轨迹，其中等距轨迹与挂载物的原始运行轨迹的间隔距离为挂载物的作业宽度。基于等距轨迹和挂载物的长度可反推出牵引车辆的控制轨迹。该补偿方法能够在保证挂载物作业宽度的前提下确定出牵引车辆的控制轨迹，能够有效避免重复作业、遗漏作业等情况的发生，提高作业的效率。
附图说明
[0042]为了更清楚地说明本专利技术实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本专利技术的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。
[0043]图1是根据一示例性实施例提供的柔性连接挂载物跟踪轨迹的补偿方法的流程图；
[0044]图2是根据一示例性实施例提供的牵引车辆和挂载物的连接图；
[0045]图3是根据一示例性实施例提供的等距轨迹的轨迹图；
[0046]图4是根据一示例性实施例提供的牵引车辆的控制轨迹图；
[0047]图5是根据一示例性实施例提供的轨迹优化的效果图；
[0048]图6是根据一示例性实施例提供的距离差异图；
[0049]图7是根据一示例性实施例提供的柔性连接挂载物跟踪轨迹的补偿装置的结构图；
[0050]图8是根据一示例性实施本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种柔性连接挂载物跟踪轨迹的补偿方法，其特征在于，包括：获取牵引车辆的原始运行轨迹中的采集点集；基于所述采集点集中的每个采集点的经纬度坐标和航向角以及挂载物的长度，确定所述挂载物的原始运行轨迹；基于作业宽度生成所述挂载物的等距轨迹，所述等距轨迹与所述原始运行轨迹的间隔距离为所述作业宽度；基于所述等距轨迹和所述挂载物的长度确定所述牵引车辆的控制轨迹。2.根据权利要求1所述的方法，其特征在于，还包括：在基于所述等距轨迹和所述挂载物的长度确定所述牵引车辆的控制轨迹之前，对所述等距轨迹进行优化，以减小所述等距轨迹中相邻点的角度跃变。3.根据权利要求1所述的方法，其特征在于，所述获取牵引车辆的原始运行轨迹中的采集点集，包括：从所述牵引车辆的原始运行轨迹的离散曲线点集中，分别剔除相邻间隔距离小于预设距离值以及速度小于预设速度值的点得到所述采集点集。4.根据权利要求1所述的方法，其特征在于，所述基于所述采集点集中的每个采集点的经纬度坐标和航向角以及挂载物的长度，确定所述挂载物的原始运行轨迹，包括：基于得到所述挂载物的原始运行轨迹，x(Tool
i
)为所述挂载物的原始运行轨迹上对应点的横坐标，x(P
i
)为第i个采集点的横坐标，ToolLen为所述挂载物的长度，θ
i
为第i个采集点处的航向角，y(Tool
i
)所述挂载物的原始运行轨迹上对应点的纵坐标，y(P
i
)为第i个采集点的纵坐标。5.根据权利要求1所述的方法，其特征在于，所述基于作业宽度生成所述挂载物的等距轨迹，包括：基于所述挂载物的原始运行轨迹的一侧的法矢量和所述挂载物的作业宽度，得到所述等距轨迹。6.根据权利要求2所述的方法，其特征在于，所述对所...

【专利技术属性】
技术研发人员：万印康，唐李征，
申请(专利权)人：北京合众思壮科技股份有限公司，
类型：发明
国别省市：