一种基于双反光柱的高精度激光定位与导航方法与流程

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

背景技术：

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

技术实现要素：

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

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

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

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

(3)筛选来源于反光柱的有效光束：预设强度阈值σ，通过检测反射激光强度i，并与σf(range)相比较，判定激光照射物是反光柱还是普通环境物体，其中f(range)与距离range负相关；

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

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

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

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

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

zk＝xk+i*yk

zl＝xl+i*tl

其中，下标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*yk

zl＝xl+i*yl

遍历所有组合后，得到：

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

附图说明

图1为本发明的硬件结构示意图；

图2为反光柱测距与数据修正示意图；

图3为移动平台运动学模型示意图；

图4为本发明方法整体流程图。

具体实施方式

下面结合附图，通过实施例对本发明作进一步地说明。

本发明提供的一种基于双反光柱的高精度激光定位与导航方法，该方法包括：

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

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

(3)筛选来源于反光柱的有效光束：

预设强度阈值σ，通过检测反射激光强度i，并与σf(range)相比较，判定激光照射物是反光柱还是普通环境物体，其中f(range)与距离range负相关；优选地，可设置f(range)＝1/range；一种可能的实现方式如下：对于所有接收到的反射光束，如果第step个激光束的激光强度intensity(step)大于预设强度阈值intensity_threshold/激光测量的距离range(step)，则将(step,intensity(step),range(step))写入激光强点的数据列表data中；对应的伪代码如下：

forstep＝1:number_laser

ifintensity(step)>intensity_threshold/range(step)

将(step,intensity(step),range(step))写入data表中

endif

endfor

(4)确定当前时刻照射到的反光柱数量及其相对激光传感器的相对坐标：根据反射光束角度的连续性，判断是否为同一个反光柱，或者，根据反射光束角度和距离的连续性，判断是否为同一个反光柱；根据属于同一反光柱的反射数据，结合反光柱半径r进行数据修正，如图2所示，得到该反光柱相对于激光传感器的相对坐标，并存入反光柱列表；

根据反射光束角度和距离的连续性，判断是否为同一个反光柱，一种可能的实现方式如下：

对于data表中所有的反光强点：

如果第i个反光强点的序号step(i)等于第i-1个反光强点的序号step(i-1)+1，并且第i和i-1的激光强点测得的距离之差range(step(i))-range(step(i-1))小于预设的距离阈值range_threshold，那么光束step(i)和step(i-1)照射到的是同一个反光柱，则将(step(i),range)赋值到当前第k个landmark子列表中，且该子列表中光束数据的组数number＝number+1；

否则，照射到的是一个新的反光柱，则当前第k个landmark子列表结束；

如果number＝1，则将(k,α,ρ)写入measure_landmark_list，其中α＝(step-1)·resolution+min_angle，ρ＝range+r；min_angle为激光传感器扫描角度的最小值；

如果number>＝2，则提取当前landmark子列表中的反光最强点的两组数据(step1,range1)和(step2,range2)，计算：

如果range1<range2

否则

同时，新建第k+1个landmark子列表，组数初始化为number＝0；将(step(i),range)赋值到新的landmark子列表中，number＝number+1；继续读取数据；

最终得到所照射到的反光柱列表measure_landmark_list；

对应的伪代码如下：

fori＝1:number_data

ifstep(i)＝step(i-1)+1&&range(step(i))-range(step(i-1))<range_threshold

then光束step(i)和step(i-1)照射到的是同一个反光柱

将(step(i),range)赋值到当前第k个landmark子列表中

number＝number+1；

else

ifnumber＝1，

then将(k,α,ρ)写入measure_landmark_list，

其中α＝(step-1)·resolution+min_angle，ρ＝range+r；

else

提取当前landmark子列表中的反光最强点的两组数据(step1,range1)和(step2,range2)，计算：

ifrange1<range2

else

endif

endif

新建第k+1个landmark子列表；

number＝0；

将(step(i),range)赋值到新的landmark子列表中，

number＝number+1

endif

endfor

其中number_data是反光强点的个数，resolution是相邻两道激光光束的夹角(分辨率)，range_threshold是判定是否照射到同一个反光柱的距离阈值，α和ρ分别代表在激光传感器相对自身的极坐标系下，该反光柱的角度和距离，measure_landmark_list为照射到反光柱的列表。

或者：仅根据反射光束角度的连续性，判断是否为同一个反光柱，一种可能的实现方式如下：

对于data表中所有的反光强点：

如果第i个反光强点的序号step(i)等于第i-1个反光强点的序号step(i-1)+1，那么光束step(i)和step(i-1)照射到的是同一个反光柱，则将(step(i),range)赋值到当前第k个landmark子列表中，且该子列表中光束数据的组数number＝number+1；

否则，照射到的是一个新的反光柱，则当前第k个landmark子列表结束；

如果number＝1，则将(k,α,ρ)写入measure_landmark_list，其中α＝(step-1)·resolution+min_angle，ρ＝range+r；min_angle为激光传感器扫描角度的最小值；

如果number>＝2，则提取当前landmark子列表中的反光最强点的两组数据(step1,range1)和(step2,range2)，计算：

如果range1<range2

否则

同时，新建第k+1个landmark子列表，组数初始化为number＝0；将(step(i),range)赋值到新的landmark子列表中，number＝number+1；继续读取数据；

最终得到所照射到的反光柱列表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中；

对应的伪代码如下：

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

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

θ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*yk

zl＝xl+i*yl

遍历所有组合后，得到：

实施例1

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

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

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

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

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