CN111044073B

CN111044073B - 基于双目激光高精度agv位置感知方法

Info

Publication number: CN111044073B
Application number: CN201911175903.2A
Authority: CN
Inventors: 张加波; 韩建超; 王颜; 刘净瑜; 张仰成; 张俊辉; 吕晶薇
Original assignee: Beijing Satellite Manufacturing Factory Co Ltd
Current assignee: Beijing Satellite Manufacturing Factory Co Ltd
Priority date: 2019-11-26
Filing date: 2019-11-26
Publication date: 2022-07-05
Anticipated expiration: 2039-11-26
Also published as: CN111044073A

Abstract

本发明基于双目激光高精度AGV位置感知方法，具体步骤为：1)安装一个激光导航传感器；2)安装反光板；3)计算实时激光导航传感器A的在运动区域内的坐标和位姿角，并获得当前激光导航传感器A位置的坐标值；4)得到激光导航传感器B坐标和位姿角；5)获得滤波后的激光导航传感器A和激光导航传感器B的当前坐标和位姿角；6)得出车体中心当前坐标位置和姿态角；7)得出偏移距离Δl以及偏差角度Δθ；8)实时解算AGV的当前位姿与目标路径的偏差，根据偏移角Δθ和偏移距离Δl实时调整车体行驶的角速度ω和偏航角

完成AGV的纠偏。

Description

基于双目激光高精度AGV位置感知方法

技术领域

本发明涉及一种基于双目激光高精度AGV位置感知方法，属于工业控制领域。

背景技术

AGV导航方式依据导引线的形式可以分为有线式和无线式。根据传感器原理进行划分，主要有：电磁导航、磁带导航、激光导航、惯性导航、视觉导航等。

电磁导航是最早应用于AGV的导航方式。该方式原理简单，技术成熟，直到目前依然有很多AGV厂家采用这种导航方式。电磁导航需要在AGV行走的路线下面埋设专门的电缆线，对电缆线通以一定频率的交流电，由AGV上的电磁感应传感器检测电磁信号实现导航。该方法可靠性高，经济实用，主要缺点在于AGV路径改变相对困难，地面施工成本较高，大大降低了系统的柔性。

磁带导航与电磁导航相近，区别是铺设在地面上的磁带替代埋设在地面一下的电缆线，通过磁传感器检测磁信号实现导航，此导航灵活性比电磁导航好，改变或扩充路径较容易，磁带铺设简单易行，但磁信号易受环境周围金属物质干扰，还因磁带外露，容易受到磨损、机械损伤和污染，导航的稳定性受环境影响较大。

惯性导航是在AGV上安装陀螺仪，在行驶地面上安装定位块，AGV可通过对陀螺仪偏差信号的计算及地面定位块信号的采集来确定自身的位置和方位。惯性导航AGV通过对已知地面地图路线进行比较来控制AGV的运动方向，实现自主导航，惯性导航方式定位准确，灵活性强，便于组合和兼容，适用领域广。缺点是陀螺仪对震动较敏感，地面条件对AGV的可靠性影响很大，后期维护成本较高，且需要校正定位。

视觉导航是对AGV行驶区域的环境进行图象处理和智能学习，实现自动导航行驶，视觉导航主要特点是精度较高，但对复杂环境的识别能力和学习能力有待提高，而且路径单一。

激光导航地面无需其它辅助定位设施；行驶路径可灵活多变，能够适合多种现场环境。缺点车型构造首先要满足激光传感器的视场范围，单一激光导航传感器无法满足多种车型导航和高精度定位的需求。

发明内容

本发明解决的技术问题是：克服现有技术的不足，提供一种基于双目激光高精度AGV位置感知方法，解决现有技术中大尺寸背覆式AGV高精度导航和定位的问题。

本发明的技术方案是：基于双目激光高精度AGV位置感知方法，步骤如下：

1)在自动导引运输车AGV车体中轴线前后两端各安装一个激光导航传感器；

2)在AGV行驶路径和激光扫描范围内安装反光板，确保任意一个激光导航传感器在任何时刻均扫到不低于3个反光板；

3)对激光导航传感器A进行标定，得到地图信息，设定初始原点和0度角；激光传感器的激光束照射在反光板上，根据反光板的距离得出实时激光导航传感器A的在运动区域内的坐标(x₁′,y₁′)和位姿角θ₁`，导航控制器通过串口实时采集传感器当前位姿信息，坐标均在第一象限，获得当前激光导航传感器A位置的坐标值；

4)将步骤3)得到的地图信息复制到激光导航传感器B中，使得激光导航传感器A和激光导航传感器B位于同一坐标系，得出激光导航传感器B坐标(x₂,y₂)和位姿角θ₂`；

5)通过滤波算法对收到的激光导航传感器A和激光导航传感器B坐标和位姿角进行滤波，获得滤波后的激光导航传感器A和激光导航传感器B的当前坐标(x₁,y₁)、(x₂,y₂)和位姿角θ₁、θ₂；

6)根据步骤5)获得的激光器的当前坐标(x₁,y₁)、(x₂,y₂)和位姿角θ₁、θ₂，通过转换公式得出车体中心当前坐标位置(x′,y′)和姿态角θ′；

7)当AGV按规定目标路径直线运行时，将步骤6)获得的AGV的当前位姿与目标路径比较对比，得出车体中心当前坐标(x′,y′)与目标终点坐标(x_f,y_f)的偏移距离Δl，以及车体中心的位姿角θ′与目标路径倾斜角θ的偏差角度Δθ。

8)AGV巡线运行过程中，实时解算AGV的当前位姿与目标路径的偏差，根据偏移角Δθ和偏移距离Δl实时调整车体行驶的角速度ω和偏航角

完成AGV的纠偏。

所述步骤1)中所述激光导航传感器安装位置位于车体前后轴线上且与车头方向平行，激光导航传感器A位于车头并与车头前进方向一致，激光导航传感器B位于车尾与车头前进方向相反，同时测量两个激光传感器中心的距离d。

所述步骤5)的具体步骤如下：

51)采集实时坐标值(x_temp,y_temp)和姿态角θ_temp加入数组中；

52)每次采集前坐标角度矩阵的值表示为

采集后坐标角度矩阵的值表示为

53)当前激光传感器滤波后坐标值为

54)当前激光传感器滤波后初始姿态角度值θ1：

A：当θ[0]≥355°且θ[1]≤5°且θ[2]≤5°

B：当θ[0]≤5°且θ[1]≥355°且θ[2]≥355°

C：当θ[0]≤5°且θ[1]≥355°且θ[2]≤5°

D：当θ[0]≥355°且θ[1]≤5°且θ[2]≥355°

E：当θ[0]≤5°且θ[1]≤5°且θ[2]≥355°

F：当θ[0]≥355°且θ[1]≥355°且θ[2]≤5°

G：当θ[0]≥355°且θ[1]≥355°且θ[2]≥355°

H：当θ[0]≤5°且θ[1]≤5°且θ[2]≤5°

将角度值转换[0，360°)之间，得出滤波后姿态角度值θ：表示的

所述步骤6)的具体过程如下：

a.当有且只有激光导航传感器A采集到有效坐标值，车体中心的位姿信息为：

b.当有且只有激光导航传感器B采集到有效坐标值，车体中心的位姿信息为：

姿态角θ′：

坐标值：

c.当激光导航传感器A和激光导航传感器B均采集到有效坐标值，车体中心的位姿信息为：

车体中心的位姿角θ′：

转换激光导航传感器B的姿态角度值θ′₂：

根据激光导航传感器A的θ₁和转换后激光导航传感器B的θ′₂，得出车体中心的位姿角θ″

将位姿角转换到[0，360°)之间，得出车体中心的位姿角θ′

所述步骤7)的具体过程为：

设行驶路径的直线方程y＝k(x-x_f)+y_f,其中

车体中心当前坐标(x′,y′)到目标终点坐标(x_f,y_f)的直线角度tanβ＝k，得出

当x′＝x_f且y′＜y_f,β＝90°；

当x′＝x_f且y′＞y_f,β＝270°；

当x′＜x_f且y′＝y_f,β＝0°；

当x′＞x_f且y′＝y_f,β＝180°；

当x′≠x_f且y′≠y_f,

得到车体与目标路径的偏移角Δθ＝θ-θ′＝β-θ′；

偏移距离

所述步骤8)的具体步骤如下：

AGV自动导航过程中，重复进行检测当前位姿、实时解算AGV的当前位姿，位姿符合性判断和姿态调整的过程，直至Δl≤2mm且Δθ≤0.1°，导航完成；将AGV的运动方向和AGV车头方向之间的夹角记为AGV的偏航角度，偏航度

取值范围为[0°,360°)，AGV向前方即AGV车头方向运动时，偏航角为0°；AGV向前方即AGV车头方向运动时，偏航角为0°；计算AGV偏航角

的步骤如下：

C.当Δθ≥0°

iii.若y′＜y_f，则AGV的偏航角为

iv.若y′≥y_f，则AGV的偏航角为

D.当Δθ＜0°

iii.若x′＜x_f，则AGV的偏航角为

iv.若x′≥x_f，则AGV的偏航角为

AGV旋转角速度ω的取值范围[-45,45]，

其中

AGV旋转方向的步骤如下：

iii.若Δθ＞0，则AGV应顺时针旋转，即ω＞0；

iv.若Δθ＜0，则AGV应逆时针旋转，即ω＜0；

若Δθ＝0,则AGV无需旋转。

本发明与现有技术相比的优点在于：

1)现有技术通常采用单一激光传感器导航方式，本发明中采用双激光传感器联合计算车体坐标，使得车体中心的坐标值计算结果更加准确，解决了单一激光传感器坐标不稳定，单一传感器遮挡导致坐标值无效的问题。

2)现有技术通常采用差速轮与单一激光导航方式相结合的运动方式，本发明将麦克纳姆轮AGV的全向运动和双激光传感器连续定位相结合，使AGV的定位精度优于±2mm，解决了差速轮定位精度低、灵活性差以及无法满足大尺寸AGV精确定位和全向移动的问题；

3)现有技术通常采用单一激光传感器导航方式，本发明中采用双激光传感器，通过在需要AGV运行的地方安装反光板，可实现全范围任意点的定位，相对于单一传感器可以减少反光板的布置数量，提高AGV运输路径布置和更改的灵活性。

附图说明

图1本发明实施例所述的反光板和激光传感器的关系示意图；

图2本发明实施例所述的传感器坐标和AGV车体中心坐标的关系示意图；

图3本发明实施例所述的车体中心坐标和姿态角与目标坐标的偏差关系示意图；3a)为目标坐标在车体坐标系第二象限示意图，3b)为目标坐标在车体坐标系第四象限示意图，3c)为目标坐标在车体坐标系第三象限示意图，3d)目标坐标在车体坐标系第一象限示意图。

图4本发明实施所述的AGV导航定位方法流程图。

具体实施方式

下面结合附图和具体实施例对本发明做进一步详细描述：

如图4所示，本发明方法具体如下：

1)在自动导引运输车AGV(Automated Guided Vehicle)前后端顶部车体中轴线处各安装一个激光传感器，如图1在AGV行驶路径和激光扫描范围内安装反光板，AGV在行驶过程中保证每次都能扫描到不低于3个反光板。安装完成后，进行标定，设置初始坐标0°角度方位。

2)当AGV在运行过程中，导航控制器通过串口/网口实时采集传感器当前位姿信息。如图2所示为传感器坐标和AGV车体中心坐标的关系示意图，传感器的中心和车体中心在同一平面，且处于中轴线上。

3)控制器实时接收当前激光传感器的坐标值和位姿角，根据接收到的传感器的数据进行滤波，获得滤波后的激光导航传感器A和激光导航传感器B的当前坐标(x₁,y₁)、(x₂,y₂)和位姿角θ₁、θ₂，滤波方式如下：

i.采集实时坐标值(x_temp,y_temp)和姿态角θ_temp加入数组中；

ii.每次采集前坐标角度矩阵的值为

采集后坐标角度矩阵的值为

iii.当前激光传感器(激光导航传感器A或激光导航传感器B)滤波后坐标值；

iv.当前激光传感器(激光导航传感器A或激光导航传感器B)滤波后初始姿态角度值θ1：

A.当θ[0]≥355°且θ[1]≤5°且θ[2]≤5°

B.当θ[0]≤5°且θ[1]≥355°且θ[2]≥355°

C.当θ[0]≤5°且θ[1]≥355°且θ[2]≤5°

D.当θ[0]≥355°且θ[1]≤5°且θ[2]≥355°

E.当θ[0]≤5°且θ[1]≤5°且θ[2]≥355°

F.当θ[0]≥355°且θ[1]≥355°且θ[2]≤5°

G.当θ[0]≥355°且θ[1]≥355°且θ[2]≥355°

H.当θ[0]≤5°且θ[1]≤5°且θ[2]≤5°

将角度值转换[0，360°)之间，得出滤波后姿态角度值θ：

4)激光传感器A根据滤波后坐标(x₁,y₁)和位姿角θ₁及激光传感器B滤波后坐标(x₂,y₂)和姿态角θ₂与车体中心当前位姿(x′,y′)和θ′的转换关系：

i.当有且只有激光器A采集到有效坐标值，车体中心的位姿信息为：

ii.当有且只有激光器B采集到有效坐标值，车体中心的位姿信息为：

姿态角θ′：

坐标值：

iii.当激光器A和激光器B均采集到有效坐标值，车体中心的位姿信息为：

车体中心的位姿角θ′

A.转换传感器B的姿态角度值θ′₂：

B.根据传感器Aθ₁和转换后传感器Bθ′₂，得出车体中心的位姿角θ′′

C.将位姿角转换到[0，360)之间，得出车体中心的位姿角θ′

5)采用激光导航方式，使AGV的车体中心朝着终点坐标和目标角度行走。如图3a)-d)所示)已知目标路径起始坐标为车体中心当前实时坐标(x′,y′)和终点坐标(x_f,y_f),由起始和终止坐标可得，行驶路径的直线方程y＝k(x-x_f)+y_f,其中

车体中心(x′,y′)到目标坐标的直线角度tanβ＝k，得出

i.当x′＝x_f且y′＜y_f,β＝90°；

ii.当x′＝x_f且y′＞y_f,β＝270°；

iii.当x′＜x_f且y′＝y_f,β＝0°；

iv.当x′＞x_f且y′＝y_f,β＝180°；

v.当x′≠x_f且y′≠y_f,

因此，当车体与目标路径的偏移角Δθ′＝θ-θ′＝β-θ′，偏移距离

6)将AGV的运动方向和AGV车头方向之间的夹角记为AGV的偏航角度，偏航角度

取值范围为[0°,360°)，AGV向前方即AGV车头方向运动时，偏航角度为0°。AGV向前方即AGV车头方向运动时，偏航角度为0°。

计算AGV偏航角度

的步骤如下：

A.当θ′≥0°

v.若y′＜y_f，则AGV的偏航角度为

vi.若y′≥y_f，则AGV的偏航角度为

B.当θ′＜0°

v.若x′＜x_f，则AGV的偏航角度为

vi.若x′≥x_f，则AGV的偏航角度为

AGV旋转角速度ω的取值范围[-45,45]，

ω的正负与旋转方向有关，大小与Δθ′、当前速度v、和比例系数k有关，即

计算AGV旋转方向的步骤如下：

v.若Δθ′＞0，则AGV应顺时针旋转；

vi.若Δθ′＜0，则AGV应逆时针旋转；

vii.若Δθ′＝0,则AGV无需旋转。

对AGV进行导航的过程为确定AGV偏航角度和AGV旋转方向的复合运动过程。

7)AGV自动导航过程中，重复进行检测当前位姿、实时解算AGV的当前位姿，位姿符合性判断和姿态调整的过程，直至当前位置(x′,y′)与目标位置(x_f,y_f)差距Δl≤2mm且Δθ≤0.1°之内，导航完成。

本发明未详细说明部分属本领域技术人员公知常识。

Claims

1.基于双目激光高精度AGV位置感知方法，其特征在于步骤如下：

4)将步骤3)得到的地图信息复制到激光导航传感器B中，使得激光导航传感器A和激光导航传感器B位于同一坐标系，得出激光导航传感器B坐标(x₂′,y₂′)和位姿角θ₂`；

6)根据步骤5)获得的激光器的当前坐标(x₁,y₁)、(x₂,y₂)和位姿角θ₁、θ₂，通过转换公式得出车体中心当前坐标位置(x′,y′)和位姿角θ′；

7)当AGV按规定目标路径直线运行时，将步骤6)获得的AGV的当前位姿与目标路径比较对比，得出车体中心当前坐标(x′,y′)与目标终点坐标(x_f,y_f)的偏移距离Δl，以及车体中心的位姿角θ′与目标路径倾斜角δ的偏移角Δθ；

完成AGV的纠偏；

所述步骤5)的具体步骤如下：

51)采集实时坐标值(x_temp,y_temp)和位姿角θ_temp加入数组中；

52)每次采集前坐标角度矩阵的值表示为

采集后坐标角度矩阵的值表示为

53)当前激光传感器滤波后坐标值为

54)当前激光传感器滤波后初始位姿角θ1：

A：当θ[0]≥355°且θ[1]≤5°且θ[2]≤5°

B：当θ[0]≤5°且θ[1]≥355°且θ[2]≥355°

C：当θ[0]≤5°且θ[1]≥355°且θ[2]≤5°

D：当θ[0]≥355°且θ[1]≤5°且θ[2]≥355°

E：当θ[0]≤5°且θ[1]≤5°且θ[2]≥355°

F：当θ[0]≥355°且θ[1]≥355°且θ[2]≤5°

G：当θ[0]≥355°且θ[1]≥355°且θ[2]≥355°

H：当θ[0]≤5°且θ[1]≤5°且θ[2]≤5°

将角度值转换[0，360°)之间，得出滤波后前激光位姿角θ：

所述步骤6)的具体过程如下：

位姿角θ′：

坐标值：

车体中心的位姿角θ′：

转换激光导航传感器B的位姿角θ′₂：

将位姿角转换到[0，360°)之间，得出车体中心的位姿角θ′

所述步骤7)的具体过程为：

设行驶路径的直线方程y＝k(x-x_f)+y_f,其中

当x′＝x_f且y′<y_f,β＝90°；

当x′＝x_f且y′>y_f,β＝270°；

当x′<x_f且y′＝y_f,β＝0°；

当x′>x_f且y′＝y_f,β＝180°；

当x′≠x_f且y′≠y_f,

得到车体与目标路径的偏移角Δθ＝δ-θ′＝β-θ′；

偏移距离

所述步骤8)的具体步骤如下：

AGV自动导航过程中，重复进行检测当前位姿、实时解算AGV的当前位姿，位姿符合性判断和姿态调整的过程，直至Δl≤2mm且Δθ≤0.1°，导航完成；将AGV的运动方向和AGV车头方向之间的夹角记为AGV的偏航角，偏航角

的步骤如下：

A.当Δθ≥0°

i.若y′<y_f，则AGV的偏航角为

ii.若y′≥y_f，则AGV的偏航角为

B.当Δθ<0°

i.若x′<x_f，则AGV的偏航角为

ii.若x′≥x_f，则AGV的偏航角为

AGV旋转角速度ω的取值范围[-45°,45°]，

其中

AGV旋转方向的步骤如下：

i.若Δθ>0，则AGV应顺时针旋转，即ω>0；

ii.若Δθ<0，则AGV应逆时针旋转，即ω<0；

若Δθ＝0,则AGV无需旋转。

2.根据权利要求1所述的基于双目激光高精度AGV位置感知方法，其特征在于：所述步骤1)中所述激光导航传感器安装位置位于车体前后轴线上且与车头方向平行，激光导航传感器A位于车头并与车头前进方向一致，激光导航传感器B位于车尾与车头前进方向相反，同时测量两个激光传感器中心的距离d。