[发明专利]一种激光导引AGV自然导航定位方法

权利要求书

1.一种激光导引AGV自然导航定位方法，其特征在于，无需人工设置反射胶贴，不需要对环境做人工标志或者改动，只需预先对行程路线周围自然环境中的固定物体作为AGV导航定位的参照物，通过对扫描的静止物体的轮廓进行正态分布变换的数学建模，完成AGV导航地图的建立和位姿的计算，用作激光导引AGV自然导航定位；具体步骤包括：

S1：传感器数据采集，完成激光扫描器、行走编码器、转向编码器、陀螺仪传感数据的采集、滤波工作；

S2：激光里程计，根据步骤S1中所采集的数据并结合AGV的运动方程，采用正态分布变换(NDT)算法完成相邻时刻激光扫描数据点的配准，即SCAN-to-SCAN的配准，并给出参考扫描和源扫描之间的运动状态估计；

所述参考扫描为k-1时刻的扫描，所述源扫描为k时刻的扫描；

S3：状态估计，分为建图和导航两种模式：

所述建图模式是根据激光扫描器的观测方程和步骤S3中激光里程计提供的不同时刻的相对状态估计及步骤S4中提供的闭环检测信息对AGV的位姿进行最优状态估计，以建立全局一致的运动轨迹；

所述导航模式是根据激光扫描器的观测方程和步骤S3中激光里程计提供的不同时刻的相对状态估计并采用NDT算法完成SCAN-to-MAP的配准，计算出AGV的最优状态；

S4：闭环检测，通过将AGV当前状态与历史状态进行比较检测AGV行驶路径是否闭环，用于建立步骤S3所述的全局一致的运动轨迹；

S5：地图构建与地图编辑，根据步骤S3的最优状态估计，建立与任务向对应的地图，并通过平移、旋转等变换方法手动调整地图的坐标系，同时手动删除地图中建立的不良NDT单元，并保存编辑后的NDT地图；

S6：导航定位，根据所述步骤S3的最优状态估计和所述步骤S5建立的NDT地图，通过NDT算法完成SCAN-to-MAP的配准，实时给出AGV位姿的最优估计，完成AGV的实时导航定位。

2.根据权利要求1所述的一种激光导引AGV自然导航定位方法，其特征在于，所述步骤S1的传感器数据采集是通过TCP/IP连接激光扫描器和AGV主控制器，通过高速数字量采样模块完成行走编码器和转向编码器的数据采集，通过但不限于RS232/RS485串行通信来实现AGV主控制器对陀螺仪的数据采集。

3.根据权利要求1或2所述的一种激光导引AGV自然导航定位方法，其特征在于，所述步骤S1的滤波是AGV主控制器依据所采集激光扫描数据的回波强度、距离等信息并结合AGV的当前运动状态进行原始数据滤波，同时AGV主控制器还依据当前AGV的运动姿态完成对陀螺仪数据的滤波工作，降低输入噪声对所述步骤S2和步骤S3的干扰。

4.根据权利要求1或2所述的一种激光导引AGV自然导航定位方法，其特征在于，所述步骤S2的AGV运动方程描述为从k-1时刻到k时刻的运动，数学描述为：X_k＝X_k-1+u_k+ω_k，其中X_k＝[x,y,θ]_k^T为k时刻AGV的状态，X_k-1＝[x,y,θ]_k-1^T为k-1时刻AGV的状态，u_k＝[Δx,Δy,Δθ]_k^T为k时刻的输入，即所述行走编码器和转向编码器的输出，ω_k为k时刻的输入噪声，至此可得到AGV在k时刻的状态估计。