1.一种考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法,其特征在于,包括:步骤1,通过激光点云数据的几何特性,提取符合预设曲率特征的点云作为特征点,建立同一特征点在两帧相邻时刻点云数据中的匹配关系,并构建代价函数,以轮式机器人位姿为变量构造ICP问题,获得轮式机器人位姿信息;
步骤2,根据步骤1获得的轮式机器人的位姿信息,检测候选动态障碍物;
步骤3,根据步骤2检测得到的候选动态障碍物,估计动态障碍物状态;
步骤1中,构建代价函数,以轮式机器人位姿为变量构造ICP问题,获得轮式机器人位姿信息的方法具体包括:步骤b1,考虑其水平方向平移及偏航方向旋转对应的二维变换矩阵 根据对应点云的距离关系,建立代价函数,将位姿解算问题转化成一个非线性最小二乘的求解问题,如下式(5):其中, 表示最优变换矩阵估计值, 表示Pk中第p个特征点在雷达坐标系中去除z轴维度后的坐标,q表示特征点的数量, 为 在Pk‑1中的去除z轴维度后的齐次坐标, 表示 在Pk中的去除z轴维度后的齐次坐标;
步骤b2,定义第k帧特征点的质心在雷达坐标系中去除z轴维度后的坐标 以及第k‑1帧特征点的质心在雷达坐标系中去除z轴维度后的坐标步骤b3,将去质心坐标定义为 将上述问题(5)解耦,转化为式(6)表示的问题:
式中, 表示最优的轮式机器人偏航方向旋转矩阵估计值, 表示单次迭代的旋转矩阵, 表示最优的水平方向平移估计值;
步骤b4,通过SVD分解求出上述问题(6)中最优 定义矩阵W为式(7):W为一个2×2矩阵,对其进行SVD分解,可得式(8):T
W=UΣV (8)其中,Σ为奇异值对角矩阵,U和V为正交矩阵;
当W满秩时, 表示为式(9):
若计算得到的 行列式为负,则取 为当前迭代最优值;
步骤b5,按照 得到轮式机器人在x方向和y方向上的平移量。
2.如权利要求1所述的考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法,其特征在于,步骤1中,所述提取符合预设曲率特征的点云作为特征点的方法具体包括:步骤a1,对点云数据进行体素滤波;
步骤b1,利用下式(1)计算点云数据中各点云的曲率值r,并按照曲率值r的大小排序,曲率值最大的点云定义为特征角点,曲率值最小的点云定义为特征平面点:式中,X(k,i)表示第k帧点云数据Pk中的第i个点云在雷达坐标系中的坐标,X(k,j)表示Pk中的第j个点云在雷达坐标系中的坐标,S表示X(k,i)的邻域。
3.如权利要求2所述的考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法,其特征在于,所述特征点为特征角点的情形下,步骤1中,所述建立同一特征点在两帧相邻时刻点云数据中的匹配关系的方法具体包括:在第k‑1帧点云数据Pk‑1中寻找Pk中的第a个特征角点X(k,a)对应的特征线段,该线段上具有点云b和点云c,其中,点云b为Pk‑1中与特征角点X(k,a)距离最近的点云,点云c为Pk‑1中在特征角点X(k,a)相邻扫描线上与特征角点X(k,a)距离最近的点云,点云线距离 表示为下式(2):式中,X(k‑1,b)表示Pk‑1中的第b个特征角点在雷达坐标系中的坐标,X(k‑1,c)表示Pk‑1中的第c个特征角点在雷达坐标系中的坐标;
所述特征点为特征平面点的情形下,步骤1中,所述建立同一特征点在两帧相邻时刻点云数据中的匹配关系的方法具体包括:在Pk‑1中寻找第d个特征平面点X(k,d)所对应的特征面,该特征面具有点云e、点云f和点云g,点云e为Pk‑1中与特征平面点X(k,d)距离最近的点云,点云f为Pk‑1中在点云e同一相邻扫描线上与点云d距离最近的点云,点云g为在相邻扫描线上与特征平面点X(k,d)距离最近的点云,点云面距离 表示为下式(3):式中,X(k‑1,e)表示Pk‑1中的第e个特征平面点在雷达坐标系中的坐标,X(k‑1,f)表示Pk‑1中的第f个特征平面点在雷达坐标系中的坐标,X(k‑1,g)表示Pk‑1中的第g个特征平面点在雷达坐标系中的坐标。
4.如权利要求3所述的考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法,其特征在于,步骤2具体包括:步骤21,根据步骤1输出的轮式机器人位姿信息,提取出可能属于动态障碍物点云;
步骤22,分割步骤21提取出的可能属于动态障碍物点云中的地面点云与非地面点云,去除地面点云,得到非地面点云;
步骤23,将整体的非地面点云按照特定策略聚类为多个点云簇,一个点云簇对应一个障碍物;
步骤24,估计步骤23获得的每个障碍物的位置、形状和尺寸信息;
其中,步骤21具体包括:
步骤211,根据里程计得到的位姿信息,将当前帧点云数据Pk左乘变换矩阵 变换到Pk‑1的坐标系中;
步骤212,根据最近邻关系,Pk与Pk‑1的交集部分确定为静态点云部分;
步骤213,将Pk与Pk‑1的非交集部分的点云进行障碍物检测操作,通过观测多帧数据的方式去除属于静态障碍物的目标。
5.如权利要求4所述的考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法,其特征在于,步骤23具体包括:步骤231,从步骤22获得的非地面点云中取出一个点云作为聚类种子点云,并对其进行栅格邻域搜索,若在栅格邻域半径ρ内存有点云,则将这些点云存入聚类生长点云队列步骤232,遍历 中每个点云,进行半径阈值为ρ的邻域栅格搜索,若邻域内存在点云,则将不属于 的点云存入聚类生长过渡队列 遍历完所有点云后,将属于 中点云存入聚类容器 m为障碍物类别;
步骤233,将 中所有点云存入 重复以上步骤直至某次生长时 为空集,则该类别聚类结束;
重复上述步骤,直至所有点云处理完毕,得到所有障碍物聚类信息 将不满足足够阈值数量的类别进行剔除。
6.如权利要求4所述的考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法,其特征在于,步骤24具体包括:步骤241,将步骤23得到的点云簇的质心定义为该障碍物的坐标;
步骤242,将移除z轴维度后的点云进行随机采样一致性直线分割,将分割出的直线方向作为该障碍物主方向;
步骤243,利用步骤242确定的主方向建立二维坐标系,再根据障碍物所对应点云簇中每个点云的空间信息,计算点云在该坐标系x′、y′两个维度上的极大、极小值,并将极大值与相应的极小值的差值作为障碍物的长和宽。
7.如权利要求4所述的考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法,其特征在于,步骤3具体包括:步骤31,利用候选动态障碍物信息根据多帧数据中障碍物的差异程度计算差异度函数,并建立关联矩阵;
步骤32,根据动态障碍物的关联情况,结合激光里程计计算得到的帧间相对运动,判断障碍物运动状态,从而得到动态障碍物的运动方向及速度。
8.如权利要求7所述的考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法,其特征在于,步骤31具体包括:步骤311,根据第k帧第u个障碍物的位置为 第k‑1帧第v个障碍物位置为障碍物的长和宽分别由 以
及步骤1计算得到的两帧点云数据相对运动为 将差异度函数 定义为下式(10):
其中, 为机器人在二维空间中位置的齐次坐标表现形式,α为距离差异权重,β为长宽比差异权重;
步骤312,Pk‑1中共检测到的障碍物的总数量为m个,记为其中,u=1,2,…,m;Pk中共检测到的障碍物的总数量为n个,记为 其中,v=1,
2,…,n,将关联矩阵 表示为下式(11):
式中, 表示由式(10)计算的差异度;
步骤313,根据步骤312中建立的关联矩阵(11),关联当前帧和上一帧点云数据中的动态障碍物,若最小差异度所对应的Pk‑1中元素的下标u>v,则表明发生障碍物丢失;若最小差异度所对应的Pk‑1中元素的下标u<v,则表明有新动态障碍物出现。
9.如权利要求8所述的考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法,其特征在于,步骤32具体包括:k‑1 k‑1 k‑1 k k k
有步骤31关联的障碍物在Pk‑1中的坐标为t =[x ,y ],Pk中的坐标为t=[x ,y],步骤1计算得到的两帧点云数据相对运动为 则利用式(12)计算障碍物的运动距离:k k‑1
根据激光雷达传感器的帧率f,则动态障碍物的速度为V=Δd×f,方向为v=t‑t ,k k‑1表示t去除z轴维度后的齐次坐标, 表示t 去除z轴维度后的齐次坐标。