[发明专利]一种室内动态环境下的语义SLAM服务机器人导航方法

权利要求书

1.一种室内动态环境下的语义SLAM服务机器人导航方法，其特征在于，

该方法包括以下步骤：

步骤（1）：利用服务机器人携带的传感器所获取的信息进行语义分割并提取特征点；

步骤（2）：根据所述步骤（1）中提取的特征点进行移动一致性检查，剔除动态点，再利用稳定的静态特征点进行相机位姿估计；

步骤（3）：根据所述步骤（2）中的静态特征点在构建好的三维点云地图基础上进行语义八叉树地图的构建；

步骤（4）：根据所述步骤（3）中语义八叉树地图，初始化服务机器人自身位姿；

步骤（5）：根据所述步骤（4）中确定的服务机器人位姿信息进行定位的同时，通过结合灰狼优化算法的DWA局部路径规划算法，求解最优速度轨迹，确定服务机器人行驶路径；

步骤（6）：根据所述步骤（5）中的最优速度轨迹，将生成的速度指令发送给服务机器人底盘执行机构，到达目标区域；

步骤（7）：根据所述步骤（6）中的速度指令，服务机器人在到达目标区域过程中若陷入局部障碍区域，则启动恢复模式，使其脱离被困区域，重新导航；

其中，所述步骤（1）包括如下步骤：

步骤（1-1）：利用服务机器人携带的传感器获取原始图像信息，使用PSPNet进行语义分割，得到的语义图像中所有相同类别标记为同一类，将语义图像中每个像素点乘以该像素点对应的语义先验权重，用结果得分的中值代替每个像素点的结果得分，得到语义一致的结果图像:

（1）

其中，j表示图像的编号，在服务机器人导航过程中，传感器连续获取图像，为图像中的语义先验权重，为经过PSPNet分割后的语义图像，为经过PSPNet分割后的结果得分图像；

步骤（1-2）：将语义图像分割为的块, 对于每个块，根据公式（1）计算中值作为该块的被选择的权重，其中，i为块的编号，一幅图像中不止一块，越高，则此块被选择的概率越大，然后在选择的块内根据梯度值进一步提取特征点，若梯度值小于50，则剔除特征点；

所述步骤（4）包括如下步骤：

步骤（4-1）：服务机器人自身搭载有Linux操作系统，服务机器人导航开始时初始化服务机器人位姿使其位于路线起始点半径0.1m范围内；

步骤（4-2）：自动建图导航算法通过传感器观测环境信息，利用贝叶斯估计即粒子滤波方法，获取服务机器人每一时刻的位姿估计：

预测：由时刻的后验概率密度函数计算，得到时刻的先验概率密度函数，

（6）

其中，表示系统t时刻的位姿状态估计，表示系统t-1时刻的位姿状态估计，表示1到t-1时刻的传感器观测环境信息；

更新：由时刻得到的先验概率密度函数，结合环境观测量，针对上一步的观测过程，获取后验概率密度，

（7）

其中，为归一化常数，表示为

（8）

其中，表示t时刻的传感器观测的环境信息，表示1到t-1时刻的传感器观测环境信息，表示系统t时刻的位姿状态估计，表示1到t-1时刻的传感器观测环境信息已知情况下对t时刻的传感器观测的环境信息的估计，表示系统t时刻的位姿状态估计下的传感器观测的环境信息，表示1到t-1时刻的传感器观测环境信息已知情况下系统t时刻的位姿状态估计；

所述步骤（5）包括如下步骤：

步骤（5-1）：在步骤（4-2）完成服务机器人位姿信息估计之后，随着时间的累积，确定服务机器人当前时刻位姿信息，累积之前所有时刻位姿信息，得知当前时刻所处位置，完成定位；

步骤（5-2）：初始化，，狼的位置和目标函数值，利用生成对抗网络对狼群位置进行扩充，初步预测狼群位置，补充局部未知区域样本数不足的缺陷；

步骤（5-3）：计算最终评价函数得分，如果目标函数值小于狼的目标函数值，则将狼的目标函数值更新为最优目标函数值，同时将狼的位置更新为最优位置，如果目标函数值介于狼和狼的目标函数值之间，则将狼的目标函数值更新为最优目标函数值，同时更新狼的位置，如果目标函数值介于狼和狼的目标函数值之间，则将狼的目标函数值更新为最优目标函数值，同时更新狼的位置；

步骤（5-4）：对于三个狼的维度，计算系数和

（9）

（10）

其中，表示随着迭代从2到0线性下降过程中与初始值2产生的差值向量，和为0到1之间的随机向量；

步骤（5-5）：包围猎物，更新，，狼的位置

（11）

（12）

其中，，，分别为，，狼的位置，，，分别表示，，狼与猎物之间的距离，，，和，，分别表示，，狼根据式（9）和（10）求出的随机系数向量，，，分别表示，，狼的移动距离，则更新的下一时刻的位置为：

（13）

其中，，，分别表示，，狼的移动距离，表示t+1时刻更新的最终位置，此位置提供DWA局部路径规划的线速度及角速度。