一种轨迹梯度顺滑的室内移动机器人精准定位导航算法的制作方法

本发明涉及导航算法，具体为一种轨迹梯度顺滑的室内移动机器人精准定位导航算法。

背景技术：

1、室内移动机器人正在大量普及，是当下热门的机器人技术产品。如扫地机器人、服务机器人和送餐机器人等，它们可以在各种场景中替代人类劳动，例如扫地机器人可以承担家庭环境清洁的大部分工作，不仅能扫地吸尘，还能完成拖地洗抹布等清洁任务。服务机器人可以帮助商场的客人寻找想要去的地方。送餐机器人可以代替餐厅服务员进行送餐工作。

2、对于室内移动机器人而言，导航功能是它们完成各种应用功能的基础，可以极大地提升工作效率和人们的生活质量。然而现有的室内移动机器人的导航算法规划出的机器人行驶路径和局部轨迹不够顺滑。由于机器人的固有电机响应特性，如果机器人控制指令不够顺滑，频繁的大幅度的速度变化将降低机器人导航的效率，同时频繁的加速减速将降低电机的使用寿命。此外，现有的室内移动机器人的导航算法通常只单独考虑全局路径规划和局部路径规划的部分，未考虑与地图实时维护更新以及定位的关系，与后者往往以非常宽松的松耦合关系存在，联系不够紧密导致规划效果不够精准。因此，设计规划精准的一种轨迹梯度顺滑的室内移动机器人精准定位导航算法是很有必要的。

技术实现思路

1、本发明的目的在于提供一种轨迹梯度顺滑的室内移动机器人精准定位导航算法，以解决上述背景技术中提出的问题。

2、为了解决上述技术问题，本发明提供如下技术方案：一种轨迹梯度顺滑的室内移动机器人精准定位导航算法，该导航算法采用导航系统，该系统包括地图维护模块、全局规划器模块、定位模块、局部规划器模块，所述地图维护模块的子算法基于预先使用的定位和建图技术构建的栅格地图和激光雷达传感器的实时感知信息，对地图进行在线维护更新，所述全局规划器模块基于地图维护模块提供的最新地图信息，负责为机器人规划出一条从起点到终点的无障碍物路径，所述定位模块基于地图信息，利用激光雷达传感器观测到的环境数据计算机器人在地图坐标系中的姿态，包含位置坐标和朝向角度，所述局部规划器模块负责跟随来自全局规划器的路径，并利用定位模块提供位姿信息进行反馈，从而实现精准的路径跟随和避障。

3、根据上述技术方案，所述地图维护模块部分以用户预先使用同步定位和建图技术构建的栅格地图为基础，分为全局地图和局部地图两部分进行维护，所谓栅格地图即以一定分辨率的正方形将平面地图划分成若干小格，即地图被离散化为一个二维矩阵，每个小格称为栅格，以不同的栅格值来表示栅格的状态，该值表示该栅格是障碍物的概率，概率为0表示是可通行区域如下图白色栅格所示，概率为1表示是障碍物如下图黑色栅格所示，概率介于0-1之间的表示机器人未探索区域；

4、基于原始的栅格地图，本算法以代价地图的形式表示全局地图和局部地图，所谓代价，即通常还会用一个0-255之间的整数表示全局地图和局部地图的栅格值，在本算中，以介于128—255的值表示该栅格的障碍物距离机器人几何中心的距离可能会发生碰撞，越大表示发生碰撞的可能性越大，0-128表示该栅格离机器人最外侧轮廓的远近程度；

5、本算法维护的全局地图是只与原始栅格地图相同大小的代价地图，而局部地图则是以机器人为中心的一个大小的正方形区域的代价地图，该局部地图的分辨率较全局地图更高，当机器人上安装的传感器如激光雷达在环境中发现了新的障碍物时，该信息将被首先更新到栅格地图中，接着再被更新到全局地图和局部地图中。

6、根据上述技术方案，所述栅格地图更新的算法如下，对于地图中的一个栅格，以p(state＝1)表示该栅格是障碍物的概率，以p(state＝0)表示该栅格是可通行区域的概率，这两个概率满足：

7、p(state＝1)+p(state＝0)＝1

8、记这两个概率的比值为：

9、

10、另记当前的激光雷达传感器对环境的观测数据为z,则给定z后该栅格为障碍物和可通行区域的概率分别为：

11、

12、

13、这两者的比值为：

14、

15、两边取对数有：

16、

17、其中仅项包含测量值，这个对数比值称为测量模型，这项的值与实际测量情况有关，取决于激光雷达的射线是否打中了当前栅格，假设开始有任何传感器观测之前某一个栅格在未来为障碍物或者可通行区域的概率均等，则logq(state)的初始值为：

18、

19、根据上述技术方案，所述全局规划器模块基于梯度下降的dijkstra路径规划算法，可以规划出一条顺滑的全局路径，dijkstra算法可以找到从起点栅格道终点栅格的最短路径，本算法的规划过程中在dijkstra算法找到最短路径的过程中增加了一个计算栅格的势能值的步骤，即最短路径附近的栅格都会更新一个势能值，然后从起终点栅格出发，依据该栅格附近8个邻居栅格的势能值计算出8个方向的势能梯度，沿着势能梯度最大的方向按照设定的步长获取下个路径点，然后从下一个路径点出发，继续沿着势能梯度最大的方向计算下一个路径点，直到达到起点附近，由于每次到下一个路径点到距离和势能梯度相关，并不是固定的一个栅格长度距离，因此规划出的路径是比较顺滑的曲线，其中，quadraticinterpolation函数为势能的二次插值函数，功能为基于当前栅格上下左右的四个邻居栅格的势能值来对当前栅格进行二次插值。

20、根据上述技术方案，所述定位模块基于占据栅格地图和当前激光雷达数据，使用基于贝叶斯滤波框架的粒子滤波算法进行定位，

21、定位过程分为个主要步骤：

22、(1)随机生成n个粒子，每个粒子表示对机器人在地图中位姿的一个概率假设；

23、(2)依据最新观测的激光雷达数据计算每个粒子的权重，粒子所在位置和观测数据一致的权重较大，否则为权重较小；

24、(3)将带权重值的粒子们组成新的集合，按照权重大小重新进行抽样，获得新粒子集合，新粒子集合中，权重最大用于表示机器人当前位姿。

25、根据上述技术方案，所述局部规划器模块基于机器人硬约束的损失函数的求解，设机器人的最大加速度的大小为a,最大角加速度的大小为β，每次规划的模拟时间为δt,机器人当前的线速度为vnow,当前的角速度为ωnow,则对于机器人而言，其在δt时间之后，线速度和角速度的取值范围为：

26、v∈[vnow-a·δt,vnow+a·δt]

27、ω∈[ωnow-β·δt,ωnow+β·δt]

28、另外，考虑到机器人的理论最大线速度和角速度的数值为：vmax和ωmax,处于安全考虑，以及不允许机器人有后退，即v≥0，则机器人的线速度和角速度的取值范围为：

29、v∈[max(0,vnow-a·δt),min(vnow+a·δt,vmax)]

30、ω∈[max(-ωmax,ωnow-β·δt),min(ωnow+β·δt,ωmax)]。

31、所述局部规划的算法步骤如下：

32、(6)将上述机器人线速度和角速度取值范围分别离散成m个取值和n个取值，m和n为人为预设的值，然后排列组合成m×n个(线速度，角速度)的取值组合；

33、(7)对于m×n个(v,ω)的每个组合，认为在δt时间范围内保持不变，模拟计算出机器人的轨迹，机器人的轨迹为一段圆弧，记机器人当前的位姿为(xnow,ynow,θnow),δt时间后机器人的轨迹为：

34、

35、

36、θt＝θnow+ωδt

37、(8)计算模拟出机器人的轨迹和障碍物的最近距离d，判断是否会和地图中的障碍物发生碰撞，剔除会发生碰撞的组合，得到剩余的轨迹集合t；

38、(9)对于集合t中的轨迹，首先将它们的速度(v,ω)、到障碍物的最近距离d、姿态角θ在集合内进行归一化计算，将它们的取值都映射到[0,1]之间；

39、按照一系列cost计算t中cost最小的轨迹的速度作为机器人的最终的控制指令。

40、根据上述技术方案，所述cost定义如下：

41、totalcost＝goalcost+obstaclecost+velocitycost

42、goalcost＝distance(goal,robotpos)

43、obstaclecost＝-min_distanace_to_obstacle

44、velocitycost＝α·v+β·ω

45、其中，goalcost为机器人当前位置robotpos到目标点goal的欧式距离，obstaclecost用于衡量机器人到障碍物的距离，velocitycost是对线速度和角速度的惩罚项。

46、与现有技术相比，本发明所达到的有益效果是：本发明，采用紧密的耦合算法，利用定位模块提供位姿信息进行反馈，从而实现精准的路径跟随和避障，由于每次到下一个路径点到距离和势能梯度相关，并不是固定的一个栅格长度距离，因此规划出的路径是比较顺滑的曲线。