一种面向饲养场的投料车自动导航定位方法及装置

本技术属于无人移动平台的自主导航与定位，特别的涉及一种面向饲养场的投料车自动导航定位方法及装置。

背景技术：

1、地面无人移动平台的应用是无人平台技术研究的热点，地面无人平台的自主导航与定位是无人平台遂行任务必须解决的关键问题，性能优越的导航与定位方法不仅能够有效提高车辆的安全性，更能够确保任务准确的完成。目前，无人驾驶车辆广泛采用多传感器融合定位方案，以覆盖不同的行驶工况。在场区内实现自主导航与定位时，常见的传感器有激光雷达、摄像头、双目相机、uwb、gnss、imu、车辆码盘等。针对不同的应用场景，研究者提出了多种导航定位方法，如gnss/imu、lidar/gnss、gnss/uwb等。

2、在众多的自主导航与定位方法中，利用激光雷达构建地图，并基于建立的地图定位的方法被广泛运用在无人移动平台开发中。然而基于激光雷达的定位技术，在面积大、未知以及动态的饲养场应用时，存在计算复杂度高以及定位误差发散等问题；其次，一些基于imu的惯性导航定位技术的误差会随着时间不断累积，导致位置和姿态的测量结果偏离实际位置，因此无法用来做长时间的高精度定位。综上，针对牲畜饲料投放车的应用需求，现有的相关技术难以应对不同天气，光线、室内室外场地等条件，且导航定位精度无法得到保障。

技术实现思路

1、本技术为解决上述提到的基于激光雷达的定位技术，在面积大、未知以及动态的饲养场应用时，存在计算复杂度高以及定位误差发散等问题；其次，一些基于imu的惯性导航定位技术的误差会随着时间不断累积，导致位置和姿态的测量结果偏离实际位置，因此无法用来做长时间的高精度定位。综上，针对牲畜饲料投放车的应用需求，现有的相关技术难以应对不同天气，光线、室内室外场地等条件，且导航定位精度无法得到保障等技术缺陷，提出一种面向饲养场的投料车自动导航定位方法及装置，其技术方案如下：

2、第一方面，本技术实施例提供了一种面向饲养场的投料车自动导航定位方法，包括：

3、获取投料车在多个位置对应的初始点云数据、初始位姿数据以及初始定位数据，并对初始点云数据、初始位姿数据以及初始定位数据进行配准处理，得到与投料车的车体坐标系所对应的目标点云数据、目标位姿数据以及目标定位数据；

4、根据目标点云数据、目标位姿数据以及目标定位数据，确定出至少三种因子所对应的转换特征；其中，转换特征包括位置转换特征、偏转角转换特征、速度转换特征以及加速度转换特征中任意至少一种；

5、基于所有因子所对应的转换特征建立车辆运动模型，并根据车辆运动模型与每种因子所对应的位置特征之间的位置关系，得到与每种因子对应的目标权重值；

6、对每种因子所对应的转换特征以及目标权重值进行加权求和计算，得到目标转换特征；

7、根据目标转换特征生成地图，并当检测到投料车遇到障碍物时，对地图进行更新处理。

8、在第一方面的一种可选方案中，对初始点云数据、初始位姿数据以及初始定位数据进行配准处理，得到与投料车的车体坐标系所对应的目标点云数据、目标位姿数据以及目标定位数据，包括：

9、分别对初始点云数据、初始位姿数据以及初始定位数据进行卡尔曼滤波处理，得到第一点云数据、第一位姿数据以及第一定位数据；

10、按照预设的时间间隔，对初始点云数据以及第一点云数据进行拉格朗日插值处理，得到第二点云数据；

11、按照预设的时间间隔，对初始位姿数据以及第一位姿数据进行拉格朗日插值处理，得到第二位姿数据；

12、按照预设的时间间隔，对初始定位数据以及第一定位数据进行拉格朗日插值处理，得到第二定位数据；

13、根据预设的旋转矩阵以及平移矩阵，对第二点云数据、第二位姿数据以及第二定位数据分别进行坐标转换处理，得到与投料车的车体坐标系所对应的目标点云数据、目标位姿数据以及目标定位数据。

14、在第一方面的又一种可选方案中，根据目标点云数据、目标位姿数据以及目标定位数据，确定出至少三种因子所对应的转换特征，包括：

15、分别计算出第n时刻对应的目标位姿数据与第n-1时刻对应的目标位姿数据之间的第一斜率，以及第n-1时刻对应的目标位姿数据与第n-2时刻对应的目标位姿数据之间的第二斜率，并当检测到第一斜率与第二斜率之间的差值超过预设差值阈值时，将与第n时刻对应的目标点云数据、目标位姿数据以及目标定位数据作为关键帧数据；

16、根据所有关键帧数据确定出至少三种因子所对应的转换特征。

17、在第一方面的又一种可选方案中，至少三种因子包括预积分因子、激光里程计因子以及北斗定位因子；

18、根据所有关键帧数据确定出至少三种因子所对应的转换特征，包括：

19、对任意两个相邻的关键帧数据中的目标位姿数据进行积分计算，得到与预积分因子对应的转换特征；

20、计算出每个关键帧数据的目标点云数据中每个点的曲率，根据所有点的曲率提取出与每个目标点云数据对应的边缘特征以及平面特征；

21、对第s个关键帧数据中的目标点云数据所对应的边缘特征以及平面特征，与前s-1个关键帧数据中的目标点云数据所对应的边缘特征以及平面特征进行匹配，得到相似度超过预设相似度阈值的匹配特征，并根据匹配特征得到与激光里程计因子对应的转换特征；

22、对任意两个相邻的关键帧数据中的目标定位数据构建线性变换函数，得到与北斗定位因子对应的转换特征。

23、在第一方面的又一种可选方案中，至少三种因子还包括闭环因子；

24、根据所有关键帧数据确定出至少三种因子所对应的转换特征，还包括：

25、计算出第m个关键帧数据中的目标位姿数据与前m-1个关键帧数据中的目标位姿数据之间的距离差值，并根据距离差值处于预设距离区间，且时间间隔大于预设间隔区间所对应的关键帧数据，以及第m个关键帧数据得到与闭环因子对应的转换特征。

26、在第一方面的又一种可选方案中，基于所有因子所对应的转换特征建立车辆运动模型，并根据车辆运动模型与每种因子所对应的位置特征之间的位置关系，得到与每种因子对应的目标权重值，包括：

27、基于预设权重值对所有因子所对应的转换特征进行加权求和计算，得到第一转换特征；

28、将第一转换特征代入至预设的表达式中，得到车辆运动模型；

29、分别计算出相同时间内所述车辆运动模型对应的位置坐标与每种因子所对应的转换特征中位置坐标之间的距离，并根据每种因子所对应的距离之间的比例关系得到与每种因子对应的目标权重值。

30、在第一方面的又一种可选方案中，根据目标转换特征生成地图，包括：

31、将与每个目标点云数据对应的边缘特征以及平面特征转换为与世界坐标系对应的目标边缘特征以及目标平面特征；

32、根据与每个目标点云数据对应的目标边缘特征以及目标平面特征、目标转换特征生成地图。

33、第二方面，本技术实施例提供了一种面向饲养场的投料车自动导航定位装置，包括：

34、信息采集模块，用于获取投料车在多个位置对应的初始点云数据、初始位姿数据以及初始定位数据，并对初始点云数据、初始位姿数据以及初始定位数据进行配准处理，得到与投料车的车体坐标系所对应的目标点云数据、目标位姿数据以及目标定位数据；

35、因子确定模块，用于根据目标点云数据、目标位姿数据以及目标定位数据，确定出至少三种因子所对应的转换特征；其中，转换特征包括位置转换特征、偏转角转换特征、速度转换特征以及加速度转换特征中任意至少一种；

36、模型构建模块，用于基于所有因子所对应的转换特征建立车辆运动模型，并根据车辆运动模型与每种因子所对应的位置特征之间的位置关系，得到与每种因子对应的目标权重值；

37、特征计算模块，用于对每种因子所对应的转换特征以及目标权重值进行加权求和计算，得到目标转换特征；

38、地图定位模块，用于根据目标转换特征生成地图，并当检测到投料车遇到障碍物时，对地图进行更新处理。

39、第三方面，本技术实施例还提供了一种面向饲养场的投料车自动导航定位装置，包括处理器以及存储器；

40、处理器与存储器连接；

41、存储器，用于存储可执行程序代码；

42、处理器通过读取存储器中存储的可执行程序代码来运行与可执行程序代码对应的程序，以用于实现本技术实施例第一方面或第一方面的任意一种实现方式提供的面向饲养场的投料车自动导航定位方法。

43、第四方面，本技术实施例提供了一种计算机存储介质，计算机存储介质存储有计算机程序，计算机程序包括程序指令，程序指令当被处理器执行时，可实现本技术实施例第一方面或第一方面的任意一种实现方式提供的面向饲养场的投料车自动导航定位方法。

44、在本技术实施例中，可在对养殖场内的投料车进行导航定位时，获取投料车在多个位置对应的初始点云数据、初始位姿数据以及初始定位数据，并对初始点云数据、初始位姿数据以及初始定位数据进行配准处理，得到与投料车的车体坐标系所对应的目标点云数据、目标位姿数据以及目标定位数据；根据目标点云数据、目标位姿数据以及目标定位数据，确定出至少三种因子所对应的转换特征；基于所有因子所对应的转换特征建立车辆运动模型，并根据车辆运动模型与每种因子所对应的位置特征之间的位置关系，得到与每种因子对应的目标权重值；对每种因子所对应的转换特征以及目标权重值进行加权求和计算，得到目标转换特征；根据目标转换特征生成地图，并当检测到投料车遇到障碍物时，对地图进行更新处理。通过对获取到的不同类型数据进行处理，得到与至少三种因子所对应的转换特征，并结合构建的车辆运动模型对该转换特征进行优化处理，以根据优化处理后的转换特征生成地图，一方面可采用激光雷达、imu以及北斗定位等传感器，发挥不同传感器各自的优点，实现优势互补；另一方面结合车辆运动模型提升定位精度，通过对转换特征进行优化处理的方式，充分考虑场景变化以及北斗信号在室内不稳定等问题，不仅提高导航定位的适用性以及精度，还可满足全天候、室内室外变化等使用要求。