1.面向复杂环境下使用雷达、激光雷达、IMU、GPS的自适应多传感器融合方法,其特征在于,包括如下步骤:
S1:通过4D毫米波雷达传感器接收雷达点云,使用雷达点云的多普勒速度初步推断自我速度,用于初步区分单帧中的动态点云和静态点云;
S2:将步骤S1中的动态点云与静态点云分别处理。动态点云采用一种区域生长的基于密度的聚类方法DBSCAN来对动态点云进行二次提取,完成对动态车辆、行人的准确提取。静态点通过随机样本一致性RANSAC从中提取地面点,从而去除地下的噪点得到可用的静态点云;将提取过后的静态点重新进行车辆自速度估计,获得机器人的绝对速度;
S3:通过激光雷达接收到点云数据,采用高频的IMU将步骤S1中处理过后的毫米波雷达与激光雷达的每一帧数据进行对齐,包括时间同步和空间校准;
S4:利用步骤S3中对齐后的毫米波雷达点云数据来辅助激光雷达实时去除每一帧中的动态点,由此可以获得只包含静态点的激光雷达点云数据和静态的毫米波雷达点云数据;
S5:对步骤S4中静态的激光雷达点云数据进行几何特征提取,如果特征点数量突然减少并维持在一个较低的数量中,则判断为机器人进入了退化区域;
S6:通过激光雷达点云的特征点数量来选择激光雷达或者毫米波雷达点云数据进行扫描到子地图的匹配。采用迭代误差状态卡尔曼滤波器将激光雷达或者毫米波雷达与IMU进行紧密耦合;在每次迭代中,计算当前位姿变量并用于导出下一次迭代的状态向量和协方差矩阵,持续进行直到收敛;
S7:通过GPS获取机器人的位姿和速度,利用S3中毫米波雷达估计的速度与S6中前端里程计得到的位姿来判断GPS的数据的可靠性,计算两者的残差来确定GPS数据在后端优化中的权重,并加入到因子图中与前端里程计共同优化。
2.如权利要求1所述的方法,其特征在于,步骤S2中,动态点云与静态点云分别处理的具体方法为:为了进一步细化动态点云的提取,采用了基于密度的空间聚类算法DBSCAN,根据周围数据点的密度来识别聚类,有效地滤除噪声点并丢弃多普勒信息不一致的低密度点云聚类;对于初步提取的静态点,先对其进行地面点提取,进而判断地面点以下的点云为毫米波雷达产生的噪声点,将其归为噪点;因此,经过毫米波雷达点云的预处理,将毫米波雷达点云R分为静态点RS,动态点RD和噪声点RN。
3.如权利要求1所述的方法,其特征在于,步骤S3中,激光雷达和毫米波雷达数据进行对齐的具体方法为:在激光雷达当前帧之前寻找时间戳最近的毫米波雷达,然后,通过IMU预积分计算这两帧之间的位姿变换,最后,将位姿变换作用到毫米波雷达的点云上,完成传感器的数据对齐。
4.如权利要求4所述的方法,其特征在于,步骤S4中,通过毫米波雷达辅助激光雷达去除动态障碍物的具体方法为:
首先,输入是一帧毫米波雷达点云Pr激光雷达点云Pl,其中,这两帧点云已经完成了数据对齐;其次,将两帧的数据都投影到xy平面,并将数据存入kd-tree中以加快搜索;最后,根据直接计算的欧式距离,将特定距离∈以内的激光雷达点云被认为是动态点;通过这种方法,得到去除动态点后的激光雷达点云集Lfiltered:其中,其中d(·)表示欧几里得距离。Li和Rj分别表示激光雷达和毫米波雷达点云序列。
5.如权利要求1所述的方法,其特征在于,步骤S5中,激光雷达点云数据几何特征的提取方法具体为:
首先,对于每个激光雷达点云数据帧,计算局部区域内的曲率,曲率的大小反映了该点相对于其邻域的几何结构复杂度;其次,根据计算出的曲率值,设定阈值,将曲率超过阈值的点提取为特征点,当机器人进入退化区域时,环境中的显著几何结构变少,曲率大的特征点数量迅速下降;最后,当检测到特征点数量突然大幅减少,并且这一数量持续维持在一个较低的水平,推断机器人进入了一个退化区域,激光雷达数据不可用,采用毫米波雷达点云数据进行点云配准来维持里程计。
6.如权利要求1所述的方法,其特征在于,步骤S6中,将激光雷达和毫米波雷达数据与IMU紧密耦合具体步骤为:将机器人状态表示为x,第i个IMU采样时刻的状态表示为:其中Ri表示SO(3)上的旋转矩阵,pi和vi分别表示位置和速度;bω和ba分别对应于来自IMU的陀螺仪和加速度计的偏差;此外,g表示世界坐标系中的重力矢量,需要对其进行估计;误差状态可以在工作点附近的切线空间中表示为δx∈R18;接收到高频IMU数据后,首先通过前向传播估计机器人的当前状态,得到误差状态和协方差在确定点云数据后,采用基于ikd-tree的扫描到子图匹配,这有利于实时计算;当判断车辆处在隧道、烟雾、高动态等复杂环境中,提前构建毫米波雷达子地图,并采用毫米波雷达来替代激光雷达进行扫描到子地图的匹配;同样,当激光雷达点云密度和特征点数量恢复到正常状态后,提前构建激光雷达子地图,并由原来的毫米波雷达惯性系统回退到激光雷达惯性系统;采用迭代误差状态卡尔曼滤波器IESKF方法迭代更新观测模型;在每次迭代中,计算当前变量δx并用于导出下一次迭代的状态向量x和协方差矩阵P,持续进行直到收敛;本质上,每次迭代都涉及求解具有先验的最小二乘问题:其中,其中k表示第j次激光雷达扫描的结束时间,表示状态预测误差,表示传播协方差,是观测残差,表示当前状态估计与观测数据之间的误差;Rj是第j个点的噪声协方差。
7.如权利要求1所述的方法,其特征在于,步骤S7中,GPS自适应权重分配可靠性判断以及图优化的具体方法为:
采用两帧间的位姿变和速度作为检验一致性的标准,其中,位姿变化可以来自前端前端里程计,速度来自采用过滤后的毫米波雷达静态点云进行自我速度估计,因此,将残差ek表示为:如果GPS状态正常,ek应该是零均值的高斯分布,否则是有偏差的。决策变量λk定义为由相关协方差矩阵A加权的残差平方和:
当离群值测试函数小于Tmin时代表GPS可以完全信任,此时后端优化可以更加信任GPS数据来降低累积误差;当测试函数大于Tmax时代表GPS数据不可用,此时后端优化将直接去除GPS因子,排除异常值的影响;当λk处在中间阈值时,认定为GPS数据存在质疑;如果长期未加入GPS数据会使得GPS数据与前端里程计数据位姿有一定的偏差,直接加入GPS因子会导致里程计跳变甚至系统崩溃;因此,中间阈值的GPS数据可以在优化过程中起到过渡作用,这样能够保证里程计平稳的融合变化,逐渐向GPS数据靠近或者远离;当GPS位于不确定区域时,采用一个平滑函数ρk进行过渡,所得到的用于成本函数而不是最小化残差平方和:其中r(·)表示传感器测量因子的残差。这种方法允许逐步整合GPS数据,使GPS数据对整个优化过程起到一个平滑变化,从而保持整个系统的稳定性。