一种基于车道线的物流AGV定位方法与流程

一种基于车道线的物流agv定位方法
技术领域
1.本发明属于slam技术领域，涉及一种物流agv的多传感器定位方案，具体涉及一种基于车道线分割的，以视觉定位为主，imu、轮速计与激光为辅助的agv二维定位方案，可用于货物流动大的物流线边仓中的agv全局定位。


背景技术：

2.agv(automated guidedvehicle)指的是搭载一定传感器，具备自主导航功能的运输机器人。与人力作业相比，agv有着明显的高工作效率与低维护成本，因而有着很大的市场前景。agv的自主导航可以分解为定位和控制两个子模块，它们对于agv相当于是“眼睛”和“腿”。由于传统的运输车控制技术已经足够成熟，因此精准的定位便是实现自主导航的关键。
3.目前agv常用的定位方法有：基于二维码的定位技术、基于射频标签(radio frequencyidentification,rfid)的定位技术、基于超宽带无线通信(ultra wideband,uwb)的定位技术以及slam技术。其中基于二维码的方案具有高精度、高鲁棒性与全局唯一性，广泛应用于物流车间中。但是二维码仅在张贴到的地点才能给出定位信息，不具备实时定位功能；更重要的是，二维码需要贴在agv途径的车道上，容易收到污渍、破损的影响，需要很频繁的人力维护。与之相比，基于射频标签(rfid)的定位技术仅需要在路标点处设立电子标签，然后通过无线射频的方式进行双向传输。电子标签需要的改造规模小，而且不易破损，但仍然需要现场改造，因此应用也受到限制。基于超宽带无线通信(uwb)的技术不需要设置外界标签，而且速度快、成本低，具有着很强的竞争力。但是uwb需要在系统中设置基站，仍然存在人工维护的成本。以上定位方法都是比较成功的定位范例，但是普遍存在着场景改造或人工维护的缺陷，这使得在实际应用中受到限制。
4.鉴于以上方法的限制，目前的agv越来越多地采用slam(simultaneous localizationand mapping)技术进行定位。slam允许移动机器人、无人机或者手持设备在未知场景下构建周围环境地图，同时求解出设备自身相对于外界环境的姿态。基于slam的定位方案与地图交互性高、感知力强，并且无需外界标签和人工维护，具有很大的发展潜力。在众多slam 技术中，以各类相机为主要传感器的视觉slam具有低成本、高语义信息的特点，而且可以与深度学习相结合提高对环境的感知能力，被认为是最具潜力的slam方案。经典的视觉 slam包括特征点法与直接法，它们的共性是都需要借助图像中的纹理信息或高频轮廓信息来恢复真实的3d环境。但是视觉slam依赖图像信息，在光照变化剧烈与纹理缺失的场合难以正常工作，这成为了视觉slam的瓶颈。为了克服视觉的缺陷，常常引入备受关注的imu (inertial measurement unit)来提供姿态先验。imu定位技术俗称惯导，完全不依赖外界环境就能够给出六自由度的姿态估计，理论上对环境变化绝对鲁棒。但imu自身具备很大的零点漂移与不确定偏置，实际中要借助复杂的动态估计才能保证精度。与之相比，结构简单的轮速计借助车轮编码器来估计小车运动，在地面平整且直行的情况下具有一定的精度。但在旋转时，车轮打滑严重，轮速计精度会迅速下降。常用的解决方法是用imu
来估计偏航角进行补偿，但由于不存在对环境的感知力，因此求得的姿态始终存在严重的累积误差。与上述传感器相比，激光雷达具有显著的高精度特点，相应的也存在较为成熟的激光slam方案。但激光slam 完全依赖周围立体环境的轮廓进行工作，在线边仓等缺乏固定设备、货物流动大的场合中，激光slam将无能为力。综上所述，目前slam技术依赖的传感器都存在各自的缺陷。因此，对于货物流动大的物流仓库内，agv的定位问题并没有足够高效、准确的技术方案。


技术实现要素：

5.针对现有技术中存在的缺陷，本发明的目的在于提供一种基于车道线的物流agv定位方法，以视觉为主，imu、轮速计与激光为辅的定位方案，能够在不需要场景改造的前提下，在货物流动大的仓库内实现实时、准确的agv定位。
6.为了达到上述目的，本发明所采用的具体技术方案如下：
7.一种基于车道线的物流agv定位方法，具体包括以下步骤：
8.步骤1)在agv上固定多个内参数标定后的鱼眼相机，执行鱼眼相机的单应性矩阵标定，存储内参数和单应性矩阵并根据上述参数将鱼眼相机获取的带有车道线的图像变换为鸟瞰图并拼接，采用神经网络对拼接后的鸟瞰图中的车道线进行语义分割；
9.步骤2)拼接后的鸟瞰图image坐标系和agv坐标系之间的关系为根据激光定位系统、鸟瞰图的帧间姿态估计方法和手眼标定方程来估计矩阵根据激光定位系统提供的agv姿态和标定的矩阵基于鸟瞰图构建关于车道线的全局地图；
10.步骤3)对拼接后的鸟瞰图和全局地图进行预处理，基于误差函数求解鸟瞰图image 坐标系相对全局地图坐标系map的姿态，计算上述误差函数中关于姿态向量的雅可比矩阵，基于上述误差函数与雅可比矩阵，采用lm法求解最优的姿态；
11.步骤4)设计退化误差修正策略，在agv重新发现非退化场景时消除累积误差；
12.步骤5)当定位系统开始启动时，给agv一个初始姿态，读取离线的地图图像并基于三层金字塔进行预处理，从鱼眼相机读取图像并执行鸟瞰图拼接和语义分割处理，得到尺度确定的二值车道线鸟瞰图，基于三层金字塔执行步骤3)的姿态优化，优化得到的姿态作为定位系统的初始值，对实时获取的任一鸟瞰图进行姿态估计，在agv行驶的过程中不断检测退化情形，若agv在退化情形中行驶了足够长的距离后，由重新发现了非退化场景，那么启用步骤4)中的误差修正策略来修正误差。
13.优选的，步骤1)具体包括以下步骤：
14.步骤1a)在agv上固定4个内参数标定后的鱼眼相机，内参数标定算法基于鱼眼相机的等距模型；
15.步骤1b)四个鱼眼相机固定在agv的前后左右四个方向，在agv周围铺设棋盘格标定布，进行相机的单应性矩阵标定，图像点坐标p
i
和地面真实二维坐标之间的关系为h为单应性矩阵；
16.步骤1c)存储内参数和单应性矩阵并根据上述参数将鱼眼相机获取的带有车道线
的图像变换为鸟瞰图并拼接；
17.步骤1d)采用神经网络对拼接后的鸟瞰图中的车道线进行语义分割。
18.优选的，步骤2)具体包括以下步骤：
19.步骤2a)拼接后的鸟瞰图image坐标系和agv坐标系之间的关系为手眼标定方程为其中的借助激光定位系统来给出，而则通过基于鸟瞰图的帧间姿态估计方法计算；
20.步骤2b)借助激光定位系统提供的agv姿态和步骤2a)中标定的矩阵基于鸟瞰图构建关于车道线的全局地图。
21.优选的，步骤2b)具体包括以下步骤：
22.步骤2b1)当agv的姿态旋转或平移量大于设定值时，将拼接的鸟瞰图纳入关键帧队列，否则获取下一帧；利用矩阵将激光定位系统获取的agv姿态转换为image坐标系相对于map坐标系的姿态，然后将该关键帧变换到map坐标系中；
23.步骤2b2)维护一个临时地图，新加入一个关键帧时，该关键帧中车道线像素将在临时地图上叠加灰度值，叠加的灰度值由像素点到agv中心的距离来确定，临时地图中灰度值大于某一阈值的点将被纳入到全局地图中；
24.步骤2b3)若有新的关键帧加入，且关键帧队列中的关键帧数量超过设定阈值，则丢弃关键帧队列中最早的关键帧，同时，在临时地图中消除该关键帧的灰度值叠加。
25.优选的，步骤3)具体包括以下步骤：
26.步骤3a)对拼接后的鸟瞰图和全局地图进行预处理，预处理为高斯模糊处理；
27.步骤3b)设鸟瞰图坐标系相对地图坐标系map的姿态先验基于误差函数求解最优姿态：e＝e
data
ω
smo
e
smo
ω
dege
e
dege
；
28.步骤3c)计算上述误差函数中关于姿态向量的雅可比矩阵；
29.步骤3d)基于上述误差函数与雅可比矩阵，采用lm法求解最优的姿态。
30.优选的，步骤3b)具体包括以下步骤：
31.步骤3b1)e
data
是核心误差项，该误差项由当前帧与全局地图的对应像素点的灰度差来组成；
32.步骤3b2)e
smo
是平滑误差项，为当前姿态与先验姿态之间的欧氏距离；
33.步骤3b3)e
dege
是退化误差项，通过先验姿态来对纵向的位移进行约束；退化项定义为：
34.其中n
dege
表示退化方向的方向向量。
35.优选的，步骤3c)具体包括以下步骤：
36.步骤3c1)计算核心误差项e
data
在点(i，j)处的雅可比矩阵
[0037][0038]
其中j
ima
即为模糊地图的图像梯度；
[0039]
步骤3c2)依次计算e
smo
和e
dege
的雅可比矩阵，总的雅可比矩阵为
[0040]
j＝[j
data j
smo j
dege
]。
[0041]
优选的，步骤4)具体包括以下步骤：
[0042]
步骤4a)对于当前帧的最高层金字塔模糊图像，利用sobel算子统计车道线像素的梯度；如果某个像素点的梯度方向与退化方向的夹角较小，则将该像素点视为非退化方向的车道线像素，将其纳入搜索模板；
[0043]
步骤4b)使用上述搜索模板，沿着退化方向，在最高层金字塔的模糊地图上进行一维搜索，统计该模板与地图图像的投影误差，根据若干个投影误差统计出所有的局部极小值，然后分别以这些局部极小值为初始值，进行仅有最高层金字塔参与的姿态优化；
[0044]
步骤4c)对于所有优化后的姿态，计算鸟瞰图与地图的双向投影误差，计算当前帧与地图的相似性；
[0045]
步骤4d)根据双向投影误差和搜索步长，对上述所有姿态进行评估，以评估值最小为原则筛选出最优的姿态；基于该姿态再次运行一次无金字塔的姿态优化，得到最终的修正姿态。
[0046]
优选的，步骤5)具体包括以下步骤：
[0047]
步骤5a)当定位系统开始启动时，读取离线的地图图像；构造关于地图的三层降采样金字塔，对每层图像进行高斯模糊处理，然后计算每层模糊地图对应的梯度图像；
[0048]
步骤5b)从四个鱼眼相机中读取图像，执行鸟瞰图拼接和语义分割处理，得到尺度确定的二值车道线鸟瞰图，对该图像进行金字塔采样与高斯模糊处理；
[0049]
步骤5c)给agv一个初始姿态，以该姿态作为先验，基于三层金字塔实施步骤3)中的姿态优化，优化得到的姿态作为定位系统的初始值；
[0050]
步骤5d)对实时获取的任一鸟瞰图进行姿态估计；
[0051]
步骤5e)在agv行驶的过程中不断检测退化情形，若agv在退化情形中行驶了足够长的距离后，由重新发现了非退化场景，那么启用步骤4)中的误差修正策略来修正误差。
[0052]
优选的，步骤5d)具体包括以下步骤：
[0053]
步骤5d1)通过轮速计获得姿态的先验，其中偏航角使用imu积分来作为补偿；设借助imu与轮速计得到agv相对于世界坐标系的上一时刻姿态与当前姿态而鸟瞰图坐标系相对地图坐标系map的上一刻姿态当前时刻的鸟瞰图姿态的先验由下式得到：
[0054][0055]
步骤5d2)基于上述先验姿态，实施步骤3)中的姿态优化；若出现退化情形，则考虑优化问题中的退化误差项；
[0056]
步骤5d3)计算一个定位置信度，该值主要通过当前帧中的车道线像素点数量m以及帧到地图成功覆盖的车道线像素数量n给出：
[0057]
其中，ω
dege
表示退化权重。
[0058]
本发明的有益效果在于：以视觉为主，imu、轮速计与激光为辅的定位方案，能够在不需要场景改造的前提下，在货物流动大的仓库内实现实时、准确的agv定位。
附图说明
[0059]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0060]
图1为本发明的流程图；
[0061]
图2为本发明中实时定位系统的ui界面；
[0062]
图3为本发明的若干场景中的车道线地图；
[0063]
图4为本发明定位系统的若干测试轨迹；
具体实施方式
[0064]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的其他实施例，都属于本发明保护的范围。
[0065]
本发明的技术思路是：在线边仓等货物流动大的仓库内，车道线是原本就存在的客观元素，基于车道线的定位可以避免对环境的改造，同时能保证对环境变化的高鲁棒性。考虑到激光slam在环境未变化时依然具备高精度，我们采用激光定位信息来构建全局一致的车道线地图。接着，参考视觉slam中的“直接法”来构建实时的全局定位系统。imu和轮速计将给出每帧姿态的先验，该先验可以加快姿态优化问题的收敛。更重要的是，该先验姿态可以在仅有单向车道的退化情形给出一个纵向的约束。但是，imu和轮速计存在不可忽视的漂移误差，因此需要设计误差修正策略。
[0066]
参考图1，一种基于车道线的agv定位方法，包括以下步骤：
[0067]
步骤1)鸟瞰图的拼接与车道线语义分割：
[0068]
步骤1a)对四个鱼眼相机进行内参数标定，以便消除图像中的畸变；标定算法基于鱼眼相机的等距模型：
[0069]
r＝fθ
[0070]
在实际操作中，选用11*9的棋盘格标定板；
[0071]
步骤1b)将四个鱼眼相机固定于agv上方的前后左右方向，在agv周围铺设棋盘格标定布，进行相机的单应性矩阵标定；假设车道线总是分布在绝对水平的地面上，那么图像点坐标p
i
和地面真实二维坐标之间存在单应性变换：
[0072][0073]
单应性矩阵h存在8个自由度；在每个图像上标记四个非共线的角点，根据标定布得出角点的真实坐标，然后求解出唯一的单应性矩阵h；为了提高精度，可以多使用一些角点，根据最小二乘法求解；
[0074]
容易发现除地平面之外的其他元素并不符合该单应性假设，基于单应性阵的投影
变换会使大部分元素发生畸变；但是仅仅关注其中的车道线信息，因此单应性矩阵足以满足要求；
[0075]
步骤1c)将相机内参数和单应性矩阵存储到离线文件中；在定位系统实时运行时，将从四个鱼眼相机中同步读取图像，并根据上述标定数据将鱼眼图像变换为鸟瞰图；然后，将四个方向的鸟瞰图拼接在一起，对于对角线处的四个重叠区域，进行了光平衡(亮度统一)和灰度融合(灰度加权平均)处理，以保证灰度特征的一致性；
[0076]
步骤1d)对于拼接完成的鸟瞰图，采用u
‑
net网络对车道线进行语义分割；其中训练集直接来源于各种光照强度与外部环境下的车道线鸟瞰图；分割后的mask图像将用于地图构建与实时定位；
[0077]
基于颜色与直线段特征的传统方法也可以在某些场合检测出车道线，但是这些方法无法在光照变化剧烈、场景中有相似纹理时保证检测的准确性；采用的神经网络能够大大提高视觉定位对光照等干扰的鲁棒性，而且其网络规模也足以支持在嵌入式设备上实时运行。
[0078]
步骤2)鸟瞰图手眼标定与地图构建：
[0079]
步骤2a)拼接后的鸟瞰图image坐标系和真实的agv坐标系之间存在相似变换：
[0080][0081]
使得：其中s表示尺度，该值在步骤1b)中就已确定；
[0082]
为了对该矩阵进行标定，驱使agv进行小幅度的充分运动，即足够的旋转和平移运动，可以通过手眼标定方程：
[0083][0084]
来估计矩阵其中的借助激光定位系统来给出，而则通过基于鸟瞰图的帧间姿态估计方法计算；
[0085]
由于手眼标定方程对误差很敏感，因此先通过直接测量得到一个粗略的值；然后进行若干次标定，剔除掉与粗略值相差过大的数据；然后再取均值作为最后的结果；进一步的，如果能在步骤1b)时尽量保证鸟瞰图与agv方向一致，那么手眼标定方程将变为线性问题，更易于求解；
[0086]
可以通过基于鸟瞰图的“frame
‑
to
‑
frame”姿态估计方法计算，该方法的具体实施与本发明中的“frame
‑
to
‑
map”姿态优化方法类似；
[0087]
步骤2b)借助激光定位系统提供的agv姿态和步骤2a)中标定的矩阵可以基于鸟瞰图构建关于车道线的全局地图：
[0088]
步骤2b1)当姿态的旋转或平移量超过一定值的时候，才考虑将该帧纳入关键帧队列，否则获取下一帧；利用矩阵将激光定位系统获取的agv姿态转换为image坐标系相对于 map坐标系的姿态，然后将该关键帧变换到map坐标系中；
[0089]
步骤2b2)维护一个临时地图；新加入一个关键帧时，该帧中车道线像素将在临时地图上叠加灰度值，叠加的数值由像素点到agv中心的距离来确定；距离越远，叠加的数值
越小，以减少边缘处因图像质量差而导致的误分割影响；然后，临时地图中灰度值大于某一阈值的点将被纳入到全局地图中；
[0090]
步骤2b3)当新的关键帧加入时，如果队列中关键帧的数量超过某一阈值，就抛弃掉队列中最早的关键帧；同时，在临时地图中消减掉它的灰度影响；这种滑动窗口式的建图方法可以将参与灰度叠加的图像约束在固定数量内，即使agv出现徘徊式的无效运动，也可以保证地图中的车道线始终具有高置信度；
[0091]
步骤3)构建基于视觉的“frame
‑
to
‑
map”姿态优化框架：
[0092]
步骤3a)对鸟瞰图和地图进行高斯模糊处理，这是因为基于直接法构建目标函数，而直接法是严重依赖图像灰度梯度来运作的，其收敛的前提是有效像素到最优解像素之间必须是梯度平滑的；
[0093]
步骤3b)假设已经获取到了鸟瞰图坐标系相对地图坐标系map的姿态先验那么将基于以下误差函数求解最优姿态：
[0094]
e＝e
data
ω
smo
e
smo
ω
dege
e
dege
[0095]
步骤3b1)其中，e
data
是核心误差项；参考“直接法”，该误差项由当前帧与地图的对应像素点的灰度差来组成：
[0096][0097]
其中，q代表当前鸟瞰图中车道线像素点的坐标集合；注意到该集合并不是以地图中的有效像素来构建的，这代表着：“只要当前帧中的车道线能尽可能地匹配到地图中，不管地图中还有多少多余车道线，都认为是实现了最优估计”；这其实是一种提高对车道线遮挡的鲁棒性的策略；另外，由于当前帧与地图都做了模糊处理，因此这里的两个灰度值在图像中都是渐变的，保证了优化问题的平滑性；
[0098]
步骤3b2)第二个误差项是平滑项，设姿态也可以记为向量p＝(t
x
，t
y
，θ)
t
，那么平滑误差项定义为当前姿态与先验姿态之间的欧氏距离：
[0099][0100]
为了避免数值问题，需要在计算欧氏距离之前给θ乘上了一个尺度因子ω
θ
＝100；的 e
smo
意义在于避免噪声的影响；在像素灰度信息很强烈时，该误差项的权重ω
smo
一般设置为较小的值；但反之，当像素灰度信息很弱时，将增大ω
smo
的值来保证优化问题不会因为个别噪声而发散；
[0101]
步骤3b3)最后的误差项是退化项e
dege
，该项仅在退化情形下才会生效；所谓退化情形指的是场景内仅有单向车道线的情况，这种情况在实际厂房中尤为严重；这时仅靠车道线的视觉信息无法判断沿车道线方向的自由度，需要先验来对纵向的位移进行约束；退化项定义为：
[0102][0103]
其中n
dege
表示退化方向的方向向量；
[0104]
步骤3c)计算上述误差函数中关于姿态向量的雅可比矩阵；
[0105]
步骤3c1)对于e
data
而言，在点(i，j)处的雅可比矩阵为：
[0106][0107]
其中j
ima
即为模糊地图的图像梯度；在具体实施中，事先计算好地图每个点的图像梯度，防止优化时的重复计算；另一方面，根据“首估计雅可比”理论(fej)，刚性变换对于姿态来说是平滑变化的，而图像是不可预测的非凸函数，主导着优化的走向；因此为了减少计算量，仅在优化的第一次迭代时才计算j
pose
，而在之后的迭代中仅仅更新j
ima
；这样做可以大大减小计算量，相比而言牺牲的收敛性则可以忽略；
[0108]
可以看出，的优化问题依赖图像梯度来保证收敛性，如果梯度存在大面积为零的情况，优化将会失败；这正是事先实施高斯模糊与降采样金字塔的原因；
[0109]
步骤3c2)对于e
smo
和e
dege
，容易计算其雅可比矩阵，不再赘述；最后，总的雅可比矩阵记为：
[0110]
j＝[j
data j
smo j
dege
]
[0111]
步骤3d)基于上述误差函数与雅可比矩阵，采用lm法求解最优的姿态即迭代构造并求解方程：
[0112]
(j
t
j λi)δp＝
‑
j
t
ε
[0113]
其中λ为梯度因子，每一次迭代都会进行调整，用来确定高斯牛顿法和梯度下降法之间的取舍关系；在收敛效果比较好时减小λ，可以通过高斯牛顿法来加快收敛速度；当收敛效果不理想时增大λ，可以通过最速下降法来改善收敛性；迭代地求解姿态增量δp，直至误差收敛到足够小，则认为得到了最优的姿态；
[0114]
步骤4)设计退化误差修正策略：
[0115]
退化情形具体定义为当前视野或当前位置下的地图中仅存在单向车道线的情况，这种情况在实际厂房中尤为严重，是影响定位精度的主要因素之一；虽然依赖轮速计的约束具有一定精度，但随着行驶距离的增长，其累计误差将不可忽视；考虑到实际情况，可以认为agv 在直行场景中是没有复杂任务的，因而不需要严格准确的纵向位置，只需要保证agv在车道线丰富的场景中不出现漂移误差即可；因此，设计了退化修正策略，用于在agv重新发现非退化场景时消除累积误差；具体实施为以下步骤：
[0116]
步骤4a)对于当前帧的最高层金字塔模糊图像，利用sobel算子统计车道线像素的梯度；如果某个像素点的梯度方向与退化方向的夹角较小，则将该像素点视为非退化方向的车道线像素，将其纳入搜索模板；
[0117]
步骤4b)使用上述搜索模板，沿着退化方向，在最高层金字塔的模糊地图上进行一维搜索，统计该模板与地图图像的投影误差：
[0118][0119][0120]
其中q是搜索模板中的车道线像素点，是模板相对于map坐标系的平移量，x是搜索步长，s
dege
是退化方向上的平移不确定度；
[0121]
s
dege
是在agv退化行驶时不断累加得到的；假设某时刻agv行驶的纵向距离为d
dege
，那么不确定度将累加特别的，如果在初次定位时就处于退化场景，那么直接赋给s
dege
一个较大的值，因为初始定位时的纵向平移量是不可靠的；
[0122]
容易发现x就是发生漂移的那一个自由度，因此需要确定最优的x值；在具体实施中， x是跳跃式增长的，跳跃的步长为该层金字塔的高斯模糊的卷积模板宽度；根据这若干个投影误差e
x
统计出所有的局部极小值，然后分别以这些局部极小值为初始值，进行仅有最高层金字塔参与的姿态优化，保证在高效率的同时实现粗匹配；事实上，其中每次优化用时大约在10ms 以内，因此退化修正策略不会对实时性产生影响，甚至可以在实时定位中频繁启用；
[0123]
步骤4c)对于所有优化后的姿态，计算鸟瞰图与地图的双向投影误差e
bid
(x)；构造该误差时，鸟瞰图中所有的像素都将视作有效像素点；与步骤3b1)中的姿态优化框架里的误差定义不同，这里误差中的有效像素囊括了当前帧和视野内的地图中所有车道线信息，因为该误差的目的在于计算当前帧与地图的相似性；
[0124]
步骤4d)根据误差e
bid
与相应的x，对上述所有姿态进行评估：
[0125][0126]
以评估值最小为原则筛选出最优的姿态；最后，基于该姿态再次运行一次无金字塔的姿态优化，得到最终的修正姿态。
[0127]
步骤5)构建实时的agv定位系统：
[0128]
基于以上的预处理和姿态估计模块，可以整合一个完整的实时定位系统；步骤如下：
[0129]
步骤5a)当定位系统开始启动时，读取离线的地图图像；构造关于地图的三层降采样金字塔，对每层图像进行高斯模糊处理，然后计算每层模糊地图对应的梯度图像；在高层金字塔中，高斯模糊的卷积模板相对较大；这样做是为了在姿态先验不可靠时提供一个“由粗到精”的优化框架，保证优化的收敛性；
[0130]
步骤5b)接着，不断地从四个鱼眼相机中读取图像，并实施步骤(1c)和(1d)中的鸟瞰图拼接和语义分割处理，得到尺度确定的二值车道线鸟瞰图；然后，对该图像进行金字塔采样与高斯模糊处理；由于姿态优化仅需要地图的地图信息，因此无需计算鸟瞰图的图像梯度；
[0131]
步骤5c)在启动定位系统时，需要给出agv一个粗略的初始姿态(通过固定位置或通过其他传感器)；以该姿态作为先验，基于三层金字塔实施步骤3)中的姿态优化，以保证对初始姿态误差的容忍性；在高层金字塔处，姿态平移量误差权重ω
smo
与ω
dege
都需要经过相应的尺度处理；优化得到的姿态将正式作为定位系统的初始值；
[0132]
步骤5d)接着，对于实时获取的任一鸟瞰图进行姿态估计：
[0133]
步骤5d1)通过轮速计获得姿态的先验，其中偏航角使用imu积分来作为补偿；设借助imu与轮速计得到agv相对于世界坐标系的上一时刻姿态与当前姿态而鸟瞰
图坐标系相对地图坐标系map的上一刻姿态那么当前时刻的鸟瞰图姿态的先验由下式得到：
[0134][0135]
步骤5d2)基于该先验姿态，实施步骤3)中的姿态优化；若出现了退化情形，则考虑优化问题中的退化误差项；一般情况下，该优化过程不需要金字塔参与；
[0136]
步骤5d3)计算一个定位置信度，该值主要通过当前帧中的车道线像素点数量m以及帧到地图成功覆盖的车道线像素数量n给出：
[0137][0138]
其中，ω
dege
表示退化权重；退化情况下，该值取0.5，否则取1；如果定位置信度过低，那么下一帧的姿态跟踪将使用三层金字塔进行优化；
[0139]
步骤5e)在agv行驶的过程中不断检测退化情形，若agv在退化情形中行驶了足够长的距离后，由重新发现了非退化场景，那么启用步骤4)中的误差修正策略来修正误差；事实上，为了提高鲁棒性，在置信度较低时也会启用修正策略。
[0140]
以下结合一个agv定位实例，对本发明的技术效果做进一步说明。
[0141]
1、实验条件：
[0142]
本发明的实验的硬件平台为：差速轮叉车式agv；运算设备：nvidiaagx；单线激光雷达；四个鱼眼相机；
[0143]
本发明的实验的软件平台为：ubuntu18.04 lts操作系统；python3.6；opencv4.5；eigen； pangolin；
[0144]
2、实验结果与分析
[0145]
参考图2，是本发明进行实时定位的ui界面。
[0146]
其中左侧展示了当前时刻的姿态与相关信息，可以看出定位的帧率在15帧左右；事实上，抛开相机的硬件延迟与语义分割模块，定位算法用时仅在20ms以内；界面中间展示了当前agv在全局地图中的位置与最近走过的轨迹信息；ui界面右侧展示了当前帧的rgb鸟瞰图、语义分割二值图像以及当前帧与地图的配准图像；注意由于叉车式agv的特殊性，舍去了后方的鱼眼相机；在一般情况下，货物的流动仅影响固定库位中的信息(如图2右上侧图像中的栈板)，对库位边缘和道路两侧车道线并不会产生严重的遮挡；而算法也可以处理一定程度的遮挡问题，见如图2右下侧的配准图像；
[0147]
参考图3，是本发明的若干车道线地图。
[0148]
车道线地图是基于激光定位信息构建的二值图像，具有全局一致性；图3(1)是一个包含退化场景的简易测试地图，总长约10m；图3(2)是一个车道线信息丰富的地图，包含许多横向与纵向的车道线；图3(3)是一个较大场景下的地图，包含了高车道线信息场景和多个长距离的退化场景，而且墙壁遮挡严重，其中退化场景的距离超过了8m；
[0149]
参考图4，是本发明两次实验的agv定位轨迹。
[0150]
图4(a)是一个退化修正测试的轨迹结果，注意在该实验中，周围场景是没有变动的，这意味着激光定位具有较高的参考价值；为了模拟agv发生长距离退化漂移的情况，在agv 的初始定位时人为施加了一个较大的退化漂移误差(
‑
2m)；从得出的agv定位轨迹可以
看出， agv可以在观测到非退化场景时成功修正到正确的姿态。图4(b)和(c)是在较大场景下的自由测试轨迹，该场景中包括较长的走廊，退化比较严重；可以看到本发明的方法可以在该场景中保证全局一致性的定位，能够解决退化场景的导致的自由度丢失问题。
[0151]
以上述依据本发明的理想实施例为启示，通过上述的说明内容，本领域技术人员完全可以在不偏离本发明技术思想的范围内，进行多样的变更以及修改。本发明的技术性范围并不局限于说明书上的内容，必须要根据权利要求书范围来确定其技术性范围。