CN111781609B - 一种agv激光导航多边定位方法 - Google Patents

Abstract

一种AGV激光导航多边定位方法，包括有车体、激光雷达和至少三个反射板，包括有以下步骤：步骤1：从激光雷达的数据中筛选出反射板的局部坐标：反射板采用直径为D的圆柱反射板，激光雷达的数据中选取与每个反射板之间距离最小的(r_i，θ_i)，并使r_i＝r_i+D/2；步骤2：利用匹配算法得到每个反射板在地图中的全局坐标；步骤3：利用三边定位算法确定出车体即时位置的一个近似值(x_a0,y_a0)；步骤4：采用基于最小二乘法的迭代搜索多路标定位算法实现车体的精确定位；步骤5：计算车体的方向角。采用基于残差平方和的最小二乘迭代算法，相比于现有技术中的三角法和三边法，该方法明显提高了AGV车体定位的精度和稳定性；提高了匹配的准确性，降低了对反射板布局的依赖性。

Description

一种AGV激光导航多边定位方法

技术领域

本发明涉及AGV高精度定位技术领域，具体涉及一种AGV激光导航多边定位方法。

背景技术

激光雷达技术广泛应用在AGV、无人驾驶等领域，基于激光雷达技术的AGV凭借其较高的稳定性，较高的定位精度，以及对场景依赖性小的特性，广泛应用在货物运输、快递运输等领域。激光雷达主要应用于AGV的自身定位，目前主流的定位方式是基于反光板的三角定位算法。

申请号为CN201910326851.8的中国专利公开了一种激光导航AGV的高精度定位方法，基于卡尔曼滤波的算法来提高定位的稳定性，解决AGV小车在运动过程中定位的延时性，这极大地提高了AGV的运行速度。

上述技术方案中，AGV小车的定位过程中只能利用三个反射板的信息，并且上述方法依赖反射板设置位置来对AGV小车进行定位，导致其定位精度低，现有技术存在改进之处。

发明内容

为解决上述技术问题，本发明提出了。本发明公开了一种基于最小二乘法的多边定位算法，可以利用三个以上的反射板进行定位，大大提高了定位精度。

为达到上述目的，本发明的技术方案如下：一种AGV激光导航多边定位方法，包括有

车体、激光雷达和至少三个反射板，包括有以下步骤：

步骤1：从所述激光雷达的数据中筛选出反射板的局部坐标：

所述反射板采用直径为D的圆柱反射板，所述激光雷达的数据中选取与每个所述反射

板之间距离最小的(r_i，θ_i)，并使r_i＝r_i+D/2；

步骤2：利用匹配算法得到每个反射板在地图中的全局坐标：

S1、在扫描得到的多个反射板中任意选取出三个反射板成一个观测三角形abc，在地图中形成与观测三角形abc对应的地图三角形ijk；

S2、在地图中找到与边d(a,b)和d(a,c)长度最接近的边d(i,j)和d(i,k)；

S3、判断d(b,c)与d(j,k)之差的绝对值，直至差小于指定值，则a与j匹配成功，反射板a的全局坐标即地图中反射板i的全局坐标；

S4、重复S1、S2、S3直到所有的所述反射板全局坐标匹配完成；

步骤3：利用三边定位算法确定出所述车体即时位置的一个近似值(x_a0,y_a0)：

步骤4：采用基于最小二乘法的迭代搜索多路标定位算法实现车体的精确定位：

所述激光雷达扫描到n个所述反射板的(n大于3)，分别以这n个所述反射板在全局坐标系中坐标为圆心，以所述激光雷达检测距离为半径画圆，所形成n个圆会交于一点，该交点为所述车体的位置；

设所述车体的坐标为(x_a，y_a)，建立这n个圆方程，将步骤3得到的(x_a0,y_a0)来表示(x_a，y_a)，用δr表示r的测量误差，则得到

求得(xa，ya)的坐标；

步骤5：计算所述车体的方向角。

通过采用上述技术方案，相比于三角法和三边法，明显提高了车体定位的精度和稳定性，在反射板匹配中通过匹配算法大大提高了匹配的准确性，降低了对反射板布局的依赖性。

本发明进一步设置为：在完成所述步骤2之后，所述车体对当前所在位置会有一个估计值，根据估计值可以推算出观测到的反射板在全局坐标的位置，再与全局坐标进行比较即可完成匹配。

本发明进一步设置为：根据所述步骤2中的历史坐标数据预测出所述车体的即时位置(x₀，y₀)，预估出观测的反射板在全局坐标系下的位置(x_i，y_i)，预估出观测的反射板在局部坐标系下的位置(x_s，y_s)，由

x_i＝x₀+x_scosα₀-y_ssinα₀

y_i＝y₀+y_ssinα₀+y_scosα₀

α₀＝α₁-θ

在地图中找出与(x_i，y_i)最接近的点，与该反射板的全局坐标为(x_k，y_h)，

判断d(x_i，y_i)与d(x_k，y_h)之差的绝对值小于ε，其中0.3m≦ε≦0.5m。

本发明进一步设置为：所述步骤3中所述三边定位算法为：以三个反射板的全局坐标(x₁，y₁)(x₂，y₂)(x₃，y₃)为圆心，分别以激光雷达测得所述车体到三个反射板的距离r₁,r₂,r₃为半径做圆，这三个圆将交于所述车体的真实位置附近，通过计算交点得到所述车体位置的近似值(x_a0，y_a0)。

本发明进一步设置为：所述车体真实位置确认包括有：通过(x₁，y₁)、(x₂，y₂)、r₁、r₂确定两个交点O₁(x_O1，y_O1)，O₂(x_O2,y_O2)，则有：x_O1＝x₁+r₁*cos(α+θ)；y_O1＝y₁+r₁*sin(α+θ)；x_O2＝x₁+r₁*cos(α-θ)；y_O2＝y₁+r₁*sin(α-θ)；其中α由三角形余弦定理有：

得到：

反射板(x₃，y₃)到O₁(x_O1，y_O1)，O₂(x_O2,y_O2)点的距离d1,d2

若d1与r₃大小相近，且之差小于容许值ξ(0.3m≦ξ≦0.5m)，则点O₁(x_O1,y_O1)为近似值(x_a0，y_a0)，反之点O₂(x_O2,y_O2)为近似值(x_a0，y_a0)。

本发明进一步设置为：所述步骤5中，

所述车体在全局坐标系中方位角为α₀，由激光雷达检测第1个激光反射板得到的车体的方位角为α₁，则有：

则检测第n个反射板得到的方位角为α_n，

则有：

则所述车体相对于全局坐标系的方向角为：

本发明进一步设置为：，

所述δr为随机值δr₁～δr_n，得到：

并定义：

得到：

用最小二乘法，求解

得到

本发明进一步设置为：

采用矩阵形式表示为：

其中：

使残差平方和最小，即

min{δ_r1 ²+δ_r2 ²+…+δ_rn ²}

用最小二乘法，求解

得到最终所述车体的坐标为：

本发明进一步设置为：所述步骤2中的指定值为0.3～0.5m。

综上所述，本发明具有以下效果：

1、采用基于残差平方和的最小二乘迭代算法，即采用改进的基于最小二乘法的迭代搜索多路标定位算法实现AGV车体的定位；

2、相比于现有技术中的三角法和三边法，该方法明显提高了AGV车体定位的精度和稳定性；

3、在反射板匹配中，提出了三角形匹配算法和历史位姿匹配相结合的办法，提高了匹配的准确性，降低了对反射板布局的依赖性。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍。

图1为AGV激光导航多边定位方法的流程图。

图2为激光雷达扫描到反射板的示意图；

图3为局部坐标内反射板的示意图；

图4为全局坐标内反射板的示意图；

图5为三边定位算法的示意图；

图6为车体、反射板在局部坐标、全局坐标的示意图。

图中：1、车体；2、激光雷达；3、反射板。

具体实施方式

下面结合附图对本发明作进一步详细的说明。

如图1所述，一种AGV激光导航多边定位方法，包括有车体、激光雷达和至少三个反射板，包括有以下步骤：

步骤1：从所述激光雷达的数据中筛选出反射板的局部坐标：

步骤2：利用匹配算法得到每个反射板在地图中的全局坐标：

步骤3：利用三边定位算法确定出所述车体即时位置的一个近似值(x_a0,y_a0)：

步骤4：采用基于最小二乘法的迭代搜索多路标定位算法实现车体的精确定位：

步骤5：计算所述车体的方向角。

如图2所示，上述步骤1包括有：反射板采用直径为D的圆柱反射板，激光雷达扫描一周，可以测得反射板距离激光头的距离、角度和反射强度数据，根据反射板的反射强度远大于周围环境的反射强度将反射板的数据与周围环境的数据分离，将周围环境的数据剔除掉。

选择出最佳反射板数据。在同一个反射板的数据中，选取距离最小的r_i那一组(r_i，θ_i)作为该反射板的数据，并将反射板的半径距离作为补偿得到：r_i＝r_i+D/2。

如图3和图4所示，所述步骤2具体包括有：在步骤1中得到了反射板到车体的距离r_i和反射板在局部坐标系下的角度θ_i,步骤2则是在预先建立的已知反射板全局坐标地图中，匹配所有观测反射板的全局坐标。

三角形匹配法：

S1、在扫描得到的多个反射板中任意选取出三个反射板成一个观测三角形abc，在地图中形成与观测三角形abc对应的地图三角形ijk；

S2、在地图中找到与边d(a,b)和d(a,c)长度最接近的边d(i,j)和d(i,k)；

S3、判断d(b,c)与d(j,k)之差的绝对值，直至差小于指定值，则a与j匹配成功，反射板a的全局坐标即地图中反射板i的全局坐标；

S4、重复S1、S2、S3直到所有的所述反射板全局坐标匹配完成；

历史位置匹配法：

在完成三角形匹配法之后，所述车体对当前所在位置会有一个估计值，根据估计值可以推算出观测到的反射板在全局坐标的位置，再与全局坐标进行比较即可完成匹配。

据上述步骤2中的历史坐标数据预测出所述车体的即时位置(x₀，y₀)，预估出观测的反射板在全局坐标系下的位置(x_i，y_i)，预估出观测的反射板在局部坐标系下的位置(x_s，y_s)，由

x_i＝x₀+x_scosα₀-y_ssinα₀

y_i＝y₀+y_ssinα₀+y_scosα₀

α₀＝α₁-θ

在地图中找出与(x_i，y_i)最接近的点，与该反射板的全局坐标为(x_k，y_h)，判断d(x_i，y_i)与d(x_k，y_h)之差的绝对值小于ε，其中0.3m≦ε≦0.5m。

如图5所示，三边定位算法为：以三个反射板的全局坐标(x₁，y₁)(x₂，y₂)(x₃，y₃)为圆心，分别以激光雷达测得所述车体到三个反射板的距离r₁,r₂,r₃为半径做圆，这三个圆将交于所述车体的真实位置附近，通过计算交点得到所述车体位置的近似值(x_a0，y_a0)。

所述车体真实位置确认包括有：通过(x₁，y₁)、(x₂，y₂)、r₁、r₂确定两个交点O₁(x_O1，y_O1)，O₂(x_O2,y_O2)，则有：x_O1＝x₁+r₁*cos(α+θ)；y_O1＝y₁+r₁*sin(α+θ)；x_O2＝x₁+r₁*cos(α-θ)；y_O2＝y₁+r₁*sin(α-θ)；其中α由三角形余弦定理有：

得到：

反射板(x₃，y₃)到O₁(x_O1，y_O1)，O₂(x_O2,y_O2)点的距离d1,d2

若d1与r₃大小相近，且之差小于容许值ξ(0.3m≦ξ≦0.5m)，则点O₁(x_O1,y_O1)为近似值(x_a0，y_a0)，反之点O₂(x_O2,y_O2)为近似值(x_a0，y_a0)。

步骤4中，分别以这n个激光反射板在全局坐标系中坐标为圆心，以激光扫描仪检测距离为半径画圆，在理想状态下这n个圆会交于一点，这个圆交点为AGV的位置。假定AGV的坐标为(x_a,y_a)，建立这n个圆方程。

这是理想状态下的方程，由于实际测量过程中必然存在误差，所以实际中n个圆的方程是一个超定方程。在n个反射板中先选出3个反射板利用步骤三中的三边定位法得到的初步解(x_a0,y_a0)来表示(x_a,y_a)。

令

用δr来表示半径r的测量误差。代入并化简得：

其中：n反射板的个数，n≥3；(x_i,y_i)：第i个反射板的(x,y)坐标，已知；(x_a,y_a)：车体的(x,y)坐标，未知，待求；r_i：由激光扫描仪测得小车到第i个反射板的距离值；δr_i：r_i的测量误差，随机值，δr_i～δr_n，为激光扫描仪系统自带。

此时，将Δx_a，Δy_a一阶线性展开，并整理得到：

为方便计算，定义：

并用r_i近似代替(1)式左侧分母，得：

采用矩阵形式表示为：

其中：

使残差平方和最小，即

min{δ_r1 ²+δ_r2 ²+…+δ_rn ²}

用最小二乘法，求解

最终AGV的坐标为：

如图6所示，步骤5包括有：AGV在全局坐标系中方位角为α₀，由激光雷达检测第1个激光反射板得到的AGV方位角为α₁，则有：

则检测第n个反射板得到的AGV方位角为α_n，

θ_i为第i个激光反射板在局部坐标下被检测到的方位角；

x_i,y_i为第i个激光反射板在全局坐标下的坐标；

x₀,y₀为小车在全局坐标下的坐标。

可得AGV机器人相对于全局坐标系的方向角为：

应当指出，对于本领域的普通技术人员来说，在不脱离本发明创造构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

Claims (9)

1.一种AGV激光导航多边定位方法，其特征在于，包括有车体、激光雷达和至少三个反射板，包括有以下步骤：

步骤1：从所述激光雷达的数据中筛选出反射板的局部坐标：

所述反射板采用直径为D的圆柱反射板，所述激光雷达的数据中选取与所述反射板之间距离最小的(r_i，θ_i)，并使r_i＝r_i+D/2；

步骤2：利用匹配算法得到每个反射板在地图中的全局坐标：

S1、在扫描得到的多个反射板中任意选取出三个反射板成一个观测三角形abc，在地图中形成与观测三角形abc对应的地图三角形ijk；

S2、在地图中找到与边d(a,b)和d(a,c)长度最接近的边d(i,j)和d(i,k)；

S3、判断d(b,c)与d(j,k)之差的绝对值，直至差小于指定值，则a与j匹配成功，

反射板a的全局坐标即地图中反射板i的全局坐标；

S4、重复S1、S2、S3直到所有的所述反射板全局坐标匹配完成；

步骤3：利用三边定位算法确定出所述车体即时位置的一个近似值(x_a0,y_a0)：

步骤4：采用基于最小二乘法的迭代搜索多路标定位算法实现车体的精确定位：

所述激光雷达扫描到n个所述反射板，其中，n大于等于3，分别以这n个所述反射板在全局坐标系中坐标为圆心，以所述激光雷达检测距离为半径画圆，所形成n个圆会交于一点，该交点为所述车体的位置；

设所述车体的坐标为(x_a，y_a)，建立这n个圆方程，将步骤3得到的(x_a0,y_a0)来表示(x_a，y_a)，用δr表示r的测量误差，则得到

求得(xa，ya)的坐标；

步骤5：计算所述车体的方向角。

2.根据权利要求1所述的AGV激光导航多边定位方法，其特征在于，在完成所述步骤2之后，所述车体对当前所在位置会有一个估计值，根据估计值可以推算出观测到的反射板在全局坐标的位置，再与全局坐标进行比较即可完成匹配。

3.根据权利要求2所述的AGV激光导航多边定位方法，其特征在于，根据所述步骤2中的历史坐标数据预测出所述车体的即时位置(x₀，y₀)，预估出观测的反射板在全局坐标系下的位置(x_i，y_i)，预估出观测的反射板在局部坐标系下的位置(x_s，y_s)，由

x_i＝x₀+x_scosα₀-y_ssinα₀

y_i＝y₀+y_ssinα₀+y_scosα₀

α₀＝α₁-θ

在地图中找出与(x_i，y_i)最接近的点，与该反射板的全局坐标为(x_k，y_h)，判断d(x_i，y_i)与d(x_k，y_h)之差的绝对值小于ε，其中0.3m≦ε≦0.5m。

4.根据权利要求1所述的AGV激光导航多边定位方法，其特征在于，所述步骤3中所述三边定位算法为：以三个反射板的全局坐标(x₁，y₁)(x₂，y₂)(x₃，y₃)为圆心，分别以激光雷达测得所述车体到三个反射板的距离r₁,r₂,r₃为半径做圆，这三个圆将交于所述车体的真实位置附近，通过计算交点得到所述车体位置的近似值(x_a0，y_a0)。

5.根据权利要求4所述的AGV激光导航多边定位方法，其特征在于，所述车体真实位置确认包括有：通过(x₁，y₁)、(x₂，y₂)、r₁、r₂确定两个交点O₁(x_O1，y_O1)，O₂(x_O2,y_O2)，则有：x_O1＝x₁+r₁*cos(α+θ)；y_O1＝y₁+r₁*sin(α+θ)；x_O2＝x₁+r₁*cos(α-θ)；y_O2＝y₁+r₁*sin(α-θ)；其中α由三角形余弦定理有：

得到：

反射板(x₃，y₃)到O₁(x_O1，y_O1)，O₂(x_O2,y_O2)点的距离d1,d2

若d1与r₃大小相近，且之差小于容许值ξ(0.3m≦ξ≦0.5m)，则点O₁(x_O1,y_O1)为近似值(x_a0，y_a0)，反之点O₂(x_O2,y_O2)为近似值(x_a0，y_a0)。

6.根据权利要求1所述的AGV激光导航多边定位方法，其特征在于，所述步骤5中，所述车体在全局坐标系中方位角为α₀，由激光雷达检测第1个激光反射板得到的车体的方位角为α₁，则有：

则检测第n个反射板得到的方位角为α_n，

则有：

则所述车体相对于全局坐标系的方向角为：

7.根据权利要求1所述的AGV激光导航多边定位方法，其特征在于，

所述δr为随机值δr₁～δr_n，得到：

并定义：

得到：

用最小二乘法，求解

得到

8.根据权利要求7所述的AGV激光导航多边定位方法，其特征在于，

采用矩阵形式表示为：

其中：

使残差平方和最小，即

min{δ_r1 ²+δ_r2 ²+…+δ_rn ²}

用最小二乘法，求解

得到最终所述车体的坐标为：

9.根据权利要求1所述的AGV激光导航多边定位方法，其特征在于，所述步骤2中的指定值为0.3～0.5m。

Images