一种基于滤波的双反光板激光定位与导航方法技术

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

A laser positioning and navigation method based on filter for double reflector

The invention discloses a double reflector filter based on laser positioning and navigation method, geometric triangulation and traditional technology is different, the invention fully excavate and use the complex domain information of the reflected light, breaking the technical bottleneck of the traditional three pieces of the reflecting plate, the operating conditions for simultaneous detection of up to two a reflector data can greatly improve the applicability of laser navigation technology; through the calculation error filtering algorithm to filter out the measuring error, greatly improve the positioning and navigation precision; finally, although the minimum requirements for detecting two reflectors, but the actual application may also detect multiple reflection in processing through the design of data fusion, to maximize the use of the reflecting plate so as to further improve the accuracy of information.

【技术实现步骤摘要】
一种基于滤波的双反光板激光定位与导航方法
本专利技术属于智能仓储物流领域，具体涉及一种基于滤波的双反光板激光定位与导航方法。
技术介绍
基于激光传感器的定位与导航技术是工业AGV、智能机器人等领域的关键性技术，相比传统有轨导航方式，该技术具有定位精度高、灵活多变等优点，适用于复杂、高动态的工业场景中。国内外现有基于反光板的激光定位技术，均采用了几何三角定位原理，每时每刻必须要求同时检测到三块反光板才可实现定位。这一数量要求实际上在复杂工业场景中很难满足，极大地制约了激光定位与导航技术的实用性。
技术实现思路
为了解决上述问题，本专利技术提供一种新型激光定位与导航方法，通过提取和利用激光测距信息中的复数域信息，将三块反光板的最低要求降低为两块，有效解决了反光板最低数量的技术瓶颈，大大提高了激光导航技术的适用性。本专利技术的目的是通过以下技术方案来实现的：一种基于滤波的双反光板激光定位与导航方法，该方法包括：(1)在工业环境中布置反光板，预设反光板世界坐标，生成反光板坐标列表；(2)安装在移动平台上的激光传感器向周围呈辐射状发射激光，并接收反射激光；(3)筛选来源于反光板的有效光束：通过检测反射激光强度I并与预设强度阈值σ相比较，判定激光照射物是反光板还是普通环境物体；(4)确定当前时刻照射到的反光板数量及其相对激光传感器的相对坐标：根据反射光束角度的连续性，判断是否为同一块反光板，或者，根据反射光束角度和距离的连续性，判断是否为同一块反光板；根据属于同一反光板的反射光束得到该反光板相对于激光传感器的相对坐标，并存入反光板列表；(5)初始化反光板列表，得到至少两个反光板的世界坐标：人工确定初始位置对应的反光板列表中至少两个反光板的世界坐标；或者，激光传感器在初始位置获取至少三个反光板返回的角度和距离，计算两两反光板之间的距离，与根据反光板坐标列表生成的反光板距离信息相匹配，得到至少两个反光板的世界坐标；(6)在动态过程中计算期望反光板列表：根据上一时刻对当前时刻激光传感器位置和角度的预测，估计激光传感器与所有反光板之间的相对距离和角度，并存入期望反光板列表；(7)在动态过程中反光板列表的匹配：计算当前时刻反光板列表与期望反光板列表中同一个反光板对应的距离之差和角度之差，当距离之差和角度之差均满足预设阈值时，匹配成功；(8)基于双反光板数据的激光传感器位姿计算：利用激光传感器测量数据的复频域信息，在匹配成功的反光板中任选两块：第l个和第k个，并计算：zk＝Xk+i*Ykzl＝Xl+i*Yl其中，下标l和k分别代表第l和第k个反光板；α和ρ分别代表在激光传感器相对自身的极坐标系下，反光板的角度和距离；X和Y分别为反光板在X和Y轴的分量；z为反光板在世界坐标系下的复数坐标；zk,l为根据第l和第k个反光板计算得到的激光传感器世界坐标，θk是根据第k块反光板的数据计算得到的激光传感器在世界坐标系下的角度；(9)激光传感器位置滤波：首先根据移动平台运动学模型进行位姿预测，然后根据上一时刻预测的位姿，并结合当前时刻的激光传感器测量数据，对当前时刻的位置进行滤波，从而得到最终的移动平台及激光传感器在世界坐标系下的准确的位置与姿态。进一步地，所述步骤(6)中，直接采用上一时刻激光传感器的位置和角度作为当前时刻的预测，或者采用滤波算法进行预测。进一步地，所述步骤(9)之前，还包括多块优化的步骤：如果激光传感器检测到三个及以上的反光板数据，可以根据多组数据中任意两组计算得到的位姿进行数据融合。进一步地，所述步骤(9)中，根据移动平台运动学模型进行位姿预测，公式如下：θt|t-1＝θt-1|t-1+Δθc其中，输入量为：上一时刻的位姿(xt-1|t-1,yt-1|t-1,θt-1|t-1)，电机编码器的分辨率resolution，减速比为speed_reduction_ratio，左、右轮编码器此刻的读数count_left、count_right，左、右轮编码器上一刻的读数last_count_right、last_count_right，采样周期ΔT，左右轮半径r，左、右轮的角速度ωL、ωR，移动平台在上一时刻的线速度vt-1，移动平台在上一时刻的转向角速度ωt-1，移动平台在上一时刻世界坐标系下的转向角度θt-1，激光传感器距离两个轮子中心点的距离d；输出量为：相对上时刻的运动增量(Δx,Δy,Δθ)，根据上一时刻数据预测的当前时刻位姿(xt|t-1,yt|t-1,θt|t-1)；根据上一时刻预测的位姿，结合当前时刻的位姿计算结果，对当前时刻的位姿进行滤波，具体如下：若当前时刻仅检测到2块反光板，记为l和k，采用线性卡尔曼滤波的公式如下：其中，输入量为：当前时刻预测的位置上一时刻的协方差矩阵Pt-1|t-1，过程噪声的协方差矩阵Q，测量噪声的协方差矩阵R；输出量为：激光传感器在世界坐标系下的坐标当前时刻更新的协方差矩阵Pt|t；若当前时刻检测到3块及以上的反光板，则采用多块数据融合的思想进行滤波；采用贯序卡尔曼滤波的公式如下：对于检测到的M个反光板中的任意两两组合，计算：zk＝Xk+i*Ykzl＝Xl+i*Yl遍历所有组合后，得到：本专利技术的有益效果如下：与传统技术中的几何三角定位不同，本专利技术深入挖掘并充分利用了反射光的复数域信息，打破了传统意义上三块反光板的技术瓶颈，其运行条件为同时检测到两块反光板的数据即可，大大提高了激光导航技术的适用性；通过滤波算法过滤掉测量误差引起的计算误差，极大地提高定位与导航精度；最后，虽然本专利技术最低要求为检测到两块反光板，但实际应用时可能会同时检测到多个反光板，通过设计数据融合处理，能够最大化利用反光板的信息，从而进一步提高精度。本专利技术的具体性能为：定位误差<1cm，角度误差<0.5°，定位频率>35Hz。附图说明图1为本专利技术的硬件结构示意图；图2为移动平台运动学模型示意图；图3为本专利技术方法整体流程图。具体实施方式下面结合附图，通过实施例对本专利技术作进一步地说明。本专利技术提供的一种基于滤波的双反光板激光定位与导航方法，该方法包括：(1)在工业环境中布置反光板，预设反光板世界坐标，生成反光板坐标列表；(2)安装在移动平台上的激光传感器向周围呈辐射状发射激光，并接收反射激光(激光传感器安装在AGV、工业车辆等移动平台上)；(3)筛选来源于反光板的有效光束：通过检测反射激光强度I并与预设强度阈值σ相比较，判定激光照射物是反光板还是普通环境物体，筛选出来源于反光板的反射光数据；(4)确定当前时刻照射到的反光板数量及其相对激光传感器的相对坐标：根据反射光束角度的连续性，判断是否为同一块反光板，或者，根据反射光束角度和距离的连续性，判断是否为同一块反光板；根据属于同一反光板的反射光束得到该反光板相对于激光传感器的相对坐标(方法为：平均、取最强光，加权平均，任取)，并存入反光板列表；根据反射光束角度和距离的连续性，判断是否为同一块反光板，一种可能的实现方式如下：对于data表中所有的反光强点：如果第i个反光强点的序号step(i)等于第i-1个反光强点的序号step(i-1)+1，并且第i和i-1的激光强点测得的距离之差distance(step(i))-distance(step(i-1))本文档来自技高网...

【技术保护点】
一种基于滤波的双反光板激光定位与导航方法，其特征在于，该方法包括：(1)在工业环境中布置反光板，预设反光板世界坐标，生成反光板坐标列表；(2)安装在移动平台上的激光传感器向周围呈辐射状发射激光，并接收反射激光；(3)筛选来源于反光板的有效光束：通过检测反射激光强度I并与预设强度阈值σ相比较，判定激光照射物是反光板还是普通环境物体；(4)确定当前时刻照射到的反光板数量及其相对激光传感器的相对坐标：根据反射光束角度的连续性，判断是否为同一块反光板，或者，根据反射光束角度和距离的连续性，判断是否为同一块反光板；根据属于同一反光板的反射光束得到该反光板相对于激光传感器的相对坐标，并存入反光板列表；(5)初始化反光板列表，得到至少两个反光板的世界坐标：人工确定初始位置对应的反光板列表中至少两个反光板的世界坐标；或者，激光传感器在初始位置获取至少三个反光板返回的角度和距离，计算两两反光板之间的距离，与根据反光板坐标列表生成的反光板距离信息相匹配，得到至少两个反光板的世界坐标；(6)在动态过程中计算期望反光板列表：根据上一时刻对当前时刻激光传感器位置和角度的预测，估计激光传感器与所有反光板之间的相对距离和角度，并存入期望反光板列表；(7)在动态过程中反光板列表的匹配：计算当前时刻反光板列表与期望反光板列表中同一个反光板对应的距离之差和角度之差，当距离之差和角度之差均满足预设阈值时，匹配成功；(8)基于双反光板数据的激光传感器位姿计算：利用激光传感器测量数据的复频域信息，在匹配成功的反光板中任选两块：第l个和第k个，并计算：...

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

【专利技术属性】
技术研发人员：林志赟，邢浩，金智，
申请(专利权)人：浙江科钛机器人股份有限公司，
类型：发明
国别省市：浙江,33