一种基于迭代卡尔曼滤波的港口环境定位方法、系统及介质与流程

本发明涉及无人集卡定位，尤其是涉及一种基于迭代卡尔曼滤波的港口环境定位方法、系统及介质。

背景技术：

1、随着世界经济的发展，我国的海上贸易份额逐年增多，为了更好的提升港口运输效率，提出了建设自动化码头的要求。随着自动驾驶技术的成熟，如今港口集装箱运营逐渐从自动化迈向智能化，无人集卡被更多的应用在港口集装箱从码头到堆场之间水平运输的这一环节。无人化的作业模式可以将作业者从繁重、单一、重复的生产过程中解放出来，大大降低运营工作运行成本，提升效率。通过搭载的高精度传感器及智能控制算法，在港口这种路线较为固定的场景，无人集卡相比于通过人来驾驶的有人集卡，具有调度方便、操作精度高的优势，在安全性和经济性上都有很高的提升潜力，因此成为下一阶段发展研究的热点。精准的定位是实现全流程智能化的前提条件，集装箱在港口内的转运首先通过岸桥将集装箱从货船转移到无人集卡，其次无人集卡接收到指令搭载集装箱从岸桥驶出前往特定集装箱堆场等待卸货，最后通过轮胎吊抓取将集装箱卸载到指定位置安置。行业内对港口自动驾驶的要求每百米横向偏差10cm，航向角变化0.2，港口内的集装箱抓取要求误差在5cm之内，为了达到提升作业效率的目的，保证定位的精度是前提。

2、现有技术中，专利（申请号：202210951408.1）公开了一种基于rtk的港口车辆高精度自适应定位解算方法，包括：步骤1，安装基准站；步骤2，获取基准站数据；步骤3，云解算平台存储基准站数

3、据，接收基准站观测的卫星数据，对数据进行分类去重、解码，保存在数据库中；步骤4，获取移动站数据，通过5g私有网络连接云解算平台，上传移动站卫星观测数据；步骤5，自适应位置解算，下发基准站卫星观测数据给移动站，移动站校验后对数据进行解码，自适应选择位置解算平台。但是常规依赖gps组合导航数据的定位方式难以满足港口环境下对自动驾驶精度的需求，gps技术在使用时要求载体与卫星有良好的可视性，无人集卡的工作场景包括岸桥、港口内部道路以及堆场，岸桥、轮胎吊等钢结构大型起重设备以及堆场内存放的集装箱都会对gps终端天线造成遮挡，导致gps定位精度下降明显甚至产生定位跳变现象；金属材质的集装箱以及靠近岸桥的海水会反射卫星的电波信号，gps终端天线受到多种反射波，绕射波、透射波的干扰也会导致信号失真，输出错误的数据；此外，gps的更新频率较低，为10hz，难以保障无人集卡实时定位的准确性，其差分服务可能会由于现场网络传输不稳定产生通信盲区导致短期中断，失去厘米级定位精度，以上这些干扰导致无人集卡无法在港口全部作业场景下都达到高精度定位要求。

4、现有技术中，专利（申请号：202011460748.1）公开了一种无导航卫星信号下的自主定位方法，采用正态分布变换同步定位与地图生成 (ndt-slam)的自主定位方法，通过正态分布变换占用地图(ndt-om)方法形成基于三维激光雷达扫描数据的ndt-om局部地图，并通过ndt-d2d匹配方法完成局部ndt-om地图之间的匹配，并在此基础上融合由惯性导航系统(ins)与里程计生成的位置估计信息与局部地图匹配信息。堆场内集装箱摆放、轮胎吊、岸桥及车辆的作业位置随着时间动态变化，道路上的标识牌、挡块等环境要素也会随着作业任务不同而变化；港口作为船舶的货物作业、加油作业、船员等监管要素的聚集之地，岸桥下可能会临时安放负责拆除绑扎杆和扭锁的扭锁箱以及货物舱盖板等物体形成障碍；目前大多数智慧运营码采用有人集卡与无人集卡混合作业的方式，有人集卡难以监管，存在违规占道、超速行驶、违停等现象，其行为存在不可控性，同样会对无人集卡的运行造成干扰。在作业频繁、动态障碍物多、环境信息变化较大的港口环境下，预先建立的高精度地图的实效性难以保证，无法提供精准的定位信息。

技术实现思路

1、鉴于以上现有技术的不足，本发明提供了一种基于迭代卡尔曼滤波的港口环境定位方法、系统及介质，不仅为无人集卡集装箱装运过程提供定位与自主避障提供信息，而且不依赖与岸基端设备的信息交互，拓展性、可移植性强，可应用在大部分无人集卡上而无需对车体进行改造；不易受复杂环境影响，使用激光雷达作为感知环境信息的传感器具有360°的全向探测范围。

2、为了实现上述目的及其他相关目的，本发明提供的技术方案如下：

3、一种基于迭代卡尔曼滤波的港口环境定位方法，所述方法包括：

4、q1.无人集卡行驶在港口道路上，基于车载激光雷达实时获取道路的点云数据信息，基于车载imu实时获取车辆的位姿数据信息，基于车载组合导航实时获取车辆的roll和pitch数据信息；

5、q2.基于所述道路的点云数据信息，采用时间同步插值优化算法分别对所述车辆的位姿数据信息和所述车辆的roll和pitch数据信息进行时间同步优化，得到同一时刻的点云数据信息、车辆的位姿数据信息和车辆的roll和pitch数据信息；

6、q3.基于所述同一时刻的车辆的位姿数据信息和车辆的roll和pitch数据信息，采用迭代卡尔曼滤波状态预测算法对车辆的位姿进行预测，得到预测后的车辆的位姿数据信息；

7、q4.基于所述预测后的车辆的位姿数据信息，采用点云去畸变算法对所述同一时刻的点云数据信息进行去畸变，得到去畸变后的点云数据信息；

8、q5.将所述去畸变后的点云数据信息输入体素空间，构建局部地图，对车辆进行定位，输出车辆的定位数据信息。

9、进一步的，在步骤q2中，所述采用时间同步插值优化算法分别对所述车辆的位姿数据信息和所述车辆的roll和pitch数据信息进行时间同步优化包括：

10、q21.基于所述道路的点云数据信息、所述车辆的位姿数据信息和所述车辆的roll和pitch数据信息，获取当前时刻的道路点云对应的上一时刻和下一时刻的车辆的姿态数据信息，并获取当前时刻的道路点云对应的上一时刻和下一时刻的车辆的roll和pitch数据信息；

11、q22.基于所述当前时刻的道路点云对应的上一时刻和下一时刻的车辆的姿态数据信息，建立时间同步插值优化函数g，

12、，

13、其中，(x,y)为车辆的位置数据信息，θ为车辆的航向角数据信息，(x0,y0)为车辆的初始位置数据信息，θ0为车辆的初始航向角数据信息，(xt,yt,θt)为t时刻车辆的姿态数据信息，(xt+1,yt+1,θt+1)为t+1时刻车辆的姿态数据信息，ɑ0，ɑ1和ɑ3为时间同步的权重因子，得到同一时刻的车辆位姿数据信息；

14、q23.基于所述当前时刻的道路点云对应的上一时刻和下一时刻的车辆的roll和pitch数据信息，建立插值优化函数h，

15、，

16、其中，m为车辆的roll或pitch数据信息，m0为车辆的roll或pitch的初始化数据信息，mt为t时刻的车辆的roll或pitch数据信息，mt+1为t+1时刻的车辆的roll或pitch数据信息，β0为车辆的roll或pitch的同步优化因子，得到同一时刻的车辆的roll和pitch数据信息。

17、进一步的，所述时间同步的权重因子ɑ0，ɑ1和ɑ3的约束条件为，

18、，

19、所述车辆的roll或pitch的同步优化因子β0的约束条件为，

20、0<β0<1。

21、进一步的，在步骤q3中，所述采用迭代卡尔曼滤波状态预测算法对车辆的位姿进行预测包括：

22、q31.基于所述同一时刻的车辆的位姿数据信息，建立车辆位姿的状态变量函数mt，

23、mt+1=amt+but+wt，

24、其中，t为不同时刻，a为车辆的状态矩阵，b为车辆的控制矩阵，ut为t时刻车辆的控制变量，wt为t时刻过程噪音，得到车辆的位姿的状态变化数据信息；

25、q32.根据所述同一时刻的车辆的roll和pitch数据信息，建立卡尔曼状态更新函数，

26、,

27、其中，k为卡尔曼增益系数，ztt为t时刻车辆的roll和pitch数据信息，i为车辆的状态系数矩阵，h为状态空间转换系数矩阵，jt为t时刻车辆的状态协方差矩阵；

28、q33.基于所述卡尔曼状态更新函数和所述车辆的位姿的状态变化数据信息，建立车辆位姿的预测函数y，

29、，

30、其中，mt+1为车辆位姿的状态变量函数,得到预测后的车辆位姿数据信息。

31、进一步的，在步骤q32中，所述卡尔曼增益系数k为，

32、k=(htr-1h+p-1)-1htr-1，

33、其中，h为状态空间转换系数矩阵，r为测量噪声矩阵，p为误差的协方差矩阵。

34、进一步的，在步骤q4中，采用点云去畸变算法对所述同一时刻的点云数据信息进行去畸变包括：

35、q41.基于所述预测后的车辆的位姿数据信息，对所述同一时刻的点云数据信息进行检索，并建立去畸变函数d，

36、d=t_lidar_imu*t_imu-end_w*t_w_imu-pn*t_imu_lidar*p_lidar-pn，

37、其中，t_lidar_imu、t_imu_lidar为激光雷达与imu之间的外参，t_w_imu-pn为在pn时刻imu在世界坐标系下的坐标，t_imu-end_w为在end时刻imu在世界坐标系下的坐标，p_lidar-pn为pn时刻雷达在世界坐标系下的坐标；

38、q42.基于所述去畸变函数d，对点云数据信息进行去畸变，得到去畸变后的点云数据信息。

39、进一步的，所述将所述去畸变后的点云数据信息输入体素空间，构建局部地图为根据伪希尔伯特空间填充曲线来建立体素内所有细分空间单元，耦合去畸变后的点云数据信息，构建局部地图。

40、为了实现上述目的及其他相关目的，本发明还提供了一种用于实现任一项所述的基于迭代卡尔曼滤波的港口环境定位方法的系统，所述系统包括：

41、多传感器数据获取模块，用于实时获取道路的点云数据信息、车辆的位姿数据信息和车辆的roll和pitch数据信息；

42、时间同步优化模块，与所述多传感器数据获取模块连接，用于采用时间同步插值优化算法分别对车辆的位姿数据信息和车辆的roll和pitch数据信息进行时间同步优化；

43、车辆位姿预测模块，与所述时间同步优化模块连接，用于采用迭代卡尔曼滤波状态预测算法对车辆的位姿进行预测；

44、点云去畸变模块，与所述车辆位姿预测模块连接，用于采用点云去畸变算法对同一时刻的点云数据信息进行去畸变；

45、局部地图构建模块，与所述点云去畸变模块连接，用于将去畸变后的点云数据信息输入体素空间，构建局部地图，对车辆进行定位。

46、进一步的，所述系统还包括实时显示模块，与所述局部地图构建模块连接，用于实时显示局部地图和车辆的定位数据信息。

47、为了实现上述目的及其他相关目的，本发明还提供了一种计算机可读存储介质，该计算机可读存储介质上存储有被编程或配置以执行任意一项所述的基于迭代卡尔曼滤波的港口环境定位方法的计算机程序。

48、本发明具有以下积极效果：

49、1.本发明通过采用时间同步插值优化算法分别对车辆的位姿数据信息和车辆的roll和pitch数据信息进行时间同步优化，结合迭代卡尔曼滤波状态预测算法对车辆的位姿进行预测，为无人集卡集装箱装运过程提供定位与自主避障提供信息，抗干扰能力强，在gps传感器信号缺失或信号弱的情况下依然能保证定位精度；不依赖与岸基端设备的信息交互，拓展性、可移植性强，可应用在大部分无人集卡上而无需对车体进行改造，不易受复杂环境影响，使用激光雷达作为感知环境信息的传感器具有360°的全向探测范围。

50、2.本发明采用点云去畸变算法对所述同一时刻的点云数据信息进行去畸变，并结合体素空间，构建局部地图，对车辆进行定位，最大程度上的降低了环境中光照、雨雪、海雾等因素的影响；根据港口应用环境特点，利用全部点云数据进行匹配，以更加充分地利用环境信息，采用数据级紧耦合的融合方式对原始数据的利用度最大，具有融合速度快、处理精度高等优点，由于环境点云在无人船的行进过程中是不断获取的，因此通过点云地图定位的方式能够得到一直更新的车辆位姿信息，从而提升了低速小位移时的位姿的准确性，满足自动化码头对水平运输车辆定位和导航的要求。