[发明专利]一种基于双反光柱的高精度激光定位与导航方法

摘要

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

主权项

1.一种基于双反光柱的高精度激光定位与导航方法，其特征在于，该方法包括：(1)在工业环境中布置反光柱，预设反光柱世界坐标，生成反光柱坐标列表；(2)安装在移动平台上的激光传感器向周围呈辐射状发射激光，并接收反射激光；(3)筛选来源于反光柱的有效光束：预设强度阈值σ，通过检测反射激光强度I，并与σf(range)相比较，判定激光照射物是反光柱还是普通环境物体，其中f(range)与距离range负相关；(4)确定当前时刻照射到的反光柱数量及其相对激光传感器的相对坐标：根据反射光束角度的连续性，判断是否为同一个反光柱，或者，根据反射光束角度和距离的连续性，判断是否为同一个反光柱；根据属于同一反光柱的反射光数据，结合反光柱半径进行数据修正，得到该反光柱相对于激光传感器的相对坐标，并存入反光柱列表；(5)初始化反光柱列表，得到至少两个反光柱的世界坐标：人工确定激光传感器所在初始位置对应的反光柱列表中至少两个反光柱的世界坐标；或者，激光传感器在初始位置获取至少三个反光柱返回的角度和距离，计算两两反光柱之间的距离，与根据反光柱坐标列表生成的反光柱距离信息相匹配，得到至少两个反光柱的世界坐标；(6)在动态过程中计算期望反光柱列表：根据上一时刻对当前时刻激光传感器位置和角度的预测，估计激光传感器与所有反光柱之间的相对距离和角度，并存入期望反光柱列表；(7)在动态过程中反光柱列表的匹配：计算当前时刻反光柱列表与期望反光柱列表中同一个反光柱对应的距离之差和角度之差，当距离之差和角度之差均满足预设阈值时，匹配成功；(8)基于双反光柱数据的激光传感器位姿计算：利用激光传感器测量数据的复频域信息，在匹配成功的反光柱中任选两个：第l个和第k个，并计算：z_k＝X_k+i*Y_kz_l＝X_l+i*Y_l其中，下标l和k分别代表第l和第k个反光柱；α和ρ分别代表在激光传感器相对自身的极坐标系下，反光柱的角度和距离；X和Y分别为反光柱在X和Y轴的分量；z为反光柱在世界坐标系下的复数坐标；z_k，l为根据第l和第k个反光柱计算得到的激光传感器世界坐标，θ_k是根据第k个反光柱的数据计算得到的激光传感器在世界坐标系下的角度；(9)激光传感器位置滤波：首先根据移动平台运动学模型进行位姿预测，然后根据上一时刻预测的位姿，并结合当前时刻的激光传感器测量数据，对当前时刻的位置进行滤波，从而得到最终的移动平台及激光传感器在世界坐标系下的准确的位置与姿态。