CN115435775A - 基于拓展卡尔曼滤波的多传感器融合slam方法 - Google Patents
基于拓展卡尔曼滤波的多传感器融合slam方法 Download PDFInfo
- Publication number
- CN115435775A CN115435775A CN202211163166.6A CN202211163166A CN115435775A CN 115435775 A CN115435775 A CN 115435775A CN 202211163166 A CN202211163166 A CN 202211163166A CN 115435775 A CN115435775 A CN 115435775A
- Authority
- CN
- China
- Prior art keywords
- pose
- time
- laser radar
- data
- sensor
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 42
- 238000001914 filtration Methods 0.000 title claims abstract description 36
- 230000004927 fusion Effects 0.000 title claims abstract description 28
- 239000011159 matrix material Substances 0.000 claims description 54
- 238000005457 optimization Methods 0.000 claims description 17
- 238000006073 displacement reaction Methods 0.000 claims description 15
- 230000001133 acceleration Effects 0.000 claims description 12
- 238000001514 detection method Methods 0.000 claims description 10
- 230000008569 process Effects 0.000 claims description 10
- 238000005259 measurement Methods 0.000 claims description 8
- 230000009466 transformation Effects 0.000 claims description 8
- 238000007781 pre-processing Methods 0.000 claims description 6
- 238000013519 translation Methods 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 4
- 230000010354 integration Effects 0.000 claims description 4
- 238000007476 Maximum Likelihood Methods 0.000 claims description 3
- 230000003416 augmentation Effects 0.000 claims description 3
- 238000007499 fusion processing Methods 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 238000009434 installation Methods 0.000 claims description 3
- 238000012886 linear function Methods 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 238000004422 calculation algorithm Methods 0.000 abstract description 9
- 238000004088 simulation Methods 0.000 abstract description 5
- 230000000875 corresponding effect Effects 0.000 description 8
- 238000010276 construction Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000013507 mapping Methods 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 206010035148 Plague Diseases 0.000 description 1
- 241000607479 Yersinia pestis Species 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 230000002596 correlated effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000012804 iterative process Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Images
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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
-
- 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
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10032—Satellite or aerial image; Remote sensing
- G06T2207/10044—Radar image
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30244—Camera pose
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Multimedia (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种基于拓展卡尔曼滤波的多传感器融合SLAM方法，针对走廊相似场景下，激光SLAM算法由于观测数据的相似性，算法性能将严重劣化，甚至完全失效的问题，本发明将里程计和IMU的数据进行预处理，通过拓展卡尔曼滤波将两者的位姿信息融合，作为激光雷达扫描匹配更精确的迭代初始位姿；为验证本算法的性能，在Melodic版本的ROS(Robot Operating System)搭建了Gazebo仿真实验环境，通过仿真实验对比，验证了算法的鲁棒性和有效性。
Description
技术领域
本发明涉及无人驾驶汽车和移动机器人在未知环境中运行时定位导航与地图构建技术领域，具体涉及一种基于拓展卡尔曼滤波的多传感器融合SLAM(同时定位和建图)方法。
背景技术
无人驾驶技术和辅助驾驶技术都离不开SLAM的发展，同时SLAM也是实现自动驾驶的关键一环。当地图和机器人的位置都事先未知时，在这种情况下，要求机器人在一个完全未知的环境中从一个未知的位置出发，在递增地建立环境的导航地图同时,利用已建立的地图来同步刷新自身的位置。在上述问题中,机器人位置和地图两者的估算是高度相关的，任何一方都无法独立获取。这样一种相辅相生、不断迭代的过程，简称为SLAM问题。
尽管SLAM近些年取得了巨大的进步，但有一些问题仍然困扰着无人驾驶界，SLAM是一个估算问题，考虑到环境的复杂性和传感器测量的不确定性，目前还没有一个比较完整的解决方案。通常的SLAM方法都是局限于用单一传感器收集环境信息，而使用单一的传感器容易出现各种问题，达不到智能驾驶的要求。
发明内容
本发明目的在于针对环境特征高度相似场景容易出现SLAM匹配失效的问题以及单一传感器建图精度不高等问题，提出了一种基于拓展卡尔曼滤波的多传感器融合SLAM(同时定位和建图)方法。
本发明属于无人驾驶汽车在未知环境中运行时定位导航与地图构建领域，具体为一种基于拓展卡尔曼滤波的多传感器融合SLAM(Simultaneous Localization andMapping)方法，本发明为了解决SLAM(同步定位与地图创建)过程中单一传感器容易出现的定位精度不高、激光点云匹配计算量大、轨迹闭合效果差、位姿累积误差大、以及各环节传感器观测数据利用不充分等问题。提出了一种基于拓展卡尔曼滤波的惯性里程计(IMU)、轮速计、单线激光雷达多传感器信息融合智能车SLAM方法。针对走廊相似场景下，激光SLAM算法由于观测数据的相似性，算法性能将严重劣化，甚至完全失效的问题，本发明将里程计和IMU的数据进行预处理，通过拓展卡尔曼滤波将两者的位姿信息融合，作为激光雷达扫描匹配更精确的迭代初始位姿；为验证本算法的性能，在Melodic版本的ROS(Robot OperatingSystem)搭建了Gazebo仿真实验环境，通过仿真实验对比，验证了算法的鲁棒性和有效性。
为实现上述目的，本发明采取如下技术方案：
一种基于拓展卡尔曼滤波的多传感器融合SLAM方法，其特征在于，包括以下步骤：
步骤S1：读取IMU、轮速计的数据信息并进行预处理；
步骤S2：采用拓展卡尔曼滤波融合IMU、轮速计预处理的位姿得到融合位姿；
步骤S3：激光雷达数据预处理，将融合后的位姿为激光雷达数据做线性插值去运动畸变；
步骤S4：将融合处理之后的位姿作为激光雷达扫描匹配的迭代初始位姿，开始SLAM前端建立局部子图；
步骤S5：对于构建完成的子图判断是否形成回环，进行SLAM后端回环检测以减少累计误差，并将当前时刻对应的位姿约束加入到非线性优化中；
步骤S6：当没有新的激光雷达Scan产生，输出全局优化后的轨迹位姿，并将产生的全局地图可视化。
进一步地，步骤S1具体包括：
对惯性测量单元IMU预积分求得惯性里程计；
IMU每个时刻进行自身3轴加速度和角速度的测量，角速度
与加速度
的测量模型分别为：
其中
为实际角速度，bg是角速度随时间缓慢变化的偏差bias,ηg是角速度白噪声，
为旋转矩阵，aw为实际加速度，gw表示重力矢量，w表示世界坐标系作为惯性系，ba是加速度随时间变化的偏差，ηa是加速度白噪声；
运动方程为：
公式中∧表示反对称矩阵；
在区间[t,t+Δt]里进行欧拉积分有：
计算轮速计数据求得轮边里程计信息：
通过轮速计得到的两轮角速度和两轮线速度求解横向位移、纵向位移和航向角；
根据运动模型计算出小车的速度v和角速度ω：
公式vL、vR分别表示左右轮的线速度，d为轮胎离底盘中心的距离；
对时间积分得到里程信息：
θ(t)＝∫ω(t)dt
x(t)＝∫v(t)cos(θ(t))dt
y(t)＝∫v(t)sin(θ(t))dt
式中θ(t)为航行角，x(t)为横轴位移，y(t)为纵轴位移；
进行位姿数据融合前时间上和空间上的统一；
以激光雷达发布的时间为基准进行时间上的统一：当每一帧激光传感器数据到来时，各传感器的时间取比基准时间小且离的最近的时间作为初始时间，当下一帧数据到来时，取比基准时间大且最近的时间作为终止时间，计算这两个时刻的时间差作为积分时间；
将得到的数据进行坐标系的变换以得到空间上的统一：根据各传感器的安装位姿，利用Eigen库进行坐标系的转换，得到变换的平移矩阵和旋转矩阵，再将其他传感器坐标系统一到小车的基座标系，W为基座标系，C为需要对齐的坐标系，X、Y、Z分别为空间坐标系下的坐标；
变换公式为：
为了方便计算改写成增广矩阵：
公式中Rc为3*3的旋转矩阵，Tc为3*1的平移矩阵：
公式中RX(α)为绕X轴旋转α角度的旋转矩阵，公式中RY(β)为绕Y轴旋转β角度的旋转矩阵，公式中RZ(θ)为绕Z轴旋转θ角度的旋转矩阵。
进一步地，步骤S2当中：
所述拓展卡尔曼滤波是标准卡尔曼滤波EKF在非线性情形下的一种扩展形式，即，将非线性函数通过一阶泰勒展开近似线性函数，然后采用卡尔曼滤波框架得出准确的下一时刻状态真值；
记无人车在各时刻的状态为x1,x2....,xk，表示为xk＝[x,y,z,pitch,roll,yaw]T，其中k是离散时间下标，x,y,z表示在空间中的位置，roll,pitch,yaw分别表示绕x,y,z轴的旋转；采用运动方程和观测方程来描述状态估计问题：
xt＝g(xt-1,ut)+εt
zt＝h(xt)+δt
公式中：g为运动方程；u为输入；εt输入噪声；h为观测方程；x为观测数据,δt为观测噪声；
标准卡尔曼滤波EKF在工作点附近，对非线性系统进行线性近似化：
g(xt-1,ut)在上一状态估计的最优值μt-1处取一阶导数得到Gt，h(xt)在当前时刻预测值
处取一阶导数得到Rt；
预测过程：
式中
为期望，
为协方差；
更新过程：
式中K为卡尔曼增益，Ht为转换矩阵，h为观测方程，；u为输入；
多传感器系统模型如下：
y1(k)＝C1x(k)+δ1
yn(k)＝Cnx(k)+δn
多传感器系统的状态方程
只有一个，观测方程y有多个，ε为运动误差，δ为单个传感器观测误差，第一个传感器的观测方程更新后得到系统的状态量x(k)及系统协方差矩阵∑(k)；将二者作为下一个传感器更新过程的系统预测状态量
和系统预测协方差矩阵
进行状态更新；将最后一个传感器更新后得到的系统的状态量x(k)及系统协方差矩阵∑(k)作为融合后输出，并将二者用于预测过程进行下一时刻的迭代，最终得到融合之后的位姿。
进一步地，在步骤S3中：
考虑激光雷达旋转一周所需时间为0.05-0.1S，即激光雷达旋转一周获取一帧数据的时间，故激光雷达产生的第一个激光点与最后一个点之间存在时间间隔，当智能车运动时，激光雷达对于同一个物体的检测距离会发生变化，即激光点所对应的激光雷达坐标系发生了变化；通过多传感器位姿对齐后经过拓展卡尔曼滤波得到各时刻的位姿估计，分别做线性插值将每个激光雷达点对应时刻做坐标变换，以实现去激光雷达运动畸变。
进一步地，在步骤S4中：
SLAM确定位姿的过程是一个非线性最小二乘问题，即将求解位姿最大估计概率问题转化为观测与估计的误差平方和最小；智能驾驶汽车的运动方程和观测方程为：
式中wk为运动噪声，vk，j为观测噪声，xk为k时刻对应的汽车位置，yj表示观测到的路标位置，uk为传感器读数或者输入，zk，j为观测数据；
对于某次观测：zk，j＝h(yj，xk)+vk，j，噪声满足高斯分布：vk～N(0，Qk，j)，一般的高斯分布为：
其中μ为期望，即最大概率的取值，∑-1表示协方差矩阵的逆矩阵；对其取负对数：
故原问题可能取值的最大概率的值问题就等价于该负数问题的最小值问题，即求原问题的最大似然：P(zj，k|xk，yj)＝N(h(yj，xk)，Qk,j)，相当于最小化：
对于最小二乘问题采用裂纹伯格-马夸尔特(L-M)迭代法求解，采用步骤S2求解得到的融合位姿作为求解汽车位姿的激光雷达迭代初始位姿，以减小当前激光雷达数据在子图内扫描匹配的区域和迭代收敛的次数；最后在匹配得分最高的位姿中插入当前激光雷达数据Scan，并做实时车辆位姿轨迹和子图输出。
进一步地，在步骤S5中：
所述回环检测是当前激光雷达数据与已经构建完成的子图进行扫描匹配，将步骤S2得到的估计位姿信息提供给SLAM后端的扫描匹配，以减少需要匹配的子图数量和降低后端优化的时间；
当匹配得分大于设定的阈值时，认为当前回环成功，并将当前位姿约束加入到非线性优化中，通过构建最小二乘问题迭代求解，给匹配进行打分，并且前端匹配是在子图内优化单次误差，后端匹配是在子图之间优化减少累计误差，只要回环一次就做一次全局优化消除累计误差。
进一步地，在步骤S6中，当不再有新的激光雷达扫描数据Scan产生时，将优化后的全局地图输出。
相比于现有技术，本发明及其优选方案所完成的工作主要包括：第一，针对单一激光雷达SLAM过程中扫描匹配耗时并且精度低的缺陷，利用拓展卡尔曼滤波融合了IMU、论速计的位姿信息，为激光雷达扫描匹配提供初始位姿，减少了迭代的次数，加速了计算时间；并且提供了更加精确的初始位姿，避免了迭代发散的问题，使得迭代之后的位姿更加精确。第二，在仿真试验中设计了走廊这种特征高度相似的场景，在这种环境下做回环检测极易出现匹配出错导致优化失效，将融合之后的信息作为位姿约束，加入到SLAM后端非线性优化中，从而减小了累计误差，最后通过实验验证了本发明融合SLAM算法的鲁棒性和有效性。
附图说明
图1为本发明实施例两轮差速底盘的运动学示意图；
图2为本发明实施例坐标变换图；
图3为本发明实施例激光雷达去运动畸变效果图；
图4为本发明实施例Gazebo实验环境示意图；
图5为本发明实施例单一激光雷达SLAM结果图1；
图6为本发明实施例单一激光雷达SLAM结果图2；
图7为本发明实施例方法SLAM结果图1；
图8为本发明实施例方法最终保存的地图；
图9为本发明实施例环境地图和本发明方法SLAM结果图2；
图10为本发明实施例方法总体流程示意图。
具体实施方式
为让本专利的特征和优点能更明显易懂，下文特举实施例，作详细说明如下：
应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步的说明。除非另有指明，本说明书使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。
需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
如图10所示，本实施例提供的方案的步骤和流程如下：
步骤S1：读取IMU、轮速计的数据信息并进行预处理；
包括：对惯性测量单元(IMU)预积分求得惯性里程计；
IMU每个时刻可实现自身3轴加速度和角速度的测量，角速度
与加速度
的测量模型分别为：
其中
为实际角速度，bg是角速度随时间缓慢变化的偏差bias，ηg是角速度白噪声，
为旋转矩阵，aw为实际加速度，gw表示重力矢量，w表示世界坐标系作为惯性系，ba是加速度随时间变化的偏差，ηa是加速度白噪声；
运动方程为：
公式中∧表示反对称矩阵；
在区间[t，t+Δt]里进行欧拉积分有：
vw(t+Δt)＝vw(t)+aw(t)·Δt
计算轮速计数据求得轮边里程计信息；
通过轮速计得到的两轮角速度和两轮线速度求解横向位移、纵向位移和航向角，如图1所示；
根据运动模型计算出小车的速度v和角速度ω：
公式vL、vR分别表示左右轮的线速度，d为轮胎离底盘中心的距离；
对时间积分得到里程信息：
θ(t)＝∫ω(t)dt
x(t)＝∫v(t)cos(θ(t))dt
y(t)＝∫v(t)sin(θ(t))dt
式中θ(t)为航行角，x(t)为横轴位移，y(t)为纵轴位移；
进行位姿数据融合前时间上和空间上的统一；
由于每个传感器发布数据的频率不同，需要将各传感器数据进行时间上和空间的统一，而激光雷达作为建图的核心传感器，故以激光雷达发布的时间为基准进行时间上的统一。
时间上的统一方案：当每一帧激光传感器数据到来时，其他传感器的时间取比这个时间小且离的最近的时间作为初始时间，当下一帧数据到来时，取比这个时间大且最近的时间作为终止时间，计算这两个时刻的时间差作为积分时间。
将得到的数据进行坐标系的变换以得到空间上的统一。根据各传感器的安装位姿，利用Eigen库进行坐标系的转换，得到变换的平移矩阵和旋转矩阵，再将其他传感器坐标系统一到小车的基座标系，如图2所示，W为基座标系，C为需要对齐的坐标系，X、Y、Z分别为空间坐标系下的坐标。
变换公式为：
为了方便计算改写成增广矩阵：
公式中Rc为3*3的旋转矩阵，Tc为3*1的平移矩阵。
公式中RX(α)为绕X轴旋转α角度的旋转矩阵，公式中RY(β)为绕Y轴旋转β角度的旋转矩阵，公式中RZ(θ)为绕Z轴旋转θ角度的旋转矩阵。
步骤S2：拓展卡尔曼滤波融合IMU、轮速计预处理的位姿得到融合位姿；
融合数据问题是一个非线性最小二乘问题(又称最小平方法)，它通过最小化误差的平方和寻找数据的最佳函数匹配。利用最小二乘法可以简便地求得未知的数据，并使得这些求得的数据与实际数据之间误差的平方和为最小。而拓展卡尔曼滤波是标准卡尔曼滤波(EKF)在非线性情形下的一种扩展形式，实质就是将非线性函数通过一阶泰勒展开近似线性函数，然后采用卡尔曼滤波框架得出准确的下一时刻状态真值，其核心思想为估计和观测反馈。
记无人车在各时刻的状态为x1，x2....，xk，表示为xk＝[x，y，z，pitch，roll，yaw]T，其中k是离散时间下标，x，y，z表示在空间中的位置，roll，pitch，yaw分别表示绕x，y，z轴的旋转。在SLAM中，通常要估计机器人的位置，那么系统的状态就指的是机器人的位姿。用运动方程和观测方程来描述状态估计问题：
xt＝g(xt-1，ut)+εt
zt＝h(xt)+δt
公式中：g-运动方程；u-输入；εt输入噪声；h-观测方程；x-观测数据，δt-观测噪声；
运动方程描述了状态xk-1是怎么推导到xk的，而观测方程描述的是从xk是怎么得到观察数据的yk，当有多个传感器数据时，观测方程就有多个，然而这种性质只适用线性高斯系统。EKF的做法是在工作点附近，对非线性系统进行线性近似化：
g(xt-1，ut)在上一状态估计的最优值μt-1处取一阶导数得到Gt，h(xt)在当前时刻预测值
处取一阶导数得到Rt。
预测过程：
式中
为期望，
为协方差；
更新过程：
式中K-卡尔曼增益，Ht-转换矩阵，h-观测方程，；u-输入；
多传感器系统模型如下：
y1(k)＝C1x(k)+δ1
yn(k)＝Cnx(k)+δn
多传感器系统的状态方程
只有一个，观测方程y有多个，ε为运动误差，δ为单个传感器观测误差，第一个传感器的观测方程更新后得到系统的状态量x(k)及系统协方差矩阵∑(k)。将二者作为下一个传感器更新过程的系统预测状态量
和系统预测协方差矩阵
进行状态更新。将最后一个传感器更新后得到的系统的状态量x(k)及系统协方差矩阵∑(k)作为融合后输出，并将二者用于预测过程进行下一时刻的迭代，最终得到融合之后的位姿。
步骤S3：激光雷达数据预处理，将融合后的位姿为激光雷达数据做线性插值去运动畸变；
由于激光雷达旋转一周所需时间为0.05-0.1S(10-20HZ)，即激光雷达旋转一周获取一帧数据的时间，故激光雷达产生的第一个激光点与最后一个点之间存在时间间隔，当智能车运动时，激光雷达对于同一个物体的检测距离会发生变化，即激光点所对应的激光雷达坐标系发生了变化。如果可以将所有的激光点与初始点坐标变化关系求出来，那么就可以解决激光雷达运动畸变问题。多传感器位姿对齐后经过拓展卡尔曼滤波得到各时刻的位姿估计，分别做线性插值将每个激光雷达点对应时刻做坐标变换，即可去激光雷达运动畸变，去运动畸变效果如图3所示。
步骤S4：将融合处理之后的位姿作为激光雷达扫描匹配的迭代初始位姿，开始SLAM前端建立局部子图；
SLAM确定位姿的过程是一个非线性最小二乘问题，即将求解位姿最大估计概率问题转化为观测与估计的误差平方和最小；智能驾驶汽车的运动方程和观测方程为：
式中wk为运动噪声，vk,j为观测噪声，xk为k时刻对应的汽车位置，yj表示观测到的路标位置，uk为传感器读数或者输入，zk,j为观测数据；
对于某次观测：zk,j＝h(yj，xk)+vk,j，噪声满足高斯分布：vk～N(0，Qk，j)，一般的高斯分布为：
其中μ为期望，即最大概率的取值，∑-1表示协方差矩阵的逆矩阵；对其取负对数：
故原问题可能取值的最大概率的值问题就等价于该负数问题的最小值问题，即求原问题的最大似然：P(zj，k|xk，yj)＝N(h(yj，xk)，Qk，j)，相当于最小化：
对于最小二乘问题采用裂纹伯格-马夸尔特(L-M)迭代法求解，本算法采用步骤S2求解得到的融合位姿作为求解汽车位姿的激光雷达迭代初始位姿，从而减小了当前激光雷达数据在子图内扫描匹配的区域和迭代收敛的次数，加速了迭代效率，为获得更精确的位姿提供了指引。最后在匹配得分最高的位姿中插入当前激光雷达数据Scan，并做实时车辆位姿轨迹和子图输出。
步骤S5：对于构建完成的子图判断是否形成回环，进行SLAM后端回环检测以减少累计误差，并将这时刻对应的位姿约束加入到非线性优化中；
回环检测是当前激光雷达数据与已经构建完成的子图进行扫描匹配，同时本算法将S2得到的估计位姿信息提供给SLAM后端的扫描匹配，从而减少了需要匹配的子图数量，大大降低了后端优化的时间。当匹配得分大于设定的阈值时，认为当前回环成功，并将当前位姿约束加入到非线性优化中，此时的优化与S4相似，依然是构建最小二乘问题迭代求解，不同的是需要给匹配进行打分，并且前端匹配是在子图内优化单次误差，后端匹配是在子图之间优化减少累计误差，只要回环一次就做一次全局优化消除一些累计误差。
步骤S6：当没有新的激光雷达Scan产生，输出全局优化后的轨迹位姿，并将产生的全局地图可视化；
当不再有新的激光雷达扫描数据Scan产生时，算法开始将优化后的全局地图输出。
本实施例在Ubuntu18.04+ROS Melodic版本的Gazebo环境中进行实验验证，图4为特征高度相似走廊实验环境，该环境为50x10(米)的回环走廊，图5、6为单一激光雷达的SLAM结果，从图片中可以看出在环境特征高度形似场景中单一的激光雷达匹配出现问题，最后导致地图的畸变甚至重叠；图7、8、9为本发明方法的实验结果，由图可以看出，本发明的方法融合了多传感器位姿信息后减少了匹配出错的问题，并大幅度减少了位姿误差，在特征高度相似场景下鲁棒性强，至此改善了单一激光雷达对于长走廊等高度相似场景的SLAM失效问题，通过多次实验验证了本方法的有效性。
以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例。但是凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。
本专利不局限于上述最佳实施方式，任何人在本专利的启示下都可以得出其它各种形式的基于拓展卡尔曼滤波的多传感器融合SLAM方法，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本专利的涵盖范围。
Claims (7)
1.一种基于拓展卡尔曼滤波的多传感器融合SLAM方法，其特征在于，包括以下步骤：
步骤S1：读取IMU、轮速计的数据信息并进行预处理；
步骤S2：采用拓展卡尔曼滤波融合IMU、轮速计预处理的位姿得到融合位姿；
步骤S3：激光雷达数据预处理，将融合后的位姿为激光雷达数据做线性插值去运动畸变；
步骤S4：将融合处理之后的位姿作为激光雷达扫描匹配的迭代初始位姿，开始SLAM前端建立局部子图；
步骤S5：对于构建完成的子图判断是否形成回环，进行SLAM后端回环检测以减少累计误差，并将当前时刻对应的位姿约束加入到非线性优化中；
步骤S6：当没有新的激光雷达Scan产生，输出全局优化后的轨迹位姿，并将产生的全局地图可视化。
2.根据权利要求1所述的基于拓展卡尔曼滤波的多传感器融合SLAM方法，其特征在于：步骤S1具体包括：
对惯性测量单元IMU预积分求得惯性里程计；
IMU每个时刻进行自身3轴加速度和角速度的测量，角速度
与加速度
的测量模型分别为：
其中
为实际角速度，bg是角速度随时间缓慢变化的偏差bias,ηg是角速度白噪声，
为旋转矩阵，aw为实际加速度，gw表示重力矢量，w表示世界坐标系作为惯性系，ba是加速度随时间变化的偏差，ηa是加速度白噪声；
运动方程为：
公式中∧表示反对称矩阵；
在区间[t,t+Δt]里进行欧拉积分有：
vw(t+Δt)＝vw(t)+aw(t)·Δt
计算轮速计数据求得轮边里程计信息：
通过轮速计得到的两轮角速度和两轮线速度求解横向位移、纵向位移和航向角；
根据运动模型计算出小车的速度v和角速度ω：
公式vL、vR分别表示左右轮的线速度，d为轮胎离底盘中心的距离；
对时间积分得到里程信息：
θ(t)＝∫ω(t)dt
x(t)＝∫v(t)cos(θ(t))dt
y(t)＝∫v(t)sin(θ(t))dt
式中θ(t)为航行角，x(t)为横轴位移，y(t)为纵轴位移；
进行位姿数据融合前时间上和空间上的统一；
以激光雷达发布的时间为基准进行时间上的统一：当每一帧激光传感器数据到来时，各传感器的时间取比基准时间小且离的最近的时间作为初始时间，当下一帧数据到来时，取比基准时间大且最近的时间作为终止时间，计算这两个时刻的时间差作为积分时间；
将得到的数据进行坐标系的变换以得到空间上的统一：根据各传感器的安装位姿，利用Eigen库进行坐标系的转换，得到变换的平移矩阵和旋转矩阵，再将其他传感器坐标系统一到小车的基座标系，W为基座标系，C为需要对齐的坐标系，X、Y、Z分别为空间坐标系下的坐标；
变换公式为：
为了方便计算改写成增广矩阵：
公式中Rc为3*3的旋转矩阵，Tc为3*1的平移矩阵：
公式中RX(α)为绕X轴旋转α角度的旋转矩阵，公式中RY(β)为绕Y轴旋转β角度的旋转矩阵，公式中RZ(θ)为绕Z轴旋转θ角度的旋转矩阵。
3.根据权利要求2所述的基于拓展卡尔曼滤波的多传感器融合SLAM方法，其特征在于：步骤S2当中：
所述拓展卡尔曼滤波是标准卡尔曼滤波EKF在非线性情形下的一种扩展形式，即，将非线性函数通过一阶泰勒展开近似线性函数，然后采用卡尔曼滤波框架得出准确的下一时刻状态真值；
记无人车在各时刻的状态为x1,x2....,xk，表示为xk＝[x,y,z,pitch,roll,yaw]T，其中k是离散时间下标，x,y,z表示在空间中的位置，roll,pitch,yaw分别表示绕x,y,z轴的旋转；采用运动方程和观测方程来描述状态估计问题：
xt＝g(xt-1,ut)+εt
zt＝h(xt)+δt
公式中：g为运动方程；u为输入；εt输入噪声；h为观测方程；x为观测数据,δt为观测噪声；
标准卡尔曼滤波EKF在工作点附近，对非线性系统进行线性近似化：
g(xt-1,ut)在上一状态估计的最优值μt-1处取一阶导数得到Gt，h(xt)在当前时刻预测值
处取一阶导数得到Rt；
预测过程：
式中
为期望，
为协方差；
更新过程：
式中K为卡尔曼增益，Ht为转换矩阵，h为观测方程，；u为输入；
多传感器系统模型如下：
y1(k)＝C1x(k)+δ1
yn(k)＝Cnx(k)+δn
多传感器系统的状态方程
只有一个，观测方程y有多个，ε为运动误差，δ为单个传感器观测误差，第一个传感器的观测方程更新后得到系统的状态量x(k)及系统协方差矩阵Σ(k)；将二者作为下一个传感器更新过程的系统预测状态量
和系统预测协方差矩阵
进行状态更新；将最后一个传感器更新后得到的系统的状态量x(k)及系统协方差矩阵Σ(k)作为融合后输出，并将二者用于预测过程进行下一时刻的迭代，最终得到融合之后的位姿。
4.根据权利要求3所述的基于拓展卡尔曼滤波的多传感器融合SLAM方法，其特征在于：在步骤S3中：
考虑激光雷达旋转一周所需时间为0.05-0.1S，即激光雷达旋转一周获取一帧数据的时间，故激光雷达产生的第一个激光点与最后一个点之间存在时间间隔，当智能车运动时，激光雷达对于同一个物体的检测距离会发生变化，即激光点所对应的激光雷达坐标系发生了变化；通过多传感器位姿对齐后经过拓展卡尔曼滤波得到各时刻的位姿估计，分别做线性插值将每个激光雷达点对应时刻做坐标变换，以实现去激光雷达运动畸变。
5.根据权利要求4所述的基于拓展卡尔曼滤波的多传感器融合SLAM方法，其特征在于：在步骤S4中：
SLAM确定位姿的过程是一个非线性最小二乘问题，即将求解位姿最大估计概率问题转化为观测与估计的误差平方和最小；智能驾驶汽车的运动方程和观测方程为：
式中wk为运动噪声，vk,j为观测噪声，xk为k时刻对应的汽车位置，yj表示观测到的路标位置，uk为传感器读数或者输入，zk,j为观测数据；
对于某次观测：zk,j＝h(yj,xk)+vk,j，噪声满足高斯分布：vk～N(0,Qk,j)，一般的高斯分布为：
其中μ为期望，即最大概率的取值，∑-1表示协方差矩阵的逆矩阵；对其取负对数：
故原问题可能取值的最大概率的值问题就等价于该负数问题的最小值问题，即求原问题的最大似然：P(zj,k|xk,yj)＝N(h(yj,xk),Qk,j)，相当于最小化：
对于最小二乘问题采用裂纹伯格-马夸尔特(L-M)迭代法求解，采用步骤S2求解得到的融合位姿作为求解汽车位姿的激光雷达迭代初始位姿，以减小当前激光雷达数据在子图内扫描匹配的区域和迭代收敛的次数；最后在匹配得分最高的位姿中插入当前激光雷达数据Scan，并做实时车辆位姿轨迹和子图输出。
6.根据权利要求5所述的基于拓展卡尔曼滤波的多传感器融合SLAM方法，其特征在于：在步骤S5中：
所述回环检测是当前激光雷达数据与已经构建完成的子图进行扫描匹配，将步骤S2得到的估计位姿信息提供给SLAM后端的扫描匹配，以减少需要匹配的子图数量和降低后端优化的时间；
当匹配得分大于设定的阈值时，认为当前回环成功，并将当前位姿约束加入到非线性优化中，通过构建最小二乘问题迭代求解，给匹配进行打分，并且前端匹配是在子图内优化单次误差，后端匹配是在子图之间优化减少累计误差，只要回环一次就做一次全局优化消除累计误差。
7.根据权利要求6所述的基于拓展卡尔曼滤波的多传感器融合SLAM方法，其特征在于：在步骤S6中，当不再有新的激光雷达扫描数据Scan产生时，将优化后的全局地图输出。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211163166.6A CN115435775A (zh) | 2022-09-23 | 2022-09-23 | 基于拓展卡尔曼滤波的多传感器融合slam方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211163166.6A CN115435775A (zh) | 2022-09-23 | 2022-09-23 | 基于拓展卡尔曼滤波的多传感器融合slam方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115435775A true CN115435775A (zh) | 2022-12-06 |
Family
ID=84248702
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211163166.6A Pending CN115435775A (zh) | 2022-09-23 | 2022-09-23 | 基于拓展卡尔曼滤波的多传感器融合slam方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115435775A (zh) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116541663A (zh) * | 2023-06-21 | 2023-08-04 | 四川信息职业技术学院 | 一种基于卡尔曼滤波提高双套站传感器数据质量的方法 |
| CN116878488A (zh) * | 2023-09-07 | 2023-10-13 | 湘江实验室 | 一种建图方法、装置、存储介质及电子装置 |
| CN117848332A (zh) * | 2024-03-07 | 2024-04-09 | 北京理工大学前沿技术研究院 | 一种车载多源融合高精度定位系统的imu噪声消除方法 |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20040073360A1 (en) * | 2002-08-09 | 2004-04-15 | Eric Foxlin | Tracking, auto-calibration, and map-building system |
| US20120086598A1 (en) * | 2010-10-08 | 2012-04-12 | Canadian Space Agency | Apparatus and methods for driftless attitude determination and reliable localization of vehicles |
| WO2017177533A1 (zh) * | 2016-04-12 | 2017-10-19 | 深圳市龙云创新航空科技有限公司 | 基于激光雷达的微型无人机操控方法及系统 |
| CN109186601A (zh) * | 2018-07-05 | 2019-01-11 | 南京理工大学 | 一种基于自适应无迹卡尔曼滤波的激光slam算法 |
| US20190323845A1 (en) * | 2016-11-09 | 2019-10-24 | The Texas A&M University System | Method and System for Accurate Long Term Simultaneous Localization and Mapping with Absolute Orientation Sensing |
| CN112734841A (zh) * | 2020-12-31 | 2021-04-30 | 华南理工大学 | 一种用轮式里程计-imu和单目相机实现定位的方法 |
| US11002859B1 (en) * | 2020-02-27 | 2021-05-11 | Tsinghua University | Intelligent vehicle positioning method based on feature point calibration |
| CN113111443A (zh) * | 2021-05-06 | 2021-07-13 | 江西昊絮智辕科技有限公司 | 智能运输小车及运动模型构建方法 |
| CN114993298A (zh) * | 2022-04-29 | 2022-09-02 | 南京航空航天大学 | 基于ekf的模板匹配vo与轮式里程计融合定位方法 |
| CN115046540A (zh) * | 2022-05-25 | 2022-09-13 | 新驱动重庆智能汽车有限公司 | 一种点云地图构建方法、系统、设备和存储介质 |
-
2022
- 2022-09-23 CN CN202211163166.6A patent/CN115435775A/zh active Pending
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20040073360A1 (en) * | 2002-08-09 | 2004-04-15 | Eric Foxlin | Tracking, auto-calibration, and map-building system |
| US20120086598A1 (en) * | 2010-10-08 | 2012-04-12 | Canadian Space Agency | Apparatus and methods for driftless attitude determination and reliable localization of vehicles |
| WO2017177533A1 (zh) * | 2016-04-12 | 2017-10-19 | 深圳市龙云创新航空科技有限公司 | 基于激光雷达的微型无人机操控方法及系统 |
| US20190323845A1 (en) * | 2016-11-09 | 2019-10-24 | The Texas A&M University System | Method and System for Accurate Long Term Simultaneous Localization and Mapping with Absolute Orientation Sensing |
| CN109186601A (zh) * | 2018-07-05 | 2019-01-11 | 南京理工大学 | 一种基于自适应无迹卡尔曼滤波的激光slam算法 |
| US11002859B1 (en) * | 2020-02-27 | 2021-05-11 | Tsinghua University | Intelligent vehicle positioning method based on feature point calibration |
| CN112734841A (zh) * | 2020-12-31 | 2021-04-30 | 华南理工大学 | 一种用轮式里程计-imu和单目相机实现定位的方法 |
| CN113111443A (zh) * | 2021-05-06 | 2021-07-13 | 江西昊絮智辕科技有限公司 | 智能运输小车及运动模型构建方法 |
| CN114993298A (zh) * | 2022-04-29 | 2022-09-02 | 南京航空航天大学 | 基于ekf的模板匹配vo与轮式里程计融合定位方法 |
| CN115046540A (zh) * | 2022-05-25 | 2022-09-13 | 新驱动重庆智能汽车有限公司 | 一种点云地图构建方法、系统、设备和存储介质 |
Non-Patent Citations (6)
| Title |
|---|
| 刘仕琦等: "基于多传感器融合的视觉SLAM算法研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》, no. 2, 15 February 2021 (2021-02-15), pages 138 - 2309 * |
| 吴佳鑫等: "带有回环约束的无人车多传感器融合SLAM系统", 《中国优秀硕士学位论文全文数据库 信息科技辑》, no. 01, 15 February 2021 (2021-02-15), pages 140 - 653 * |
| 夏卫星等: "基于自适应四参数估计的电罗经航向运动建模", 《舰船科学技术 》, vol. 38, no. 19, 31 October 2016 (2016-10-31), pages 125 - 128 * |
| 胡尊刚: "基于单目视觉的移动机器人SLAM技术与目标检测技术研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》, no. 02, 15 February 2021 (2021-02-15), pages 138 - 936 * |
| 陈光祯等: "《智能无线机器人：人工智能算法与应用》", 31 July 2022, 机械工业出版社, pages: 152 - 156 * |
| 陈靖等: "《增强现实技术及其应用案例》", 31 January 2022, 北京理工大学出版社 , pages: 122 - 126 * |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116541663A (zh) * | 2023-06-21 | 2023-08-04 | 四川信息职业技术学院 | 一种基于卡尔曼滤波提高双套站传感器数据质量的方法 |
| CN116541663B (zh) * | 2023-06-21 | 2023-09-19 | 四川信息职业技术学院 | 一种基于卡尔曼滤波提高双套站传感器数据质量的方法 |
| CN116878488A (zh) * | 2023-09-07 | 2023-10-13 | 湘江实验室 | 一种建图方法、装置、存储介质及电子装置 |
| CN116878488B (zh) * | 2023-09-07 | 2023-11-28 | 湘江实验室 | 一种建图方法、装置、存储介质及电子装置 |
| CN117848332A (zh) * | 2024-03-07 | 2024-04-09 | 北京理工大学前沿技术研究院 | 一种车载多源融合高精度定位系统的imu噪声消除方法 |
| CN117848332B (zh) * | 2024-03-07 | 2024-05-03 | 北京理工大学前沿技术研究院 | 一种车载多源融合高精度定位系统的imu噪声消除方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Qin et al. | Lins: A lidar-inertial state estimator for robust and efficient navigation | |
| CN107063280B (zh) | 一种基于控制采样的智能车辆路径规划系统及方法 | |
| CN109211251B (zh) | 一种基于激光和二维码融合的即时定位与地图构建方法 | |
| CN115435775A (zh) | 基于拓展卡尔曼滤波的多传感器融合slam方法 | |
| CN110160542B (zh) | 车道线的定位方法和装置、存储介质、电子装置 | |
| CN107246876B (zh) | 一种无人驾驶汽车自主定位与地图构建的方法及系统 | |
| CN112083726B (zh) | 一种面向园区自动驾驶的双滤波器融合定位系统 | |
| CN112347840A (zh) | 视觉传感器激光雷达融合无人机定位与建图装置和方法 | |
| Li et al. | Collaborative mapping and autonomous parking for multi-story parking garage | |
| Hasberg et al. | Simultaneous localization and mapping for path-constrained motion | |
| CN112965063B (zh) | 一种机器人建图定位方法 | |
| CN114018248B (zh) | 一种融合码盘和激光雷达的里程计方法与建图方法 | |
| US11158065B2 (en) | Localization of a mobile unit by means of a multi hypothesis kalman filter method | |
| CN111707272A (zh) | 一种地下车库自动驾驶激光定位系统 | |
| Kang et al. | Vins-vehicle: A tightly-coupled vehicle dynamics extension to visual-inertial state estimator | |
| CN114001733B (zh) | 一种基于地图的一致性高效视觉惯性定位算法 | |
| CN113920198B (zh) | 一种基于语义边缘对齐的由粗到精的多传感器融合定位方法 | |
| CN111060099A (zh) | 一种无人驾驶汽车实时定位方法 | |
| CN114323033A (zh) | 基于车道线和特征点的定位方法、设备及自动驾驶车辆 | |
| CN116337045A (zh) | 一种基于karto和teb的高速建图导航方法 | |
| Peng et al. | Vehicle odometry with camera-lidar-IMU information fusion and factor-graph optimization | |
| CN111257853B (zh) | 一种基于imu预积分的自动驾驶系统激光雷达在线标定方法 | |
| CN115577320A (zh) | 一种基于数据插值的多传感器异步数据融合方法 | |
| CN111812669A (zh) | 绕机检查装置及其定位方法、存储介质 | |
| Si et al. | TOM-Odometry: A generalized localization framework based on topological map and odometry |
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 |