[发明专利]导航定位方法、装置、系统及存储介质

权利要求书

1.一种导航定位方法，其特征在于，应用于装车机的控制设备，所述装车机上安装有工业相机和多个激光雷达，与所述控制设备配合的激光发射器用于照射激光线至货车的车厢底板上，所述控制设备与所述激光雷达和所述工业相机通信连接，所述方法包括：

实时获取一组采集数据，所述采集数据包括在同一位置同一时刻，各所述激光雷达采集自身所正对的货车车厢区域的点云数据，以及所述工业相机采集的所述货车的车厢底板图像；

根据所述点云数据得到每个所述激光雷达与所述货车的车厢边沿的距离，根据所有所述距离，计算出所述装车机与所述货车中轴线的第一偏角，以及所述装车机在所述货车车厢中的偏移量；

根据所述第一偏角和所述偏移量实时控制所述装车机在所述货车车厢中的行驶；

从所述车厢底板图像中提取出激光线数据，根据所述激光线数据计算出所述装车机与激光线间的第二偏角，将由同一组所述采集数据获得的所述第一偏角和所述第二偏角作为一组偏角数据进行记录；

在装车机到达装车位置后，根据多组所述偏角数据拟合出偏角估计值，根据所述偏角估计值调整所述装车机的装车角度。

2.根据权利要求1所述的导航定位方法，其特征在于，所述激光雷达为三个，包括第一雷达、第二雷达和第三雷达，所述第一雷达安装于所述装车机的一侧，所述第二雷达安装于所述装车机的另一侧，且所述第一雷达和第二雷达的连线与所述装车机的宽度方向平行，所述第三雷达与所述第二雷达位于所述装车机的同一侧；

所述根据所有所述距离，计算出所述装车机与所述货车中轴线的第一偏角，以及所述装车机在所述货车车厢中的偏移量的步骤，包括：

基于所述第二雷达和所述第三雷达与所述货车的车厢边沿的距离，以及所述第二雷达和所述第三雷达之间的安装距离，计算出所述装车机与所述货车中轴线的第一偏角；

根据所述第一偏角，所述第一雷达与所述货车的车厢边沿的距离，以及所述第二雷达与所述货车的车厢边沿的距离，计算出所述装车机在所述货车车厢中的偏移量。

3.根据权利要求2所述的导航定位方法，其特征在于，所述计算出所述装车机与所述货车中轴线的第一偏角的步骤，包括：

利用第一偏角计算公式，计算出所述装车机与所述货车中轴线的第一偏角；

其中，所述第一偏角计算公式包括：θ_ladar表征第一偏角，FR表征第二雷达与货车的车厢边沿的距离，BR表征第三雷达与货车的车厢边沿的距离，L表征第二雷达与第三雷达之间的安装距离；

所述计算出所述装车机在所述货车车厢中的偏移量的步骤，包括：

利用偏移量计算公式，得到所述装车机在所述货车车厢中的偏移量；

其中，所述偏移量计算公式包括：FL表征第一雷达与货车的车厢边沿的距离，shfit表征偏移量。

4.根据权利要求1所述的导航定位方法，其特征在于，所述从所述车厢底板图像中提取出激光线数据，根据所述激光线数据计算出所述装车机与激光线间的第二偏角的步骤，包括：

将所述车厢底板图像中像素值大于灰度阈值的像素点作为目标像素点，并提取各所述目标像素点的坐标作为激光线数据；

基于各个所述坐标，拟合出关于各个所述目标像素点的拟合直线；

根据所述拟合直线的系数，采用三角函数，得到所述装车机与激光线间的第二偏角。

5.根据权利要求4所述的导航定位方法，其特征在于，所述根据所述拟合直线的系数，采用三角函数，得到所述装车机与激光线间的第二偏角的步骤，包括：

基于所述拟合直线的系数，利用第二偏角计算公式，得到所述装车机与激光线间的第二偏角；

所述第二偏角计算公式包括：

其中，θ_laser表征第二偏角，k表征拟合直线的系数。