[发明专利]一种高精度循迹和控制的巡逻车路径规划及跟踪算法

说明书

本发明提供一种高精度循迹和控制的巡逻车路径规划及跟踪算法。该方法包括以下步骤：步骤1，栅格化巡逻车使用地的地理；步骤2，设计巡逻车路径规划模型；步骤3，根据运动学模型求解巡逻车的航向角和速度；步骤4，巡逻车速度、航向角和位置闭环控制；步骤5，处理巡逻路线上的异常情况，更新寻优路径。本发明实现了高速及高精度的巡逻车路轨规划寻优算法，提高了巡逻车路径寻优的效率，同时在巡逻车的控制问题中，提出了双环串联PID闭环的控制系统，有效地克服巡逻车运动时的非线性因素和系统摩擦。

技术领域

本发明涉及巡逻车循迹领域，特别是涉及一种高精度循迹和控制的巡逻车路径规划及跟踪算法。

背景技术

随着社会不断发展，各领域的技术研究不断深入，各种智能代理技术逐渐融入人们的日常工作和生活，帮助人们从枯燥的重复劳动中解脱出来，提高了生产效率，降低了生产成本。

巡逻车作为目前建筑、安防行业运用最广泛的机器人，其在安全监控、巡逻监测等方面有巨大的价值。巡逻车路径规划的目标是利用合适的路径规划方法寻找从起点到终点的无碰撞的最优路径，在可控的结构化工厂环境中，巡逻车具有一定的自主性，能够感知周围环境，检测障碍物，并规划一条无碰撞的路径导航到目的地。但在矿山、岩洞、施工工地等复杂的动态施工空间环境中，对巡逻车的应用要实现完全自主，尚未得到充分解决，并且在特定的平面布置图上规划巡逻车的路径需要有关起点和终点(即现场通道和目标工作区)、静态障碍物(如墙、柱)和动态危险区域(如材料、工人和设备工作区)的信息.还要实现几何空间的定位及距离测算。因此，有必要根据在不同时间节点处于现场不同位置进行的施工活动、工作空间分布和变化实现机器人系统全局路径规划和路径跟踪控制。

发明内容

为解决上述问题，本发明在实时采集的行驶场地数据基础上，提出了一种高精度循迹和控制的巡逻车路径规划及跟踪算法。为寻找巡逻车的最优路径，设计了一种巡逻车路径规划模型，同时为了提高巡逻车运行时的准确率，提出了巡逻车速度、航向角和位置闭环控制，并通过实时更新最优路径，来处理异常出现的障碍物。本发明提供一种高精度循迹和控制的巡逻车路径规划及跟踪算法，具体步骤如下，其特征在于：

步骤1，栅格化巡逻车使用地的地理：采集巡逻车行驶场地的数据，根据场地数据将场地地图栅格化，栅格块设置为正方形，横纵向的栅格移动代价设为10，对角线的栅格移动代价设为14，每个栅格都对应一个地理中的坐标范围，同时将栅格分为空区域、障碍内区域和目标区域；

步骤2，设计巡逻车路径规划模型：将起点和坐标加入模型中，同时遍历栅格搜索巡逻车路径，找到估价函数值最小的最优路径，将最优路径作为巡逻车的移动路径；

步骤3，根据运动学模型求解巡逻车的航向角和速度：通过陀螺仪所检测的角度、速度和加速度建立巡逻车的运动学模型；

步骤4，巡逻车速度、航向角和位置闭环控制：设计巡逻车速度、航向角和位置闭环控制双环串联PID，首先设计巡逻车的速度值和航向角的PID，实现速度和航向角的闭环控制，同时再接入巡逻车的位置闭环，最终实现巡逻车的位置跟踪；

步骤5，巡逻路线上的异常情况：如果路线出现障碍物后，后台监控软件通过通讯将更新后的栅格地图发送给巡逻车的控制软件，控制软件通过通讯中断接收更新后的数据，并根据更新后的栅格地图重复步骤2至步骤4。

进一步，步骤2中设计巡逻车路径规划模型的过程可以表示为：

步骤2.1，将巡逻车起点加入开放列表中，作为当前节点；

步骤2.2，查找与当前节点相邻的节点，并将可达到的节点放入开放列表中，同时将当前节点设为这些节点的父节点；

步骤2.3，把当前节点从开放列表中移除放入封闭列表中，并在开放列表中寻找遍历寻找估价函数值最小的节点，同时将估价函数值最小的节点放入关闭列表里，并重复步骤2.2和步骤2.3，直到将终点放入封闭列表中，则停止寻优；