[发明专利]一种基于滤波的双反光板激光定位与导航方法

权利要求书

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

遍历所有组合后，得到：