[发明专利]地下铲运机自主行驶和避障运动控制及目标路径规划方法

摘要

本发明公开了一种地下铲运机自主行驶和避障运动控制及目标路径规划方法，根据井下巷道情况，先规划出主目标路径，然后规划出依附于主目标路径的局部避障目标路径，在铲运机控制器中存储主目标路径数据，及局部避障目标路径的起始点、终止点和相对于主目标路径的偏移距离数据，由测量系统的测量数据或由轨迹推算模型，计算出的铲运机横向位置偏差、航向角偏差及航向角偏差变化率，三偏差通过融合规则形成综合偏差反馈信号作为PID校正控制的输入量，该控制指令输入给铲运机的电液比例转向控制系统，驱动车辆转向并跟踪目标路径，实现地下铲运机自主导航和避障运动控制，方便、灵活、可靠。

主权项

1.一种地下铲运机自主行驶和避障运动控制及目标路径规划方法，用来控制地下铲运机跟踪规划出的主目标路径和局部避障目标路径，其特征在于，该方法包括以下主要步骤：A、根据地下巷道情况进行地下铲运机主目标路径和局部避障目标路径规划，在地下铲运机主目标路径和局部避障目标路径规划完成后，沿主目标路径曲线弧长s_m(k)存储主目标路径曲率K_m(k)系列值，沿主目标路径曲线弧长s_m(k)存储局部避障目标路径的起始点s_mbb(i)和终止点s_mbe(i)及局部避障目标路径相对于主目标路径的横向偏离位移δ_b0(i)，主目标路径和局部避障目标路径保存在自主行驶控制器的存储介质中；上式中：k=1，2，3，…；i为局部避障目标路径数量，i=1，2，3，…；B、用采样周期T作为测量和控制间隔，当车辆沿主目标路径行驶时，用车辆的定位参考点相对于主目标路径的位置P(s_m(k),δ(k))和航向角偏差β(k)，及车辆转向角α(k)，来表示车辆的位姿；当车辆沿局部避障目标路径行驶时，仍然使用车辆的定位参考点相对于主目标路径的位置P(s_m(k),δ(k))和航向角偏差β(k)，及车辆转向角α(k)，来表示车辆的位姿；C、判断铲运机是否到达避障位置，当铲运机没有到达局部避障目标路径位置时，输入自主行驶控制器的横向位置偏差为δ_b(k)，δ_b(k)=δ(k)；当铲运机到达局部避障目标路径位置时，自主行驶控制器输入的横向位置偏差为δ_b(k)，δ_b(k)=δ(k)+δ_b0(i)，δ_b0(i)为局部避障目标路径相对于主目标路径的横向偏离位移；D、计算铲运机跟踪轨迹航向角变化率计算目标路径投影车速v_m(k)，计算目标路径航向角变化率计算航向角偏差变化率计算下一采样时刻航向角偏差β(k+1)，计算下一采样时刻横向位置偏差δ(k+1)及δ_b(k+1)，计算下一采样时刻目标路径曲线弧长s_m(k+1)，由目标路径曲率存储控制子系统得到下一采样时刻目标路径曲率K_m(k+1)及K_mb(k+1)的数值；E判断横向位置偏差δ_b(k)的数值，当-ε_δ1≤δ_b(k)≤ε_δ1时，δ_b(k)=δ_b(k)。当δ_b(k)＞ε_δ1时，δ_b(k)=ε_δ1；当δ_b(k)＜-ε_δ1时，δ_b(k)=-ε_δ1，其中ε_δ1的取值范围为，0＜ε_δ1≤1（m）；F、计算铲运机横向位置偏差δ_b(k)、航向角偏差β(k)及航向角偏差变化率三种偏差融合而成综合偏差反馈信号e(k)，作为PID校正控制器的输入；G、判断横向位置偏差|δ_b(k)|及航向角偏差|β(k)|的数值，当|δ_b(k)|≤ε_δ2且|β(k)|≤ε_β同时满足时，PID自主行驶控制器才进行综合偏差反馈信号e(k)的PID控制，此时PID的积分环节起作用，输出此时的转向控制电压u(k);当|δ_b(k)|≤ε_δ2和|β(k)|≤ε_β不能同时满足时，PID自主行驶控制器仅进行综合偏差反馈信号的PD控制，此时PID的积分环节不起作用，输出此时的转向控制电压u(k)；H、转向控制电压u(k)作为铲运机电液比例转向控制系统的控制电压，电液比例转向控制系统驱动转向油缸，实现铲运机自主调整转向角度，使铲运机行驶轨迹自动跟踪目标路径；I、当采样次数k=1时，航向角偏差β(k)，横向位置偏差δ(k)和目标路径曲线弧长s_m(k)都是实测值或由实测值计算得到，航向角偏差变化率是计算值；当k≥2时，航向角偏差β(k)，横向位置偏差δ(k)，目标路径曲线弧长s_m(k)既可以是实测值也可是推算值，航向角偏差变化率是计算值。