[发明专利]一种无光动态开放环境下AGV车辆建图与自主导航避障方法

权利要求书

1.一种无光动态开放环境下AGV车辆建图与自主导航避障方法，其特征在于，包括如下步骤：

S1、获取多传感器参数，对多传感器进行相互标定得到多传感器之间的外参，其中，所述多传感器包括：里程计、激光雷达、深度相机和惯性导航；

S2、利用步骤S1标定的多传感器外参融合2D激光雷达参数求解车辆位置姿态信息,具体包括：

S21、通过里程计与惯性导航获取的信息融合求解车辆在不平整路况上的位置姿态；

S22、通过激光雷达与深度相机获取的信息融合求解车辆在未知空间上的位置姿态，其中，具体包括如下子步骤：

S221、分别利用激光雷达与深度相机从位置环境中获取二维激光数据和点云图像；

S222、将步骤S221得到的点云图像进行处理得到虚拟的二维激光数据；

S223、将步骤S222得到的虚拟的二维激光数据和激光雷达测量的数据进行融合得到车辆的位置姿势；

S23、将步骤S21和步骤S22中求解的车辆位置姿态通过UKF融合优化，得到优化后的车辆位置姿态；

S3、对步骤S2求解的车辆位置姿态信息进行检测及优化，通过优化后的位姿关系对栅格地图进行调整，得到未知三维场景的全局地图；

S4、扫描构建局部地图融合步骤S3得到的未知三维场景的地图对车辆进行动态环境的三维避障与导航。

2.根据权利要求1所述的无光动态开放环境下AGV车辆建图与自主导航避障方法，其特征在于，步骤S1具体包括：里程计与激光雷达外参标定、深度相机与激光雷达外参标定以及深度相机与惯性导航的外参标定，其中：

所述里程计与激光雷达外参标定方法为：以激光雷达的扫描-匹配数据作为真值，里程计测量到的数据作为测量值，通过直接线性法求解出两者之间的外参；

所述深度相机与激光雷达外参标定方法为：通过相机捕获的标定板点云拟合出标定板平面的距离，然后将激光点云通过外参旋转到相机坐标系，再通过旋转后的激光点云到平面的距离进行优化求解相机与激光雷达之间的外参；

所述深度相机与惯性导航的外参标定方法为：通过对标定板进行角点提取计算深度相机的旋转量，通过对陀螺仪数据积分求解惯性导航的旋转量，然后通过最小二乘法求解两者之间的外参。

3.根据权利要求1所述的无光动态开放环境下AGV车辆建图与自主导航避障方法，其特征在于，步骤S21中信息融合的方式为：

通过上一时刻的状态对下一时刻的状态进行预测后，采用复合位姿模型的观测量进行校正。

4.根据权利要求1所述的无光动态开放环境下AGV车辆建图与自主导航避障方法，其特征在于，步骤S3中使用最小二乘法求解车辆的最优位姿，表示为：

其中，函数含义为使目标函数达到最小时所取得变量值，ξ为AGV车辆位姿，T_ξ为转换矩阵，T_ξ表示将地图点h_k由当前扫描帧到局部地图中的位姿变换关系，k为地图点序号，K为地图点的个数，M_smooth表示对局部地图中概率值进行平滑处理函数。

5.根据权利要求4所述的无光动态开放环境下AGV车辆建图与自主导航避障方法，其特征在于，步骤S4具体包括：

S41、加载全局地图后使用2D匹配车辆的位置，并对车辆位置周围的动态障碍进行检测；

S42、在移动导航的同时进行局部地图的扫描并在局部地图中构建动态人工势力场，对移动车辆进行动态环境的避障与导航。

6.根据权利要求5所述的一种无光动态开放环境下AGV车辆建图与自主导航避障方法，其特征在于，步骤S42具体包括：

S421、在传统人工势力场上加入速度项，在局部地图中构建动态人工势力场模型；

S422、对其中的动态人工势力场进行计算得到动态障碍物场景的势力函数；

S423、根据步骤S422得到的势力函数对障碍物场景进行势力场中的受力分析，得到车辆在动态障碍物下运动导航的总势力场。