[发明专利]移动机器人空间位姿点云校正方法及系统

权利要求书

1.一种移动机器人空间位姿点云校正方法，其特征在于：

包括以下步骤：

步骤一：建立障碍物检测模型，定位误差点区域，确定校正对象；

步骤二：基于误差点区域，利用三维空间位姿数据算法获得障碍物三维空间位姿数据；

步骤三：基于改进后的EM算法估算出人为校正因子参数；

步骤四：基于人为校正因子参数，利用可视化地图编辑工具进行地图修正；

步骤二具体包括以下步骤：

S201，基于卷积神经网络对相机获取的图像数据进行二维目标检测；

S202，对点云数据进行数据过滤，数据过滤函数为式(3)：

d_max＝d_avg+λ×S (3)

d_avg表示每一个点云与邻近的点云之间距离的平均值；d_max表示点云之间的最大距离值；λ是标准偏差系数，S为标准偏差，用来表示点云分布的分散程度；

S203，在利用图像数据获取对应的雷达数据时，利用投影矩阵，提取二维范围框所对应的降噪之后的点云数据Q；

S204，将点云数据Q进行旋转矩阵变换，得到旋转后的点云数据Q′，变换后的数据样本的数量N是根据每次旋转角度α来设定的，旋转后的点云数据Q′计算公式为式(4)：

N＝π/α

通过调整范围框长宽比例，增加范围框覆盖区域；

S205，通过点云网络结构获取障碍物的三维空间位姿数据，用[x,y,z]来表示三维空间位姿数据，经过转换网络将点云数据进行多维度提升处理，获得点云的多维数据，经过池化，将点云的多维数据维度降低到6维度，用g＝[x,y,z,l,w,h]来表示障碍物空间位姿数据，其中x,y,z,l,w,h分别表示点云网络结构识别出的障碍物物体质心坐标以及障碍物长宽高信息；

步骤三具体包括以下步骤，

S301，定义s表示障碍物空间位姿数据g＝[x,y,z,l,w,h]中缺失的位姿数据，完全数据表示为(g,s)，参数β表示未知参数，参数β表示人为输入的位姿值，取值范围是在实数范围内取值；p(g,β|(g,s))表示的是完全数据下，空间位姿数据概率密度函数；通过概率公式推导出基于完全数据的似然函数为式(5)：

S302,在障碍物空间位姿数据g和参数β估计值已知的前提下，将对数似然函数进行期望求解；

S303,基于所述期望，进行参数β的求解，获得能够使得似然函数期望值最大的β，能够使得期望值最大的β值是校正因子。

2.根据权利要求1所述的一种移动机器人空间位姿点云校正方法，其特征在于：

步骤一具体包括以下步骤：

S101，基于栅格地图建模方法，定义地图上机器人的初始位置坐标为(x_rot,y_rot)，定义探测的障碍物的方向向量基于方向向量障碍物相对于机器人的角度值为θ，障碍物与机器人点位之间的距离d，通过增加d值探测障碍物，当检测到方向向量上存在障碍物，则计算障碍物的观测值平面位姿数据，其中障碍物的二维坐标信息计算公式为式(1)：

(x_obstacle,y_obstacle)表示障碍物的二维坐标信息；调整θ角度，其中0°≤θ≤90°，更新方向向量调整距离d，探测其他障碍物点位的二维坐标信息；

S102，基于读取的障碍物的点云数据、机器人的位姿和机器人的速度信息，得到占据栅格地图，占据栅格地图的每一个单元格都存在一个占据可能性P，占据可能性P的取值范围是0≤P≤100，设定了空闲单元和占用单元边界值为P_b＝65，其中单元格的单元状态判定标准公式为式(2)：

占用单元表示当前单元格为障碍物占据状态，空闲单元表示当前单元格为自由状态；

S103，在一个单元格的地图区域，当障碍物被检测到后，进行计数统计，统计机器人观测到的障碍物的二维坐标信息和障碍物数量；当检测到的障碍物数量达到设定的阈值，表示所述地图区域特征点分布明显；相反，如果检测到的障碍物数量低于设定的阈值，则表示所述地图区域特征点分布稀疏，所述地图区域确定为误差点区域，校正对象是误差点区域所包含的点云数据。