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

权利要求书

1.一种高精度循迹和控制的巡逻车路径规划及跟踪算法，具体步骤如下，其特征在于：

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

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

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

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

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

2.根据权利要求1所述的一种高精度循迹和控制的巡逻车路径规划及跟踪算法，其特征在于：步骤2中设计巡逻车路径规划模型的过程可以表示为：

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

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

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

评估函数f(n)具体公式为：

f(n)＝g(n)+h(n)

其中，g(n)是第n个节点到上一个节点的移动代价最佳路径值；h(n)是选择第n个节点后，起点到终点的最佳路径的启发值。

3.根据权利要求1所述的一种高精度循迹和控制的巡逻车路径规划及跟踪算法，其特征在于：步骤3中根据运动学模型求解巡逻车的航向角和速度的过程可以表示为：

将巡逻车运动学模型设为：

式中，巡逻车速度的微分值、是航向角度的微分值、为路径横向坐标的微分值、为路径纵向坐标的微分值、a是加速度、R是转弯半径、是航向角；并通过微分方程求解巡逻车的定积分值：

式中，v_k+1为巡逻车在k+1的速度值，为巡逻车在k+1的航向角，X_k+1为巡逻车在k+1的路径横向坐标，Y_k+1为巡逻车在k+1的路径纵向坐标，v_k为巡逻车在k的速度值，为巡逻车在k的航向角，X_k为巡逻车在k的路径横向坐标，Y_k为巡逻车在k的路径纵向坐标，t为时间变量；巡逻车在时刻k+1和时刻k之间，设行驶的距离为Δd，行驶的航向角为则可得：

式中Δt是时刻k+1和时刻k的间隔时间。

4.根据权利要求1所述的一种高精度循迹和控制的巡逻车路径规划及跟踪算法，其特征在于：步骤4中巡逻车速度、航向角和位置闭环控制的过程可以表示为：

为提高巡逻车控制精度、减小系统的超调量，其中实现速度、航向角和位置的闭环控制均采用积分分离的PID控制，在算法中引入逻辑功能，调节器输出采样点值为：

其中，u(i)是PID的输出，e(i)是i时刻系统误差，e(i-1)是i-1时刻系统误差，i为离散化的时间变量，K_p，K_l1，K_d，分别是PID算法的比例系数、积分系数和微分系数，e(j)是j时刻的系统误差，A为PID控制系统门限，当偏差较大时，PID的积分项不起作用，偏差在门限之内时，引入积分算法，来控制巡逻车的运行。