CN110068832A - 一种激光导航agv的高精度定位方法 - Google Patents

一种激光导航agv的高精度定位方法 Download PDF

Info

Publication number
CN110068832A
CN110068832A CN201910326851.8A CN201910326851A CN110068832A CN 110068832 A CN110068832 A CN 110068832A CN 201910326851 A CN201910326851 A CN 201910326851A CN 110068832 A CN110068832 A CN 110068832A
Authority
CN
China
Prior art keywords
agv
follows
formula
locating method
precision locating
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Withdrawn
Application number
CN201910326851.8A
Other languages
English (en)
Inventor
丁旭
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
C Lo Matt Automation Technology (suzhou) Co Ltd
Original Assignee
C Lo Matt Automation Technology (suzhou) Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by C Lo Matt Automation Technology (suzhou) Co Ltd filed Critical C Lo Matt Automation Technology (suzhou) Co Ltd
Priority to CN201910326851.8A priority Critical patent/CN110068832A/zh
Publication of CN110068832A publication Critical patent/CN110068832A/zh
Withdrawn legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Electromagnetism (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

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

Description

一种激光导航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高速运动过程中，激光雷达的精确定位。
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (7)

1.一种激光导航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)
步骤六：计算激光雷达位置坐标：
2.根据权利要求1所述的一种激光导航AGV的高精度定位方法，其特征在于：高精度定位方法基于卡尔曼滤波的融合定位方法，这其中：
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θ)。
3.根据权利要求2所述的一种激光导航AGV的高精度定位方法，其特征在于：卡尔曼滤波的融合定位方法主要包括四个步骤：
①预测过程；
②错误协方差矩阵；
③观测和匹配过程；
④评估过程。
4.根据权利要求3所述的一种激光导航AGV的高精度定位方法，其特征在于：预测过程主要是根据上一时刻的状态推算出下一时刻的状态，所用公式为：
5.根据权利要求3所述的一种激光导航AGV的高精度定位方法，其特征在于：错误协方差矩阵所用公式为：
6.根据权利要求3所述的一种激光导航AGV的高精度定位方法，其特征在于：观测和匹配过程所用公式为：
7.根据权利要求3所述的一种激光导航AGV的高精度定位方法，其特征在于：评估过程所用公式为：
CN201910326851.8A 2019-04-23 2019-04-23 一种激光导航agv的高精度定位方法 Withdrawn CN110068832A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910326851.8A CN110068832A (zh) 2019-04-23 2019-04-23 一种激光导航agv的高精度定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910326851.8A CN110068832A (zh) 2019-04-23 2019-04-23 一种激光导航agv的高精度定位方法

Publications (1)

Publication Number Publication Date
CN110068832A true CN110068832A (zh) 2019-07-30

Family

ID=67368405

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910326851.8A Withdrawn CN110068832A (zh) 2019-04-23 2019-04-23 一种激光导航agv的高精度定位方法

Country Status (1)

Country Link
CN (1) CN110068832A (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110471031A (zh) * 2019-08-28 2019-11-19 佛山市兴颂机器人科技有限公司 一种基于反光板的激光位置定位方法及系统
CN110530399A (zh) * 2019-09-06 2019-12-03 苏州寻迹智行机器人技术有限公司 双轮差速移动机器人里程计标定的轮间距修正方法
CN111381244A (zh) * 2020-03-05 2020-07-07 三一机器人科技有限公司 车辆的定位增强方法、装置、车辆和可读存储介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104102222A (zh) * 2014-07-31 2014-10-15 广州大学 一种agv精确定位的方法
KR20150020900A (ko) * 2013-08-19 2015-02-27 부경대학교 산학협력단 레이저 스캐너를 이용한 위치인식 및 맵핑 시스템을 이용한 위치인식 방법
CN106020202A (zh) * 2016-07-15 2016-10-12 东南大学 一种基于卡尔曼滤波的模糊pid控制方法
CN106969768A (zh) * 2017-04-22 2017-07-21 深圳力子机器人有限公司 一种无轨导航agv的精确定位及停车方法
CN107192387A (zh) * 2017-05-23 2017-09-22 北京理工大学 一种基于无迹卡尔曼滤波的组合定位方法
CN107239076A (zh) * 2017-06-28 2017-10-10 仲训昱 基于虚拟扫描与测距匹配的agv激光slam方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20150020900A (ko) * 2013-08-19 2015-02-27 부경대학교 산학협력단 레이저 스캐너를 이용한 위치인식 및 맵핑 시스템을 이용한 위치인식 방법
CN104102222A (zh) * 2014-07-31 2014-10-15 广州大学 一种agv精确定位的方法
CN106020202A (zh) * 2016-07-15 2016-10-12 东南大学 一种基于卡尔曼滤波的模糊pid控制方法
CN106969768A (zh) * 2017-04-22 2017-07-21 深圳力子机器人有限公司 一种无轨导航agv的精确定位及停车方法
CN107192387A (zh) * 2017-05-23 2017-09-22 北京理工大学 一种基于无迹卡尔曼滤波的组合定位方法
CN107239076A (zh) * 2017-06-28 2017-10-10 仲训昱 基于虚拟扫描与测距匹配的agv激光slam方法

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110471031A (zh) * 2019-08-28 2019-11-19 佛山市兴颂机器人科技有限公司 一种基于反光板的激光位置定位方法及系统
CN110530399A (zh) * 2019-09-06 2019-12-03 苏州寻迹智行机器人技术有限公司 双轮差速移动机器人里程计标定的轮间距修正方法
CN111381244A (zh) * 2020-03-05 2020-07-07 三一机器人科技有限公司 车辆的定位增强方法、装置、车辆和可读存储介质

Similar Documents

Publication Publication Date Title
CN106772435B (zh) 一种无人机避障方法和装置
CN106595631B (zh) 一种躲避障碍物的方法及电子设备
CN110068832A (zh) 一种激光导航agv的高精度定位方法
CN107239076B (zh) 基于虚拟扫描与测距匹配的agv激光slam方法
CN112083725B (zh) 一种面向自动驾驶车辆的结构共用多传感器融合定位系统
CN108088456A (zh) 一种具有时间一致性的无人驾驶车辆局部路径规划方法
CN109143207A (zh) 激光雷达内参精度验证方法、装置、设备及介质
CN111324848B (zh) 移动激光雷达测量系统车载轨迹数据优化方法
CN103149937A (zh) 一种基于曲率补偿的横侧向曲线航迹跟踪方法
CN112154303B (zh) 高精度地图定位方法、系统、平台及计算机可读存储介质
CN106444772B (zh) 自动引导车轮系舵角自动调整方法、装置和自动引导车
CN107643533A (zh) 一种无人机定位方法、装置、系统及无人机
CN108734780A (zh) 用于生成地图的方法、装置和设备
CN110068334A (zh) 一种磁导航agv的高精度定位方法
CN111693059B (zh) 环岛的导航方法、装置、设备及存储介质
CN112034479A (zh) 一种应用于煤矿井下智能巡检无人机的定位方法及系统
CN112558072B (zh) 车辆定位方法、装置、系统、电子设备及存储介质
CN114572240A (zh) 车辆行驶控制方法、装置、车辆、电子设备及存储介质
CN107289945A (zh) 一种无人机导航方法
CN110806585A (zh) 一种基于树干聚类跟踪的机器人定位方法及系统
CN112365743A (zh) 航空器航迹定位数据偏移的修正方法及装置
CN106292685A (zh) 模型飞机及其飞行控制方法和系统
CN103853893B (zh) 一种从点云数据检测飞机姿态的参数模型匹配方法
CN115388892A (zh) 一种基于改进rbpf-slam算法的多传感器融合slam方法
CN117193377A (zh) 确保收敛的无人机飞行时间最优实时轨迹优化方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
WW01 Invention patent application withdrawn after publication
WW01 Invention patent application withdrawn after publication

Application publication date: 20190730