[发明专利]一种基于地标识别的AGV导航定位方法

权利要求书

1.一种基于地标识别的AGV导航定位方法，其特征在于，包括粗定位和精定位，粗定位实现基于视觉地标解算的路径规划，精定位实现基于视觉地标的精定位调姿量解算；所述基于视觉地标解算的路径规划包括：

(1)基于地标解算的精定位与粗定位分界方式；

将路径规划过程分为精定位与粗定位阶段：粗定位导航过程为初始站位导航至分界地标的过程；精定位导航过程为，粗定位真实驶达分解地标站位导航至目标站位的过程；距离目标站位最近的视觉地标为分界地标；记建立的视觉地标位姿索引库中，地标位姿为N_tag表示地标的数量，M表示地标位姿的集合，地标标号i的位姿为M_i；记目标站位的位姿为T_Goal，D_i为标号i的地标与目标站位的位姿关系，则地标与站位的位姿关系表示为：

D_i＝M_i-T_Goal,i∈{1，2,3…，N_tag} (1)

记g为分界地标序号，||D_g||为分界地标与站位的最小二范数，则可得：

||D_g||＝||D_i||_min,i∈{1，2,3…，N_tag} (2)；

(2)基于AGV相关不确定度与地标解算的导航分段方法：

路径规划其实是一个动态的过程，在循迹过程中常需重新规划路线，重新规划路线时以当前位姿为初始站位，故当时位姿精度水平，影响AGV路径规划算法的鲁棒性；为提高循迹过程中AGV运动精度，提高循迹可靠性，结合解算地标校准AGV位姿方法，在粗定位导航阶段的路径规划中，设计基于AGV相关不确定度与地标解算的分段导航方法；

粗定位导航路径中，因地标测量不确定度影响AGV运动控制的精度，为此，分别测算如下不确定度：分界地标位姿测量不确定度σ_Mg、路径规划坐标系与运行环境坐标系耦合不确定度σ_T、AGV所载视觉传感器解算视觉地标不确定度σ_C以及运动控制不确定度σ_m，因此，初始点至地标g不确定度汇总表述为∑σ_g：

式中，σ_m与AGV实际运动距离密切相关，也是AGV运动误差的主要来源，考虑不确定度极值，运动距离d_m、运动单位距离的不确定度定义为σ_u，运动不确定度可表述为σ_m＝σ_u·d_m；f表示函数关系，为保证不确定度计算的鲁棒性，在无法获得不确定度具体函数关系前提下，考虑不确定度单向极限偏差；

相机视距d_F与地标尺寸d_M同样影响分段规划次数N，结合不确定度分析可得如下关系：

从而得到分段规划次数N：

(3)分段导航规划中分段地标的选取方法：

分段的参考距离d_Seg为：

式中，d(T_Start,M_g)表示T_Start与M_g之间的曼哈顿距离；

记选取的N个地标标号分别为i₁,i₂,…,i_N-1,g，则i_j地标的选取方法为：

(4)提升AGV运行稳定性的基于地标的路径规划算法：

各分段中，设计有初始站位至目标站位的规划；路径规划算法中，为提高AGV转站运动平稳性减少AGV运动方向突变，引入拓展方向概念；栅格地图的路径搜索中，节点n-1拓展方向表示为D_n-1,n，则路径搜索过程中的代价函数计算公式为：

f(n)是节点n的综合优先级，h(n)是节点n距离终点的预计代价；g(n)是节点距离起点n的代价，当拓展节点的拓展方向与上一结点相同时，该拓展节点曼哈顿距离值减小为实际的k_same倍，当拓展节点与上一拓展节点不同时，该拓展节点曼哈顿距离值增大为实际距离的k_diff倍；当选择下一个遍历节点时，总会选取f(n)最小的节点，距离采用曼哈顿距离；依次计算搜索节点n,即可规划处初始站位至目标站位的路径；

所述基于视觉地标的精定位调姿量解算包括如下内容：

粗定位导航过程完成后，即为精定位导航过程，精定位导航的核心为解算精定位调姿量；

分界地标的位姿为M_g＝(x_g,y_g,θ_g)，记其对应的位姿矩阵为

则AGV识别地标解算的自身位姿为坐标为式中下标(0,2)表示第0行第2列：

目标站位位姿为T_Goal，识别地标时刻的AGV与理论目标站位的偏差为Δp：

精定位调姿过程，调姿量为偏差ΔT与里程计位姿的差值，其中里程计位姿记录为：

调姿量为：

x方向、y方向以及θ角度方向的调姿裕度为：

精定位调姿结束判定条件为：