CN111457929A - 一种基于地理信息系统的物流车辆自主路径规划与导航方法 - Google Patents

Abstract

本发明公开了一种基于地理信息系统的物流车辆自主路径规划与导航方法，包括步骤为：建立物流车辆自身运动模型及车身自带激光雷达的观测模型；从GIS上规划出一条从起始点到目标送货点的最佳路线；利用ArcGIS软件将园区路线上的经纬度坐标点转换为物流车系统中可识别的直角栅格坐标；保证物流车辆与障碍物的安全距离，实时解算物流车行进时的无碰撞速度轨迹，同时躲避前方可能的动态或静态障碍物，将速度指令发送给物流车底盘执行机构，到达目标送货区域。通过上述方式，本发明基于GIS规划的全局路线，对整个导航过程具有全局引导作用，避免了物流车辆在进行局部规划时陷入局部区域，可提升园区内物流输送的安全性和可靠性。

Description

一种基于地理信息系统的物流车辆自主路径规划与导航方法

技术领域

本发明涉及无人车辆自主导航领域，特别是涉及一种基于地理信息系统的物流车辆自主路径规划与导航方法。

背景方法

当代社会电子商务方法、物联网方法的快速发展，物流输送特别是厂家园区、仓库等地区的物流运送能力变得日益重要。随着人工智能方法的出现，物流输送工具和方式正在向智能化、无人化方向快速发展，成为物流输送体系的重要环节。早期传统的物流输送环节主要依靠快递人员及交通工具完成，近年来人工智能方法尤其是无人驾驶方法的出现，使得物流运送能力得到了显著提升，极大缓解了物流输送货物积压等的巨大压力，整个过程也更加无人化和安全可靠。

上世纪80年代以来，美国等就研发出能自动识别货物并规划路线的自动物流输送系统。国内京东物流也正式投入使用了国内首个最大的机器人仓群进行无人化货物输送，全面提升了运营效率。事实上，针对园区物流的自主输送包括两个关键步骤：精准便捷的区域输送路线和快速安全的车辆避障能力。目前物流输送环节主要依靠磁导航或GPS定位导航实现，难以根据园区内的实际建筑及道路分布规划配送路线并规划避障路径。对于车辆运送过程中的实时动态避障，由于货物体积较大且实时性要求高，不能满足输送的安全性要求。实时规划园区物流配送路线并实现有效安全的避障是今后物流车辆自动配送领域的必然趋势。

发明内容

本发明提供一种基于GIS的物流车辆路径规划方法。不仅能够实现园区等区域物流输送路线的有效规划，也能满足车辆的实时避障要求。

为解决上述方法问题，本发明采用的一个方法方案是：提供一种基于GIS的物流车辆路径规划方法，其特征在于包括以下步骤：

一种基于地理信息系统的物流车辆自主路径规划与导航方法，其特征在于，包括以下步骤：

步骤(1)、建立物流车辆与车载激光雷达的运动模型及环境观测模型；

步骤(2)、根据所述步骤(1)中物流车辆与激光雷达运动及观测的数学模型，在GIS中绘制园区地图，规划物流车出发点到目标送货点的全局路线图；

步骤(3)、根据所述步骤(2)中绘制好的GIS园区地图及全局路线，对路线中各点经纬度坐标进行转换和提取；

步骤(4)、根据所述步骤(3)中转换所得直角栅格坐标，确定车辆行驶路径，并利用AMCL初始化车体自身位姿；

步骤(5)、根据所述步骤(4)中确定的车辆姿态信息进行定位的同时，通过局部路径规划，解算最优速度轨迹；

步骤(6)、根据所述步骤(5)中速度轨迹，将生成的速度指令发送给物流车底盘执行机构，到达目标送货区域。

本发明还提供另一种基于地理信息系统的物流车辆自主路径规划与导航方法，其特征在于包括以下步骤：

步骤(1)建立物流车辆自身运动学模型，其中车辆自身采用二轮运动差分模型，由车体前部导轮保持车体平衡；

步骤(2)确定激光雷达观测模型；

步骤(3)在ArcGIS中绘制所在园区地图，导出从起始点至目标区域的全局路线经纬度序列数据；

步骤(4)借助ArcGIS软件，将全局路线中各经纬度坐标转换为直角栅格坐标，并生成车辆的速度和位移指令；

步骤(5)通过AMCL初始化物流车辆在栅格地图中的位姿，根据所述步骤(4)中的指令，由物流车自带的自动建图算法进行建图导航任务；

步骤(6)以DWA算法为基础，提出向轨迹评价函数的速度可行域区间加入无碰撞安全系数，使得物流车与障碍物保持适当安全距离；

步骤(7)局部规划器计算速度指令发送给车辆底盘执行机构；物流车行进过程中，由于位置误差或进入局部未知区域等导致物流车行进途中受困，此时进入车辆行为恢复模式，处理导航异常情况；

步骤(8)按照既定路线，物流车将货物输送到目标送货点。

所述步骤(1)的具体步骤如下：

步骤(1-1)建立物流车辆自身运动模型：运动模型的输入主要由三个因素构成，分别是控制输入量u_t、物流车上一时刻状态X_t-1、来自于周边环境及车体本身的噪声w_t，则描述车体系统状态变化X_t为：

X_t＝f(X_t-1，u_t，w_t)

其中，f为系统的状态转移函数，w_t一般为零均值白噪声；可结合园区等实际的环境状况，物流车的运动模型可表示为：

其中，X_r表示为车辆的位姿矩阵

为系统噪声矩阵；

步骤(1-2)建立物流车辆观测模型，其观测模型可写为

其中，Y_t为t时刻系统的观测量，h为观测函数，δ_(t)是观测过程中的误差。

所述步骤(2)的具体步骤如下：

步骤(2-1)建立物流车的2D激光雷达模型。设在雷达的一个扫描周期内，可获得数据点个数为n,则可观测到的扫描数据点个数

雷达获取的扫描数据可表示为(ρ_i，θ_i),其中ρ_i为雷达到探测点的距离，θ_i为激光雷达和探测点间连线和扫描范围中轴线夹角。

所述步骤(3)的具体步骤如下：

步骤(3-1)绘制物流车所在园区地图前的数据准备工作如一些主要地点的点、线、面等矢量数据和对应的栅格数据；

步骤(3-2)通过ArcGIS制图软件，加载所需要的地图数据，主要来源为国家基础地理信息库，并对数据进行渲染操作，增强地图路线等细节信息；

步骤(3-3)在ArcGIS中切换到地图制图模块，首先根据园区实际大小选择绘制地图的纸张规格，其次添加关键的地图图件信息如图例、图名、比例尺及经纬网格；其中经纬网格显示设置为度分秒形式，便于之后对其进行坐标转换操作；

步骤(3-4)将制图所用边界矢量数据添加到地图中，设置好保存格式、地图分辨率等同时输出地图；

步骤(3-5)在地图中选取物流车辆园区内的行进路线，导出经纬度数据到Excel表。

所述步骤(4)的具体步骤如下：

步骤(4-1)通过ArcMap导入经纬度数据Excel表并可视化，此时可看见所述步骤(3)中规划好的路线，并将数据导出为shp格式文件；

步骤(4-2)由ArcToolbox选择数据管理工具中的投影，选择数据集，设定输入坐标为经纬度坐标，输出坐标为直角坐标；

步骤(4-3)构造平面坐标，利用Feature的几何字段来获取平面坐标数据；在输出文件中找到Output.dbf文件，将其存入Excel表，坐标转换完成。

所述步骤(5)的具体步骤如下：

步骤(5-1)物流车自身搭载有Linux操作系统，车辆导航开始时启动AMCL定位初始化物流车位姿使其尽量靠近输送起始点；

步骤(5-2)自动建图导航算法通过雷达观测环境信息，利用贝叶斯估计即粒子滤波方法，获取物流车辆每一时刻的位姿估计：

预测：由t-1时刻的后验概率密度函数计算p(x_t-1|y_1：t-1)，得到t时刻的先验概率密度函数p(x_t|y_1：t-1)，

p(x_t|y_1：t-1)＝∫p(x_t|x_t-1)p(x_t-1|y_1：t-1)dx_t-1

其中，x表示系统的状态估计，y表示系统观测量；

更新：由t时刻得到的先验概率密度函数p(x_t|y_1：t-1)，结合环境观测量y_t，针对上一步的观测过程，获取后验概率密度p(x_t|y_1：t)，

其中，p(y_t|y_1：t-1)为归一化常数，表示为

p(y_t|y_1：t-1)＝∫p(y_t|x_t)p(x_t|y_1：t-1)dx_t

步骤(5-3)使用扩展卡尔曼滤波算法进行特征估计，获取周围局部区域的栅格地图；计算得到p(m|x_1：t，y_1：t),其中m表示环境中相互独立的路标；

步骤(5-4)通过Rao-Blackwellized粒子滤波方法，将同时定位与建图问题转化为上述求得的位姿估计及基于位姿估计的环境特征估计，

p(x_1：t，m|y_1：t，u_1：t-1)＝p(m|x_1：t，y_1：t，u_1：t-1)p(x_1：t|y_1：t，u_1：t-1)

＝p(m|x_1：t，y_1：t)p(x_1：t|y_1：t，u_1：t-1)

其中，t是采样时间，x_1：t是物流车辆的运动轨迹，y_1：t为2D激光雷达获得的观测信息，u_1：t-1是里程计测量值，其中里程计主要是车身内部自带的光电编码器，获取车身位姿的改变。

所述步骤(6)的具体步骤如下：

步骤(6-1)根据车辆所有可行速度包括线速度及角速度，生成速度可行区间，

V_s＝{(v，ω)|v∈[0，v_max]，ω∈[-ω_max，ω_max]}

其中，v_max，ω_max分别为对应的最大线速度和角速度；

步骤(6-2)由于车辆电机性能限制，同时考虑单个速度采样间隔，计算车辆真实速度变化范围，

其中车辆当前时刻速度为(v_c，ω_c)，线加速度取值边界为

角加速度取值边界为

步骤(6-3)考虑到车辆传感器自身存在噪声误差及采样间隔内的位姿误差等，且需保障物流车与障碍物保持足够安全距离，使得车辆及时在障碍物前停止运动，应满足

其中r_min为(v，ω)状态下的模拟速度轨迹距离障碍物的最近距离，k₁，k₂是添加的无碰撞安全系数，经过大量路径规划实验，可取k₁范围为0.6～1.0，k₂可取范围0.8～1.2；

步骤(6-4)通过以上各范围限定，车辆最终的速度采样空间应满足以上三个速度范围限制，取其交集得

v_r＝V_s∩V_d∩v_a

步骤(6-5)物流车根据生成的速度采样空间进行速度采样，并解算每一时间间隔内对应的所有的速度轨迹，通过评价得到最优速度轨迹，轨迹评价函数如下，

G(v，ω)α.heading(v，ω)+β.dist(v，ω)+γ.vel(v，ω)

其中，heading(v，ω)描述物流车辆与目标区域的朝向一致性，dist(v，ω)表示当前运行速度轨迹中距离障碍物的最小距离，vel(v，ω)衡量车辆当前运行速度，三者结合所得最高的评价函数得分可认为是最优速度轨迹。

所述步骤(7)的具体步骤如下：

步骤(7-1)异常情况检测：在每一时刻，物流车所在的栅格地图中都存在一个局部目标栅格，当其检测到自身在Δt时间内均处于目标栅格中，则认为进入行为恢复模式；

步骤(7-2)重定位：启动行为恢复模式后，物流车通过激光雷达重新扫描并观测周边环境并重新匹配已有栅格地图；

步骤(7-3)重规划路径：当物流车重新规划出下一时刻的速度轨迹即局部路径时，表示物流车脱离被困区域，则继续向目标送货区行进。

本发明的有益效果是：本发明的物流车辆路径规划方法，满足智能物流配送的实时性和安全性要求，不仅能够实现物流输送，而且提出了园区物流输送的全局路线规划方法，在动态避障时在可行速度域上加入安全系数，保障物流车行进时的安全可靠，同时节省了人力物力，为今后发展更为智能化的物流运送工具提供依据。

附图说明

图1是本发明物流车辆自主路径规划方法一较佳实施例的流程图；

图2是本发明物流车辆自主路径规划方法中部分路线经纬度序列图；

图3是本发明物流车辆自主路径规划方法中GIS坐标转换图；

图4是本发明物流车辆自主路径规划方法中速度采样轨迹示意图。

具体实施方式

下面结合附图对本发明的较佳实施例进行详细阐述，以使本发明的优点和特征能更易于被本领域方法人员理解，从而对本发明的保护范围做出更为清楚明确的界定。

如图1至图4，请参阅图1，本发明提供一种物流车辆路径规划的方法，包括步骤为：

(1)建立物流车辆的运动模型及环境观测模型，具体步骤如下：

1)建立物流车辆自身运动模型。运动模型的输入主要由三个因素构成，分别是控制输入量u_t、物流车上一时刻状态X_t-1、来自于周边环境及车体本身的噪声w_t，则描述车体系统状态变化X_t为：

X_t＝f(X_t-1，u_t，w_t)

其中，f为系统的状态转移函数，w_t一般为零均值白噪声；可结合园区等实际的环境状况，物流车的运动模型可表示为：

其中，X_r表示为车辆的位姿矩阵

为系统噪声矩阵；

2)建立物流车辆观测模型。其观测模型可写为

(2)建立物流车辆所搭载激光雷达的环境观测模型，具体步骤如下：

1)设在雷达的一个扫描周期内，可获得数据点个数为n,则可观测到的扫描数据点个数

雷达获取的扫描数据可表示为(ρ_i，θ_i),其中ρ_i为雷达到探测点的距离，θ_i为激光雷达和探测点间连线和扫描范围中轴线夹角。

(3)采集物流园区内的环境分布信息，绘制园区的经纬地图，规划物流车辆的行进路线并导出为经纬度序列Excel表，具体步骤如下：

1)绘制物流车所在园区地图前的数据准备工作如一些主要地点的点、线、面等矢量数据和对应的栅格数据；

2)打开ArcGIS制图软件，加载所需要的地图数据(主要来源为国家基础地理信息库)，并对数据进行渲染操作，增强地图路线等细节信息；

3)在ArcGIS中切换到地图制图模块，首先根据园区实际大小选择绘制地图的纸张规格，其次添加关键的地图图件信息如图例、图名、比例尺及经纬网格。其中经纬网格显示设置为度分秒形式，便于之后对其进行坐标转换操作；

4)将制图所用边界矢量数据添加到地图中，设置好保存格式、地图分辨率等并输出地图；

5)在地图中选取物流车辆园区内的行进路线，导出经纬度数据到Excel表。图2为一实施例通过ArcGIS进行路线规划及提取经纬度数据表，由表中可以看到部分所规划路线上的经纬度数据较为精确，且具有一定连续性，满足在后续进行坐标转换的要求；

(4)根据经纬度数据对规划好的园区路线进行显示，并转化为物流车平台下可识别的直角坐标数据，具体步骤如下：

1)通过ArcMap导入经纬度数据Excel表并可视化，此时显示规划好的园区路线，并将数据导出为shp格式文件；

2)打开ArcToolbox,选择数据管理工具中的投影，选择数据集，设定输入坐标为经纬度坐标，输出坐标为直角坐标；

3)构造平面坐标，利用Feature的几何字段来获取平面坐标数据。在输出文件中找到Output.dbf文件，将其存入Excel表，坐标转换完成。

图3为一实施例进行坐标转换示意图，由图中可以看到实时显示出的园区路线，并将其转化为直角栅格坐标；

(5)物流车辆根据规划好的路线数据，借助激光雷达扫描匹配周围环境信息，运行AMCL定位和自主建图导航算法，具体步骤如下：

1)物流车自身搭载有Linux操作系统，车辆导航开始时启动AMCL定位初始化物流车位姿使其尽量靠近输送起始点；

2)自动建图导航算法通过雷达观测环境信息，利用贝叶斯估计即粒子滤波方法，获取物流车辆每一时刻的位姿估计：

预测。由t-1时刻的后验概率密度函数计算p(x_t-1|y_1：t-1)，得到t时刻的先验概率密度函数p(x_t|y_1：t-1)，

p(x_t|y_1：t-1)＝∫p(x_t|x_t-1)p(x_t-1|y_1：t-1)dx_t-1

其中，x表示系统的状态估计，y表示系统观测量；

更新。由t时刻得到的先验概率密度函数p(x_t|y_1：t-1)，结合环境观测量y_t，针对上一步的观测过程，获取后验概率密度p(x_t|y_1：t)，

其中，p(y_t|y_1：t-1)为归一化常数，表示为

p(y_t|y_1：t-1)＝∫p(y_t|x_t)p(x_t|y_1：t-1)dx_t

3)使用扩展卡尔曼滤波算法进行特征估计，获取周围局部区域的栅格地图。计算得到p(m|x_1：t，y_1：t),其中m表示环境中相互独立的路标；

4)通过Rao-Blackwellized粒子滤波方法，将同时定位与建图问题转化为上述求得的位姿估计及基于位姿估计的环境特征估计，

p(x_1：t，m|y_1：t，u_1：t-1)＝p(m|x_1：t，y_1：t，u_1：t)p(x_1：t|y_1：t，u_1：t)

＝p(m|x_1：t，y_1：t)p(x_1：t|y_1：t，u_1：t-1)

其中，t是采样时间，x_1：t是物流车辆的运动轨迹，y_1：t为2D激光雷达获得的观测信息，u_1：t-1是里程计测量值，其中里程计主要是车身内部自带的光电编码器，获取车身位姿的改变；

(6)物流车在园区行进过程中，根据DWA路径规划算法，实时避开出现的障碍物及局部未知区域，具体步骤如下：

1)根据车辆所有可行速度包括线速度及角速度，生成速度可行区间，

V_s＝{(v，ω)|v∈[0，v_max]∧ω[-ω_max，ω_max]}

其中，v_max，ω_max分别为对应的最大线速度和角速度；

2)由于车辆电机性能限制，同时考虑单个速度采样间隔，计算车辆真实速度变化范围，

其中车辆当前时刻速度为(v_c，ω_c)，线加速度取值边界为

角加速度取值边界为

3)考虑到车辆传感器自身存在噪声误差及采样间隔内的位姿误差等，且需保障物流车与障碍物保持足够安全距离，使得车辆及时在障碍物前停止运动，应满足

其中r_min为(v，ω)状态下的模拟速度轨迹距离障碍物的最近距离，k₁，k₂是添加的无碰撞安全系数，经过大量路径规划实验，可取k₁范围为0.6～1.0，k₂可取范围0.8～1.2；

4)通过以上各速度范围限定，车辆最终的速度采样空间应满足以上三个速度范围限制，取其交集即

v_r＝V_s∩V_d∩v_a

5)物流车根据生成的速度采样空间进行速度采样，并解算每一时间间隔内对应的所有速度轨迹，通过评价得到最优速度轨迹，轨迹评价函数如下，

G(v，ω)＝α.heading(v，ω)+β.dist(v，ω)+γ.vel(v，ω)

其中，heading(v，ω)描述物流车辆与目标区域的朝向一致性，dist(v，ω)表示当前运行速度轨迹中距离障碍物的最小距离，vel(v，ω)衡量车辆当前运行速度，三者结合综合所得最高的评价函数得分对应的速度轨迹可认为是最优速度轨迹；由图4物流车辆局部路径规划示意图可看到最优速度轨迹总是最接近于最终设定好的目标点，而接近甚至与障碍物发生碰撞的速度轨迹将被自动剔除。在障碍物分布较为密集且无规律情况下，在速度可行域区间加入无碰撞安全系数的措施进一步提高了路径可靠性。

表1为10次加入无碰撞安全系数的DWA算法运行时长及速度轨迹与所设障碍的平均距离，由表可看出，速度轨迹与障碍物的最大安全距离为1.128759m,最小安全距离为0.992853m，基本都稳定地保持在1m范围内，且算法运行时长保持在27s左右，同时保证了算法的实时性。由此可知，基于无碰撞安全系数的DWA局部规划能有效地避开障碍物，满足安全性要求。

表1.

(7)物流车在行进过程中遭遇未知情况无法继续导航时进入行为恢复模式，具体步骤如下：

1)异常情况检测。在每一时刻，物流车所在的栅格地图中都存在一个局部目标栅格，当其检测到自身在Δt时间内均处于目标栅格中，则认为进入行为恢复模式；

2)重定位。启动行为恢复模式后，物流车通过激光雷达重新扫描并观测周边环境并重新匹配已有栅格地图；

3)重规划路径。当物流车重新规划出下一时刻的速度轨迹即局部路径时，表示物流车脱离被困区域，继续向目标送货区行进。

针对物流输送环节中货物运输量大、车辆自主性不高等问题，本发明提出了一种基于GIS的全局路径规划方法实现了园区内物流输送路线的自主选择，同时，提出一种加入无碰撞安全系数的DWA规划算法，实现了物流车辆的实时安全避障。

基于GIS的园区物流配送收集所在园区的规模大小、道路建筑分布等详细信息，实现了园区地图的精确建模和路线可视化。通过选择园区物流配送路线生成经纬度数据并将其转化为车辆实际可识别的直角栅格坐标，基本实现了物流配送路线的自主选择与生成。

基于已有的全局配送路线，以DWA局部避障算法为基础，加入无碰撞安全系数使得车辆与障碍物始终保持安全距离。当障碍物分布情况更为复杂及数量上升时，算法规划出的局部路径仍然较好地选择与障碍物保持安全距离，且实时性较高。综上所述，在GIS全局路径基础上依靠加入安全系数后DWA算法可满足园区物流车辆输送要求。

以上所述仅为本发明的实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他方法领域，均同理包括在本发明的专利保护范围内。

Claims (9)

1.一种基于地理信息系统的物流车辆自主路径规划与导航方法，其特征在于，包括以下步骤：

步骤(1)、建立物流车辆与车载激光雷达的运动模型及环境观测模型；

步骤(2)、根据所述步骤(1)中物流车辆与激光雷达运动及观测的数学模型，在GIS中绘制园区地图，规划物流车出发点到目标送货点的全局路线图；

步骤(3)、根据所述步骤(2)中绘制好的GIS园区地图及全局路线，对路线中各点经纬度坐标进行转换和提取；

步骤(4)、根据所述步骤(3)中转换所得直角栅格坐标，确定车辆行驶路径，并利用AMCL初始化车体自身位姿；

步骤(5)、根据所述步骤(4)中确定的车辆姿态信息进行定位的同时，通过局部路径规划，解算最优速度轨迹；

步骤(6)、根据所述步骤(5)中速度轨迹，将生成的速度指令发送给物流车底盘执行机构，到达目标送货区域。

2.一种基于地理信息系统的物流车辆自主路径规划与导航方法，其特征在于包括以下步骤：

步骤(1)建立物流车辆自身运动学模型，其中车辆自身采用二轮运动差分模型，由车体前部导轮保持车体平衡；

步骤(2)确定激光雷达观测模型；

步骤(3)在ArcGIS中绘制所在园区地图，导出从起始点至目标区域的全局路线经纬度序列数据；

步骤(4)借助ArcGIS软件，将全局路线中各经纬度坐标转换为直角栅格坐标，并生成车辆的速度和位移指令；

步骤(5)通过AMCL初始化物流车辆在栅格地图中的位姿，根据所述步骤(4)中的指令，由物流车自带的自动建图算法进行建图导航任务；

步骤(6)以DWA算法为基础，提出向轨迹评价函数的速度可行域区间加入无碰撞安全系数，使得物流车与障碍物保持适当安全距离；

步骤(7)局部规划器计算速度指令发送给车辆底盘执行机构；物流车行进过程中，由于位置误差或进入局部未知区域等导致物流车行进途中受困，此时进入车辆行为恢复模式，处理导航异常情况；

步骤(8)按照既定路线，物流车将货物输送到目标送货点。

3.根据权利要求2所述的基于地理信息系统的物流车辆自主路径规划与导航方法，其特征在于，所述步骤(1)的具体步骤如下：

步骤(1-1)建立物流车辆自身运动模型：运动模型的输入主要由三个因素构成，分别是控制输入量u_t、物流车上一时刻状态X_t-1、来自于周边环境及车体本身的噪声w_t，则描述车体系统状态变化X_t为：

X_t＝f(X_t-1，u_t，w_t)

其中，f为系统的状态转移函数，w_t一般为零均值白噪声；可结合园区等实际的环境状况，物流车的运动模型可表示为：

其中，X_r表示为车辆的位姿矩阵

为系统噪声矩阵；

步骤(1-2)建立物流车辆观测模型，其观测模型可写为

其中，Y_t为t时刻系统的观测量，h为观测函数，δ_(t)是观测过程中的误差。

4.根据权利要求2所述的基于地理信息系统的物流车辆自主路径规划与导航方法，其特征在于，所述步骤(2)的具体步骤如下：

步骤(2-1)建立物流车的2D激光雷达模型。设在雷达的一个扫描周期内，可获得数据点个数为n,则可观测到的扫描数据点个数

雷达获取的扫描数据可表示为(ρ_i，θ_i),其中ρ_i为雷达到探测点的距离，θ_i为激光雷达和探测点间连线和扫描范围中轴线夹角。

5.根据权利要求2所述的基于地理信息系统的物流车辆自主路径规划与导航方法，其特征在于，所述步骤(3)的具体步骤如下：

步骤(3-1)绘制物流车所在园区地图前的数据准备工作如一些主要地点的点、线、面等矢量数据和对应的栅格数据；

步骤(3-2)通过ArcGIS制图软件，加载所需要的地图数据，主要来源为国家基础地理信息库，并对数据进行渲染操作，增强地图路线等细节信息；

步骤(3-3)在ArcGIS中切换到地图制图模块，首先根据园区实际大小选择绘制地图的纸张规格，其次添加关键的地图图件信息如图例、图名、比例尺及经纬网格；其中经纬网格显示设置为度分秒形式，便于之后对其进行坐标转换操作；

步骤(3-4)将制图所用边界矢量数据添加到地图中，设置好保存格式、地图分辨率等同时输出地图；

步骤(3-5)在地图中选取物流车辆园区内的行进路线，导出经纬度数据到Excel表。

6.根据权利要求2所述的基于地理信息系统的物流车辆自主路径规划与导航方法，其特征在于，所述步骤(4)的具体步骤如下：

步骤(4-1)通过ArcMap导入经纬度数据Excel表并可视化，此时可看见所述步骤(3)中规划好的路线，并将数据导出为shp格式文件；

步骤(4-2)由ArcToolbox选择数据管理工具中的投影，选择数据集，设定输入坐标为经纬度坐标，输出坐标为直角坐标；

步骤(4-3)构造平面坐标，利用Feature的几何字段来获取平面坐标数据；在输出文件中找到Output.dbf文件，将其存入Excel表，坐标转换完成。

7.根据权利要求2所述的基于地理信息系统的物流车辆自主路径规划与导航方法，其特征在于，所述步骤(5)的具体步骤如下：

步骤(5-1)物流车自身搭载有Linux操作系统，车辆导航开始时启动AMCL定位初始化物流车位姿使其尽量靠近输送起始点；

步骤(5-2)自动建图导航算法通过雷达观测环境信息，利用贝叶斯估计即粒子滤波方法，获取物流车辆每一时刻的位姿估计：

预测：由t-1时刻的后验概率密度函数计算p(x_t-1|y_1：t-1)，得到t时刻的先验概率密度函数p(x_t|y_1：t-1)，

p(x_t|y_1：t-1)＝∫p(x_t|x_t-1)p(x_t-1|y_1：t-1)dx_t-1

其中，x表示系统的状态估计，y表示系统观测量；

更新：由t时刻得到的先验概率密度函数p(x_t|y_1：t-1)，结合环境观测量y_t，针对上一步的观测过程，获取后验概率密度p(x_t|y_1：t)，

其中，p(y_t|y_1：t-1)为归一化常数，表示为

p(y_t|y_1：t-1)＝∫p(y_t|x_t)p(x_t|y_1：t-1)dx_t步骤(5-3)使用扩展卡尔曼滤波算法进行特征估计，获取周围局部区域的栅格地图；计算得到p(m|x_1：t，y_1：t),其中m表示环境中相互独立的路标；

步骤(5-4)通过Rao-Blackwellized粒子滤波方法，将同时定位与建图问题转化为上述求得的位姿估计及基于位姿估计的环境特征估计，

p(x_1：t，m|y_1：t，u_1：t-1)＝p(m|x_1：t，y_1：t，u_1：t-1)p(x_1：t|y_1：t，u_1：t-1)

＝p(m|x_1：t，y_1：t)p(x_1：t|y_1：t，u_1：t-1)

其中，t是采样时间，x_1：t是物流车辆的运动轨迹，y_1：t为2D激光雷达获得的观测信息，u_1：t-1是里程计测量值，其中里程计主要是车身内部自带的光电编码器，获取车身位姿的改变。

8.根据权利要求2所述的基于地理信息系统的物流车辆自主路径规划与导航方法，其特征在于，所述步骤(6)的具体步骤如下：

步骤(6-1)根据车辆所有可行速度包括线速度及角速度，生成速度可行区间，

V_s＝{(v，ω)|v∈[0，v_max]，ω∈[-ω_max，ω_max]}

其中，v_max，ω_max分别为对应的最大线速度和角速度；

步骤(6-2)由于车辆电机性能限制，同时考虑单个速度采样间隔，计算车辆真实速度变化范围，

其中车辆当前时刻速度为(v_c，ω_c)，线加速度取值边界为

角加速度取值边界为

步骤(6-3)考虑到车辆传感器自身存在噪声误差及采样间隔内的位姿误差等，且需保障物流车与障碍物保持足够安全距离，使得车辆及时在障碍物前停止运动，应满足

其中r_min为(v，ω)状态下的模拟速度轨迹距离障碍物的最近距离，k₁，k₂是添加的无碰撞安全系数，经过大量路径规划实验，可取k₁范围为0.6～1.0，k₂可取范围0.8～1.2；

步骤(6-4)通过以上各范围限定，车辆最终的速度采样空间应满足以上三个速度范围限制，取其交集得

v_r＝V_s∩V_d∩v_a

步骤(6-5)物流车根据生成的速度采样空间进行速度采样，并解算每一时间间隔内对应的所有的速度轨迹，通过评价得到最优速度轨迹，轨迹评价函数如下，

G(v，ω)＝α.heading(v，ω)+β.dist(v，ω)+γ.vel(v，ω)

其中，heading(v，ω)描述物流车辆与目标区域的朝向一致性，dist(v，ω)表示当前运行速度轨迹中距离障碍物的最小距离，vel(v，ω)衡量车辆当前运行速度，三者结合所得最高的评价函数得分可认为是最优速度轨迹。

9.根据权利要求2所述的基于地理信息系统的物流车辆自主路径规划与导航方法，其特征在于，所述步骤(7)的具体步骤如下：

步骤(7-1)异常情况检测：在每一时刻，物流车所在的栅格地图中都存在一个局部目标栅格，当其检测到自身在Δt时间内均处于目标栅格中，则认为进入行为恢复模式；

步骤(7-2)重定位：启动行为恢复模式后，物流车通过激光雷达重新扫描并观测周边环境并重新匹配已有栅格地图；

步骤(7-3)重规划路径：当物流车重新规划出下一时刻的速度轨迹即局部路径时，表示物流车脱离被困区域，则继续向目标送货区行进。

Images