CN104729500A - 一种激光导航agv的全局定位方法 - Google Patents

Abstract

本发明公开了一种激光导航AGV的全局定位方法，所述的AGV的全局定位包括AGV在环境地图中方向和位置的确定；其中，首先，通过读取安装在AGV本体上的罗盘数据来确定方向信息；其次，利用激光雷达探测到的路标信息，并采用Markov方法来确定位置信息；所述Markov方法包括时间更新和观测更新两个阶段。本发明有效地解决了AGV初始位姿精确定位问题，使得AGV不需要每次都在一个已知的位置地点启动，去除了给AGV的应用带来这种极大的不便，即AGV即使任意位置启动，也能给定AGV一个相对精确的初始位姿。

Description

一种激光导航AGV的全局定位方法

技术领域

本发明属于AGV技术领域，尤其涉及一种激光导航AGV的全局定位方法。

背景技术

根据以往的研究，在利用特征地图对AGV进行定位时需要进行数据关联，在此基础上建立观测与地图中路标之间的对应关系。在现有的数据关联方法中，最近邻域法是一种比较典型的独立相容方法。最近邻域法计算简单、速度快，但是它忽略了地图中路标间的相关性，不能保证得到一致的假设，更重要的是，它是一种局部关联方法，需要给定误差较小的初始值，误差较大的初始值极易导致错误的假设。

考虑最近邻域法忽略了地图中路标间的相关性，有研究者采用更加严格的联合相容检验，提出联合相容分支定界算法。联合相容分支定界算法利用了地图中路标间的相关性，主要依赖于路标间的相对误差而不是绝对误差。联合相容分支定界算法的可靠性要好于最近邻域法。但是，由于数据关联的本质是产生一个观测与地图中路标之间的唯一匹配假设，这要求地图中满足一定数量的路标所构成的所有子地图具有唯一性。然而，这种唯一性并不一定总能得到满足，这时也需要给定AGV一个适当精度的初始位姿。

在基于特征地图的AGV定位中，通常采用扩展卡尔曼滤波方法实现AGV定位。扩展卡尔曼滤波方法也需要给定一个相对精确的初始位姿，初始位姿不精确，会导致扩展卡尔曼滤波定位也不精确甚至导致定位任务失败。由于AGV在环境中的启动位置并不确定，除了AGV自身携带的传感器，没有其它任何可以用于确定AGV初始位姿的定位***，除非AGV每次都在一个位置已知的地点启动，这给AGV的应用带来极大的不便。

发明内容

针对于上述现有技术的缺陷，本发明的目的在于提供一种激光导航AGV的全局定位方法，以解决现有技术的特征地图中AGV初始位姿未知情况下的全局定位问题。

为达到上述目的，本发明的一种激光导航AGV的全局定位方法，所述的AGV的全局定位包括AGV在环境地图中方向和位置的确定；其中，首先，通过读取安装在AGV本体上的罗盘数据来确定方向信息；其次，利用激光雷达探测到的路标信息，并采用Markov方法来确定位置信息。

优选地，采用Markov方法来确定位置信息时，首先将AGV的连续位置空间离散化，具体为：定义AGV在环境中的坐标为(x,y)，定义AGV在地图中需要进行全局定位的矩形区域为[X_min，X_max；Y_min，Y_max]，其中，X_min和X_max为x方向的最小值和最大值，Y_min和Y_max为y方向的最小值和最大值；

将矩形区域[X_min，X_max；Y_min，Y_max]以分辨率[ResX，ResY]离散化为一个个小的矩形单元，其中，ResX和ResY分别为x和y方向离散化的分辨率，通常设定ResX＝ResY，一个矩形单元称之为一个栅格，用表示，其中，k为时间，其初值为零，i为栅格索引，为栅格赋予一个信度，表示为整个矩形区域的栅格构成一幅信度图像。

优选地，上述的Markov方法包括时间更新和观测更新两个阶段。

优选地，上述的Markov方法的时间更新是给出一种基于频域处理的时间更新计算方法，具体过程为：

a.通过傅里叶变换将信度图像从空域变换到频域，变换公式为：

$F(u,v)=\underset{x=0}{\overset{M-1}{\mathrm{\Σ}}}\underset{y=0}{\overset{N-1}{\mathrm{\Σ}}}f(x,y)e^{-i2\mathrm{\π}(\frac{\mathrm{ux}}{M}+\frac{\mathrm{vy}}{N})}$

式中，f和F分别为空域图像和频域图像，令M和N分别为x和y方向的栅格数，u和v分别为x和y方向的位置量；e为自然指数函数；

b.对频域图像进行乘法操作，具体计算公式为：

$F(u,v)e^{-i2\mathrm{\π}({\mathrm{ux}}_0/M+{\mathrm{vy}}_0/N)}$

x₀和y₀分别为信度图像根据AGV运动量需要在空域中的x和y方向的平移量；

c.在频域中对信度图像进行模糊化操作，具体公式为：

F(u,v)·H(u,v)

其中，F为利用傅里叶变换的周期性将低频部分移至图像中心后的频域图像；其中，D₀为人为设置的滤波参数；

d.通过傅里叶逆变换将信度图像从频域变换回空域，完成一次Markov方法的时间更新阶段，具体公式为：

$f(x,y)=\underset{u=0}{\overset{M-1}{\mathrm{\Σ}}}\underset{v=0}{\overset{N-1}{\mathrm{\Σ}}}F(u,v)e^{i2\mathrm{\π}(\frac{\mathrm{ux}}{M}+\frac{\mathrm{vy}}{N})}$

定义时间更新的信度图像令 $\stackrel{\‾}{\mathrm{Bel}}\left({\hat{X}}_{k,i}\right)=f(x,y).$

优选地，上述的Markov方法的观测更新是给出一种基于高斯核平滑的观测更新计算方法，具体过程为：

a.对路标位置函数进行连续化，定义AGV在某一时刻观测到n个路标，每一个路标的位置在激光雷达坐标系下表示为((ρ_i,θ_i)，i∈(1,n)，定义该时刻的路标位置函数对该函数进行连续化操作，具体公式为：

$\mathrm{\ρ}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\exp \{-{(\mathrm{\θ}-{\mathrm{\θ}}_i)}^2/{\left({2\mathrm{\σ}}_i\right)}^2\}$

式中，exp为高斯核函数；σ_i是核的带宽，此处，σ_i取为λ/ρ_i；λ为大于零的常数，可通过人工经验进行调节和确定；

b.对连续化后的路标位置函数ρ进行等角度间隔离散化，将以等角度Δθ离散化为τ个值，得到离散化后的路标位置函数为：

$\mathrm{\ρ}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\exp \{-{({\mathrm{\θ}}_{\min }+\mathrm{r\Δ\θ}-{\mathrm{\θ}}_i)}^2/{\left(2{\mathrm{\σ}}_i\right)}^2\}$

式中，Δθ为r∈(0,τ-1)；

c.进行观测似然计算，具体公式为：

$p\left(z_k\right|{\hat{X}}_{k,i},M)=\underset{i=1}{\overset{\mathrm{\τ}}{\mathrm{\Π}}}\left(\frac{{\mathrm{\ρ}}_{\max }-|{\mathrm{\ρ}}_z\left({\mathrm{\θ}}_i\right)-{\mathrm{\ρ}}_m\left({\mathrm{\θ}}_i\right)|}{{\mathrm{\ρ}}_{\max }}\right)$

式中，ρ_max为AGV最远能探测到的路标的距离；ρ_z为AGV实际观测平滑后的观测值；ρ_m为AGV在地图中处的观测平滑后的观测值；

d.根据观测似然值对信度图像进行更新，具体公式为：

$\mathrm{Bel}\left({\hat{X}}_{k,i}\right)=\mathrm{\ηp}\left(z_k\right|{\hat{X}}_{k,i},M)\stackrel{\‾}{\mathrm{Bel}}\left({\hat{X}}_{k,i}\right)$

式中，η＝1/p(z_k|Z_0:k-1,U_0:k)，为归一化常数。

本发明的有益效果：

本发明有效地解决了AGV初始位姿精确定位问题，使得AGV不需要每次都在一个已知的位置地点启动，去除了给AGV的应用带来这种极大的不便，即AGV即使任意位置启动，也能给定AGV一个相对精确的初始位姿。

附图说明

图1绘示本发明一种激光导引AGV的全局定位方法示意图。

图2绘示本发明的实施例中激光导航AGV***硬件结构图。

图3绘示本发明的定位方法AGV位置空间离散化示意图。

图4绘示本发明的基于频域处理的时间更新方法流程图。

图5绘示本发明的高斯核平滑原理图。

图6绘示本发明的参数λ对平滑结果的影响示意图。

具体实施方式

为了便于本领域技术人员的理解，下面结合实施例与附图对本发明一种激光雷达导引AGV的全局定位方法作进一步的说明，使本发明的目的、技术方案及效果更加清楚、明确，实施方式提及的内容并非对本发明的限定。

应当理解的是，本发明中所描述的具体实施例仅仅是对本发明精神作举例说明，对本领域普通技术人员来说，可以做各种各样的修改或补充或采用类似的方式替代，所有这些改进和变换都应属于本发明所附权利要求的保护范围。

参照图2所示，本发明一种激光雷达导引AGV的全局定位方法所采用的激光导航AGV***的硬件结构，主要包括AGV本体、控制***、驱动***和传感***。其中，AGV本体尺寸为长100cm、宽60cm、高40cm；配置了两个差速驱动的主动轮和四个万向轮，主动轮位于车体几何中心的两侧，四个万向轮分别位于车体的正前、正后、左中和右中的位置；控制***由基于PC的上位控制计算机和基于ARM的嵌入式下位控制计算机组成；驱动***由两组电机驱动器、直流伺服电机和减速器组成；传感***由两个编码器、一个激光雷达和一个电子罗盘组成。

在本实施例中，设置激光雷达扫描分辨率为0.5°，距离测量分辨率为1mm，在该测量距离分辨率设置下激光雷达的最远探测距离为8m。

在本实施例中，基于ARM的嵌入式下位控制计算机主要负责采集编码器数据并进行航迹推算，采集电子罗盘数据，并将航迹推测结果与电子罗盘数据通过串口发送至上位控制计算机，同时接收上位控制计算机发送过来的速度控制命令，并通过PWM控制AGV的运动。

在AGV运动所在平面的直角坐标系中，AGV的位姿表示为以AGV历史观测、运动控制量和地图为条件的k时刻AGV位姿的后验分布，即p(X_k|Z_0:k,U_0:k,M)进行定位问题估计。

式中各变量含义如下：

X_k——k时刻的AGV位姿；

Z_0:k——从0到k时刻的AGV对路标的所有观测，Z_0:k＝{z₀,z₁,...,z_k}，其中z_k为k时刻的观测；

U_0:k——从0到k时刻的AGV所有运动控制输入，U_0:k＝{u₀,u₁,...,u_k}，其中u_k为k时刻的控制输入；

M——环境地图，本文中其中n为路标(特征点)数量，m_i＝(x_i,y_i)^T，x_i和y_i为路标在二维世界坐标系下的坐标。

AGV的运动控制模型状态转移矩阵的概率分布表示为：p(X_k|X_k-1,u_k)。

AGV在给定位姿X和地图M的情况下，观测模型的后验分布表示为：p(z_k|X_k,M)。

所述的AGV的全局定位包括确定AGV在环境地图中的位置和方向，其中，方向通过读取安装在AGV本体上的罗盘数据来确定；位置利用激光雷达探测到的路标信息，本实施例采用Markov方法来确定；该Markov方法包括时间更新和观测更新两个阶段，具体实现步骤如下所示：

在宽敞的室内环境下，选择在环境尺寸20m×20m空房间环境中进行Markov全局定位。对房间整***置进行坐标设置，首先确定一个坐标系原点和X、Y轴，把所有AGV走过的路线放在第一象限，确保所有的位置坐标都是整数；在室内非对称的布置20块挡板，并计算好各块挡板的对应位置，将AGV置于环境中，使其漫游一段时间，截取前100s的原始传感器数据进行AGV定位；时间更新采用本发明提出的基于频域处理的时间更新方法，观测更新采用本发明提出的基于高斯核平滑的观测更新方法。

参照图1所示，首先，将AGV的连续位置空间离散化，AGV在地图中需要进行全局定位的矩形区域为[0,20；0,20]，将矩形区域[0,20；0,20]以分辨率[0.1,0.1]离散化为一个个小的矩形单元，其中，0.1和0.1分别为x和y方向离散化的分辨率，通常设定等值。一个矩形单元称之为一个栅格，用表示，其中，k为时间，i为栅格索引。为栅格赋予一个信度，表示为整个矩形区域的栅格构成一幅信度图像，如图3所示。

AGV在处预测的信度为对应的校正后的信度为则离散化后的时间更新方程式和观测更新方程式可以分别表示为：

$\begin{array}{c}\stackrel{\‾}{\mathrm{Bel}}\left({\hat{X}}_{k,i}\right)=\underset{j\∈\mathrm{\Θ}}{\mathrm{\Σ}}p\left({\hat{X}}_{k,i}\right|{\hat{X}}_{k-1,j},u_k)\mathrm{Bel}\left({\hat{X}}_{k-1,j}\right)\\ \mathrm{Bel}\left({\hat{X}}_{k,i}\right)=\mathrm{\ηp}\left(z_k\right|{\hat{X}}_{k,i},M)\stackrel{\‾}{\mathrm{Bel}}\left({\hat{X}}_{k,i}\right)\end{array}$

利用本实施例中得到的AGV数据进行AGV全局定位，采用了Markov方法的时间更新，即一种基于频域处理的时间更新计算方法，该方法的具体过程为：

a.通过傅里叶变换将信度图像从空域变换到频域，就是利用二维离散傅里叶变换公式：将信度的空域图像f(x,y)经傅里叶变换变换到频域，得到信度的频域图像F(u,v)；式中，f和F分别为空域图像和频域图像；x和y方向的栅格数均为100；

b.信度图像在空域中的非整数倍单位栅格平移操作，利用空域图像和频域图像之间的关系：获得新的信度频域图像，公式中的x₀和y₀是任意实数，实现信度图像的非整数倍单位栅格平移；

c.信度图像在频域中的模糊化操作为：F(u,v)·H(u,v)；其中，F为利用傅里叶变换的周期性将低频部分移至图像中心后的频域图像；其中，D₀为人为设置的滤波参数；

d.通过傅里叶逆变换将信度图像从频域变换回空域，完成一次Markov方法的时间更新阶段，如图4所示。

本实施例继续进行高斯核平滑的观测更新计算方法，该方法的具体过程为：

a.AGV在某一时刻观测到3块挡板即3个路标，任意一个路标可以表示为函数ρ_i＝ρ_iδ((θ-θ_i)，将所有观测到的路标所对应的函数求和，得到一个非光滑、非连续的路标位置函数模型：如图5所示；

b.对非光滑、非连续的路标位置函数进行连续化，用高斯核函数取代上式中的ρ_iδ(θ-θ_i)，得到连续的路标位置函数为：

式中，exp为高斯核函数；σ_i是核的带宽，此处，σ_i取为λ/ρ_i；λ为大于零的常数，λ对平滑结果的影响参照图6所示，λ越大，所得路标位置函数越平滑；

c.对连续化后的路标位置函数进行等角度间隔离散化：将自变量θ∈(θ_min,θ_max)以等角度Δθ分成τ段，则任意一个路标位置可表示为：

$\mathrm{\ρ}=\underset{i=1}{\overset{3}{\mathrm{\Σ}}}\exp \{-{({\mathrm{\θ}}_{\min }+\mathrm{r\Δ\θ}-{\mathrm{\θ}}_i)}^2/{\left(2{\mathrm{\σ}}_i\right)}^2\},$ 式中，Δθ为

d.进行观测似然的计算： $p\left(z_k\right|{\hat{X}}_{k,i},M)=\underset{i=1}{\overset{\mathrm{\τ}}{\mathrm{\Π}}}\left(\frac{{\mathrm{\ρ}}_{\max }-|{\mathrm{\ρ}}_z\left({\mathrm{\θ}}_i\right)-{\mathrm{\ρ}}_m\left({\mathrm{\θ}}_i\right)|}{{\mathrm{\ρ}}_{\max }}\right),$

其中，ρ_max为AGV最远能探测到的路标的距离；ρ_z为AGV实际观测平滑后的观测值；ρ_m为AGV在地图中处的观测平滑后的观测值；如此便完成一次观测更新，此时可能会得到多个AGV状态信度的可能位置。

为了获取准确的AGV定位信息，我们利用采集到的原始激光传感器数据，反复进行时间更新和观测更新，最终得到一个准确的AGV位置信度的收敛值，基于Markov的AGV定位的具体算法如下：

初始化k＝0；

For每一个栅格

$\mathrm{Bel}\left({\hat{X}}_{k,i}\right)=p\left({\hat{X}}_{k,i}\right);$ //初始化概率栅格

End

While TRUE

k＝k+1；

通过基于频域处理的时间更新模型计算

w＝0；

For每一个栅格

计算观测似然

$\mathrm{Bel}\left({\hat{X}}_{k,i}\right)=p\left(z_k\right|{\hat{X}}_{k,i},M)\stackrel{\‾}{\mathrm{Bel}}\left({\hat{X}}_{k,i}\right);$

$w=w+\mathrm{Bel}\left({\hat{X}}_{k,i}\right);$

End

For每一个栅格

$\mathrm{Bel}\left({\hat{X}}_{k,i}\right)=w^{-1}\·\mathrm{Bel}\left({\hat{X}}_{k,i}\right);$

End

End

本发明具体应用途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以作出若干改进，这些改进也应视为本发明的保护范围。

Claims (10)

1.一种激光导航AGV的全局定位方法，其特征在于，所述的AGV的全局定位包括AGV在环境地图中方向和位置的确定；首先，通过读取安装在AGV本体上的罗盘数据来确定方向信息；其次，利用激光雷达探测到的路标信息，并采用Markov方法来确定位置信息。

2.如权利要求1所述的一种激光导航AGV的全局定位方法，其特征在于，采用Markov方法来确定位置信息时，首先将AGV的连续位置空间离散化，具体为：定义AGV在环境中的坐标为(x,y)，定义AGV在地图中需要进行全局定位的矩形区域为[X_min，X_max；Y_min，Y_max]，其中，X_min和X_max为x方向的最小值和最大值，Y_min和Y_max为y方向的最小值和最大值；

将矩形区域[X_min，X_max；Y_min，Y_max]以分辨率[ResX，ResY]离散化为一个个小的矩形单元，其中，ResX和ResY分别为x和y方向离散化的分辨率，通常设定ResX＝ResY，一个矩形单元称之为一个栅格，用表示，其中，k为时间，其初值为零，i为栅格索引，为栅格赋予一个信度，表示为整个矩形区域的栅格构成一幅信度图像。

3.如权利要求1所述的一种激光导航AGV的全局定位方法，其特征在于，所述Markov方法包括时间更新和观测更新两个阶段。

4.如权利要求3所述的一种激光导航的AGV全局定位方法，其特征在于，所述的时间更新阶段采用一种基于频域处理的时间更新方法，该方法通过傅里叶变换将信度图像从空域变换到频域，利用空域图像和频域图像之间的关系实现信度图像的非整数倍栅格平移和对信度图像的模糊化操作，然后通过傅里叶逆变换将信度图像从频域变换回空域。

5.如权利要求4所述的一种激光导航AGV的全局定位方法，其特征在于，所述空域图像和频域图像之间的关系为：

$f(x-x_0,y-y_0)\⇔F(u,v)e^{-i2\mathrm{\π}({\mathrm{ux}}_0/M+{\mathrm{vy}}_0/N)}$

其中，f为空域图像；F为频域图像；M和N分别为x和y方向的栅格数；u和v分别为x和y方向的位置量；x₀和y₀分别为信度图像根据AGV运动量需要在空域中的x和y方向的平移量。

6.如权利要求4所述的一种激光导航AGV的全局定位方法，其特征在于，所述的对信度图像的模糊化操作为：

F(u,v)·H(u,v)

其中，F为利用傅里叶变换的周期性将低频部分移至图像中心后的频域图像；其中，D₀为人为设置的滤波参数。

7.如权利要求3所述的一种激光导航AGV的全局定位方法，其特征在于，所述的观测更新阶段采用一种基于高斯核平滑的观测更新方法，该方法通过高斯核函数对提取的路标进行连续化，对连续化后的虚拟环境轮廓进行等角度间隔离散化，最后建立观测似然模型计算观测似然。

8.如权利要求7所述的一种激光导航AGV的全局定位方法，其特征在于，所述对提取的路标进行连续化，具体过程为：

a.定义AGV在某一时刻观测到n个路标，则该n个路标在激光雷达极坐标系下的位置分别为(ρ₁,θ₁)，(ρ₂,θ₂)，......，(ρ_n,θ_n)，将θ视为自变量，ρ视为因变量，则任意一个路标可以表示为函数ρ_i＝ρ_iδ((θ-θ_i)，其中δ函数为：

$\mathrm{\δ}\left(x\right)=\left\{\begin{array}{cc}1& x=0\\ 0& x\≠0\end{array}\right.$

将所有观测到的路标所对应的函数求和，得到一个非光滑、非连续的路标位置函数：

$\mathrm{\ρ}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\ρ}}_i\mathrm{\δ}(\mathrm{\θ}-{\mathrm{\θ}}_i)$

b.对非光滑、非连续的路标位置函数进行连续化，用高斯核函数取代上式中的ρ_iδ(θ-θ_i)，得到连续的路标位置函数为：

$\mathrm{\ρ}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\exp \{-{(\mathrm{\θ}-{\mathrm{\θ}}_i)}^2/{\left({2\mathrm{\σ}}_i\right)}^2\}$

式中，exp为高斯核函数；σ_i是核的带宽，此处，σ_i取为λ/ρ_i；λ为大于零的常数，λ越大，所得路标位置函数越平滑。

9.如权利要求7所述的一种激光导航AGV的全局定位方法，其特征在于，所述对连续化后的路标位置函数进行等角度间隔离散化，具体方法为：

将自变量θ∈(θ_min,θ_max)以等角度Δθ分成τ段，其中，θ_min和θ_max分别为θ的最小值和最大值，由此可得离散化的路标位置函数：

$\mathrm{\ρ}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\exp \{-{({\mathrm{\θ}}_{\min }+\mathrm{r\Δ\θ}-{\mathrm{\θ}}_i)}^2/{\left({2\mathrm{\σ}}_i\right)}^2\}$

式中，Δθ为

10.如权利要求7所述的一种激光导航AGV的全局定位方法，其特征在于，所述观测似然的计算方法为：

$p\left(z_k\right|{\hat{X}}_{k,i},M)=\underset{i=1}{\overset{\mathrm{\τ}}{\mathrm{\Π}}}\left(\frac{{\mathrm{\ρ}}_{\max }-|{\mathrm{\ρ}}_z\left({\mathrm{\θ}}_i\right)-{\mathrm{\ρ}}_m\left({\mathrm{\θ}}_i\right)|}{{\mathrm{\ρ}}_{\max }}\right)$

其中，ρ_max为AGV最远能探测到的路标的距离；ρ_z为AGV实际观测平滑后的观测值；ρ_m为AGV在处的观测平滑后的观测值。