AGV室外定位切换方法、计算机装置及程序产品与流程

agv室外定位切换方法、计算机装置及程序产品
技术领域
1.本发明涉及定位技术领域，特别是一种agv室外定位切换方法、计算机装置及程序产品。


背景技术：

2.agv以及各类移动机器人的自主导航作业在社会生产生活中发挥着重要作用，广泛地应用于工业、农业、服务业中的各种场景，agv以及各类机器人的工作场景也不仅仅局限于厂区、车间、商场等室内场景，比如大型农场中逐渐采用agv进行农作物的运输。
3.虽然室外定位大多数时候均可通过gps提供的定位信息解决，但是gps信号会受到天气以及各种遮挡物的影响，单纯的基于gps的室外定位方法已经不能满足人们对导航作业任务的连续性和鲁棒性的需求。于是在以gps定位信息为主的前提下，添加其他传感器来辅助定位成为了研究热点，其中就包括基于轮式里程计、imu、摄像头的辅助定位方法。经过调研发现，基于轮式里程计以及imu的辅助定位方法在无gps定位信息时虽然能够提供定位所需的位姿信息，但是其定位误差随着时间逐渐累积，最终变得不可靠，而且轮式里程计在不平坦路段容易产生滑移，对定位精度有着很大影响。而现有的视觉定位方案也无法保证导航所需的定位精度。所以选用激光雷达作为辅助定位手段，通过点云匹配进行位姿估计来保证gps信号不可靠时的定位信息供给。
4.激光里程计通过当前帧扫描的点云数据与上一帧的点云数据进行匹配，找到两帧点云数据之间的对应关系，通过最小化匹配点之间的距离来求解最优位姿变化关系，从而实现实时的位姿估计，但是由于帧间匹配存在一定误匹配的概率，所以提供的位姿估计误差随时间累积严重。于是产生了基于帧间匹配以及局部子图匹配的优化后的激光里程计，通过与实时构建的子图的匹配来减少误匹配的概率，提供了激光里程计的精度，但是仍存在误差累积的问题。所以设计一种克服位姿估计误差累积问题的匹配方法成为解决该工程问题的关键。
5.现有的定位切换方法本质上是一种基于卡尔曼滤波的融合定位方法，在某一定位信息源不可靠时将其剔除，在融合定位中gps定位数据是高精度，而基于激光雷达的实时建图定位的定位精度较低，当gps定位信号不可靠时，基于卡尔曼滤波的融合定位中会剔除gps数据或者减小gps数据的采样频率，此时基于激光雷达的slam定位是主要的定位来源，但是slam本质上是一种位姿估计方法，即便是有回环检测进行位姿修正，也不能消除误差累积带来的建图和定位误差，如果gps信号长期不可靠，势必定位精度会逐渐下降甚至不可用，而本发明基于激光雷达点云数据与先验真值地图(由gps真值位姿构建)进行匹配，属于一种绝对定位，不存在误差的累积。


技术实现要素：

6.本发明所要解决的技术问题是，针对现有技术不足，提供一种agv室外定位切换方法、计算机装置及程序产品，持续、稳定地为agv执行室外导航作业任务提供可靠的定位信
息。
7.为解决上述技术问题，本发明所采用的技术方案是：一种agv室外定位切换方法，包括以下步骤：
8.s1、采集agv工作场景数据，建立工作场景先验全局点云地图；
9.s2、利用所述工作场景先验全局点云地图构建gps信号不可靠区域的表征方式，判断当前gps信号的可用状态；
10.s3、若当前gps信号为不可用状态，则对激光雷达定位信息可用性进行判断，若激光雷达定位信息可用则切换至激光雷达定位；若采用激光雷达完成定位初始化，当gps信号为可用状态，且激光雷达位姿与gps位姿差值小于给定阈值时，切换至gps定位。
11.本发明通过对工作场景进行建图和关键帧点云数据以及关键帧位姿信息的储存，为gps信号不可用时基于激光雷达的定位方案提供了特征信息；通过在构建的全局点云地图中对gps信号遮挡区域进行标定，为导航作业过程中对gps可用性的判断提供了辅助信息；通过构建gps信号可用性的判据，为gps信号可用性的准确判断以及导航定位信息的有效性提供了有力保障；通过两种定位初始化方案的设计，保证了agv在任意条件下都能完成定位初始化；根据gps信号以及基于激光雷达的定位信息的可用性判别结果，提出了一种agv导航作业过程中的定位模式选择与切换机制，保证了导航作业任务执行的流畅与稳定。
12.步骤s1的具体实现过程包括：
13.控制agv以小于第一设定阈值的速度前进，记录agv进入遮挡区域的时间戳ts以及agv驶出遮挡区域的时间戳te；
14.根据标记的信号遮挡区域的时间戳选择不同的建图位姿获取方式，若当前数据不在信号遮挡区域，则用于建图的位姿采用gps提供的位姿信息；若当前数据属于信号遮挡区，则用于建图的位姿采用基于点云匹配提供的位姿信息；
15.根据实时获取的位姿信息判断车辆移动距离，当移动距离大于第二设定阈值时，保存关键帧位姿以及关键帧点云；
16.拼接关键帧位姿以及关键帧点云，得到全局点云地图。
17.本发明通过对gps信号遮挡区域的数据进行标记，在信号遮挡区域选择基于点云配准提供的位姿进行建图，在非遮挡区域选择基于gps的位姿进行建图，这样灵活地选择建图时的位姿信息来源，提高了建图的高质量，对于基于先验地图的匹配定位而言，地图的精度很大程度决定了定位的精度，所以这种基于不同位姿来源的建图方法为后期基于地图的定位提供了有力保障。
18.步骤s2中，利用所述工作场景先验全局点云地图构建gps信号不可靠区域的表征方式的具体实现过程包括：
19.读取全局点云地图，对全局点云地图中的m个信号遮挡区域进行标定，标记信号遮挡区域的边界点，记录边界点在地图中的坐标信息；
20.基于边界点对信号遮挡区域进行膨胀；
21.对任一一个信号遮挡区域，设标记了n个边界点，在n个边界点中任意选择3个点形成一组排列，计算每一种排列对应的外接圆半径大小，选择外接圆半径最大的排列作为信号遮挡区域的标志边界点；n》3；
22.计算由信号遮挡区域的标志边界点构成的外接圆圆心在地图中的坐标(x
block1
,yblock1
)以及外界圆半径r1；
23.对全局点云地图中所有信号遮挡区域均进行上述膨胀操作，得到膨胀区中心坐标序列(x
block1
,y
block1
),(x
block2
,y
block2
),...,(x
blockm
,y
blockm
)和膨胀区半径序列r1,r2,...,rm，每一个膨胀中心坐标加上该膨胀中心对应的膨胀区域半径代表先验地图中一个圆形gps信号不可靠区域。
24.本发明对地图中的不可靠区域进行膨胀操作，能够避免车辆在临近gps信号不可靠区域时定位信号收到干扰，保证了定位信号的稳定性与鲁棒性。
25.步骤s2中，判断当前gps信号的可用状态的具体实现过程包括：
26.读取gps原始数据，解析gps原始数据，得到当前定位状态标志位信息，该标志位信息在车辆能获取自身的实时位置与姿态时为真，此时gps可靠性标志位a＝1，否则gps可靠性标志位a＝0；
27.根据当前位姿与标定的gps信号不可靠区域的位置关系，得出gps可靠性标志位b；
28.根据当前agv在地图中的位姿信息、车辆速度信息以及数据跳变阈值得出gps信号可靠性标志位c；
29.若gps可靠性判别标志位a＝1、b＝1、c＝1同时满足，则将gps可用性标志位置为true，即gps信号为可用状态；否则置为false，即gps信号为不可用状态。
30.本发明引入多种gps可用性判断依据，判断过程中不单纯依赖gps自身提供的定位状态标志位信息，还引入了基于先验地图信号不可靠区域的判据以及基于车辆行驶位姿连续性的判断依据，使定位切换的判断、决策过程置信度更高、更具鲁棒性，不会因为单个判据出现误判而做出错误的切换决策。
31.根据当前位姿与标定的gps信号不可靠区域的位置关系，得出gps可靠性标志位b的具体实现过程包括：
32.1)读取agv当前在全局点云地图中的位置信息(pose.x,pose.y)、信号遮挡膨胀中心坐标序列以及膨胀区半径序列ri，其中i＝1,2,3,...,m；
33.2)计算agv当前位置与膨胀中心坐标序列中信号遮挡区域i膨胀中心的距离di；
34.3)若di《ri，则此时agv处于信号不可靠区域内，可靠性标志位b＝0，停止计算di；若di＞ri且信号遮挡膨胀中心坐标序列遍历完成，则可靠性标志位b＝1；若di＞ri但信号遮挡膨胀中心坐标序列未完成遍历，则返回步骤2)。
35.本发明在先验地图中对gps信号不可靠区域进行标定，能够为定位切换过程中gps信号的可用性判断提供一个基于先验地图的判据，使定位切换判断更有完备性。
36.根据当前agv在地图中的位姿信息、车辆速度信息以及数据跳变阈值得出gps信号可靠性标志位c的具体实现过程包括：
37.计算agv当前运行线速度v和转向角速度w；
38.读取当前时刻agv在地图中的位姿信息(pose.x,pose.y,heading)以及上一时刻agv在地图中的位姿信息(pose.x
last
,pose.y
last
,heading
last
)；
39.计算位置变换阈值hd：hd＝k
×v×
t；其中t为gps信号发布的时间间隔，k为阈值扩张系数；
40.计算偏航角变换阈值hh：hh＝k
×w×
t；
41.计算当前时刻与上一时刻agv的位置差值d和姿态差值h：
[0042][0043]
h＝heading-heading
last
；
[0044]
若d《hd且h《hh，则gps信号可靠性标志位c置为1，否则gps信号可靠性标志位c置为0；若gps可靠性判别标志位a＝1、b＝1、c＝1同时满足，则将gps可用性标志位置为true，否则置为false。
[0045]
本发明对车辆行驶过程中位姿的连续性进行分析，能够监测车辆位姿发生异常跳变的情况，推断出当前定位信息异常的发生，从而进行相应的定位切换操作，引入此判据为定位切换判断增加了运动学约束，使定位切换判断更有完备性。
[0046]
步骤s3中，采用激光雷达完成定位初始化的具体实现过程包括：
[0047]
读取保存的关键帧位姿以及关键帧特征点云；
[0048]
读取当前时刻不可靠的gps位姿信息(pose
gps
.x
vague
,pose
gps
.y
vague
,heading
vague
)作为粗定位信息；(pose
gps
.x,pose
gps
.y)是gps提供的二维地图位置坐标信息，heading
gps
是gps提供的姿态信息；
[0049]
在读取的关键帧位姿序列中寻找距离粗定位信息为第三设定阈值以内的关键帧位姿以及该关键帧位姿对应的关键帧特征点云；
[0050]
根据关键帧位姿对关键帧特征点云进行拼接，得到两张局部特征点云地图，即平面特征点云地图flatmap和边缘线特征点云地图cornermap；
[0051]
获取当前帧点云中的平面线特征点云以及边缘线特征点云；
[0052]
找到边缘线特征点云地图cornermap中与边缘线特征点corneri最近的两个点，计算corneri到这两个点形成的直线的距离d_corneri；对当前帧点云中的平面特征点flati，找到平面特征点云地图flatmap中与平面特征点flati最近的三个点，计算flati到这三个点形成的平面的距离d_flati；
[0053]
当前帧的n个边缘线特征点对应有距离序列{d_corner1,d_corner2,...,d_cornern}，构造目标函数：
[0054][0055]
获取使f
corner
的最小的位姿变换关系[t
x
,ty,θ
yaw
]，t
x
是agv中心在建立的先验全局点云地图所对应的地图坐标系下x方向的位移变化量，该地图坐标系的原点为建图初始时刻车辆的起点，该地图坐标系x轴定义为初始时刻过车辆后轴中心的纵向中轴线，车辆后轴中心沿纵向中轴线指向车头的方向为x轴正方向；地图坐标系的y轴定义为初始时刻过车辆后轴中心并垂直于车辆纵向中轴线，车辆后轴中心指向车头的左侧为y轴正方向；ty是agv中心在地图坐标系y方向的位移变化量，θ
yaw
是agv偏航角变化量；
[0056]
利用下式获取初始位姿在地图坐标系中的位姿描述
[0057]
(pose
lidar
.x,pose
lidar
.y,heading
lidar
)：
[0058][0059]
完成定位初始化pose.x＝pose
lidar
.x,pose.y＝pose
lidar
.y,heading＝heading
lidar
；q0,q1,q2,q3为描述姿态变化的四元数；t
cx
为tc在x方向上的位移变化量，t
cy
为
tc在y方向上的位置变化量；tc＝tvt，t是粗定位位姿到正确初始位姿的旋转矩阵，tv是全局点云地图原点到粗定位位姿的位姿旋转矩阵。
[0060]
上述过程通过提取环境点云的边缘特征信息来获取车辆x、y方向上的位移以及偏航角的变化，这样不需要对整块点云进行配准，能够减少运算量从而保证定位的实时性。同时基于平面特征的匹配为整个配准过程提供约束，避免单纯基于边缘特征产生误匹配，保证了定位的鲁棒性和准确性。
[0061]
步骤s3中，采用基于激光雷达点云配准的定位导航过程中，若gps信号重新变为可用状态，且激光雷达位姿与gps位姿差值小于给定阈值时，切换至gps定位的具体实现过程包括：
[0062]
若d《d
max
且h《h
max
，则认为当前状态切换不会发生位姿跳变，定位模式切换至基于gps的定位；否则，认为当前状态下发生定位模式切换会影响导航功能执行，继续选用激光雷达提供的位姿信息进行导航；其中，
[0063][0064]
h＝heading-heading
gps
；
[0065]
(pose
gps
.x,pose
gps
.y,heading
gps
)为gps提供的位姿；(pose.x,pose.y,heading)为当前时刻agv位姿；(pose
gps
.x,pose
gps
.y)是gps提供的二维地图位置坐标信息，heading
gps
是gps提供的姿态信息。
[0066]
采用基于gps进行的定位导航过程中，若gps可用性标志位由true变为false，且基于激光雷达点云配准的位姿与上一时刻gps位姿差值小于给定阈值时，切换至基于激光雷达点云配准的定位的具体实现过程包括：
[0067]
若d
lidar
《d
max
且h
lidar
《h
max
，则认为当前状态切换不会发生位姿跳变，定位模式切换至基于激光雷达点云配准的定位；否则，认为当前状态下发生定位模式切换会影响导航功能执行，此时等待gps信号恢复正常，如果超过t1分钟gps信号仍未恢复正常，则发送异常报告指令至用户终端，用户进行人工干预；其中，
[0068][0069][0070]
是gps上一时刻的位姿；(pose
lidar
.x，pose
lidar
.y，heading
lidar
)是基于激光雷达点云配准的当前时刻位姿；
[0071]
本发明在判断当前gps信号为可用状态后，不立即进行定位切换，而是利用基于先验地图的匹配定位所提供的位姿与gps提供的位姿进行比对，只有两个位姿的差值在设置的阈值范围内才进行切换，这样能够进一步确保gps信号的可用性，并且能够避免因为贸然切换产生的定位跳变对导航规划功能产生不良影响。
[0072]
本发明还提供了一种计算机装置，包括存储器、处理器及存储在存储器上的计算机程序；所述处理器执行所述计算机程序，以实现本发明方法的步骤。
[0073]
本发明还提供了一种计算机程序产品，包括计算机程序/指令；该计算机程序/指令被处理器执行时实现本发明方法的步骤。
[0074]
与现有技术相比，本发明所具有的有益效果为：
[0075]
1)本发明克服了agv在室外工作场景中gps信号不可用时的定位问题，保证agv在
执行导航作业任务时能够不受天气以及遮挡区域(比如隧道、农场中的大棚)的影响，保证导航的连续域稳定。
[0076]
2)通过构建gps信号可用性的三重判据，为gps信号可用性的准确判断以及导航定位信息的有效性提供了有力保障；
[0077]
3)利用存储的关键帧点云数据以及关键帧位姿信息与实时激光雷达获取的点云数据进行匹配来获取基于激光雷达的定位信息，避免了gps信号不可用时的定位信息缺失；
[0078]
4)通过两种定位初始化方案的设计，保证了agv在任意条件下都能完成定位初始化；根据gps信号以及基于激光雷达的定位信息的可用性判别结果，提出了一种agv导航作业过程中的定位模式选择与切换机制，保证了导航作业任务执行的流畅与稳定。
[0079]
5)相比现有的定位切换方法，本发明在基于激光雷达的定位部分采用建好的先验点云地图进行匹配定位，先验地图基于gps信号进行构建，避免了随时间产生的累计误差带来的影响；且本发明方法对先验地图的遮挡区域进行了标定，并且设计了基于gps定位数据连续性的可靠性判断依据，为定位方案的切换提供了更可靠的依据。
附图说明
[0080]
图1为本发明实施例方法流程图；
[0081]
图2为本发明gps信号可用性判别流程图；
[0082]
图3为本发明基于采集的数据建图流程图；
[0083]
图4为室外gps和激光雷达点云匹配提供的定位轨迹对比图；
[0084]
图5为定位模式由基于gps的定位切换至基于激光雷达点云匹配的定位后30s内与gps真值坐标的误差曲线；
[0085]
图6为定位模式由切换回基于gps的定位模式后5s内与切换前基于激光雷达点云配准的位置坐标的差值曲线。
具体实施方式
[0086]
本发明实施一种agv室外定位切换方法及系统，包括以下步骤：
[0087]
步骤1、人工操控搭载组合导航系统和多线激光雷达的agv对工作场景进行数据采集，并基于解析后的gps数据以及激光雷达点云数据对工作场景进行建图与特征保存；
[0088]
步骤2、在建立的工作场景先验全局点云地图中进行gps信号不可靠区域的标定，并将标定后的gps不可靠区域根据边界点进行膨胀，得到导航坐标系下gps不可靠区域的位置表征方式；
[0089]
步骤3、基于gps稳定定位定向的可靠性原始标志位a、由当前位姿与标定的gps信号遮挡区域的位置决定的gps信号可靠性标志位b以及由前后时刻gps位姿信息连续性判断决定的gps信号可靠性标志位c，协同判断当前gps信号的可用状态；
[0090]
步骤4、基于激光雷达和gps的agv室外定位初始化，初始时刻对gps信号的可用性进行判别，根据判别结果选择对应的定位初始化模式，得到agv在室外场景中的初始位姿；
[0091]
步骤5、根据定位初始化的模式以及实时对gps信号、激光雷达定位信息的可用性判断的结果进行agv导航作业过程中定位方案的选择与切换；
[0092]
各步骤具体实现过程如下：
[0093]
步骤1为人工操控搭载组合导航系统和多线激光雷达的agv对工作场景进行数据采集，并基于解析后的gps数据以及激光雷达点云数据对工作场景进行建图与特征保存。
[0094]
步骤1.1、对gps原始数据进行解析；
[0095]
步骤1.1.1、从串口中读取gps原始数据，按照厂商给出的数据解析标准进行解析(不同厂商数据协议规范不同)，得到当前地理位置信息(latitude,longitude,altitude)；
[0096]
步骤1.1.2、选择gps坐标原点，记录其经纬度信息(latitude0,longitude0,altitude0)；
[0097]
步骤1.1.3、基于geographiclib库以及选择的gps坐标原点的经纬度信息对坐标原点进行重置，并基于库函数中的geo_converter类得到当前位置相对坐标原点的位姿信息(pose
gps
.x,pose
gps
.y,pose
gps
.z,heading
gps
,pitch
gps
,roll
gps
)；
[0098]
步骤1.2、控制搭载多线激光雷达和组合导航系统的agv在指定工作场景中进行数据采集；
[0099]
步骤1.2.1、在天气晴朗且行人、车辆较少的时间段人工操控搭载激光雷达以及gps的agv开始工作场景的数据采集，采集信息包括gps提供的位姿信息，以ros中的nav_msgs::odometry数据格式进行记录，激光雷达提供的点云数据，以ros中的sensor_msgs::pointcloud2数据格式进行记录，所有的数据以rosbag的形式进行保存；
[0100]
步骤1.2.2、控制车辆以小于0.5m/s的速度进行数据采集，车辆行驶过程中尽量沿直线行驶且行驶过程中靠近环境特征明显的一侧；
[0101]
步骤1.2.3、在车辆即将进入工作场景中的遮挡区域(棚内垄间工作区、植被茂密处、隧道等)，人工通过无线设备发送信号至车载工控机，车载工控机接收到信号后在ros中记录进入遮挡区域的时间戳ts；
[0102]
步骤1.2.4、完成信号遮挡区域的数据采集工作即将从信号遮挡区域驶出的时候，人工通过无线设备发送信号至车载工控机，车载工控机接收到信号后在ros中记录驶出遮挡区域的时间戳te；
[0103]
步骤1.2.5、车辆完成所有工作区域的数据采集工作后回到起始位置，回到起始位置后不立即停止数据录制，保持agv继续运动，沿初始时刻行驶的路径继续进行移动，至少达到10s以上；
[0104]
步骤1.3、回放录制的rosbag格式的数据集，并基于slam方法进行全局三维点云地图的构建；
[0105]
步骤1.3.1、根据标记的信号遮挡区域的时间戳选择不同的位姿来源进行建图所需信息的保存；
[0106]
步骤1.3.1.1、如果当前数据时间戳不属于标记的信号遮挡的时间段，则此时gps位姿信息可靠，将gps提供的位姿信息gps_pose(pose
gps
.x,pose
gps
.y,pose
gps
.z,heading
gps
,pitch
gps
,roll
gps
)赋值给slam算法基于当前帧点云与实时构建的局部点云子图匹配求解的位姿lidar_maping_pose(pose
lidar
.x,pose
lidar
.y,pose
lidar
.z,heading
lidar
,pitch
lidar
,roll
lidar
)
[0107]
步骤1.3.1.2、如果当前数据时间戳属于标记的信号遮挡的时间段，则将gps的获得的绝对位姿信息作为激光里程计的初值，同时停止gps_pose对lidar_mapping_pose的赋值操作，在遮挡区域完成基于激光雷达的点云配准进行实时的位姿估计，并通过回环检测
对位姿进行修正；
[0108]
步骤1.3.2、根据实时获取的位姿信息来对移动距离进行判断，当位置移动大于2m时进行关键帧位姿以及关键帧点云的保存；
[0109]
步骤1.3.3、基于保存的关键帧位姿以及关键帧点云进行地图的拼接，得到全局点云地图；
[0110]
步骤1.3.3.1、读取步骤1.3.2中保存的n个关键帧位姿key_frame_pose_1,key_frame_pose_2,...,key_frame_pose_n，以及n个关键帧点云key_frame_1,key_frame_2,...,key_frame_n；
[0111]
步骤1.3.3.2、基于多线激光雷达相邻的两个扫描线上对应点与地面的夹角来进行地面点的滤除，对每一个关键帧点云都进行地面点的滤除，但是不完全删除，作为点云的特征之一保存；
[0112]
步骤1.3.3.3、基于初始时刻的初始位姿init_pose(即地图原点)，将每一个滤除地面点后的关键帧点云数据按照其对应的关键帧位姿导入全局地图中，得到一张拼接好的稠密的点云地图；
[0113]
步骤1.3.3.4、利用pcl库中的voxel体素网格滤波器进行降采样，去除空间中位置相近的点云数据，得到一张去除地面点后的稀疏全局点云地图；
[0114]
步骤1.3.3.5、将进行下采样后的点云地图进行2维坍缩，得到可以用于导航任务的栅格地图；
[0115]
步骤1.3.4、对保留的关键帧点云进行特征提取，得到相应的平面特征地图、边缘线特征地图；
[0116]
步骤1.3.4.1、读取步骤1.3.3.2得到的去除地面点后的关键帧点云；
[0117]
步骤1.3.4.2、将点云数据投影至一个h*w的图像中，每个点云对应图像中的一个像素格，其中h为多线激光雷达的线数，w可由多线激光雷达水平方向的分辨率δ求得：
[0118]
w＝360
°
/δ
[0119]
比如velodyne-16激光雷达，h＝16，δ＝0.2
°
，w＝360
°
/0.2
°
＝1800；
[0120]
步骤1.3.4.3、根据投影点云在图像中的相邻位置关系以及其原点云与激光雷达坐标系原点之间的距离进行聚类，删除聚类少于30个点云的点云簇；
[0121]
步骤1.3.4.4、对聚类后的k个点云簇内进行点云表面平滑度的计算，计算每一个点云簇内包含的nk个点附近的粗糙度：
[0122][0123]
其中s为第k个点云簇内点p
ki
在图像上的投影点所在行两侧各选5个点所组成的点集，r
ki
是第k个点云簇内激光点与激光雷达坐标系原点的距离，r
kj
是第k个点云簇内点p
ki
对应的点集s中的点云与激光雷达坐标系原点的距离；
[0124]
步骤1.3.4.5、将计算出来的粗糙度c
ki
与阈值c
th
进行比较，如果c
ki
》c
th
则属于边缘特征，若c
ki
《c
th
则属于平面特征；
[0125]
步骤1.3.4.6、将点云中被判断为平面特征的点提取出来组成平面特征点云，将点云中被判断为边缘特征的点提取出来组成边缘特征点云，保存每一个关键帧点云对应的关键平面特征点云以及边缘特征点云；
[0126]
步骤1.3.4.7、基于初始时刻的初始位姿init_pose(即地图原点)，将所有平面特征按照其对应的关键帧位姿导入全局地图中，得到全局平面特征地图；将所有边缘特征按照其对应的关键帧位姿导入全局地图中，得到全局边缘特征地图；
[0127]
步骤2为在建立的工作场景先验全局点云地图中进行gps信号不可靠区域的标定，并将标定后的gps不可靠区域根据边界点进行膨胀，得到导航坐标系下gps不可靠区域的位置表征方式；
[0128]
步骤2.1、读取步骤1.3.3.4中得到的全局点云地图；
[0129]
步骤2.2、人工对全局点云地图中的m个不可靠区域进行标定，标记不可靠区域的边界点，记录边界点在地图中的坐标信息；
[0130]
步骤2.3、基于边界点对信号遮挡区域进行膨胀；
[0131]
步骤2.3.1、对信号遮挡区域1，假设人工标记了n个边界点(n》3)，在n个边界点中任意选择3个点形成一组排列，这样的排列有个；
[0132]
步骤2.3.2、计算每一种排列对应的外接圆半径大小，选择外接圆半径最大的排列作为信号遮挡区域的标志边界点；
[0133]
步骤2.3.3、计算由信号遮挡区域的标志边界点构成的外接圆圆心在地图中的坐标(x
block1
,y
block1
)以及外界圆半径r1，对地图中每一个信号遮挡区域都进行该膨胀操作，得到膨胀区中心坐标序列(x
block1
,y
block1
),(x
block2
,y
block2
),...,(x
blockm
,y
blockm
)和膨胀区半径序列r1,r2,...,rm；
[0134]
步骤3、基于gps稳定定位定向的可靠性原始标志位a、由当前位姿与标定的gps信号遮挡区域的位置决定的gps信号可靠性标志位b以及由前后时刻gps位姿信息连续性判断决定的gps信号可靠性标志位c，协同判断当前gps信号的可用状态；
[0135]
步骤3.1、读取gps原始数据，解析gps原始数据，得到当前定位状态标志位信息，该标志位信息在车辆能够成功获取自身的实时位置与姿态时为真，此时gps可靠性标志位a＝1，否则gps可靠性标志位a＝0
[0136]
步骤3.2、根据当前位姿与先验地图中标定的信号不可靠区域的位置关系得出gps可靠性标志位b；
[0137]
步骤3.2.1、读取agv当前在地图中的位置信息(pose.x,pose.y)；
[0138]
步骤3.2.2、读取步骤2.3.3中得到的信号遮挡膨胀中心坐标序列以及膨胀区半径序列；
[0139]
步骤3.2.3、计算agv当前位置与膨胀中心坐标序列中信号遮挡区域i膨胀中心的距离di：
[0140][0141]
步骤3.2.4、如果di《ri,则此时agv处于信号不可靠区域内，可靠性标志位b＝0，停止计算di；
[0142]
步骤3.2.5、如果di＞ri并且信号遮挡膨胀中心坐标序列遍历完成，则可靠性标志位b＝1，否则返回步骤3.2.3；
[0143]
步骤3.3、根据当前agv在地图中的位姿信息、车辆速度信息以及数据跳变阈值得出gps信号可靠性标志位c；
[0144]
步骤3.3.1、根据轮式里程计信息得出当前车辆运行线速度v和转向角速度w；
[0145]
步骤3.3.2、读取当前agv在地图中的位姿信息(pose.x,pose.y,heading)以及上一时刻agv在地图中的位姿信息(pose.x
last
,pose.y
last
,heading
last
)；
[0146]
步骤3.3.3、计算位置变换阈值hd：
[0147]
hd＝k
×v×
t
[0148]
其中t为gps信号发布的时间间隔，k为阈值扩张系数，取值1.2～1.5可调；
[0149]
步骤3.3.4、计算偏航角变换阈值hh：
[0150]hh
＝k
×w×
t
[0151]
其中t为gps信号发布的时间间隔，k为阈值扩张系数，取值1.2～1.5可调；
[0152]
步骤3.3.5、计算当前时刻与上一时刻agv的位置差值d和姿态差值h：
[0153][0154]
h＝heading-heading
last
[0155]
如果d《hd且h《hh则gps信号可靠性标志位c置为1，否则gps信号可靠性标志位c置为0；
[0156]
步骤3.4、如果gps可靠性判别标志位a＝1、b＝1、c＝1同时满足，则将gps可用性标志位置为true，否则置为false；
[0157]
步骤4、基于激光雷达和gps的agv室外定位初始化，初始时刻对gps信号的可用性进行判别，根据判别结果选择对应的定位初始化模式，得到agv在室外场景中的初始位姿；
[0158]
步骤4.1、agv启动后，读取步骤3中的gps可用性标志位的判断结果；
[0159]
步骤4.2、如果gps可用性标志位的判断结果为true，则使用gps提供的位姿进行定位初始化pose.x＝pose
gps
.x,pose.y＝pose
gps
.y,heading＝heading
gps
定位初始化过程完成；
[0160]
步骤4.3、如果gps可用性标志位的判断结果为false，则使用激光雷达匹配定位来完成定位初始化；
[0161]
步骤4.3.1、读取步骤1中得到的关键帧位姿以及关键帧特征点云；
[0162]
步骤4.3.2、读取此时不可靠的gps位姿信息(pose
gps
.x
vague
,pose
gps
.y
vague
,heading
vague
)作为粗定位；
[0163]
步骤4.3.3、在读取的关键帧位姿序列中寻找粗定位附近10m内的关键帧位姿以及其对应的关键帧特征点云；
[0164]
步骤4.3.4、根据关键帧位姿对关键帧特征点云进行拼接，得到两张局部特征点云地图，平面特征点云地图flatmap和边缘线特征点云地图cornermap；
[0165]
步骤4.3.5、对当前帧点云执行步骤1.3.4.2-步骤1.3.4.6，得到当前帧中的平面线特征点云以及边缘特征点云；
[0166]
步骤4.3.6、对当前帧点云中的边缘特征点corneri，找到边缘特征点云地图cornermap中与之最近的两个点(需保证两个点来自两条不同的激光线)，计算corneri到这两个点形成的直线的距离d_corneri；
[0167]
步骤4.3.7、对当前帧点云中的平面特征点flati，找到平面特征点云地图flatmap中与之最近的三个点(需保证其中三个点不属于同一条激光线)，计算flati到这三个点形成的平面的距离d_flati；
[0168]
步骤4.3.8、当前帧的n个边缘特征点对应有距离序列{d_corner1,d_corner2,...,d_cornern}，构造目标函数：
[0169][0170]
通过levenberg-marquardt法求解一个非线性最优化问题，可以得到一个使f
corner
的最小的位姿变换关系[t
x
,ty,θ
yaw
]，t
x
是agv中心在建立的先验全局点云地图所对应的地图坐标系下x方向的位移变化量，该坐标系的原点为建图初始时刻车辆的起点，ty是agv中心在地图坐标系y方向的位移变化量，θ
yaw
是agv偏航角变化量；
[0171]
步骤4.3.9、利用下式获取初始位姿在地图坐标系中的位姿描述(pose
lidar
.x,pose
lidar
.y,heading
lidar
)：
[0172][0173]
完成定位初始化pose.x＝pose
lidar
.x,pose.y＝pose
lidar
.y,heading＝heading
lidar
；q0,q1,q2,q3为描述姿态变化的四元数；t
cx
为tc在x方向上的位移变化量，t
cy
为tc在y方向上的位置变化量；tc＝tvt，t是粗定位位姿到正确初始位姿的旋转矩阵，tv是全局点云地图原点到粗定位位姿的位姿旋转矩阵。
[0174]
步骤4.3.10、利用步骤4.3.10中得到的位姿信息进行定位初始化，pose.x＝pose
lidar
.x,pose.y＝pose
lidar
.y,heading＝heading
lidar
[0175]
步骤5、步骤5、根据定位初始化的模式以及实时对gps信号、激光雷达定位信息的可用性判断的结果进行agv导航作业过程中定位方案的选择与切换；
[0176]
步骤5.1、定位初始化过程完成后，agv等待导航任务的发布；
[0177]
步骤5.2、导航任务发布后，依据定位初始化时使用的初始化模式来选择后续导航任务执行初始阶段的定位模式；
[0178]
步骤5.2.1、如果定位初始化模式为基于gps位姿信息的初始化，则agv继续使用gps提供的位姿信息来执行导航任务，并按照步骤3中的gps可用性判断流程来实时进行信号检测以及导航任务是否执行结束的判断，如果导航任务结束，则关闭定位模块；
[0179]
步骤5.2.1.1、如果执行导航任务过程时检测到gps的可用性标志位跳变为false，则agv暂停执行导航作业任务；
[0180]
步骤5.2.1.2、agv在原点静止等待30s，如果gps可用性标志位恢复为true且持续时间超过30s，则继续使用gps提供的位姿信息进行导航作业任务；如果等待时间过后，gps可用性标志位仍为false，则进行激光雷达定位可用性判断；
[0181]
步骤5.2.1.3、利用上一时刻的gps位姿信息作为点云配准的初始位姿信息，根据步骤4.3.1-4.3.10的流程执行，得到基于激光雷达点云配准的位姿信息pose
lidar
(pose
lidar
.x,pose
lidar
.y,heading
lidar
)；
[0182]
步骤5.2.1.4、计算位置差值d以及偏航角差值h：
[0183][0184]
[0185]
步骤5.2.1.5、将计算得到的位置差值d
lidar
与位置差值阈值d
max
比较，将偏航角差值h
lidar
与偏航角差值阈值h
max
比较，如果d
lidar
《d
max
且h
lidar
《h
max
则激光雷达定位可用性标志位置为true，否则置为false；
[0186]
步骤5.2.1.5.1、如果激光雷达定位可用性标志位为true，将当前基于点云配准得到的位姿信息作为步骤4.3.3中搜索关键帧位姿与关键帧特征点云的先验信息，根据步骤4.3.1-4.3.10的流程执行得到新时刻的基于点云配准的位姿信息；
[0187]
步骤5.2.1.5.2、如果激光雷达定位可用性标志位为false，则继续等待gps信号恢复正常，如果超过15分钟(t1)gps信号仍未恢复正常，则发送异常报告指令至用户终端，用户进行人工干预；
[0188]
步骤5.2.2、如果定位初始化模式为基于激光雷达匹配定位的初始化，则选择激光雷达定位作为导航任务执行的定位信息来源；
[0189]
步骤5.2.2.1、将当前时刻激光雷达定位提供的位姿信息赋值给位姿存储变量并将其作为点云配准的初始位姿信息；
[0190]
步骤5.2.2.2、根据步骤4.3.1-4.3.10的流程执行，得到新时刻基于激光雷达点云配准的位姿信息pose
lidar
(pose
lidar
.x,pose
lidar
.y,heading
lidar
)；
[0191]
步骤5.2.2.3、按照步骤3中的gps可用性判断流程来实时进行信号检测以及导航任务是否执行结束的判断，如果导航任务结束，则定位系统关闭；
[0192]
如果执行导航任务过程时检测到gps的可用性标志位变为true，则将gps提供的位姿(pose
gps
.x,pose
gps
.y,heading
gps
)与当前位姿(pose.x,pose.y,heading)进行差值比较，计算位置差值d以及偏航角差值h：
[0193][0194]
h＝heading-heading
gps
[0195]
步骤5.2.2.4、将计算得到的位置差值d与位置差值阈值d
max
比较，将偏航角差值h与偏航角差值阈值h
max
比较，如果d《d
max
且h《h
max
则认为当前状态切换不会发生位姿跳变，不会影响agv规划控制正常运行与稳定性，定位模式切换为基于gps的定位；否则，认为当前状态下发生定位模式切换会影响导航功能执行，继续选用激光雷达提供的位姿信息进行导航。
[0196]
在室外环境中记录一段轨迹中由gps和点云配准提供的位置信息，其中包含8832组坐标数据，节选部分数据记录在表1中，其可视化结果如图4所示，图中虚线部分为gps的定位轨迹，实线部分为基于激光雷达点云配准的定位轨迹，基于点云配准的定位与基于组合导航的定位误差不超过10cm，证明了激光雷达作为切换备选方案的可行性与可靠性。
[0197]
表1基于点云配准和gps的定位坐标(节选)
[0198]
基于点云配准获取的定位坐标(x,y)/(m,m)基于gps获取的定位坐标(x,y)/(m,m)(14.3864212036,-60.009727478)(14.6258982895,-60.0205248978)(14.3467760086,-61.1161613464)(14.6013512498,61.0777746375)(14.3234176636,-62.1469535828)(14.5866234915,-62.1128597022)(14.3094949722,-63.1125450134)(14.577787321,-63.1191309194)
(14.2816371918,-64.1092300415)(14.5817162362,-64.0832894497)
……
(14.1234521866,-75.0794677734)(14.5542356198,-75.099078066)(14.120757103,-76.0172805786)(14.521832858,-76.005608644)(14.0550909042,-77.2397613525)(14.5051414361,-77.2302006878)(14.0451145172,-78.134437561)(14.5041605748,-78.1311900775)(14.0101280212,-79.1993026733)(14.5031799617,-79.1973057105)
[0199]
在室外导航过程中人工对gps信号进行屏蔽与开启，模拟触发定位切换的条件，使agv完成由基于gps的定位切换至基于激光雷达点云配准的定位，并在行驶一段距离后重新切换回基于gps的定位模式，重复进行这样的模拟测试。这里选取一组测试数据进行展示，并绘制出切换至基于激光雷达点云配准的定位后30s内与gps位置真值的误差曲线图，以及切换回基于gps的定位后5s内与切换前基于激光雷达点云配准的定位位置的差值曲线，如图5、图6所示。可以看出，两次切换发现切换时刻位置误差基本上都能够控制在10cm以内，不会产生位姿跳变而影响导航功能。
[0200]
表2包含两次定位切换的导航定位坐标和gps真值坐标(节选)
[0201]
导航过程中使用的定位坐标(x,y)/(m,m)基于gps提供的真值坐标(x,y)/(m,m)(23.5787726874，-65.8486973888)(23.5787726874，-65.8486973888)(23.5777908288，-65.9107581696)(23.5777908288，-65.9107581696)(23.5768090487，-66.0359879579)(23.5768090487，-66.0359879579)
……
(23.5905635375，-72.0514508357)(23.5905635375，-72.0514508357)(23.6102035613，-73.1585706658)(23.6102035613，-73.1585706658)(定位由gps切换至激光雷达)(定位由gps切换至激光雷达)(23.1712560654，-73.4919281006)(23.6141317374，-73.5154201405)(23.1480178833，-74.1129226685)(23.5915479369，-74.0817247755)(23.1380710602，-74.9416046143)(23.5611089526，-74.9273026886)
……
(23.9201078415，-83.214225769)(23.4658712963，-83.2223903636)(23.8575553894，-84.0822296143)(23.4334685071，-84.0923496495)(23.8493518829，-84.9674911499)(23.4167766363，-84.9623086508)(23.8528795242，-85.2276992798)(23.4089214894，-85.2227424147)(定位由激光雷达切换至gps)(定位由激光雷达切换至gps)(23.4079396293，-85.2848031931)(23.4079396293，-85.2848031931)(23.4030302353，-85.5186393428)(23.4030302353，-85.5186393428)(23.3853564012，-86.369758463)(23.3853564012，-86.369758463)
……
(23.351978714，-93.1487898792)(23.351978714，-93.1487898792)(23.3205575379，-93.7793718845)(23.3205575379，-93.7793718845)(23.3077928661，-94.1949574575)(23.3077928661，-94.1949574575)