[发明专利]地下自主铲运机的导航控制方法

摘要

本发明公开了一种地下自主铲运机的导航控制方法，该方法包括：首先规划出目标路径，在车辆跟踪目标路径时，根据已知的初始偏差，将横向位置偏差、航向角偏差、航向角变化率偏差构成综合反馈，作为导航控制器的输入，结合PID校正控制，计算出车辆的转向控制指令，输入给车辆的电液转向控制系统，使车辆转向角发生改变，再运用该导航控制器内部构建的车辆轨迹精确推算模型，精确计算出车辆的跟踪轨迹与目标路径之间存在的偏差值，再用这些偏差值作为反馈来计算新的转向控制指令，如此反复，使车辆不断跟踪目标路径，实现地下铲运机自主导航。

主权项

1.一种地下自主铲运机的导航控制方法，其特征在于，包括步骤：A、以预先规划好的铲运机运动轨迹作为铲运机的目标路径，以铲运机的定位参考点P的实际运动轨迹为铲运机的跟踪轨迹，沿所述目标路径的曲线弧长s_m(k)存储路径曲率K_m(k)系列值和起始点航向角其中，k为采样次数，k＝1，2，3，…，用采样周期T作为测量和控制间隔；以所述目标路径上的航向角与所述跟踪轨迹航向角的差值定义为航向角偏差用所述铲运机的定位参考点P与该点投影于所述目标路径上的点P_m的距离定义为铲运机的横向位置偏差δ；B、用所述铲运机的定位参考点P投影于所述目标路径的点P_m的曲线弧长s_m(k)和铲运机在该点的横向位置偏差δ(k)作为所述铲运机的相对位置P(s_m(k)，δ(k))；用所述铲运机的相对位置P(s_m(k)，δ(k))及铲运机在该位置处的航向角偏差β(k)和铲运机转向角α(k)来表示所述铲运机在该位置处的位姿；所述铲运机为前部与后部铰接的结构，前部与后部的铰接处安装有角位移传感器，所述角位移传感器直接测量所述铲运机转向角α(k)；C、通过所述铲运机的定位参考点P的车速v_g(k)计算该点在所述目标路径上的投影车速计算所述目标路径的航向角变化率和所述跟踪轨迹的航向角变化率并计算航向角变化率偏差所述目标路径的航向角变化率的计算公式为：所述跟踪轨迹的航向角变化率的计算公式为：式中，l₁为铲运机前部驱动桥的中点到铰接中心线的距离，l₂为铲运机后部驱动桥的中点到铰接中心线的距离；D、由所述航向角变化率偏差航向角偏差β(k)和横向位置偏差δ(k)三个参数构成综合反馈e(k)，该综合反馈用于铲运机的转向控制；所述综合反馈其中，K₁、K₂和K₃为预先存储的反馈系数，K₁、K₂和K₃的选取范围均为1～100。