一种激光导航AGV的高精度定位方法与流程

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

背景技术：

激光雷达技术广泛应用在agv、无人驾驶等领域，基于激光雷达技术的agv凭借其较高的稳定性，较高的定位精度，以及对场景依赖性小的特性，广泛应用在货物运输、快递运输等领域。

激光雷达主要应用于agv的自身定位，目前主流的定位方式是基于反光板的三角定位算法。但这种算法有着天然的局限性，要求反光板有着较严格的布局以及有些位置无法定位，其次，当agv高速运行过程中激光雷达定位具有延迟性质，从而导致agv定位的不精确。

技术实现要素：

本发明解决的技术问题在于克服现有技术的agv定位不精确的缺陷，提供一种激光导航agv的高精度定位方法。所述一种激光导航agv的高精度定位方法具有精度、稳定性和效率高，并且适用性广等特点。

为实现上述目的，本发明提供如下技术方案：一种激光导航agv的高精度定位方法，包括以下步骤：

步骤一：计算虚拟反光板坐标,假设三个反光板的绝对坐标为(x1，y1),(x2，y2),(x3，y3)，反光板扫描到的角度分别为则：

x'1＝x1-x2

y'1＝y1-y2

y'3＝y3-y2

x'3＝x3-x2

步骤二：计算三个cot()值：

步骤三：计算虚拟圆心坐标：

x'12＝x'1+t12y'1

y'12＝y'1-t12x'1

y'23＝y'3+t23x'3

x'23＝x'3-t23y'3

x'31＝(x'3+x'1)+t31(y'3-y'1)

y'31＝(y'3+y'1)-t31(x'3-x'1)

步骤四：计算虚拟过度值：

k'31＝x'1x'3+y'1y'3+t31(x'1y'3-x'3y'1)

步骤五：计算虚拟d值：

d＝(x'12-x'23)(y'23-y'31)-(y'12-y'23)(x'23-x'31)步骤六：计算激光雷达位置坐标：

优选的，高精度定位方法基于卡尔曼滤波的融合定位方法，这其中：

agv小车运动学公式如下所示：

根据运动学公式，可以推导出agv叉车旋转角度方程如下所示：

通过迭代的方式可以根据k-1时刻的运动学参数，推导k时刻的转角，公式如下：

卡尔曼滤波有两个过程，分别是预测过程和更新过程，根据k-1时刻的状态量xk-1，控制输入量uk-1，噪声干扰量wk-1，来计算出k时刻的状态量，公式如下：

xk＝f(xk-1,uk-1,wk-1)

对于agv，控制输入量为v，γ：

激光雷达获得的反光板坐标为zk，其中vθ为激光雷达的测量噪声：

zk＝h(xk,vθ)。

优选的，卡尔曼滤波的融合定位方法主要包括四个步骤：

①预测过程；

②错误协方差矩阵；

③观测和匹配过程；

④评估过程。

优选的，预测过程主要是根据上一时刻的状态推算出下一时刻的状态，所用公式为：

优选的，错误协方差矩阵所用公式为：

优选的，观测和匹配过程所用公式为：

优选的，评估过程所用公式为：

与现有技术相比，本发明的有益效果是：

本发明提出了一种高效，稳定的定位算法，极大的提高了agv小车定位的稳定性，并且相比于其他三角定位算法，有着更高的适应性，对反光板的布局依赖性较小，并提出了一种基于卡尔曼滤波的算法来提高定位的稳定性，解决agv小车在运动过程中定位的延时性，这极大地提高了agv的运行速度，提高了agv的工作效率。

附图说明

图1为本发明的流程图；

图2为激光雷达三角定位的原理图；

图3改进的定位方法图；

图4agv小车的运动学模型图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图1-4，本发明提供一种技术方案：一种激光导航agv的高精度定位方法，包括以下步骤：

步骤一：计算虚拟反光板坐标,假设三个反光板的绝对坐标为(x1，y1),(x2，y2),(x3，y3)，反光板扫描到的角度分别为则：

x'1＝x1-x2

y'1＝y1-y2

y'3＝y3-y2

x'3＝x3-x2

步骤二：计算三个cot()值：

步骤三：计算虚拟圆心坐标：

x'12＝x'1+t12y'1

y'12＝y'1-t12x'1

y'23＝y'3+t23x'3

x'23＝x'3-t23y'3

x'31＝(x'3+x'1)+t31(y'3-y'1)

y'31＝(y'3+y'1)-t31(x'3-x'1)

步骤四：计算虚拟过度值：

k'31＝x'1x'3+y'1y'3+t31(x'1y'3-x'3y'1)

步骤五：计算虚拟d值：

d＝(x'12-x'23)(y'23-y'31)-(y'12-y'23)(x'23-x'31)

步骤六：计算激光雷达位置坐标：

高精度定位方法基于卡尔曼滤波的融合定位方法，这其中：

agv小车运动学公式如下所示：

根据运动学公式，可以推导出agv叉车旋转角度方程如下所示：

通过迭代的方式可以根据k-1时刻的运动学参数，推导k时刻的转角，公式如下：

卡尔曼滤波有两个过程，分别是预测过程和更新过程，根据k-1时刻的状态量xk-1，控制输入量uk-1，噪声干扰量wk-1，来计算出k时刻的状态量，公式如下：

xk＝f(xk-1,uk-1,wk-1)

对于agv，控制输入量为v，γ：

激光雷达获得的反光板坐标为zk，其中vθ为激光雷达的测量噪声：

zk＝h(xk,vθ)。

卡尔曼滤波的融合定位方法主要包括四个步骤：

①预测过程：主要是根据上一时刻的状态推算出下一时刻的状态，所用公式为：

②错误协方差矩阵：所用公式为：

③观测和匹配过程：所用公式为：

④评估过程：所用公式为：

工作原理：

激光雷达三角定位原理如图2所示，c代表激光雷达，a1,a2,a3代表三个反光板，代表激光雷达扫到反光板的角度，ρ1，ρ2，ρ3为三个反光板到激光雷达的距离，通过这些测得的参数以及a1,a2,a3在坐标系中的绝对坐标值即可计算激光雷达当前时刻的位姿，从而计算agv小车的位姿。

传统的三角定位有一定的局限性，例如，某些特殊位置无法进行定位，激光雷达获得的反光板距离会产生波动，导致定位的稳定性较差，故提出一种全新的定位方式，定位方法如图3所示。

对a1ca2圆弧分析有如下公式：

c，a1，a2分别代表(x+iy),(x1+iy1),(x2+iy2),则推导出：

通过以上公式得到圆的方程：

其中，

综上，每个圆周的计算方式可用下面公式总结：

相比于传统的三角定位算法，新的定位算法步骤如下所示：

1.计算虚拟反光板坐标,假设三个反光板的绝对坐标为(x1，y1),(x2，y2),(x3，y3),反光板扫描到的角度分别为则：

x'1＝x1-x2

y'1＝y1-y2

y'3＝y3-y2

x'3＝x3-x2

2.计算三个cot()值

3.计算虚拟圆心坐标

x'12＝x'1+t12y'1

y'12＝y'1-t12x'1

y'23＝y'3+t23x'3

x'23＝x'3-t23y'3

x'31＝(x'3+x'1)+t31(y'3-y'1)

y'31＝(y'3+y'1)-t31(x'3-x'1)

4.计算虚拟过度值

k'31＝x'1x'3+y'1y'3+t31(x'1y'3-x'3y'1)

5.计算虚拟d值

d＝(x'12-x'23)(y'23-y'31)-(y'12-y'23)(x'23-x'31)

6.计算激光雷达位置坐标。

通过上述的改进三角定位算法，可以极大提高定位的稳定性，以及定位的局限性，这种定位算法对反光板布局的要求性较低，并且，避免了激光雷达波动导致的定位的不稳定。

传统的以及改良的定位方法一般适用于静止或者低速运动的agv小车，当agv小车在高速运动过程中，激光雷达发出的射线需要时间返回，这会导致定位的延时性，故提出了一种全新的方法，提高激光叉车的速度且有效的降低了agv小车定位的延时性。

agv小车的运动学模型如图4所示，agv小车运动学公式如下所示：

根据运动学公式，可以推导出agv叉车旋转角度方程如下所示：

通过迭代的方式可以根据k-1时刻的运动学参数，推导k时刻的转角，公式如下：

卡尔曼滤波有两个过程，分别是预测过程和更新过程，根据k-1时刻的状态量xk-1，控制输入量uk-1，噪声干扰量wk-1，来计算出k时刻的状态量，公式如下：

xk＝f(xk-1,uk-1,wk-1)

对于agv，控制输入量为v，γ，

激光雷达获得的反光板坐标为zk，其中vθ为激光雷达的测量噪声，

zk＝h(xk,vθ)

①预测过程

根据上一时刻的状态推算出下一时刻的状态

②错误协方差矩阵

③观测和匹配过程

④评估过程

通过以上扩展卡尔曼滤波，可以有效的实现agv高速运动过程中，激光雷达的精确定位。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。