[发明专利]一种机器人建图定位方法

权利要求书

1.一种机器人建图定位方法，包括建图定位和地图优化的过程，其特征在于，具体包括：

在初始地图中，以初始位置为中心获取环境里程数据和雷达数据，并通过里程数据对雷达数据进行预处理；该过程包括如下步骤：通过雷达扫描获取点云数据，并结合里程数据进行点云矫正；

对点云数据进行观测，去除点云数据中包含的动态物体生成的点云数据；具体按照如下规则进行：设定一个octomap，对每帧点云数据中的每个激光点在octomap中对应的体素占有概率，其中第n个激光点的体素占有概率以下式进行表达

其中，z₁,…z_T分别为t＝1,…T时刻激光点云的观测数据；

从点云中挑选部分数据作为特征点并留取备用；

利用经过预处理的雷达数据与初始地图进行匹配，将经过预处理的雷达数据更新至初始地图中；

利用环境里程数据与更新后的地图进行变换处理，优化机器人在地图中的位姿，包括对新进入帧与邻近关键帧组成的局部地图进行匹配进行位姿估计，具体按照如下方法进行：

将新进入的关键帧作为观测数据F^t；

选举邻近的若干关键帧组成局部地图Q^t＝{F^t-k,…,F^t-1}；

建立观测数据与局部地图之间的约束关系以得到位姿x_i；基于位姿x_i将每帧点云的激光点转换到预设地图的坐标系下进行地图拼接，更新地图。

2.根据权利要求1所述的机器人建图定位方法，其特征在于，所述的以初始位置为中心获取环境里程数据，包括：

通过传感器模块和编码模块分别获取对应的加速度数据，并按照如下方法进行融合生成里程数据

p_i＝p_i-1+R_i-1·v_i·Δt

其中，其中，p_i为时刻i机器人的位姿，w_i为IMU采集到的机器人的角速度，R_i表示机器人姿态信息的旋转矩阵，v_i为码盘采集到的机器人的速度，Δt采样周期。

3.根据权利要求1所述的机器人建图定位方法，其特征在于，所述的通过雷达扫描获取点云数据，并结合里程数据进行点云矫正，具体按照如下规则进行：

l'_i＝⁰T_i·l_i

其中，一帧点云数据中包括n个激光束，按时间先后顺序每个激光束对应的坐标分别为{l₀,l₁,…l_n-1}，每个激光束同一时刻下对应的里程计姿态为{C₀,C₁,…C_n-1}，⁰T_i为任一位姿C_i相对于C₀的转换矩阵，l'_i为第i个激光束矫正后的坐标。

4.根据权利要求1所述的机器人建图定位方法，其特征在于，对每个观测数据的体素占有概率设置阈值η，体素占有概率小于η的激光点为动态物体，动态物体对应的激光点进行消除处理，并将体素占有概率大于等于η的激光点数据进行保留，保留的激光点的数据用于更新octomap。

5.根据权利要求1所述的机器人建图定位方法，其特征在于，所述的从点云中挑选部分数据作为特征点并留取备用，按照如下规则进行：

其中，i表示当前激光点，s表示周围激光点，r表示激光点的半径。