CN107289946A - 一种基于双反光板的高精度激光定位与导航方法 - Google Patents

一种基于双反光板的高精度激光定位与导航方法 Download PDF

Info

Publication number
CN107289946A
CN107289946A CN201710571647.3A CN201710571647A CN107289946A CN 107289946 A CN107289946 A CN 107289946A CN 201710571647 A CN201710571647 A CN 201710571647A CN 107289946 A CN107289946 A CN 107289946A
Authority
CN
China
Prior art keywords
mrow
msub
mtd
mtr
centerdot
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201710571647.3A
Other languages
English (en)
Other versions
CN107289946B (zh
Inventor
林志赟
邢浩
金智
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhejiang Titanium Robot Ltd By Share Ltd
Original Assignee
Zhejiang Titanium Robot Ltd By Share Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Zhejiang Titanium Robot Ltd By Share Ltd filed Critical Zhejiang Titanium Robot Ltd By Share Ltd
Priority to CN201710571647.3A priority Critical patent/CN107289946B/zh
Publication of CN107289946A publication Critical patent/CN107289946A/zh
Application granted granted Critical
Publication of CN107289946B publication Critical patent/CN107289946B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/46—Indirect determination of position data
    • G01S17/48—Active triangulation systems, i.e. using the transmission and reflection of electromagnetic waves other than radio waves

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Electromagnetism (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Medicines That Contain Protein Lipid Enzymes And Other Medicines (AREA)
  • Optical Radar Systems And Details Thereof (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

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

Description

一种基于双反光板的高精度激光定位与导航方法
技术领域
本发明属于智能仓储物流领域，具体涉及一种基于双反光板的高精度激光定位与导航方法。
背景技术
基于激光传感器的定位与导航技术是工业AGV、智能机器人等领域的关键性技术，相比传统有轨导航方式，该技术具有定位精度高、灵活多变等优点，适用于复杂、高动态的工业场景中。国内外现有基于反光板的激光定位技术，均采用了几何三角定位原理，每时每刻必须要求同时检测到三块反光板才可实现定位。这一数量要求实际上在复杂工业场景中很难满足，极大地制约了激光定位与导航技术的实用性。
发明内容
为了解决上述问题，本发明提供一种新型激光定位与导航方法，通过提取和利用激光测距信息中的复数域信息，将三块反光板的最低要求降低为两块，有效解决了反光板最低数量的技术瓶颈，大大提高了激光导航技术的适用性。
本发明的目的是通过以下技术方案来实现的：一种基于双反光板的高精度激光定位与导航方法，该方法包括：
(1)在工业环境中布置反光板，预设反光板世界坐标，生成反光板坐标列表；
(2)安装在移动平台上的激光传感器向周围呈辐射状发射激光，并接收反射激光；
(3)筛选来源于反光板的有效光束：预设强度阈值σ，通过检测反射激光强度I，并与σf(d)相比较，判定激光照射物是反光板还是普通环境物体，其中f(d)与距离d负相关；
(4)确定当前时刻照射到的反光板数量及其相对激光传感器的相对坐标：根据反射光束角度的连续性，判断是否为同一块反光板，或者，根据反射光束角度和距离的连续性，判断是否为同一块反光板；根据属于同一反光板的反射光束得到该反光板相对于激光传感器的相对坐标，并存入反光板列表；
(5)初始化反光板列表，得到至少两个反光板的世界坐标：人工确定初始位置对应的反光板列表中至少两个反光板的世界坐标；或者，激光传感器在初始位置获取至少三个反光板返回的角度和距离，计算两两反光板之间的距离，与根据反光板坐标列表生成的反光板距离信息相匹配，得到至少两个反光板的世界坐标；
(6)在动态过程中计算期望反光板列表：根据上一时刻对当前时刻激光传感器位置和角度的预测，估计激光传感器与所有反光板之间的相对距离和角度，并存入期望反光板列表；
(7)在动态过程中反光板列表的匹配：计算当前时刻反光板列表与期望反光板列表中同一个反光板对应的距离之差和角度之差，当距离之差和角度之差均满足预设阈值时，匹配成功；
(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)激光传感器位置滤波：首先根据移动平台运动学模型进行位姿预测，然后根据上一时刻预测的位姿，并结合当前时刻的激光传感器测量数据，对当前时刻的位置进行滤波，从而得到最终的移动平台及激光传感器在世界坐标系下的准确的位置与姿态。
进一步地，所述步骤(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为本发明方法整体流程图。
具体实施方式
下面结合附图，通过实施例对本发明作进一步地说明。
本发明提供的一种基于双反光板的高精度激光定位与导航方法，该方法包括：
(1)在工业环境中布置反光板，预设反光板世界坐标，生成反光板坐标列表；
(2)安装在移动平台上的激光传感器向周围呈辐射状发射激光，并接收反射激光(激光传感器安装在AGV、工业车辆等移动平台上)；
(3)筛选来源于反光板的有效光束：
预设强度阈值σ，通过检测反射激光强度I，并与σf(d)相比较，判定激光照射物是反光板还是普通环境物体，其中f(d)与距离d负相关，优选地，可设置f(d)＝1/d；一种可能的实现方式如下：对于所有接收到的反射光束，如果第step个激光束的激光强度intensity(step)大于预设强度阈值intensity_threshold/激光测量的距离distance(step)，则将(step,intensity(step),distance(step))写入激光强点的数据列表data中；对应的伪代码如下：
for step＝1:number_laser
if intensity(step)>intensity_threshold/distance(step)
将(step,intensity(step),distance(step))写入data表中
endif
endfor
(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},使得|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中；
对应的伪代码如下：
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,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的真实位置与姿态。本发明的整体流程如图3所示。

Claims (4)

1.一种基于双反光板的高精度激光定位与导航方法，其特征在于，该方法包括：
(1)在工业环境中布置反光板，预设反光板世界坐标，生成反光板坐标列表；
(2)安装在移动平台上的激光传感器向周围呈辐射状发射激光，并接收反射激光；
(3)筛选来源于反光板的有效光束：预设强度阈值σ，通过检测反射激光强度I，并与σf(d)相比较，判定激光照射物是反光板还是普通环境物体，其中f(d)与距离d负相关；
(4)确定当前时刻照射到的反光板数量及其相对激光传感器的相对坐标：根据反射光束角度的连续性，判断是否为同一块反光板，或者，根据反射光束角度和距离的连续性，判断是否为同一块反光板；根据属于同一反光板的反射光束得到该反光板相对于激光传感器的相对坐标，并存入反光板列表；
(5)初始化反光板列表，得到至少两个反光板的世界坐标：人工确定初始位置对应的反光板列表中至少两个反光板的世界坐标；或者，激光传感器在初始位置获取至少三个反光板返回的角度和距离，计算两两反光板之间的距离，与根据反光板坐标列表生成的反光板距离信息相匹配，得到至少两个反光板的世界坐标；
(6)在动态过程中计算期望反光板列表：根据上一时刻对当前时刻激光传感器位置和角度的预测，估计激光传感器与所有反光板之间的相对距离和角度，并存入期望反光板列表；
(7)在动态过程中反光板列表的匹配：计算当前时刻反光板列表与期望反光板列表中同一个反光板对应的距离之差和角度之差，当距离之差和角度之差均满足预设阈值时，匹配成功；
(8)基于双反光板数据的激光传感器位姿计算：利用激光传感器测量数据的复频域信息，在匹配成功的反光板中任选两块：第l个和第k个，并计算：
<mrow> <msub> <mi>w</mi> <mi>k</mi> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <msub> <mi>&amp;rho;</mi> <mi>k</mi> </msub> </mfrac> <msup> <mi>e</mi> <mrow> <mo>-</mo> <mi>i</mi> <mo>*</mo> <msub> <mi>&amp;alpha;</mi> <mi>k</mi> </msub> </mrow> </msup> </mrow>
<mrow> <msub> <mi>w</mi> <mi>l</mi> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <msub> <mi>&amp;rho;</mi> <mi>l</mi> </msub> </mfrac> <msup> <mi>e</mi> <mrow> <mi>i</mi> <mo>*</mo> <mrow> <mo>(</mo> <mi>&amp;pi;</mi> <mo>-</mo> <msub> <mi>&amp;alpha;</mi> <mi>l</mi> </msub> <mo>)</mo> </mrow> </mrow> </msup> </mrow>
zk＝Xk+i*Yk
zl＝Xl+i*Yl
<mrow> <msub> <mi>z</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>l</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mrow> <msub> <msup> <mi>w</mi> <mn>2</mn> </msup> <mi>k</mi> </msub> </mrow> <mrow> <msub> <msup> <mi>w</mi> <mn>2</mn> </msup> <mi>k</mi> </msub> <mo>+</mo> <msub> <msup> <mi>w</mi> <mn>2</mn> </msup> <mi>l</mi> </msub> </mrow> </mfrac> <msub> <mi>z</mi> <mi>k</mi> </msub> <mo>+</mo> <mfrac> <mrow> <msub> <msup> <mi>w</mi> <mn>2</mn> </msup> <mi>l</mi> </msub> </mrow> <mrow> <msub> <msup> <mi>w</mi> <mn>2</mn> </msup> <mi>k</mi> </msub> <mo>+</mo> <msub> <msup> <mi>w</mi> <mn>2</mn> </msup> <mi>l</mi> </msub> </mrow> </mfrac> <msub> <mi>z</mi> <mi>l</mi> </msub> </mrow>
<mrow> <msub> <mi>&amp;theta;</mi> <mi>k</mi> </msub> <mo>=</mo> <mi>arg</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <msub> <mi>z</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>z</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>l</mi> </mrow> </msub> </mrow> <msub> <mi>&amp;rho;</mi> <mi>k</mi> </msub> </mfrac> <mo>&amp;CenterDot;</mo> <msup> <mi>e</mi> <mrow> <mi>i</mi> <mo>&amp;CenterDot;</mo> <msub> <mi>&amp;alpha;</mi> <mi>k</mi> </msub> </mrow> </msup> <mo>)</mo> </mrow> </mrow>
其中，下标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)中，根据移动平台运动学模型进行位姿预测，公式如下：
<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;omega;</mi> <mi>L</mi> </msub> <mo>&amp;CenterDot;</mo> <mi>&amp;Delta;</mi> <mi>T</mi> <mo>=</mo> <mn>2</mn> <mi>&amp;pi;</mi> <mo>&amp;CenterDot;</mo> <mi>r</mi> <mi>e</mi> <mi>s</mi> <mi>o</mi> <mi>l</mi> <mi>u</mi> <mi>t</mi> <mi>i</mi> <mi>o</mi> <mi>n</mi> <mo>&amp;CenterDot;</mo> <mfrac> <mn>1</mn> <mrow> <mi>s</mi> <mi>p</mi> <mi>e</mi> <mi>e</mi> <mi>d</mi> <mo>_</mo> <mi>r</mi> <mi>e</mi> <mi>d</mi> <mi>u</mi> <mi>c</mi> <mi>t</mi> <mi>i</mi> <mi>o</mi> <mi>n</mi> <mo>_</mo> <mi>r</mi> <mi>a</mi> <mi>t</mi> <mi>i</mi> <mi>o</mi> </mrow> </mfrac> <mrow> <mo>(</mo> <mrow> <mi>c</mi> <mi>o</mi> <mi>u</mi> <mi>n</mi> <mi>t</mi> <mo>_</mo> <mi>l</mi> <mi>e</mi> <mi>f</mi> <mi>t</mi> <mo>-</mo> <mi>l</mi> <mi>a</mi> <mi>s</mi> <mi>t</mi> <mo>_</mo> <mi>c</mi> <mi>o</mi> <mi>u</mi> <mi>n</mi> <mi>t</mi> <mo>_</mo> <mi>l</mi> <mi>e</mi> <mi>f</mi> <mi>t</mi> </mrow> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;omega;</mi> <mi>R</mi> </msub> <mo>&amp;CenterDot;</mo> <mi>&amp;Delta;</mi> <mi>T</mi> <mo>=</mo> <mn>2</mn> <mi>&amp;pi;</mi> <mo>&amp;CenterDot;</mo> <mi>r</mi> <mi>e</mi> <mi>s</mi> <mi>o</mi> <mi>l</mi> <mi>u</mi> <mi>t</mi> <mi>i</mi> <mi>o</mi> <mi>n</mi> <mo>&amp;CenterDot;</mo> <mfrac> <mn>1</mn> <mrow> <mi>s</mi> <mi>p</mi> <mi>e</mi> <mi>e</mi> <mi>d</mi> <mo>_</mo> <mi>r</mi> <mi>e</mi> <mi>d</mi> <mi>u</mi> <mi>c</mi> <mi>t</mi> <mi>i</mi> <mi>o</mi> <mi>n</mi> <mo>_</mo> <mi>r</mi> <mi>a</mi> <mi>t</mi> <mi>i</mi> <mi>o</mi> </mrow> </mfrac> <mrow> <mo>(</mo> <mrow> <mi>c</mi> <mi>o</mi> <mi>u</mi> <mi>n</mi> <mi>t</mi> <mo>_</mo> <mi>r</mi> <mi>i</mi> <mi>g</mi> <mi>h</mi> <mi>t</mi> <mo>-</mo> <mi>l</mi> <mi>a</mi> <mi>s</mi> <mi>t</mi> <mo>_</mo> <mi>c</mi> <mi>o</mi> <mi>u</mi> <mi>n</mi> <mi>t</mi> <mo>_</mo> <mi>r</mi> <mi>i</mi> <mi>g</mi> <mi>h</mi> <mi>t</mi> </mrow> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced>
<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>v</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>&amp;CenterDot;</mo> <mi>&amp;Delta;</mi> <mi>T</mi> <mo>=</mo> <mfrac> <mi>r</mi> <mn>2</mn> </mfrac> <mrow> <mo>(</mo> <msub> <mi>&amp;omega;</mi> <mi>R</mi> </msub> <mo>&amp;CenterDot;</mo> <mi>&amp;Delta;</mi> <mi>T</mi> <mo>+</mo> <msub> <mi>&amp;omega;</mi> <mi>L</mi> </msub> <mo>&amp;CenterDot;</mo> <mi>&amp;Delta;</mi> <mi>T</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;omega;</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>&amp;CenterDot;</mo> <mi>&amp;Delta;</mi> <mi>T</mi> <mo>=</mo> <mfrac> <mi>r</mi> <mi>L</mi> </mfrac> <mrow> <mo>(</mo> <msub> <mi>&amp;omega;</mi> <mi>R</mi> </msub> <mo>&amp;CenterDot;</mo> <mi>&amp;Delta;</mi> <mi>T</mi> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mi>L</mi> </msub> <mo>&amp;CenterDot;</mo> <mi>&amp;Delta;</mi> <mi>T</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced>
<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;Delta;x</mi> <mi>c</mi> </msub> <mo>=</mo> <mi>&amp;Delta;</mi> <mi>T</mi> <mo>&amp;CenterDot;</mo> <msub> <mi>v</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>&amp;CenterDot;</mo> <msub> <mi>cos&amp;theta;</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;Delta;y</mi> <mi>c</mi> </msub> <mo>=</mo> <mi>&amp;Delta;</mi> <mi>T</mi> <mo>&amp;CenterDot;</mo> <msub> <mi>v</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>&amp;CenterDot;</mo> <msub> <mi>sin&amp;theta;</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;Delta;&amp;theta;</mi> <mi>c</mi> </msub> <mo>=</mo> <mi>&amp;Delta;</mi> <mi>T</mi> <mo>&amp;CenterDot;</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced>
θt|t-1＝θt-1|t-1+Δθc
<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;theta;</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>&amp;theta;</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mn>2</mn> <mi>&amp;pi;</mi> </mrow> </mtd> <mtd> <mrow> <mo>(</mo> <mrow> <msub> <mi>&amp;theta;</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>&gt;</mo> <mi>&amp;pi;</mi> </mrow> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;theta;</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>&amp;theta;</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mo>(</mo> <mrow> <mo>-</mo> <mi>&amp;pi;</mi> <mo>&lt;</mo> <msub> <mi>&amp;theta;</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>&lt;</mo> <mi>&amp;pi;</mi> </mrow> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;theta;</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>&amp;theta;</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <mn>2</mn> <mi>&amp;pi;</mi> </mrow> </mtd> <mtd> <mrow> <mo>(</mo> <mrow> <msub> <mi>&amp;theta;</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>&lt;</mo> <mo>-</mo> <mi>&amp;pi;</mi> </mrow> <mo>)</mo> </mrow> </mtd> </mtr> </mtable> </mfenced>
<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>&amp;Delta;</mi> <mi>x</mi> <mo>=</mo> <msub> <mi>&amp;Delta;x</mi> <mi>c</mi> </msub> <mo>+</mo> <mi>d</mi> <mo>*</mo> <msub> <mi>cos&amp;theta;</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mi>d</mi> <mo>*</mo> <msub> <mi>cos&amp;theta;</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&amp;Delta;</mi> <mi>y</mi> <mo>=</mo> <msub> <mi>&amp;Delta;y</mi> <mi>c</mi> </msub> <mo>+</mo> <mi>d</mi> <mo>*</mo> <msub> <mi>sin&amp;theta;</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mi>d</mi> <mo>*</mo> <msub> <mi>sin&amp;theta;</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced>
<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>x</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>x</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <mi>&amp;Delta;</mi> <mi>x</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>y</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>y</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <mi>&amp;Delta;</mi> <mi>y</mi> </mrow> </mtd> </mtr> </mtable> </mfenced>
其中，输入量为：上一时刻的位姿(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，采用线性卡尔曼滤波的公式如下：
<mrow> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>x</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>y</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow> 2
<mrow> <msub> <mover> <mi>P</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>P</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <mi>Q</mi> </mrow>
<mrow> <msub> <mi>H</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>Re</mi> <mrow> <mo>(</mo> <msub> <mi>w</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>w</mi> <mi>l</mi> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mi>Im</mi> <mrow> <mo>(</mo> <msub> <mi>w</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>w</mi> <mi>l</mi> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>Im</mi> <mrow> <mo>(</mo> <msub> <mi>w</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>w</mi> <mi>l</mi> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mi>Re</mi> <mrow> <mo>(</mo> <msub> <mi>w</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>w</mi> <mi>l</mi> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>
<mrow> <msub> <mi>K</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>P</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>&amp;CenterDot;</mo> <msubsup> <mi>H</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> <mi>T</mi> </msubsup> <mo>&amp;CenterDot;</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>H</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> </msub> <mo>&amp;CenterDot;</mo> <msub> <mover> <mi>P</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>&amp;CenterDot;</mo> <msubsup> <mi>H</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> <mi>T</mi> </msubsup> <mo>+</mo> <mi>R</mi> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> </mrow>
<mrow> <msub> <mi>Z</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>l</mi> </mrow> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mi>Re</mi> <mo>(</mo> <msub> <mi>w</mi> <mi>k</mi> </msub> <mo>&amp;CenterDot;</mo> <msub> <mi>z</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>w</mi> <mi>l</mi> </msub> <mo>&amp;CenterDot;</mo> <msub> <mi>z</mi> <mi>l</mi> </msub> <mo>)</mo> </mtd> </mtr> <mtr> <mtd> <mi>Im</mi> <mo>(</mo> <msub> <mi>w</mi> <mi>k</mi> </msub> <mo>&amp;CenterDot;</mo> <msub> <mi>z</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>w</mi> <mi>l</mi> </msub> <mo>&amp;CenterDot;</mo> <msub> <mi>z</mi> <mi>l</mi> </msub> <mo>)</mo> </mtd> </mtr> </mtable> </mfenced> </mrow>
<mrow> <msub> <mi>X</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>K</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> </msub> <mo>&amp;CenterDot;</mo> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>l</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>H</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> </msub> <mo>&amp;CenterDot;</mo> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mi>P</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>P</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>K</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> </msub> <mo>&amp;CenterDot;</mo> <msub> <mi>H</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> </msub> <mo>&amp;CenterDot;</mo> <msub> <mover> <mi>P</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow>
其中，输入量为：当前时刻预测的位置上一时刻的协方差矩阵Pt-1|t-1，过程噪声的协方差矩阵Q，测量噪声的协方差矩阵R；输出量为：激光传感器在世界坐标系下的坐标当前时刻更新的协方差矩阵Pt|t；
若当前时刻检测到3块及以上的反光板，则采用多块数据融合的思想进行滤波；采用贯序卡尔曼滤波的公式如下：
<mrow> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>x</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>y</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow>
<mrow> <msub> <mover> <mi>P</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>P</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <mi>Q</mi> </mrow>
对于检测到的M个反光板中的任意两两组合，计算：
<mrow> <msub> <mi>&amp;omega;</mi> <mi>k</mi> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <msub> <mi>&amp;rho;</mi> <mi>k</mi> </msub> </mfrac> <msup> <mi>e</mi> <mrow> <mo>-</mo> <mi>i</mi> <mo>*</mo> <msub> <mi>&amp;alpha;</mi> <mi>k</mi> </msub> </mrow> </msup> </mrow>
<mrow> <msub> <mi>&amp;omega;</mi> <mi>l</mi> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <msub> <mi>&amp;rho;</mi> <mi>l</mi> </msub> </mfrac> <msup> <mi>e</mi> <mrow> <mi>i</mi> <mo>*</mo> <mrow> <mo>(</mo> <mi>&amp;pi;</mi> <mo>-</mo> <msub> <mi>&amp;alpha;</mi> <mi>l</mi> </msub> <mo>)</mo> </mrow> </mrow> </msup> </mrow>
zk＝Xk+i*Yk
zl＝Xl+i*Yl
<mrow> <msub> <mi>H</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>Re</mi> <mrow> <mo>(</mo> <msub> <mi>w</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>w</mi> <mi>l</mi> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mi>Im</mi> <mrow> <mo>(</mo> <msub> <mi>w</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>w</mi> <mi>l</mi> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>Im</mi> <mrow> <mo>(</mo> <msub> <mi>w</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>w</mi> <mi>l</mi> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mi>Re</mi> <mrow> <mo>(</mo> <msub> <mi>w</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>w</mi> <mi>l</mi> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>
<mrow> <msub> <mi>K</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>P</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>&amp;CenterDot;</mo> <msubsup> <mi>H</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> <mi>T</mi> </msubsup> <mo>&amp;CenterDot;</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>H</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> </msub> <mo>&amp;CenterDot;</mo> <msub> <mover> <mi>P</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>&amp;CenterDot;</mo> <msubsup> <mi>H</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> <mi>T</mi> </msubsup> <mo>+</mo> <mi>R</mi> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> </mrow>
<mrow> <msub> <mi>Z</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>l</mi> </mrow> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mi>Re</mi> <mo>(</mo> <msub> <mi>w</mi> <mi>k</mi> </msub> <mo>&amp;CenterDot;</mo> <msub> <mi>z</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>w</mi> <mi>l</mi> </msub> <mo>&amp;CenterDot;</mo> <msub> <mi>z</mi> <mi>l</mi> </msub> <mo>)</mo> </mtd> </mtr> <mtr> <mtd> <mi>Im</mi> <mo>(</mo> <msub> <mi>w</mi> <mi>k</mi> </msub> <mo>&amp;CenterDot;</mo> <msub> <mi>z</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>w</mi> <mi>l</mi> </msub> <mo>&amp;CenterDot;</mo> <msub> <mi>z</mi> <mi>l</mi> </msub> <mo>)</mo> </mtd> </mtr> </mtable> </mfenced> </mrow>
<mrow> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>K</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> </msub> <mo>&amp;CenterDot;</mo> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>l</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>H</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> </msub> <mo>&amp;CenterDot;</mo> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mover> <mi>P</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>P</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>K</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> </msub> <mo>&amp;CenterDot;</mo> <msub> <mi>H</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> </msub> <mo>&amp;CenterDot;</mo> <msub> <mover> <mi>P</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow>
遍历所有组合后，得到：
<mrow> <msub> <mi>X</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow>
<mrow> <msub> <mi>P</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>P</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>.</mo> </mrow> 4
CN201710571647.3A 2017-07-13 2017-07-13 一种基于双反光板的高精度激光定位与导航方法 Active CN107289946B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710571647.3A CN107289946B (zh) 2017-07-13 2017-07-13 一种基于双反光板的高精度激光定位与导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710571647.3A CN107289946B (zh) 2017-07-13 2017-07-13 一种基于双反光板的高精度激光定位与导航方法

Publications (2)

Publication Number Publication Date
CN107289946A true CN107289946A (zh) 2017-10-24
CN107289946B CN107289946B (zh) 2019-10-22

Family

ID=60100359

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710571647.3A Active CN107289946B (zh) 2017-07-13 2017-07-13 一种基于双反光板的高精度激光定位与导航方法

Country Status (1)

Country Link
CN (1) CN107289946B (zh)

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107816944A (zh) * 2017-10-26 2018-03-20 重庆华渝电气集团有限公司 平面激光定位测量装置及定位方法
CN108775864A (zh) * 2018-04-10 2018-11-09 深圳普智联科机器人技术有限公司 一种基于反光板的激光标定系统及标定方法
CN109358616A (zh) * 2018-09-12 2019-02-19 黄海宁 一种对自动导航物体位置坐标及方向偏差的校正方法
CN109613550A (zh) * 2018-12-28 2019-04-12 芜湖哈特机器人产业技术研究院有限公司 一种基于反光板的激光雷达地图构建及定位方法
CN109631919A (zh) * 2018-12-28 2019-04-16 芜湖哈特机器人产业技术研究院有限公司 一种融合反光板和占据栅格的混合导航地图构建方法
CN109885049A (zh) * 2019-02-12 2019-06-14 北京航空航天大学 一种基于航位推算的激光导引agv自动建图和路径匹配方法
CN110031817A (zh) * 2018-12-28 2019-07-19 芜湖哈特机器人产业技术研究院有限公司 一种激光雷达反光板的快速匹配方法
CN110471072A (zh) * 2019-08-19 2019-11-19 华晟（青岛）智能装备科技有限公司 一种反光柱位置辨识方法及系统
CN115220012A (zh) * 2022-09-20 2022-10-21 成都睿芯行科技有限公司 一种基于反光板定位方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH10132560A (ja) * 1996-10-31 1998-05-22 Komatsu Ltd 位置計測方法及びその装置
CN202988597U (zh) * 2012-12-11 2013-06-12 重庆起重机厂有限责任公司 一种起重机多点自动定位系统
CN205080152U (zh) * 2015-11-03 2016-03-09 王小波 一种测量机动车速度的装置
CN105446334A (zh) * 2015-11-13 2016-03-30 上海诺力智能科技有限公司 一种引导车导航系统及一种引导车导航方法
CN106091924A (zh) * 2016-05-30 2016-11-09 深圳普智联科机器人技术有限公司 一种使用激光校正贴码误差的系统和方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH10132560A (ja) * 1996-10-31 1998-05-22 Komatsu Ltd 位置計測方法及びその装置
CN202988597U (zh) * 2012-12-11 2013-06-12 重庆起重机厂有限责任公司 一种起重机多点自动定位系统
CN205080152U (zh) * 2015-11-03 2016-03-09 王小波 一种测量机动车速度的装置
CN105446334A (zh) * 2015-11-13 2016-03-30 上海诺力智能科技有限公司 一种引导车导航系统及一种引导车导航方法
CN106091924A (zh) * 2016-05-30 2016-11-09 深圳普智联科机器人技术有限公司 一种使用激光校正贴码误差的系统和方法

Cited By (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107816944A (zh) * 2017-10-26 2018-03-20 重庆华渝电气集团有限公司 平面激光定位测量装置及定位方法
CN107816944B (zh) * 2017-10-26 2019-11-08 重庆华渝电气集团有限公司 平面激光定位测量装置及定位方法
CN108775864A (zh) * 2018-04-10 2018-11-09 深圳普智联科机器人技术有限公司 一种基于反光板的激光标定系统及标定方法
CN109358616A (zh) * 2018-09-12 2019-02-19 黄海宁 一种对自动导航物体位置坐标及方向偏差的校正方法
CN110031817A (zh) * 2018-12-28 2019-07-19 芜湖哈特机器人产业技术研究院有限公司 一种激光雷达反光板的快速匹配方法
CN109631919A (zh) * 2018-12-28 2019-04-16 芜湖哈特机器人产业技术研究院有限公司 一种融合反光板和占据栅格的混合导航地图构建方法
CN109613550A (zh) * 2018-12-28 2019-04-12 芜湖哈特机器人产业技术研究院有限公司 一种基于反光板的激光雷达地图构建及定位方法
CN110031817B (zh) * 2018-12-28 2022-05-27 芜湖哈特机器人产业技术研究院有限公司 一种激光雷达反光板的快速匹配方法
CN109631919B (zh) * 2018-12-28 2022-09-30 芜湖哈特机器人产业技术研究院有限公司 一种融合反光板和占据栅格的混合导航地图构建方法
CN109613550B (zh) * 2018-12-28 2023-04-07 芜湖哈特机器人产业技术研究院有限公司 一种基于反光板的激光雷达地图构建及定位方法
CN109885049A (zh) * 2019-02-12 2019-06-14 北京航空航天大学 一种基于航位推算的激光导引agv自动建图和路径匹配方法
CN109885049B (zh) * 2019-02-12 2021-06-04 北京航空航天大学 一种基于航位推算的激光导引agv自动建图和路径匹配方法
CN110471072A (zh) * 2019-08-19 2019-11-19 华晟（青岛）智能装备科技有限公司 一种反光柱位置辨识方法及系统
CN115220012A (zh) * 2022-09-20 2022-10-21 成都睿芯行科技有限公司 一种基于反光板定位方法

Also Published As

Publication number Publication date
CN107289946B (zh) 2019-10-22

Similar Documents

Publication Publication Date Title
CN107289946A (zh) 一种基于双反光板的高精度激光定位与导航方法
CN107356931B (zh) 一种基于滤波的双反光板激光定位与导航方法
CN107144855A (zh) 一种基于双反光板的激光定位与导航方法
CN107390227B (zh) 一种基于数据筛选的双反光板激光定位与导航方法
CN107144853A (zh) 一种基于数据筛选的双反光柱激光定位与导航方法
CN107144854A (zh) 一种基于双反光柱的激光定位与导航方法
CN112083725B (zh) 一种面向自动驾驶车辆的结构共用多传感器融合定位系统
CN106123908B (zh) 车辆导航方法和系统
CN106226780B (zh) 基于激光扫描雷达的多旋翼室内定位系统及实现方法
CN110208842A (zh) 一种车联网环境下车辆高精度定位方法
CN105404844A (zh) 一种基于多线激光雷达的道路边界检测方法
CN107632308A (zh) 一种基于递归叠加算法的车辆前方障碍物轮廓检测方法
CN104749598B (zh) 一种产生gnss掩星路径的方法
CN103617328A (zh) 一种飞机三维姿态解算方法
CN108508471A (zh) 一种无人驾驶车辆定位方法及装置
CN107346025B (zh) 一种基于滤波的双反光柱激光定位与导航方法
CN103369466A (zh) 一种地图匹配辅助室内定位方法
CN113238072B (zh) 一种适用于车载光电平台的运动目标解算方法
CN107167323A (zh) 无人驾驶汽车的场地测试方法及系统、设备与可读介质
CN107238841A (zh) 一种基于双反光柱的高精度激光定位与导航方法
CN113554705B (zh) 一种变化场景下的激光雷达鲁棒定位方法
CN107463871A (zh) 一种基于角特征加权的点云匹配方法
CN110442014B (zh) 一种基于位置的移动机器人rfid伺服方法
CN115112103B (zh) 一种ladcp与组合惯性导航系统联合观测系统及方法
CN111279216B (zh) 对停车行定向的检测

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant