CN107356931B - 一种基于滤波的双反光板激光定位与导航方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明属于智能仓储物流领域，具体涉及一种基于滤波的双反光板激光定位与导航方法。

背景技术

基于激光传感器的定位与导航技术是工业AGV、智能机器人等领域的关键性技术，相比传统有轨导航方式，该技术具有定位精度高、灵活多变等优点，适用于复杂、高动态的工业场景中。国内外现有基于反光板的激光定位技术，均采用了几何三角定位原理，每时每刻必须要求同时检测到三块反光板才可实现定位。这一数量要求实际上在复杂工业场景中很难满足，极大地制约了激光定位与导航技术的实用性。

发明内容

为了解决上述问题，本发明提供一种新型激光定位与导航方法，通过提取和利用激光测距信息中的复数域信息，将三块反光板的最低要求降低为两块，有效解决了反光板最低数量的技术瓶颈，大大提高了激光导航技术的适用性。

本发明的目的是通过以下技术方案来实现的：一种基于滤波的双反光板激光定位与导航方法，该方法包括：

(1)在工业环境中布置反光板，预设反光板世界坐标，生成反光板坐标列表；

(2)安装在移动平台上的激光传感器向周围呈辐射状发射激光，并接收反射激光；

(3)筛选来源于反光板的有效光束：通过检测反射激光强度I并与预设强度阈值σ相比较，判定激光照射物是反光板还是普通环境物体；

(4)确定当前时刻照射到的反光板数量及其相对激光传感器的相对坐标：根据反射光束角度的连续性，判断是否为同一块反光板，或者，根据反射光束角度和距离的连续性，判断是否为同一块反光板；根据属于同一反光板的反射光束得到该反光板相对于激光传感器的相对坐标，并存入反光板列表；

(5)初始化反光板列表，得到至少两个反光板的世界坐标：人工确定初始位置对应的反光板列表中至少两个反光板的世界坐标；或者，激光传感器在初始位置获取至少三个反光板返回的角度和距离，计算两两反光板之间的距离，与根据反光板坐标列表生成的反光板距离信息相匹配，得到至少两个反光板的世界坐标；

(6)在动态过程中计算期望反光板列表：根据上一时刻对当前时刻激光传感器位置和角度的预测，估计激光传感器与所有反光板之间的相对距离和角度，并存入期望反光板列表；

(7)在动态过程中反光板列表的匹配：计算当前时刻反光板列表与期望反光板列表中同一个反光板对应的距离之差和角度之差，当距离之差和角度之差均满足预设阈值时，匹配成功；

(8)基于双反光板数据的激光传感器位姿计算：利用激光传感器测量数据的复频域信息，在匹配成功的反光板中任选两块：第l个和第k个，并计算：

z_k＝X_k+i*Y_k

z_l＝X_l+i*Y_l

其中，下标l和k分别代表第l和第k个反光板；α和ρ分别代表在激光传感器相对自身的极坐标系下，反光板的角度和距离；X和Y分别为反光板在X和Y轴的分量；z为反光板在世界坐标系下的复数坐标；z_k,l为根据第l和第k个反光板计算得到的激光传感器世界坐标，θ_k是根据第k块反光板的数据计算得到的激光传感器在世界坐标系下的角度；

(9)激光传感器位置滤波：首先根据移动平台运动学模型进行位姿预测，然后根据上一时刻预测的位姿，并结合当前时刻的激光传感器测量数据，对当前时刻的位置进行滤波，从而得到最终的移动平台及激光传感器在世界坐标系下的准确的位置与姿态。

进一步地，所述步骤(6)中，直接采用上一时刻激光传感器的位置和角度作为当前时刻的预测，或者采用滤波算法进行预测。

进一步地，所述步骤(9)之前，还包括多块优化的步骤：如果激光传感器检测到三个及以上的反光板数据，可以根据多组数据中任意两组计算得到的位姿进行数据融合。

进一步地，所述步骤(9)中，根据移动平台运动学模型进行位姿预测，公式如下：

θ_t|t-1＝θ_t-1|t-1+Δθ_c

其中，输入量为：上一时刻的位姿(x_t-1|t-1,y_t-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，移动平台在上一时刻的线速度v_t-1，移动平台在上一时刻的转向角速度ω_t-1，移动平台在上一时刻世界坐标系下的转向角度θ_t-1，激光传感器距离两个轮子中心点的距离d；输出量为：相对上时刻的运动增量(Δx,Δy,Δθ)，根据上一时刻数据预测的当前时刻位姿(x_t|t-1,y_t|t-1,θ_t|t-1)；

根据上一时刻预测的位姿，结合当前时刻的位姿计算结果，对当前时刻的位姿进行滤波，具体如下：若当前时刻仅检测到2块反光板，记为l和k，采用线性卡尔曼滤波的公式如下：

其中，输入量为：当前时刻预测的位置上一时刻的协方差矩阵P_t-1|t-1，过程噪声的协方差矩阵Q，测量噪声的协方差矩阵R；输出量为：激光传感器在世界坐标系下的坐标当前时刻更新的协方差矩阵P_t|t；

若当前时刻检测到3块及以上的反光板，则采用多块数据融合的思想进行滤波；采用贯序卡尔曼滤波的公式如下：

对于检测到的M个反光板中的任意两两组合，计算：

z_k＝X_k+i*Y_k

z_l＝X_l+i*Y_l

遍历所有组合后，得到：

本发明的有益效果如下：与传统技术中的几何三角定位不同，本发明深入挖掘并充分利用了反射光的复数域信息，打破了传统意义上三块反光板的技术瓶颈，其运行条件为同时检测到两块反光板的数据即可，大大提高了激光导航技术的适用性；通过滤波算法过滤掉测量误差引起的计算误差，极大地提高定位与导航精度；最后，虽然本发明最低要求为检测到两块反光板，但实际应用时可能会同时检测到多个反光板，通过设计数据融合处理，能够最大化利用反光板的信息，从而进一步提高精度。本发明的具体性能为：定位误差<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))小于预设的距离阈值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},使得|D_k,l-R_i,j|＜R_i,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,X_i,Y_i,ρ_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个，并计算：

z_k＝X_k+i*Y_k

z_l＝X_l+i*Y_l

其中，下标l和k分别代表第l和第k个反光板；α和ρ分别代表在激光传感器相对自身的极坐标系下，反光板的角度和距离；X和Y分别为反光板在X和Y轴的分量；z为反光板在世界坐标系下的复数坐标；z_k,l为根据第l和第k个反光板计算得到的激光传感器世界坐标，θ_k是根据第k块反光板的数据计算得到的激光传感器在世界坐标系下的角度；

(9)多块优化：如果激光传感器检测到三个及以上的反光板数据，可以根据多组数据中任意两组计算得到的位姿进行数据融合。可选的方案有：选取反射光强最强的两组数据的计算结果作为激光传感器当前位姿，或在多组数据中两两组合进行位姿计算，然后求取他们的平均值，或加权平均值(如权重与反射光强度相关)。

(10)激光传感器位置滤波：首先根据移动平台运动学模型进行位姿预测，公式如下：

θ_t|t-1＝θ_t-1|t-1+Δθ_c

其中，输入量为：上一时刻的位姿(x_t-1|t-1,y_t-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，移动平台在上一时刻的线速度v_t-1，移动平台在上一时刻的转向角速度ω_t-1，移动平台在上一时刻世界坐标系下的转向角度θ_t-1，激光传感器距离两个轮子中心点的距离d；输出量为：相对上时刻的运动增量(Δx,Δy,Δθ)，根据上一时刻数据预测的当前时刻位姿(x_t|t-1,y_t|t-1,θ_t|t-1)；

然后根据上一时刻预测的位姿，结合当前时刻的位姿计算结果，对当前时刻的位置进行滤波；若当前时刻仅检测到2块反光板，记为l和k，一种可能的线性卡尔曼滤波的公式如下：

其中，输入量为：当前时刻预测的位置上一时刻的协方差矩阵P_t-1|t-1，过程噪声的协方差矩阵Q，测量噪声的协方差矩阵R；输出量为：激光传感器在世界坐标系下的坐标当前时刻更新的协方差矩阵P_t|t；

若当前时刻检测到3块及以上的反光板，则采用多块数据融合的思想进行滤波；一种可能的方案是贯序卡尔曼滤波，伪代码如下：

对于检测到的M个反光板中的任意两两组合，计算：

z_k＝X_k+i*Y_k

z_l＝X_l+i*Y_l

遍历所有组合后，得到：

实施例1

如图1所示，本发明的硬件构成主要包括激光传感器、工控机、车载控制电路与车载电源；所述的工控机、激光传感器与车载控制电路通过RS232/CAN/SPI等方式进行通信；车载电源对激光传感器和车载控制电路经DC/DC模块进行直流供电，经DC/AC模块对车载工控机进行交流供电。

进一步地，在工业环境(地图)中合适的位置放置一定数量的反光板，以AGV为例，使得AGV在地图中的任何位置都能够成功检测到2块或以上的反光板；激光传感器发射激光并检测反射信号。

进一步地，工控机计算反光板间距，初始化时采用静态匹配，随后均采用动态匹配来识别反光板，并从反光板列表中获取反光板世界坐标信息。

进一步地，工控机提取激光传感器测距数据中的复数域信息，利用两块反光板的数据进行AGV坐标位置和姿态的计算；若同时检测到3块以上的反光板信息，则将反光板数据进行两两组合，在每周组合下分别计算，然后以计算结果的平均值作为AGV的位置和姿态。

进一步地，工控机建立AGV运动模型，并在此基础上利用卡尔曼滤波来精确估计AGV的真实位置与姿态。本发明的整体流程如图3所示。

Claims (3)

1.一种基于滤波的双反光板激光定位与导航方法，其特征在于，该方法包括：(1)在工业环境中布置反光板，预设反光板世界坐标，生成反光板坐标列表；

(2)安装在移动平台上的激光传感器向周围呈辐射状发射激光，并接收反射激光；

(3)筛选来源于反光板的有效光束：通过检测反射激光强度I并与预设强度阈值σ相比较，判定激光照射物是反光板还是普通环境物体；

(4)确定当前时刻照射到的反光板数量及其相对激光传感器的相对坐标：根据反射光束角度的连续性，判断是否为同一块反光板，或者，根据反射光束角度和距离的连续性，判断是否为同一块反光板；根据属于同一反光板的反射光束得到该反光板相对于激光传感器的相对坐标，并存入反光板列表；

(5)初始化反光板列表，得到至少两个反光板的世界坐标：人工确定激光传感器所在初始位置对应的反光板列表中至少两个反光板的世界坐标；或者，激光传感器在初始位置获取至少三个反光板返回的角度和距离，计算两两反光板之间的距离，与根据反光板坐标列表生成的反光板距离信息相匹配，得到至少两个反光板的世界坐标；

(6)在动态过程中计算期望反光板列表：根据上一时刻对当前时刻激光传感器位置和角度的预测，估计激光传感器与所有反光板之间的相对距离和角度，并存入期望反光板列表；

(7)在动态过程中反光板列表的匹配：计算当前时刻反光板列表与期望反光板列表中同一个反光板对应的距离之差和角度之差，当距离之差和角度之差均满足预设阈值时，匹配成功；

(8)基于双反光板数据的激光传感器位姿计算：利用激光传感器测量数据的复频域信息，在匹配成功的反光板中任选两块：第l个和第k个，并计算：

z_k＝X_k+i*Y_k

z_l＝X_l+i*Y_l

其中，下标l和k分别代表第l和第k个反光板；α和ρ分别代表在激光传感器相对自身的极坐标系下，反光板的角度和距离；X和Y分别为反光板在X和Y轴的分量；z为反光板在世界坐标系下的复数坐标；z_k,l为根据第l和第k个反光板计算得到的激光传感器世界坐标，θ_k是根据第k块反光板的数据计算得到的激光传感器在世界坐标系下的角度；

(9)激光传感器位置滤波：首先根据移动平台运动学模型进行位姿预测，然后根据上一时刻预测的位姿，并结合当前时刻的激光传感器测量数据，对当前时刻的位置进行滤波，从而得到最终的移动平台及激光传感器在世界坐标系下的准确的位置与姿态。

2.根据权利要求1所述的基于滤波的双反光板激光定位与导航方法，其特征在于，所述步骤(9)之前，还包括多块优化的步骤：如果激光传感器检测到三个及以上的反光板数据，可以根据多组数据中任意两组计算得到的位姿进行数据融合。

3.根据权利要求1所述的基于滤波的双反光板激光定位与导航方法，其特征在于，所述步骤(9)中，根据移动平台运动学模型进行位姿预测，公式如下：

θ_t|t-1＝θ_t-1|t-1+Δθ_c

其中，输入量为：上一时刻的位姿(x_t-1|t-1,y_t-1|t-1,θ_t-1|t-1)，电机编码器的分辨率resolution，减速比为speed_reduction_ratio，左、右轮编码器此刻的读数count_left、count_right，左、右轮编码器上一刻的读数last_count_left、last_count_right，采样周期ΔT，左右轮半径r，左、右轮的角速度ω_L、ω_R，移动平台在上一时刻的线速度v_t-1，移动平台在上一时刻的转向角速度ω_t-1，移动平台在上一时刻世界坐标系下的朝向角度θ_t-1，激光传感器距离两个轮子中心点的距离d；输出量为：相对上时刻的运动增量(Δx,Δy,Δθ)，根据上一时刻数据预测的当前时刻位姿(x_t|t-1,y_t|t-1,θ_t|t-1)；

根据上一时刻预测的位姿，并结合当前时刻的激光传感器测量数据，对当前时刻的位姿进行滤波，具体如下：若当前时刻仅检测到2块反光板，记为l和k，采用线性卡尔曼滤波的公式如下：

其中，输入量为：当前时刻预测的位置上一时刻的协方差矩阵P_t-1|t-1，过程噪声的协方差矩阵Q，测量噪声的协方差矩阵R；输出量为：激光传感器在世界坐标系下的坐标当前时刻更新的协方差矩阵P_t|t；

若当前时刻检测到3块及以上的反光板，则采用多块数据融合的思想进行滤波；采用贯序卡尔曼滤波的公式如下：

对于检测到的M个反光板中的任意两两组合，计算：

z_k＝X_k+i*Y_k

z_l＝X_l+i*Y_l

遍历所有组合后，得到：