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

本发明专利技术提供了一种刚性连接挂载物跟踪轨迹的补偿方法、装置和相关设备，可以首先获取牵引车辆的原始运行轨迹中的采集点集。然后基于采集点集中的每个采集点的经纬度坐标和航向角，确定与牵引车辆刚性连接的挂载物及其原始运行轨迹的镜像运行轨迹，镜像距离为挂载物长度的整数倍。再基于作业宽度在挂载物的原始运行轨迹的一侧生成第一等距轨迹，在镜像运行轨迹的同一侧生成第二等距轨迹。然后将第一等距轨迹和第二等距轨迹加权平均后，得到车辆的第一参考轨迹。通过优化镜像邻项数和挂载物长度的倍数使得到的第一参考轨迹和牵引车辆的轨迹具有更高的相容度，可有效减小跟踪曲线的偏差，保证作业的一致性。保证作业的一致性。保证作业的一致性。

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


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

技术介绍

[0002]在实际的工程作业中，通常是以车头牵引一个车厢或机具的挂载形式开展，载具通常都是以一种刚性连接的方式连接到车头。以农业应用为例，在较大的田地上，通过拖拉机刚性挂载不同的农具设备来进行作业。在进行作业路径的设计时，通常是以拖拉机的位置来进行设计的，当作业路径仅为直线作业时，并不会带来影响。但为了符合实际使用的需求，还需要进行许多曲线路径的跟踪控制。在跟踪该类路径时，由于设计的路径是以车头的位置为基础，故农具中心相对此类跟踪曲线就存在一个偏差。当需要执行多条平行作业路径时，仅计算车辆位置的等距平行曲线，并不能保证农具中心位置保持同样的一个等距。在实际作业上的表现就是相邻的两行要么重复作业，要么漏掉作业。如何对刚性连接挂载物的轨迹进行更为精准的补偿，以更好的跟踪精度进行作业已经成为亟需解决的技术问题。

技术实现思路

[0003]为了解决现有技术存在的偏差大、精度低的问题，本专利技术提供了一种刚性连接挂载物跟踪轨迹的补偿方法、装置和相关设备，其具有跟踪更为精准、轨迹偏差更小等特点。
[0004]根据本专利技术具体实施方式提供的一种刚性连接挂载物跟踪轨迹的补偿方法，包括：
[0005]获取牵引车辆的原始运行轨迹中的采集点集；
[0006]基于镜像距离以及所述采集点集中的每个采集点的经纬度坐标和航向角，确定与所述牵引车辆刚性连接的挂载物的原始运行轨迹的镜像运行轨迹，所述镜像距离为所述挂载物长度的整数倍，所述镜像运行轨迹与所述挂载物的原始运行轨迹相平行；
[0007]基于作业宽度在所述挂载物的原始运行轨迹的一侧生成第一等距轨迹，在所述镜像运行轨迹的同一侧生成第二等距轨迹，所述第一等距轨迹与所述挂载物的原始运行轨迹之间的间隔距离为所述挂载物的作业宽度，所述第二等距轨迹与所述镜像运行轨迹之间的间隔距离为所述挂载物的作业宽度；
[0008]将所述第一等距轨迹和所述第二等距轨迹加权平均后，得到所述牵引车辆的第一参考轨迹。
[0009]进一步地，所述挂载物跟踪轨迹的补偿方法还包括：
[0010]在所述挂载物的第一等距轨迹靠近所述挂载物的原始运行轨迹的一侧生成第三等距轨迹，在所述第二等距轨迹靠近所述挂载物的原始运行轨迹的一侧生成第四等距轨迹，所述第三等距轨迹与所述第一等距轨迹之间的间隔距离为所述挂载物的作业宽度，所述第四等距轨迹与所述第二等距轨迹之间的间隔距离为所述挂载物的作业宽度；
[0011]将所述第三等距轨迹和所述第四等距轨迹加权平均后，得到所述牵引车辆的第二
参考轨迹；
[0012]基于所述第二参考轨迹和所述牵引车辆的原始运行轨迹的重合度，对所述第一参考轨迹的有效性进行验证。
[0013]进一步地，所述刚性连接挂载物跟踪轨迹的补偿方法还包括：
[0014]若所述第一参考轨迹无效，则基于调整后的镜像距离和挂载物长度的倍数对所述第一参考轨迹和所述第二参考轨迹进行更新后，对更新后的第一参考轨迹的有效性进行验证。
[0015]进一步地，所述获取牵引车辆的原始运行轨迹中的采集点集，包括：
[0016]从所述牵引车辆的原始运行轨迹的离散曲线点集中，分别剔除相邻间隔距离小于预设距离值以及速度小于预设速度值的点得到所述采集点集。
[0017]进一步地，基于镜像距离以及所述采集点集中的每个采集点的经纬度坐标和航向角，确定与所述牵引车辆刚性连接的挂载物的原始运行轨迹的镜像运行轨迹，包括：
[0018]基于
[0019][0020]得到所述镜像运行轨迹，x(MirP
i
)为第i个n邻项k镜像采集点横坐标，MirP
i
为第i个n邻项k镜像采集点，x(P)为采集点P的横坐标，P为所述牵引车辆的后轴中心点，k为挂载物长度的倍数，len为所述挂载物的长度，θ
i
为第i个n邻项k镜像点的航向角，y(MirP
i
)为第i个n邻项k镜像点纵坐标，y(P)为采集点P的纵坐标，n为挂载物镜像邻项数。
[0021]进一步地，所述基于作业需求在所述挂载物的原始运行轨迹的一侧生成第一等距轨迹，在所述镜像运行轨迹的一侧生成第二等距轨迹，包括：
[0022]基于所述采集点集中采集点的坐标确定法矢量；
[0023]基于所述法矢量和所述作业宽度分别确定所述第一等距轨迹和所述第二等距轨迹。
[0024]进一步地，所述刚性连接挂载物跟踪轨迹的补偿方法还包括：
[0025]若所述第二参考轨迹与所述牵引车辆的原始运行轨迹的偏差的最大值不大于预设阈值，则将所述第一参考轨迹作为所述牵引车辆的实际运行轨迹。
[0026]根据本专利技术具体实施方式提供的一种刚性连接挂载物跟踪轨迹的补偿装置，包括：
[0027]点集获取模块，用于获取牵引车辆的原始运行轨迹中的采集点集；
[0028]轨迹生成模块，用于基于镜像距离以及所述采集点集中的每个采集点的经纬度坐标和航向角，确定与所述牵引车辆刚性连接的挂载物的原始运行轨迹的镜像运行轨迹，所述镜像距离为所述挂载物长度的整数倍；
[0029]轨迹镜像模块，用于基于作业宽度在所述挂载物的原始运行轨迹的一侧生成第一等距轨迹，在所述镜像运行轨迹的同一侧生成第二等距轨迹，所述第一等距轨迹与所述挂载物的原始运行轨迹之间的间隔距离为所述挂载物的作业宽度，所述第二等距轨迹与所述镜像运行轨迹之间的间隔距离为所述挂载物的作业宽度；以及
[0030]轨迹合成模块，用于将所述第一等距轨迹和所述第二等距轨迹加权平均后，得到所述牵引车辆的第一参考轨迹。
[0031]根据本专利技术具体实施方式提供的一种设备，包括：存储器和处理器；
[0032]所述存储器，用于存储程序；
[0033]所述处理器，用于执行所述程序，实现如上所述的刚性连接挂载物跟踪轨迹的补偿方法的各个步骤。
[0034]根据本专利技术具体实施方式提供的一种存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时，实现上所述的刚性连接挂载物跟踪轨迹的补偿方法的各个步骤。
[0035]本专利技术所提供的刚性连接挂载物跟踪轨迹的补偿方法，可以首先获取牵引车辆的原始运行轨迹中的采集点集。然后基于镜像距离以及采集点集中的每个采集点的经纬度坐标和航向角，确定与牵引车辆刚性连接的挂载物的原始运行轨迹的镜像运行轨迹，镜像距离为挂载物长度的整数倍。再基于作业宽度在挂载物的原始运行轨迹的一侧生成第一等距轨迹，在镜像运行轨迹的一侧生成第二等距轨迹，其中第一等距轨迹与挂载物的原始运行轨迹之间的间隔距离为挂载物的作业宽度，第二等距轨迹与镜像运行轨迹之间的间隔距离为挂载物的作业宽度，第一等距轨迹相对于挂载物的原始运行轨迹的方位与第二等距轨迹相对于镜像运行轨迹的方位相同。最后将第一等距轨迹和第二等距轨迹加权本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种刚性连接挂载物跟踪轨迹的补偿方法，其特征在于，包括：获取牵引车辆的原始运行轨迹中的采集点集；基于镜像距离以及所述采集点集中的每个采集点的经纬度坐标和航向角，确定与所述牵引车辆刚性连接的挂载物的原始运行轨迹的镜像运行轨迹，所述镜像距离为所述挂载物长度的整数倍；基于作业宽度在所述挂载物的原始运行轨迹的一侧生成第一等距轨迹，在所述镜像运行轨迹的同一侧生成第二等距轨迹，所述第一等距轨迹与所述挂载物的原始运行轨迹之间的间隔距离为所述挂载物的作业宽度，所述第二等距轨迹与所述镜像运行轨迹之间的间隔距离为所述挂载物的作业宽度；将所述第一等距轨迹和所述第二等距轨迹加权平均后，得到所述牵引车辆的第一参考轨迹。2.根据权利要求1所述的方法，其特征在于，还包括：在所述第一等距轨迹靠近所述挂载物的原始运行轨迹的一侧生成第三等距轨迹，在所述第二等距轨迹靠近所述挂载物的原始运行轨迹的一侧生成第四等距轨迹，所述第三等距轨迹与所述第一等距轨迹之间的间隔距离为所述挂载物的作业宽度，所述第四等距轨迹与所述第二等距轨迹之间的间隔距离为所述挂载物的作业宽度；将所述第三等距轨迹和所述第四等距轨迹加权平均后，得到所述牵引车辆的第二参考轨迹；基于所述第二参考轨迹和所述牵引车辆的原始运行轨迹的重合度，对所述第一参考轨迹的有效性进行验证。3.根据权利要求2所述的方法，其特征在于，还包括：若所述第一参考轨迹无效，则基于调整后的镜像距离和挂载物长度的倍数对所述第一参考轨迹和所述第二参考轨迹进行更新后，对更新后的第一参考轨迹的有效性进行验证。4.根据权利要求1所述的方法，其特征在于，所述获取牵引车辆的原始运行轨迹中的采集点集，包括：从所述牵引车辆的原始运行轨迹的离散曲线点集中，分别剔除相邻间隔距离小于预设距离值以及速度小于预设速度值的点得到所述采集点集。5.根据权利要求1所述的方法，其特征在于，基于镜像距离以及所述采集点集中的每个采集点的经纬度坐标和航向角，确定与所述牵引车辆刚性连接的挂载物的原始运行轨迹的镜像运行轨迹，包括：基于得到所述镜像运行轨迹，x(MirP
i
)为第i个n邻项k镜像采集点横坐标，MirP
i

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