地下铲运机跟踪轨迹推算模型研究.pdf

第6 2 卷第6期 有 色 金属( 矿山部分) 2 0 1 0 年1 1 月 地下铲运机跟踪轨迹推算模型研究 石峰 ， 战凯， 顾洪枢， 冯孝华， 郭鑫 ( 北京矿冶研究总院， 北京 1 0 0 0 7 0 ) 摘要： 路径跟踪是实现地下铲运机自动化作业的基本任务之一。本文对地下铲运机跟踪轨迹特性进行了研 究， 对轨迹参数进行了定义， 推导出了轨迹偏差与车辆结构尺寸、 转向角、 转向角速率、 车速、 路径曲率的函数关系， 建立了轨迹推算精确模型， 为地下铲运机导航控制器的设计奠定了基础。 关键词： 地下铲运机； 跟踪轨迹； 推算； 模型 中图分类号： T D 4 2 2 4 文献标识码iA 文章编号： 1 6 7 1 4 1 7 2 ( 2 0 1 0 ) 0 6 0 0 6 6 0 4 A S t u d y o n Un d e r g r o u n d L o a d e r T r a c k in g T r a c k Ca l c u l a t i o n Mo d e l S HI Fe n g，ZHAN Ka i，GU Ho n g s h u，F ENG Xia o h u a，GUO Xin ( B e i j i n g G e n e r a l R e s e a r c h I n s t i t u t e o f Mi n in g a n d M e t a l l u r g y ，B e i j in g 1 0 0 0 7 0 ) A b s t r a c t ：T r a c k in g p a t h is o n e o f b a s ic t a s k f o r u n d e r g r o u n d l o a d e r a u t o - d ri v e I n t h is p a p e r ，u n d e r gro u n d l o a d e r t r a c k in g t r a c k f e a t u r e s a r e s t u d ie d ，t r a c k p a r a me t e r s a r e d e f in e d，f u n c t io n s a r e d e d u c e d a mo n g t r a c k d e v ia t io n a n d t h e v e h i c l e s t r u c t u r e s iz e ，s t e e ri n g a n g l e ，s t e e ri n g a n g l e v e l o c it y， v e h ic l e s p e e d， p a t h c u r v a t u r e T r a c k c a l c u l a t in g p r e c is e mo d e l is b u il t T h e r e f o r e a r e l ia b l e b a s is is l a id f o r u n d e r g r o u n d l o a d e r n a v ig a t io n c o n t r o l l e r d e s ig n Ke y wo r d s ：u n d e r gro u n d l o a d e r ；t r a c k in g t r a c k ；c a l c u l a t io n；mo d e l 0 引言 随着地下矿开采深度增加 ， 采矿条件越来越恶 劣 ， 对人的安全威胁也越来越大， 遥控采矿、 自动化 矿山开采技术应运而生 J 。地下铲运机是地下无 轨采矿的关键设备 ， 实现地下铲运机在井下巷道 内 无人操纵 、 自主行驶 ， 可使操作工人远离井下恶劣 、 危险的工作环境 ， 保护铲运机司机人身安全 ， 提高采 矿作业效率 ， 降低采矿成本。 路径跟踪是实现地下铲运机 自动化作业 的基本 任务之一 _ 3 J ， 路径跟踪与控制的 目标是使得车辆以 尽可能小的误差跟踪 目标路径 。地下铲运机为了适 应井下低矮 、 狭窄、 多弯 的巷道环境 ， 将车身设计成 低矮、 细长、 铰接式车体结构。例如地下 4立方铲运 机的长度一般为 9 6米 ， 宽度 2 5米 ， 高 2 3米 ， 铲 运机车身两侧外廓与巷道壁的距离一般仅为 1 米左 右， 这么长的车体要在狭窄、 多弯的巷道内行驶， 其 难度是可想而知的 ， 即使是熟练的铲运机司机 ， 驾驶 基金项 目： 国家“ 十一五” 科技支撑计划支持项 目( 2 0 0 6 B AB 1 1 B 0 2 ) 作者简介 ： 石峰( 1 9 6 1 一) ， 男 ， 教授级高级工程师 ， 流体传动专业 ， 主 要从事地下无轨采矿设备开发研究 。 铲运机时刮蹭巷道壁也时有发生。 由于井下巷道断面不规则 ， 巷道壁凹凸不平 ， 使 得沿墙行走导航方式受到一定程度 的限制 。另外 ， 井下巷道内经常会沿巷道壁放置通风机 、 电气开关 柜等设施 ， 形成局部障碍 ， 使得地下铲运机并不总是 等距离地沿墙行走， 因此， 在地下巷道环境下 ， 有必 要事先规划 出目标路径 ， 并采用相对定位与绝对定 位相结合的方法 ， 来确定铲运机相对于 目标路径的 偏差， 通过导航控制器的控制作用 ， 使地下铲运机 自 主跟踪 目标路径 。 因此有必要对地下铲运机路径跟踪轨迹特性进 行深入研究， 找出地下铲运机跟踪轨迹偏差之 间的 相互影响规律 ， 以及跟踪轨迹偏差与车辆运行状态 参数之间相互影响规律， 这是实现轨迹推算控制 的 基 础 。 1 铲运机轨迹曲线和轨迹参数定义 铲运机轨迹曲线和轨迹参数定义见图 1 。以巷 道内路面为坐标平面建立 xY坐标 系， 选取铲运 机前方驱动桥的中点为铲运机的定位参考点 ， 以该 点在 x Y坐标平面上的投影点 P来代表铲运机的 位置 ， 取该点作为定位参考点的好处是 ， 当铲运机跟 第 6期 石峰等： 地下铲运机跟踪轨迹推算模型研究 6 7 踪过程 中， 铲运机在该点的速度方 向与前面车体纵 向轴线方向一致 ， 方便定位计算。以 P点的速度方 向( 轨迹切线方向) 作为车辆的参考方向， 其与 x轴 正方向所成的夹角定义为铲运机的航 向角 ， 取从 x轴逆时针旋转形成 的航 向角为 正， 反之 为负。P 点的运动轨迹即为铲运机 的跟踪轨迹； 预先规划好 的理想 的 P点运动轨迹作为铲运机的 目标路径 ， 要 求铲运机来跟踪 。P点在 目标路径上 的垂直投影点 P 的速度方向与 X轴正方 向所成的夹角 ， 定义为 目 标路径的航向角 。 目标路径上的航向角 与铲 运机跟踪轨迹航向角 的差值 ， 定义为航向角偏差 J B ， 即卢= 一 ， 该参数反映了 自主铲运机运动方 向与 目标路径预定 的跟踪方 向的偏差 。铲运机跟踪 轨迹上 P点与 目标路径上 P 点的距离定义为铲运 机的横 向位置偏差 6 。取 P点在 目标路径 曲线右侧 的横向位置偏差为正 ， 反之为负。该参数可反 映出 铲运机在巷道 内的横向定位情况 。 图 1 轨迹 曲线和轨迹参数定义 Fig 1 Tr ac k c u r ve a nd t r a c k pa r a m e t e r s de f in iti o n 图中曲线 A为预先规划好的 目标路径 ， 曲线 B 为铲运机实际跟踪轨迹。 为跟踪轨迹在 P点的 曲率 ， 当曲率中心 O 在车辆轨迹曲线前进方 向的左 侧时定义为正 曲率 ， 曲率半径 尺 为正 ， 反之为负 。 为 目标路径在 P 点 的曲率 ， 为其 曲率半径 ， 曲率 中心 O 。S 为 目标路径 曲线 的弧长 ， 取铲运 机运动方 向为正方 向。O L 为铲运机转向角， 即铲运 机前后车体 的相对转动角。使地下铲运机反时针转 弯的转向角 为正 ， 反之为负。转 向角和转 向角变 化速率决定 了车辆的转弯半径大小和铲运机跟踪轨 迹， 自主铲运机正是通过对转向角的不断调整来 实 现跟踪 目标路径 。为了计算方便 ， 地下铲运机定 位 P点位置坐标由对应的 目标路径点的曲线弧长和铲 运机横 向位置偏差 6来表示 ， 即 P ( s ， 。铲运机 P点跟踪车速为 ， 车速 与轨迹 曲线 B相切 ， 其 目标路径上 P 点投影车速为 ， 投影车速 与轨 迹 曲线 A相切。 2 轨迹推算模型分析 2 1 投影车速与铲运机车速的关系分析 轨迹参数几何关 系图见 图 2 。铲运机定位点 P 在 目标路径上 的投影点为 P ， 为铲 运机车速 的投影跟随速度， 投影点 P 的瞬时曲率中心为 O ， 此时关 于曲率 中心 O 的角速度 为 ， 投影速度 与角速度 的关系为 ： m R m 6 0 图 2轨迹 参数 几何 关系图 Fig 2 Tr a c k pa r a m e t e rs g e ome t r ic r e l a t ion d ia gr am 将铲运机车速 分 解成两个垂 直分量 V g c o s 3 和 U g s in 3 ， 由图中可知 V g c 0 与 同向， U g s in 3 与 垂直 ， 由于 为铲运机车速 的投影跟随速度 ， 的分量 V g c o 绕瞬时曲率中心 O 的角速度也为 ， 即 ： 一 将( 2 ) 式代人( 1 ) 式整理得 ： ： 1 ) g C O S ( 3 ) 1 + 6 2 2 横向位置偏差与航向角偏差的关系分析 由于铲运 机 车速 在 6方 向存在 速 度分 量 V g s i ， 在 6方 向速度分量为零 ， 因此铲运机车速 在艿 方向的速度分量 V g s i n l3 是产生横向位置偏差 变化 的原因， 有 ： d 8= v s s i ( 4 ) 2 3 航向角与曲率及车速的关系分析 6 8 有 色 金属( 矿山部分) 第6 2 卷 由 曲 线 的 曲 率 公 式 K = I，将 S 定 义 为 轨 迹 曲线的弧长 ， 并定义铲运机运动方 向为正方向； 将 定义为轨迹曲线的航向角， 并定义从 x轴逆时针旋 转形成的航向角为正 ， 反之 为负 ； 将 定义为轨迹 曲线的曲率 ， 并定义当曲率 中心 0在车辆轨迹曲线 前进方向的左侧时曲率为正， 反之为负。在作出如 上 定 义 后 ， 曲 率 公 式K ： l I 可 改 写 成： I n S l K= ( 5 ) 由 于 车 速 ： 孪， 因 此 有： n = v K ( 6 )d t 、 对于 目标路径有： K m ( 7 ) ( 8 ) 对于跟踪轨迹有 ： 警 K g ( 9 ) K s = ( 1 0 ) 将( 3 ) 式代人( 7 ) 式整理得： ： v g K, c o s ,8 ( 1 1 )d t 1 K m 6 、 上式反映了目标路径投影车速航向角 与铲 运机车速 目标路径曲率 K 横 向位置偏差 6及 航 向角偏差 的关系。 2 4 轨迹航向角与曲率、 转向角、 车速的关系分析 车辆前后车体运动学关系见图 3 ， 其前车体转 动角速度为 0 3 ， 此时转动的瞬时中心为 0 点 ， 转动 半径为 R ， 后车体转动角速度为 ， 此时转动的瞬 时中心为 0 z 点 ， 转动半径为 R ， 前后车体转向角为 铲运机中央铰接点为 J 点， 铲运机前方桥 中点 为 g 点 ， 该点车速为 ， 铲运机后方桥 中点为 q点 ， 该点车速为 。 。由铲运机前后车体在铰接点处速度 相等有 ： g + v j g q +vj q 铲运机前后车体在中央铰接点处平行于后方桥 轴线方向的速度分量也应相等 ， 忽略铲运机跟踪中 的侧滑 ， 有 ： 图 3 车辆前后车体运动学关系 Fig 3 Kine mat ic r e l a t ion o f f r o nt a nd r e a r o ft he ve hic l e V g s i ns 一g Z l C O S O =0+2 1 2 由前后车体转向的角速度等于前后车体角速度 之差 ， 即 - ( JO g - 0 3 2 , 代入上式得 ： c l y g s l no l+z 2 由 1 g =K 代入上式得 ： d ( 1 2 ) 该公式反映了铲运机定位参考点的轨迹 曲线上 每一点的曲率与前后车架转向角、 转向角变化速率 、 车速之间的关系。 由上式也可得到 ： ： ( 1 3 ) g m 2 由 = K s ， 代入( 1 2 ) 式得 ： 一 d t l 2 + 1 C O S O ( 1 4 ) 该公式反映了铲运机定位参考点的轨迹曲线上 每一点的航向角度随时间的变化速率与前后车架转 向角、 转 向角变化速率、 车速 、 铲运机结构参数之间 的关系。 由K ： ， 代入( 1 2 ) 式得： 第 6期 石峰等： 地下铲运机跟踪轨迹推算模型研究 6 9 一 d 5 gV g ( 1 2 +1 1 C O S ) ( 1 5 ) 该公式反映 了铲运机定位参考点的轨迹曲线上 每一点 的航 向角随轨迹 曲线弧长的变化率与前后车 架转向角 、 转 向角变化速率 、 车速之间的关系。 2 5 铲运机前方、 后方定位参考点车速之间的关系 分析 同样 ， 铲运机前后车体在 中央铰接点处垂直于 后方桥轴线方向的速度分量也应相等， 即： d ： 。 。 + z in 。 由 ： 带 入上式 整理 得 ： ： g h 一 + cos u 可见后方车体定位参考点的速度 V 与前方车 体定位参考点的速度 有关外 ， 还与转向角 、 铲运 机 结 构 车 体 参 数 z 、 fz 、 转 向 角 变 化 速 率 警 有 关 ， 如 果假定 是常数 ， 则 U g h 就不是常数了， 只是 Y g h 很接 近 g 。 3 跟踪轨迹推算数学模型 由 = 一 ， 对时间求导可得 ： ： 一 ( 1 7 ) J - _一 f l ， I d d f d 将( 1 1 ) 、 ( 1 4 ) 式代人( 1 7 ) 得 ： 望 ： 一 t1 11 ( 1 8d t 1 + K m Z ，+f l c o s 上式反应 了航 向角偏差 、 横 向位 置偏 差 、 转 向 角、 目标路径曲率、 车速 、 铲运机结构参数之间的关 系 。 可见铲运机前方车体定位参考点的轨迹 曲线变 化情况与车速 、 转 向角 、 转 向角变化速率 及结 构参数 z 、 f ： 有关 ， 其中 z 、 f 为铲运机结构参数 ， 铲 运机制造完成后 即不会改变 。车速 对铲运机轨 迹曲线有影响 ， 由于车速 V 一般在正常跟踪时选取 一 个适宜值 ， 这样只有转 向角 O d 和转向角变化速率 J 一 是可变量 ， 因此 ， 对铲运机轨迹的控制 ， 主要是对 U j _ 铲运机转向角和转向角变化速率- 5 7 的控制。, 4 结论 1 ) 本文首先对 铲运机跟踪轨迹 曲线 和轨迹参 数