智能掘进机横向优化控制方法及系统

文档序号:24655948发布日期:2021-04-13 21:02阅读:来源:国知局

技术特征:
1.一种智能掘进机横向优化控制方法,其特征在于,包括以下步骤:s1、扫描矿井巷道,获取巷道左巷壁边界点、右巷壁边界点的位置坐标,并拟合生成左巷壁、右巷壁的边界线;s2、构建掘进机运动学模型,并基于所述运动学模型与左巷壁、右巷壁的边界线生成不同铰接角速度下的轨迹簇;s3、遍历轨迹簇中的每条轨迹,根据每条轨迹相对中心线的平均误差、最大误差、终端误差以及期望控制输入设计控制评价函数;所述中心线是指左巷壁的边界线与右巷壁的边界线之间的中线,且左巷壁的边界线与右巷壁的边界线关于该中线对称;s4、根据控制评价函数,从轨迹簇中选择最优运动轨迹,并计算最优期望铰接角;优选地,根据所述最优运动轨迹相对中心线的平均误差、最大误差、终端误差进行巷壁碰撞分级预警;优选地,还包括:s5、根据最优期望铰接角更新掘进机位置与姿态,判断当前位置是否到达终点,若到达终点则停止,否则返回步骤s1。2.根据权利要求1所述的智能掘进机横向优化控制方法,其特征在于,步骤s1的具体实现过程包括:1)掘进机前向激光雷达扫描矿井巷道,得到球坐标系下的激光点集合ω
point
={point
i
|i=1、2、

、n

1、n},其中,point
i
=(r
i

i

i
)表示第i个激光点,n为集合中激光点的个数;激光雷达与第i个激光点的连线为r
i
,r
i
在xoy平面的投影为ω
i
表示r
i
与之间的夹角,α
i
表示r
i
与激光雷达正y轴方向的夹角,r
i
表示激光雷达到第i个激光点的距离;x,y,z轴是指激光雷达三维直角坐标系的坐标轴;2)将球坐标系下的每个激光点均转换到车辆坐标系下,得到车辆坐标系下的激光点集合θ
pointinveh
={pointinveh
i
|i=1、2、

、n

1、n},其中,车辆坐标系是以掘进机前桥中点为原点,以车辆航向为轴方向,以垂直车辆航向向左的方向为轴方向,以垂直轴和轴且竖直向上的方向为轴方向;轴方向;分别表示车辆坐标系下第i个激光点的轴、轴与轴坐标;3)滤除激光点集合θ
pointinveh
={pointinveh
i
|i=1、2、

、n

1、n}中高度h
i
小于h
ground
的激光点,其中,h
i
=abs(z
i
),h
ground
表示地面激光点的高度阈值;4)将经步骤3)处理后的激光点集合φ
pointinwall
={pointinwall
c
|c=1、2、

、m

1、m}投影到车辆坐标系的平面,得到二维激光点集合ξ
pointintwodim
={pointintwodim
c
|c=1、2、

、m

1、m},其中表示第c个二维激光点;5)从所述二维激光点集合ξ
pointintwodim
={pointintwodim
c
|c=1、2、

、m

1、m}中提取左、右巷壁的边界点,采用三次样条插值分别拟合左、右巷壁的边界点,生成左、右巷壁边界线;优选地,从所述二维激光点集合ξ
pointintwodim
={pointintwodim
c
|c=1、2、

、m

1、m}中提取左、右巷壁的边界点的具体实现过程包括:5a)从车辆坐标系原点开始,沿着轴按照固定间隔space
p
进行等间隔采样,直至采样到集合ξ
pointintwodim
中轴坐标的最大值处,将采样点集合记为p
samplingpoint

{samplingpoint
j
|j=1、2、

、j

1、j},其中,j表示第j个采样点,是第j个采样点的横坐标,j为采样点的个数;5b)遍历采样点集合p
samplingpoint
中的每个采样点;对于每个所述采样点,确定集合ξ
pointintwodim
中距离该采样点最近的左侧激光点和右侧激光点,即得到该采样点对应的左巷壁边界点和右巷壁边界点;将所有采样点对应的左巷壁边界点的集合记为bound
leftpoint
={leftpoint
j
|j=1、2、

、j

1、j},将所有采样点对应的右巷壁边界点的集合记为bound
rightpoint
={rightpoint
j
|j=1、2、

、j

1、j};表示第j个左巷壁边界点,表示第j个右巷壁边界点,表示第j个左巷壁边界点的坐标,表示第j个左巷壁边界点的坐标,表示第j个右巷壁边界点的坐标,表示第j个左巷壁边界点的坐标,上标left表示左侧巷壁,上标right表示右侧巷壁;优选地,确定集合ξ
pointintwodim
中距离任一采样点最近的左侧激光点和右侧激光点的具体实现过程包括:

计算当前采样点与集合ξ
pointintwodim
中每个激光点的距离;

集合ξ
pointintwodim
中轴坐标为正值且与所述当前采样点距离最小的激光点即为左巷壁边界点,集合ξ
pointintwodim
中轴坐标为负值且与所述当前采样点距离最小的激光点即为右巷壁边界点。3.根据权利要求2所述的智能掘进机横向优化控制方法,其特征在于,步骤2)的具体实现过程包括:a)利用以下转换公式将第i个激光点从球坐标系转换到激光雷达三维直角坐标系:其中,x
i
,y
i
,z
i
是第i个激光点在激光雷达三维直角坐标下的坐标;b)将第i个激光点从倾斜的激光雷达三维直角坐标系转换到水平直角坐标系,转换公式为:式为:是指第i个激光点在激光雷达水平直角坐标下的坐标,β为激光雷达向下倾斜的角度;若激光雷达未向下倾斜,则β=0;c)将第i个激光点从水平直角坐标系转换到车辆坐标系,转换公式为:其中,为激光雷达水平直角坐标系轴与车辆坐标系的夹角;δx、δy、δz为激光雷达相对于车辆坐标系原点的偏移量。4.根据权利要求1所述的智能掘进机横向优化控制方法,其特征在于,步骤s2的具体实现过程包括:步骤1,以掘进机前桥中点为参考点,构建掘进机运动学模型如下:
其中,(x
f
,y
f
)为前桥中点坐标,θ
f
为前桥航向角,v为前桥车速,γ为掘进机铰接角,l
f
为前桥轴距,l
r
为后桥轴距;步骤2,分别向左、右巷壁边界线预瞄轴长度为l
pre
的距离,得到左预瞄点和右预瞄点计算预瞄距离l
pre
:l
pre
=l
min
+p
·
v;其中,l
min
表示最短预瞄距离,p为时间常数;步骤3,以车辆坐标系原点到预瞄点连线的中垂线与轴的交点为转向中心,计算得到左转向半径和右转向半径和右转向半径其中,其中,sign()为取符号函数;则左边界约束和右边界约束下的转向半径范围为步骤4,考虑掘进机执行机构的左右转向半径限幅与左边界约束、右边界约束下的转向半径范围求交集得到转向半径区间为[

∞,r
right
]∪[r
left
,+∞];步骤5,根据掘进机运动学模型以及转向半径区间,计算得到转向半径区间[

∞,r
right
]∪[r
left
,+∞]对应的铰接角速度区间为其中计算公式如下:步骤6,将铰接角速度区间按照长度为space
γ
的固定间隔采样进行离散化,将离散后的铰接角速度序列记为υ
γ
={γ
k
|k=1、2、

、k

1、k},其中,k表示铰接角速度序列中的第k个铰接角速度,k为铰接角速度序列中离散铰接角速度数量;将铰接
角速度序列υ
γ
中的铰接角速度代入掘进机运动学模型,推算得到不同铰接角速度下的轴长度为l
pre
的轨迹簇;其中,k为轨迹簇中轨迹数量,k表示轨迹簇中的第k条轨迹。5.根据权利要求1所述的智能掘进机横向优化控制方法,其特征在于,步骤s3的具体实现过程包括:i)将轨迹簇中每一条轨迹的轴按照长度为的固定间隔采样进行离散化,得到离散后的轨迹点并计算得到每个轨迹点对应左巷壁边界线和右巷壁边界线的最短距离其中,j表示轨迹中的第j个轨迹点,j=1、2、

、j

1、j,j为轨迹簇中每条轨迹的轨迹点数量,ii)将每个轨迹点对应左巷壁边界线和右巷壁边界线的最短距离相减,得到每条轨迹相对中心线的误差errordis
k,j
;iii)根据errordis
k,j
计算得到每条轨迹相对中心线的平均误差最大误差终端误差iv)设计控制评价函数如下:其中,w1、w2、w3、w4为权重系数,w1、w4为常数。6.根据权利要求5所述的智能掘进机横向优化控制方法,其特征在于,其中,w
2,2
>>w
2,1
、w
3,2
>>w
3,1
;表示最大误差界值;为终端误差界值。7.根据权利要求1所述的智能掘进机横向优化控制方法,其特征在于,步骤s4中,选择最小的控制评价函数f
min
所对应最优运动轨迹,进一步确定最优运动轨迹对应的铰接角速度并计算得到最优期望铰接角γ
opt
:其中,t
c
为控制周期;γ
opt,last
为上一周期的最优期望铰接角,γ
opt,last
的初始值为0。8.根据权利要求1所述的智能掘进机横向优化控制方法,其特征在于,步骤s4中,根据最优运动轨迹相对中心线的平均误差最大误差终端误差计算巷壁碰撞预警等级的具体实现过程包括:a)根据最大误差计算最大误差等级:其中,其中,为常数;根据平均误差计算平均误差等级:
其中,其中,为常数;根据终端误差计算终端误差等级:其中,其中,为常数;b)根据误差等级level
max
、level
ave
、level
end
计算巷壁碰撞预警等级:其中,warn_value=level
max
·
weight
max
+level
ave
·
weight
ave
+level
end
·
weight
end
;warn_level=4为四级风险预警,建议安全员接管;warn_level=3为三级风险预警,提醒安全员注意;warn_level=2为二级风险预警,warn_level=1为一级风险预警,此时为较安全状态,可进行自动驾驶;weight
max
、weight
ave
、weight
end
为常数。9.根据权利要求1所述的智能掘进机横向优化控制方法,其特征在于,步骤s5中,判断当前位置是否到达终点的具体实现过程包括:从二维激光点集合ξ
pointintwodim
={pointintwodim
c
|c=1、2、

、m

1、m}中找到轴坐标最大的点pointbound,即为最远边界点;判断最远边界点的轴坐标值是否小于终点判别阈值r
end
;若小于终点判别阈值r
end
,则表示到达终点,否则表示还未到达终点。10.一种智能掘进机横向优化控制系统,其特征在于,包括计算机设备;所述计算机设备被配置或编程为用于执行权利要求1~9之一所述方法的步骤。
当前第2页1 2 3 
网友询问留言 已有0条留言
  • 还没有人留言评论。精彩留言会获得点赞!
1