[发明专利]一种面向饲养场的投料车自动导航定位方法及装置

权利要求书

1.一种面向饲养场的投料车自动导航定位方法，其特征在于，包括：

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

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

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

对每种所述因子所对应的转换特征以及目标权重值进行加权求和计算，得到目标转换特征；根据所述目标转换特征生成地图，并当检测到所述投料车遇到障碍物时，对所述地图进行更新处理；

其中，所述根据所述目标点云数据、所述目标位姿数据以及所述目标定位数据，确定出至少三种因子所对应的转换特征，包括：

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

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

其中，所述至少三种因子包括预积分因子、激光里程计因子以及北斗定位因子；

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

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

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

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

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

2.根据权利要求1所述的方法，其特征在于，所述对所述初始点云数据、所述初始位姿数据以及所述初始定位数据进行配准处理，得到与所述投料车的车体坐标系所对应的目标点云数据、目标位姿数据以及目标定位数据，包括：

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

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

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

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

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

3.根据权利要求1所述的方法，其特征在于，所述至少三种因子还包括闭环因子；

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

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