CN107390227B - 一种基于数据筛选的双反光板激光定位与导航方法 - Google Patents

一种基于数据筛选的双反光板激光定位与导航方法 Download PDF

Info

Publication number
CN107390227B
CN107390227B CN201710572152.2A CN201710572152A CN107390227B CN 107390227 B CN107390227 B CN 107390227B CN 201710572152 A CN201710572152 A CN 201710572152A CN 107390227 B CN107390227 B CN 107390227B
Authority
CN
China
Prior art keywords
reflector
laser sensor
laser
angle
list
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.)
Active
Application number
CN201710572152.2A
Other languages
English (en)
Other versions
CN107390227A (zh
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.)
Zhejiang Titanium Robot Ltd By Share Ltd
Original Assignee
Zhejiang Titanium Robot Ltd By Share 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 Zhejiang Titanium Robot Ltd By Share Ltd filed Critical Zhejiang Titanium Robot Ltd By Share Ltd
Priority to CN201710572152.2A priority Critical patent/CN107390227B/zh
Publication of CN107390227A publication Critical patent/CN107390227A/zh
Application granted granted Critical
Publication of CN107390227B publication Critical patent/CN107390227B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/42—Simultaneous measurement of distance and other co-ordinates
    • 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
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/16—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using electromagnetic waves other than radio waves

Abstract

本发明公开了一种基于数据筛选的双反光板激光定位与导航方法，与传统技术中的几何三角定位不同，本发明深入挖掘并充分利用了反射光的复数域信息，打破了传统意义上三块反光板的技术瓶颈，其运行条件为同时检测到两块反光板的数据即可，大大提高了激光导航技术的适用性；虽然本发明最低要求为检测到两块反光板，但实际应用时可能会同时检测到多个反光板，通过设计数据融合处理，能够最大化利用反光板的信息，从而进一步提高精度。

Description

一种基于数据筛选的双反光板激光定位与导航方法
技术领域
本发明属于智能仓储物流领域，具体涉及一种基于数据筛选的双反光板激光定位与导航方法。
背景技术
基于激光传感器的定位与导航技术是工业AGV、智能机器人等领域的关键性技术，相比传统有轨导航方式，该技术具有定位精度高、灵活多变等优点，适用于复杂、高动态的工业场景中。国内外现有基于反光板的激光定位技术，均采用了几何三角定位原理，每时每刻必须要求同时检测到三块反光板才可实现定位。这一数量要求实际上在复杂工业场景中很难满足，极大地制约了激光定位与导航技术的实用性。
发明内容
为了解决上述问题，本发明提供一种新型激光定位与导航方法，通过提取和利用激光测距信息中的复数域信息，将三块反光板的最低要求降低为两块，有效解决了反光板最低数量的技术瓶颈，大大提高了激光导航技术的适用性。
本发明的目的是通过以下技术方案来实现的：一种基于数据筛选的双反光板激光定位与导航方法，该方法包括：
(1)在工业环境中布置反光板，预设反光板世界坐标，生成反光板坐标列表；
(2)安装在移动平台上的激光传感器向周围呈辐射状发射激光，并接收反射激光；
(3)筛选来源于反光板的有效光束：预设强度阈值σ，通过检测反射激光强度I，并与σf(d)相比较，判定激光照射物是反光板还是普通环境物体，其中f(d)与距离d负相关；
(4)确定当前时刻照射到的反光板数量及其相对激光传感器的相对坐标：根据反射光束角度的连续性，判断是否为同一块反光板，或者，根据反射光束角度和距离的连续性，判断是否为同一块反光板；根据属于同一反光板的反射光束得到该反光板相对于激光传感器的相对坐标，并存入反光板列表；
(5)初始化反光板列表，得到至少两个反光板的世界坐标：人工确定初始位置对应的反光板列表中至少两个反光板的世界坐标；或者，激光传感器在初始位置获取至少三个反光板返回的角度和距离，计算两两反光板之间的距离，与根据反光板坐标列表生成的反光板距离信息相匹配，得到至少两个反光板的世界坐标；
(6)在动态过程中计算期望反光板列表：根据上一时刻对当前时刻激光传感器位置和角度的预测，估计激光传感器与所有反光板之间的相对距离和角度，并存入期望反光板列表；
(7)在动态过程中反光板列表的匹配：计算当前时刻反光板列表与期望反光板列表中同一个反光板对应的距离之差和角度之差，当距离之差和角度之差均满足预设阈值时，匹配成功；
(8)基于双反光板数据的激光传感器位姿计算：利用激光传感器测量数据的复频域信息，在匹配成功的反光板中任选两块：第l个和第k个，并计算：
zk＝Xk+i*Yk
zl＝Xl+i*Yl
其中，下标l和k分别代表第l和第k个反光板；α和ρ分别代表在激光传感器相对自身的极坐标系下，反光板的角度和距离；X和Y分别为反光板在X和Y轴的分量；z为反光板在世界坐标系下的复数坐标；zk,l为根据第l和第k个反光板计算得到的激光传感器世界坐标，θk是根据第k块反光板的数据计算得到的激光传感器在世界坐标系下的角度；
进一步地，所述步骤(6)中，直接采用上一时刻激光传感器的位置和角度作为当前时刻的预测，或者采用滤波算法进行预测。
进一步地，所述步骤(8)之后，还包括多块优化的步骤：如果激光传感器检测到三个及以上的反光板数据，可以根据多组数据中任意两组计算得到的位姿进行数据融合，得到最终的移动平台及激光传感器在世界坐标系下的位置及角度。
本发明的有益效果如下：与传统技术中的几何三角定位不同，本发明深入挖掘并充分利用了反射光的复数域信息，打破了传统意义上三块反光板的技术瓶颈，其运行条件为同时检测到两块反光板的数据即可，大大提高了激光导航技术的适用性；虽然本发明最低要求为检测到两块反光板，但实际应用时可能会同时检测到多个反光板，通过设计数据融合处理，能够最大化利用反光板的信息，从而进一步提高精度。本发明的具体性能为：定位误差<1cm，角度误差<0.5°，定位频率>35Hz。
附图说明
图1为本发明的硬件结构示意图；
图2为移动平台运动学模型示意图；
图3为本发明方法整体流程图。
具体实施方式
下面结合附图，通过实施例对本发明作进一步地说明。
本发明提供的一种基于数据筛选的双反光板激光定位与导航方法，该方法包括：
(1)在工业环境中布置反光板，预设反光板世界坐标，生成反光板坐标列表；
(2)安装在移动平台上的激光传感器向周围呈辐射状发射激光，并接收反射激光(激光传感器安装在AGV、工业车辆等移动平台上)；
(3)筛选来源于反光板的有效光束：
预设强度阈值σ，通过检测反射激光强度I，并与σf(d)相比较，判定激光照射物是反光板还是普通环境物体，其中f(d)与距离d负相关，优选地，可设置f(d)＝1/d；一种可能的实现方式如下：对于所有接收到的反射光束，如果第step个激光束的激光强度intensity(step)大于预设强度阈值intensity_threshold/激光测量的距离distance(step)，则将(step,intensity(step),distance(step))写入激光强点的数据列表data中；对应的伪代码如下：
for step＝1:number_laser
if intensity(step)>intensity_threshold/distance(step)
将(step,intensity(step),distance(step))写入data表中
endif
endfor
(4)确定当前时刻照射到的反光板数量及其相对激光传感器的相对坐标：根据反射光束角度的连续性，判断是否为同一块反光板，或者，根据反射光束角度和距离的连续性，判断是否为同一块反光板；根据属于同一反光板的反射光束得到该反光板相对于激光传感器的相对坐标(方法为：平均、取最强光，加权平均，任取)，并存入反光板列表；
根据反射光束角度和距离的连续性，判断是否为同一块反光板，一种可能的实现方式如下：
对于data表中所有的反光强点：
如果第i个反光强点的序号step(i)等于第i-1个反光强点的序号step(i-1)+1，并且第i和i-1的激光强点测得的距离之差distance(step(i))-distance(step(i-1))小于预设的距离阈值distance_threshold，那么光束step(i)和step(i-1)照射到的是同一块反光板，则将(step(i),α,ρ)赋值到上一个landmark结构体中；
否则照射到的是一块新的反光板，开始一个新的landmark结构体；计算：α＝(step(i)-1)·resolution+α；ρ＝distance(step(i))
将(step(i),α,ρ)写入新的landmark结构体中；
最终得到由landmark结构体组成的所照射到的反光板列表measure_landmark_list；
对应的伪代码如下：
for i＝1:number_data
if step(i)＝step(i-1)+1&&distance(step(i))-distance(step(i-1))<distance_threshold
then光束step(i)和step(i-1)照射到的是同一块反光板
else
α＝(step(i)-1)·resolution+α；ρ＝distance(step(i))
将(step(i),α,ρ)写入landmark
endif
endfor
其中number_data是反光强点的个数，resolution是相邻两道激光光束的夹角(分辨率)，distance_threshold是判定是否照射到同一块反光板的距离阈值，α和ρ分别代表在激光传感器相对自身的极坐标系下，该反光板的角度和距离，measure_landmark_list为照射到反光板的列表。
或者：仅根据反射光束角度的连续性，判断是否为同一块反光板，一种可能的实现方式如下：
对于data表中所有的反光强点：
如果第i个反光强点的序号step(i)等于第i-1个反光强点的序号step(i-1)+1，那么光束step(i)和step(i-1)照射到的是同一块反光板，则将(step(i),α,ρ)赋值到上一个landmark结构体中；
否则照射到的是一块新的反光板，开始一个新的landmark结构体，计算：
α＝(step(i)-1)·resolution+α；ρ＝distance(step(i))
将(step(i),α,ρ)写入新的landmark结构体中；
最终得到由landmark结构体组成的所照射到的反光板列表measure_landmark_list；
(5)初始化反光板列表，得到至少两个反光板的世界坐标：人工确定初始位置对应的反光板列表中至少两个反光板的世界坐标；或者，激光传感器在初始位置获取至少三个反光板返回的角度和距离，计算两两反光板之间的距离，与根据反光板坐标列表生成的反光板距离信息相匹配，得到至少两个反光板的世界坐标；一种可能的实现方式如下：
计算任意两个反光板之间的距离
计算任意两个反光强点之间的距离
对于measure_landmark_list列表中所照射到的M个反光板所有两两组合(l,k)：找到i,j∈{1,…,N},使得|Dk,l-Ri,j|＜Ri,j·threshold；则i添加到correspondence_list(k)，j添加到correspondence_list(l)；
找到correspondence_list(k)中出现频次最高的值，即为反光强点k对应的反光板c(k)，如果找不到这样的点，则从measure_landmark_list中去掉点i，并且令M＝M-1；
如果measure_landmark_list中包含的反光强点个数M<＝2，则警告处理。
(6)在动态过程中计算期望反光板列表：根据上一时刻对当前时刻激光传感器位置和角度的预测，估计激光传感器与所有反光板之间的相对距离和角度，并存入期望反光板列表；(直接采用上一时刻激光传感器的位置和角度作为当前时刻的预测，或者采用滤波算法进行预测)一种可能的实现方式如下：对于第i块反光板，i取1到N，计算：
其中函数arctan2(x,y)返回原点至点(x,y)的方位角，取值为(-π,π]。
如果ρt|t-1小于最大距离阈值max_distance，并且αt|t-1大于激光传感器扫描角度的最小值min_angle减去角度阈值threshold_angle，αt|t-1小于激光传感器扫描角度的最大值max_angle加上角度阈值threshold_angle，则将添加到期望的反光列表estimated_landmark_list中；
对应的伪代码如下：
for i＝1:N
如果ρt|t-1＜max_distance&&min_angle-threshold_angle＜αt|t-1＜max_angle+threshold_angle
则将添加到期望的反光列表estimated_landmarks_list
End
(7)在动态过程中反光板列表的匹配：计算当前时刻反光板列表与期望反光板列表中同一个反光板对应的距离之差和角度之差，当距离之差和角度之差均满足预设阈值时，匹配成功；一种可能的实现方式如下：
对于measure_landmark_list的所有反光板，找到{i,Xi,Yi,ρt|t-1,αt|t-1}，使得：
计算测量点(ρk,αk)和世界坐标系的误差：
如果dist<threshold_distance，则c(k)＝i
如果找不到这样的点，则从measure_landmark_list中去掉点i，并令M＝M-1；如果measure_landmark_list中包含的反光强点个数M<＝2，则警告处理。
(8)基于双反光板数据的激光传感器位姿计算：利用激光传感器测量数据的复频域信息，在匹配成功的反光板中任选两块：第l个和第k个，并计算：
zk＝Xk+i*Yk
zl＝Xl+i*Yl
其中，下标l和k分别代表第l和第k个反光板；α和ρ分别代表在激光传感器相对自身的极坐标系下，反光板的角度和距离；X和Y分别为反光板在X和Y轴的分量；z为反光板在世界坐标系下的复数坐标；zk,l为根据第l和第k个反光板计算得到的激光传感器世界坐标，θk是根据第k块反光板的数据计算得到的激光传感器在世界坐标系下的角度；
(9)多块优化：如果激光传感器检测到三个及以上的反光板数据，可以根据多组数据中任意两组计算得到的位姿进行数据融合，得到最终的移动平台及激光传感器在世界坐标系下的位置及角度。可选的方案有：选取反射光强最强的两组数据的计算结果作为激光传感器当前位姿，或在多组数据中两两组合进行位姿计算，然后求取他们的平均值，或加权平均值(如权重与反射光强度相关)。
实施例1
如图1所示，本发明的硬件构成主要包括激光传感器、工控机、车载控制电路与车载电源；所述的工控机、激光传感器与车载控制电路通过RS232/CAN/SPI等方式进行通信；车载电源对激光传感器和车载控制电路经DC/DC模块进行直流供电，经DC/AC模块对车载工控机进行交流供电。
进一步地，在工业环境(地图)中合适的位置放置一定数量的反光板，以AGV为例，使得AGV在地图中的任何位置都能够成功检测到2块或以上的反光板；激光传感器发射激光并检测反射信号。
进一步地，工控机计算反光板间距，初始化时采用静态匹配，随后均采用动态匹配来识别反光板，并从反光板列表中获取反光板世界坐标信息。
进一步地，工控机提取激光传感器测距数据中的复数域信息，利用两块反光板的数据进行AGV坐标位置和姿态的计算；若同时检测到3块以上的反光板信息，则将反光板数据进行两两组合，在每周组合下分别计算，然后以计算结果的平均值作为AGV的位置和姿态。本发明的整体流程如图3所示。

Claims (2)

1.一种基于数据筛选的双反光板激光定位与导航方法，其特征在于，该方法包括：
(1)在工业环境中布置反光板，预设反光板世界坐标，生成反光板坐标列表；
(2)安装在移动平台上的激光传感器向周围呈辐射状发射激光，并接收反射激光；
(3)筛选来源于反光板的有效光束：预设强度阈值σ，通过检测反射激光强度I，并与σf(d)相比较，判定激光照射物是反光板还是普通环境物体，其中f(d)与距离d负相关；
(4)确定当前时刻照射到的反光板数量及其相对激光传感器的相对坐标：根据反射光束角度的连续性，判断是否为同一块反光板，或者，根据反射光束角度和距离的连续性，判断是否为同一块反光板；根据属于同一反光板的反射光束得到该反光板相对于激光传感器的相对坐标，并存入反光板列表；
(5)初始化反光板列表，得到至少两个反光板的世界坐标：人工确定激光传感器所在初始位置对应的反光板列表中至少两个反光板的世界坐标；或者，激光传感器在初始位置获取至少三个反光板返回的角度和距离，计算两两反光板之间的距离，与根据反光板坐标列表生成的反光板距离信息相匹配，得到至少两个反光板的世界坐标；
(6)在动态过程中计算期望反光板列表：根据上一时刻对当前时刻激光传感器位置和角度的预测，估计激光传感器与所有反光板之间的相对距离和角度，并存入期望反光板列表；
(7)在动态过程中反光板列表的匹配：计算当前时刻反光板列表与期望反光板列表中同一个反光板对应的距离之差和角度之差，当距离之差和角度之差均满足预设阈值时，匹配成功；
(8)基于双反光板数据的激光传感器位姿计算：利用激光传感器测量数据的复频域信息，在匹配成功的反光板中任选两块：第l个和第k个，并计算：
zk＝Xk+i*Yk
zl＝Xl+i*Yl
其中，下标l和k分别代表第l和第k个反光板；α和ρ分别代表在激光传感器相对自身的极坐标系下，反光板的角度和距离；X和Y分别为反光板在X和Y轴的分量；z为反光板在世界坐标系下的复数坐标；zk，l为根据第l和第k个反光板计算得到的激光传感器世界坐标，θk是根据第k块反光板的数据计算得到的激光传感器在世界坐标系下的角度。
2.根据权利要求1所述的基于数据筛选的双反光板激光定位与导航方法，其特征在于，所述步骤(8)之后，还包括多块优化的步骤：如果激光传感器检测到三个及以上的反光板数据，可以根据多组数据中任意两组计算得到的位姿进行数据融合，得到最终的移动平台及激光传感器在世界坐标系下的位置及角度。
CN201710572152.2A 2017-07-13 2017-07-13 一种基于数据筛选的双反光板激光定位与导航方法 Active CN107390227B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710572152.2A CN107390227B (zh) 2017-07-13 2017-07-13 一种基于数据筛选的双反光板激光定位与导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710572152.2A CN107390227B (zh) 2017-07-13 2017-07-13 一种基于数据筛选的双反光板激光定位与导航方法

Publications (2)

Publication Number Publication Date
CN107390227A CN107390227A (zh) 2017-11-24
CN107390227B true CN107390227B (zh) 2019-10-18

Family

ID=60339586

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710572152.2A Active CN107390227B (zh) 2017-07-13 2017-07-13 一种基于数据筛选的双反光板激光定位与导航方法

Country Status (1)

Country Link
CN (1) CN107390227B (zh)

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109143258A (zh) * 2018-09-06 2019-01-04 苏州元谋智能机器人系统有限公司 无轨导航agv的定位方法
CN109633681A (zh) * 2018-12-05 2019-04-16 芜湖智久机器人有限公司 一种反光板识别方法及装置
CN109631919B (zh) * 2018-12-28 2022-09-30 芜湖哈特机器人产业技术研究院有限公司 一种融合反光板和占据栅格的混合导航地图构建方法
CN111745635B (zh) * 2019-03-28 2022-03-04 苏州科瓴精密机械科技有限公司 识别反光标方法、移动机器人定位方法及移动机器人系统
CN110471072B (zh) * 2019-08-19 2021-04-02 华晟（青岛）智能装备科技有限公司 一种反光柱位置辨识方法及系统
CN111538035B (zh) * 2020-05-29 2022-08-30 三一机器人科技有限公司 定位方法、装置和系统
CN111722202B (zh) * 2020-08-05 2021-04-27 湖北工业大学 一种基于回波强度的反光板位置拟合方法及系统
CN112130166B (zh) * 2020-09-04 2023-11-28 江苏智库智能科技有限公司 基于反光板网络的agv定位方法及装置

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH10132560A (ja) * 1996-10-31 1998-05-22 Komatsu Ltd 位置計測方法及びその装置
CN202988597U (zh) * 2012-12-11 2013-06-12 重庆起重机厂有限责任公司 一种起重机多点自动定位系统
CN205080152U (zh) * 2015-11-03 2016-03-09 王小波 一种测量机动车速度的装置
CN105446334A (zh) * 2015-11-13 2016-03-30 上海诺力智能科技有限公司 一种引导车导航系统及一种引导车导航方法
CN106091924B (zh) * 2016-05-30 2018-08-07 深圳普智联科机器人技术有限公司 一种使用激光校正贴码误差的系统和方法

Also Published As

Publication number Publication date
CN107390227A (zh) 2017-11-24

Similar Documents

Publication Publication Date Title
CN107390227B (zh) 一种基于数据筛选的双反光板激光定位与导航方法
CN107144855B (zh) 一种基于双反光板的激光定位与导航方法
CN107144853B (zh) 一种基于数据筛选的双反光柱激光定位与导航方法
CN107289946B (zh) 一种基于双反光板的高精度激光定位与导航方法
CN107144854B (zh) 一种基于双反光柱的激光定位与导航方法
CN107356931B (zh) 一种基于滤波的双反光板激光定位与导航方法
Chung et al. The detection and following of human legs through inductive approaches for a mobile robot with a single laser range finder
Kang et al. Automatic targetless camera–lidar calibration by aligning edge with gaussian mixture model
CN111157982A (zh) 基于岸基雷达的智能船舶船岸协同目标跟踪系统和方法
US9528828B2 (en) Method and system for determining position and orientation of a measuring instrument
KR102012179B1 (ko) 3차원 평면 검출 기반의 라이다-카메라 캘리브레이션 장치 및 방법
CN110411366A (zh) 一种道路积水深度的检测方法及电子设备
CN107346025B (zh) 一种基于滤波的双反光柱激光定位与导航方法
US20050088642A1 (en) Apparatus and method for detecting obstacles
US20220114363A1 (en) Method and System for Indoor Multipath Ghosts Recognition
CN112184765B (zh) 一种用于水下航行器的自主跟踪方法
CN108828518A (zh) 一种货车车厢内部堆垛车定位方法
CN105631893A (zh) 一种通过拍照检测电容安装正确与否的方法及装置
CN109029465A (zh) 一种无人艇基于毫米波雷达的跟踪和避障系统
Pyo et al. Beam slice-based recognition method for acoustic landmark with multi-beam forward looking sonar
US20210237768A1 (en) Distance estimating device and storage medium storing computer program for distance estimation
CN110456377A (zh) 一种基于三维激光雷达的卫星来袭异物检测方法和系统
Yu Development of real-time acoustic image recognition system using by autonomous marine vehicle
CN112731371A (zh) 一种激光雷达与视觉融合的集成化目标跟踪系统及方法
CN107238841B (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
GR01 Patent grant
GR01 Patent grant