[发明专利]一种建图定位方法、系统、终端及介质

权利要求书

1.一种建图定位方法，其特征在于，包括：

基于BIM和CAD地图，提取激光雷达高度栅格占用信息，形成占用栅格地图；

提取BIM三维信息，形成3D点云地图；

同时加载占用栅格地图和3D点云地图，作为导航地图；

设立事件触发机制，根据实际环境中的情况，选择基于导航地图的定位策略，进行定位；

定位成功后，根据导航地图与实际环境的匹配情况，实时插入新增的激光雷达数据进入导航地图，消除实际环境和理论BIM之间的误差；

其中：

所述根据实际环境中的情况，选择基于导航地图的定位策略，进行定位，包括：

首先采用占用栅格地图进行定位，根据激光雷达打到的障碍物信息配合占用栅格地图中占用和非占用状态，估计目前位置概率，根据激光点在占用栅格上的数量，判断此时的定位是否准确；

在位置周围对状态变化量x、y、yaw进行非线性优化，其中，x、y、yaw分别代表平面上的x轴坐标、y轴坐标以及航向角；将更多的激光点打到占用栅格上，得到概率最大的状态变量state：

p(state|input，laser_observer，occupy_map，before_state)

其中，state表示状态变量(x，y，yaw)，input代表控制输入即轮式里程计信息，laser_observer表示激光雷达的观测值，occupy_map表示占用栅格图输入，before_state表示上一时刻的状态变量；

当机体和水平面的夹角θ大于设定的角度阈值angle_threshold时，采用3D点云地图进行定位，将激光点云数据在先验位置匹配3D点云地图中的点云，通过非线性优化，得到重投影误差最小的状态变量state′：

p′(state′|input，laser_observe，point_cloud，before_state′)

其中，state′表示状态变量(x，y，yaw，θ)，input代表控制输入即轮式里程计信息，laser_observer表示激光雷达的观测值，point_cloud表示3D点云输入，before_state′表示上一时刻的状态变量；

当机体和水平面的夹角θ小于设定的角度阈值angle_threshold时，重新采用占用栅格地图定位；

其中：在采用3D点云地图进行定位的过程中，所述非线性优化，采用固定z轴数值，对其他自由度上的状态进行优化的方式。

2.根据权利要求1所述的建图定位方法，其特征在于，所述提取激光雷达高度栅格占用信息，包括：

对BIM的尺寸进行固定，按照一定比例尺对激光雷达高度的水平面进行切片，之后将每个像素灰度化，删除色彩信息；设定灰度阈值，小于等于阈值则认定为非占用栅格，大于阈值则认定为占用栅格，包括：

将BIM按照比例尺进行缩放，并且根据比例尺算出每个像素代表的长度；

将彩色图像按照一定参数转为灰度图像，得到：

Gray＝(R*param1+G*param2+B*param3+param4)/1000；

其中，Gray表示图像灰度，R、G、B表示构成原图像的红绿蓝三通道值，param表示这三通道的权重；

对得到的灰度图像进行归一化，确立灰度阈值threshold，当Gray＞threshold时，认为这是一个障碍物，标记为用于代表墙面的占用栅格，否则认为是非占用栅格；

对灰度图像中的像素进行合并，以1合n个像素为合并原则，对一个区域内的所有像素进行合并处理，使其变为一个像素；进行N次合并后，建立N+1层金字塔地图，即为具有多层结构的占用栅格地图；所述占用栅格地图中，未合并像素和/或每个像素代表的实际距离小于等于设定距离阈值的部分属于高精度地图部分，每个像素代表的实际距离大于设定距离阈值的部分属于低精度地图部分，其中，高精度地图部分用于定位，低精度地图部分用于路径规划。