专利名称：地下铲运机自主行驶和避障运动控制及目标路径规划方法
技术领域：
本发明涉及一种自主行驶和避障控制技术，尤其涉及一种地下铲运机自主行驶和避障运动控制及目标路径规划方法。
背景技术：
随着现有矿产资源逐步全面转入地下开采，并向深部和难采矿体发展，开采规模逐渐增大，采矿条件愈来愈恶劣，对人的安全威胁也愈来愈大，遥控采矿技术应运而生。地下铲运机是地下无轨采矿的关键设备，实现地下铲运机在井下巷道内无人操纵、自主行驶，可使操作人员远离井下恶劣、危险的工作环境，保护铲运机司机人员安全、提高采矿作业效率并降低采矿成本。地下铲运机自主行驶首先要解决自身在巷道内的自主行驶及避障运动控制问题及目标路径规划问题。地下铲运机自主行驶研究在国内刚刚起步，国外在此方面已有较多的研究。如澳大利亚彼得里德利对铲运机运动轨迹和轨迹控制进行了研究，构建了自主铲运机自主行驶控制模型，该自主行驶控制模型在小曲率路径跟踪时误差较小，但在目标路径曲率较大时，跟踪误差较大，不能满足控制精度要求。美国丰约翰对轮式机器人路径跟踪问题进行了研究，但将目标路径局限于直线段和圆弧段，让轮式机器人跟踪每一个直线段或圆弧段，给出跟踪直线段或圆弧段的计算办法，该方法不能对其他形状曲线路径进行跟踪计算，同时该研究也未涉及铰接车辆情况，故无法应用于地下铲运机。北京矿冶研究总院发明了一种基于车辆轨迹精确推算模型的地下铲运机自主行驶控制方法，可较精确计算出铲运机的跟踪轨迹与目标路径之间存在的偏差值，构成综合偏差来控制铲运机转向并跟踪目标路径，该方法没有考虑避障运动控制问题，另外，当跟踪偏差较大时，控制系统的稳定性和控制精度会受到较大影响。所以，需要进一步研究解决地下铲运机在较大偏差情况下，自主行驶控制器的稳定性问题，并提高自主行驶控制精度。避障运动控制是地下智能铲运机一项必不可少的功能。现有避障技术是用几段圆弧线组成的局部避障目标路径，并与其他目标路径相连接，形成一条连续的目标路径，这样做在理论上是可行的，但在实际应用时会很复杂，比如事先要存储大量的目标路径曲率，不同弧线段可能对应几种不同的曲率需要存储，同时还需要存储不同弧线的切换点位置数据，这些情况带来自主行驶避障控制系统数据管理难度大，而且避障目标路径的规划也很复杂，需要大量计算工作，实际避障控制时需要准确找到不同弧线的切换点，否则会严重影响避障控制效果，所以该方法应用时也很不方便；另外，在铲运机铲装矿石出矿过程中，除了在巷道中心线位置铲装料堆中部的矿石外，铲运机还需要偏左或偏右不断变换位置去铲装料堆左右两边的矿石，通常每铲运一次，换一个铲取位置，这样铲取效率才高。要规划出每一条偏离巷道主目标路径的路径数据量较大，也比较麻烦；为了降低井下巷道开拓成本，井下巷道的形状和尺寸一般允许有一定程度的不规则情况存在，这给井下巷道目标路径规划带来困难，并使自主行驶控制器存储目标路径数据量增多，目前也没有解决这一问题的较好方法。
发明内容
本发明的目的是提供一种方便、灵活、可靠的地下铲运机自主行驶和避障运动控制及目标路径规划方法。本发明的目的是通过以下技术方案实现的:本发明的地下铲运机自主行驶和避障运动控制及目标路径规划方法，用来控制地下铲运机跟踪规划出的主目标路径和局部避障目标路径，其特征在于，该方法包括以下主要步骤:A、根据地下巷道情况进行地下铲运机主目标路径和局部避障目标路径规划，在地下铲运机主目标路径和局部避障目标路径规划完成后，沿主目标路径曲线弧长Sm(k)存储主目标路径曲率Km(k)系列值，沿主目标路径曲线弧长Sm(k)存储局部避障目标路径的起始点smbb(i)和终止点Snte(i)及局部避障目标路径相对于主目标路径的横向偏离位移δ b0 (i)，主目标路径和局部避障目标路径保存在自主行驶控制器的存储介质中；上式中:k=l，2，3，…；i为局部避障目标路径数量，i = 1，2，3，…；B、用采样周期T作为测量和控制间隔，当车辆沿主目标路径行驶时，用车辆的定位参考点相对于主目标路径的位置P(sm(k)，δ (k))和航向角偏差β (k)，及车辆转向角a (k)，来表示车辆的位姿；当车辆沿局部避障目标路径行驶时，仍然使用车辆的定位参考点相对于主目标路径的位置P(sm(k)，δ (k))和航向角偏差β (k)，及车辆转向角a (k)，来表示车辆的位姿；C、判断铲运机是否到达避障位置，当铲运机没有到达局部避障目标路径位置时，输入自主行驶控制器的横向位置偏差为sb(k)，sb(k) = s (k);当铲运机到达局部避障目标路径位置时，自主行驶控制器输入的横向位置偏差为Sb(k)，5b(k)=5 (k)+5b0(i),δΜ( )为局部避障目标路径相对于主目标路径的横向偏离位移；D、计算铲运机跟踪轨迹航向角变化率Τ，计算目标路径投影车速Vm(k)，计算
目标路径航向角变化率
权利要求
1.一种地下铲运机自主行驶和避障运动控制及目标路径规划方法，用来控制地下铲运机跟踪规划出的主目标路径和局部避障目标路径，其特征在于，该方法包括以下主要步骤: A、根据地下巷道情况进行地下铲运机主目标路径和局部避障目标路径规划，在地下铲运机主目标路径和局部避障目标路径规划完成后，沿主目标路径曲线弧长Sm(k)存储主目标路径曲率Km (k)系列值，沿主目标路径曲线弧长Sm (k)存储局部避障目标路径的起始点sfflbb(i)和终止点Snte(i)及局部避障目标路径相对于主目标路径的横向偏离位移δΜα)，主目标路径和局部避障目标路径保存在自主行驶控制器的存储介质中； 上式中:k=l，2，3，…；i为局部避障目标路径数量，i=l，2，3，…； B、用采样周期T作为测量和控制间隔，当车辆沿主目标路径行驶时，用车辆的定位参考点相对于主目标路径的位置P(sm(k)，δ (k))和航向角偏差β (k)，及车辆转向角a (k)，来表示车辆的位姿；当车辆沿局部避障目标路径行驶时，仍然使用车辆的定位参考点相对于主目标路径的位置P(sm(k)，δ (k))和航向角偏差β (k)，及车辆转向角a (k)，来表示车辆的位姿； C、判断铲运机是否到达避障位置，当铲运机没有到达局部避障目标路径位置时，输入自主行驶控制器的横向位置偏差为Sb(k)，5b(k)=5 (k);当铲运机到达局部避障目标路径位置时，自主行驶控制器输入的横向位置偏差为Sb(k)，5b(k)=5 (k)+5b0(i), 5b0(i)为局部避障目标路径相对于主目标路径的横向偏离位移； D、计算铲运机跟踪轨 迹航向角变化率计算目标路径投影车速Vm(k)，计算目标路径航向角变化率^计算航向角偏差变化率f，计算下一采样时刻航向角偏差β (k+1)，计算下一采样时刻横向位置偏差δ (k+1)及Sb(k+1)，计算下一采样时刻目标路径曲线弧长Sm(k+Ι)，由目标路径曲率存储控制子系统得到下一采样时刻目标路径曲率Km (k+1)及 Kmb (k+1)的数值； E判断横向位置偏差5b(k)的数值，当_ε δ1^ 5b(k)彡ε S1时，5b(k)=5b(k)0当δ b(k) > ε S1 时，δ b (k) = ε Sl ;当 δ b (k) < - ε Sl 时，δ b (k) =- ε Sl,其中 ε δ1 的取值范围为，O < ε S1 彡 I (m)； F、计算铲运机横向位置偏差Sb(k)、航向角偏差β(k)及航向角偏差变化率T三种偏差融合而成综合偏差反馈信号e (k)，作为PID校正控制器的输入； G、判断横向位置偏差ISb(k) I及航向角偏差I β (k) I的数值，当I Sb(k) I彡ε S2且β (k) I ( ε 0同时满足时，PID自主行驶控制器才进行综合偏差反馈信号e(k)的PID控制，此时PID的积分环节起作用，输出此时的转向控制电压u(k);当I δ b(k) I彡ε S2和β (k) I ( ε 0不能同时满足时，PID自主行驶控制器仅进行综合偏差反馈信号的H)控制，此时PID的积分环节不起作用，输出此时的转向控制电压u (k)； H、转向控制电压u(k)作为铲运机电液比例转向控制系统的控制电压，电液比例转向控制系统驱动转向油缸，实现铲运机自主调整转向角度，使铲运机行驶轨迹自动跟踪目标路径； 1、当采样次数k=l时，航向角偏差β(k)，横向位置偏差δ (k)和目标路径曲线弧长Sffl(k)都是实测值或由实测值计算得到，航向角偏差变化率7是计算值；当k > 2时，航向角偏差β (k)，横向位置偏差δ (k)，目标路径曲线弧长sm(k)既可以是实测值也可是推算值，航向角偏差变化率，是计算值。
2.如权利要求1所述的地下铲运机自主行驶和避障运动控制及目标路径规划方法，其特征在于，所述步骤A中，在地下铲运机主目标路径和局部避障目标路径规划过程中，先不考虑避障情况，进行铲运机主目标路径的规划，在主目标路径规划完毕后，再进行与主目标路径等距偏移的局部避障目标路径的规划。
3.如权利要求1所述的地下铲运机自主行驶和避障运动控制及目标路径规划方法，其特征在于，局部避障目标路径的曲率Kmb (k)与主目标路径曲率Km (k)的关系为:
4.如权利要求1所述的地下铲运机自主行驶和避障运动控制及目标路径规划方法，其特征在于: 跟踪轨迹航向角变化率
5.如权利要求1所述的地下铲运机自主行驶控制方法，其特征在于，主目标路径曲率Km(k)及局部避障目标路径曲率Kmb(k)与主目标路径弧长Sm(k) —一对应。
6.如权利要求1所述的地下铲运机自主行驶和避障运动控制及目标路径规划方法，其特征在于，计算综合反馈e(k)的公式为:
7.如权利要求1所述的地下铲运机自主行驶和避障运动控制及目标路径规划方法，其特征在于，综合反馈e(k)经PID控制器校正后，计算输出转向控制电压u(k)的方法为: u (k) =U (k-1) + Δ u (k) 其中，u(k-l)为前一时刻控制电压，Au(k)为转向控制电压的增量，且AuGOZKpka)-e(k-l)]+eKie(k)+Kd[e(k)-2e(k-l)+e(k-2)]，其中，e(k-l)为 e (k)前一时刻的综合反馈值，e(k-2)为e(k-l)前一时刻的综合反馈值,Kp为比例系数,Ki为积分系数，
8.如权利要求1所述的地下铲运机自主行驶和避障运动控制及目标路径规划方法，其特征在于，所述采样周期T选取范围为，20≤T≤400msο
9.如权利要求1所述的地下铲运机自主行驶和避障运动控制及目标路径规划方法，其特征在于，所存储的比例系数Kp的取值范围为，1 ≤ Kp ≤ 200，所述积分系数Ki的取值范围为，O≤Ki≤25，所述微分系数Kd的取值范围为，O≤Kd≤5。
全文摘要
本发明公开了一种地下铲运机自主行驶和避障运动控制及目标路径规划方法，根据井下巷道情况，先规划出主目标路径，然后规划出依附于主目标路径的局部避障目标路径，在铲运机控制器中存储主目标路径数据，及局部避障目标路径的起始点、终止点和相对于主目标路径的偏移距离数据，由测量系统的测量数据或由轨迹推算模型，计算出的铲运机横向位置偏差、航向角偏差及航向角偏差变化率，三偏差通过融合规则形成综合偏差反馈信号作为PID校正控制的输入量，该控制指令输入给铲运机的电液比例转向控制系统，驱动车辆转向并跟踪目标路径，实现地下铲运机自主导航和避障运动控制，方便、灵活、可靠。
文档编号G05B13/04GK103197675SQ20131008005
公开日2013年7月10日 申请日期2013年3月13日 优先权日2013年3月13日
发明者石峰, 顾洪枢, 战凯, 冯孝华, 郭鑫, 李建国, 段辰玥, 陈强 申请人:北京矿冶研究总院, 北矿机电科技有限责任公司