Research on pitch control of coal mine roadheader based on fuzzy neural network PID
-
摘要: 目前煤矿掘进机俯仰控制主要采用PID控制方法,在掘进机俯仰控制时变性与液压系统非线性情况下的控制精度不高。掘进机俯仰控制通过控制液压缸行程实现,将传统PID算法与模糊控制、神经网络等相结合,可有效提高液压缸行程控制精度。提出了一种基于模糊神经网络PID的煤矿掘进机俯仰控制方法。通过分析掘进机支撑部运动学关系,得到俯仰角与支撑部液压缸的数学关系;介绍了掘进机俯仰控制液压系统工作原理,建立了液压系统及其传递函数模型;将模糊控制与神经网络相结合,形成模糊神经网络,利用模糊神经网络优化PID控制参数,再结合支撑机构数学模型和液压系统传递函数模型,建立掘进机俯仰角模糊神经网络PID控制模型,实现煤矿掘进机俯仰机构自动精确控制。该方法可使掘进机俯仰机构更加快速、准确到达预设位置,解决掘进机俯仰控制中的时变性与非线性难题。仿真结果表明:模糊神经网络PID控制算法相较于模糊PID和PID控制算法,跟踪误差分别降低了69.34%和74.49%。通过液压缸位移控制模拟煤矿掘进机在突变工况和跟随工况下的俯仰控制,结果表明:模糊神经网络PID控制算法相比模糊PID和PID控制算法,俯仰控制跟踪误差最小,对位置信号的平均响应时间分别缩短了27.22%和50.33%,动态控制性能更好。Abstract: Currently, PID control method is mainly used for the pitch control of coal mine roadheader, and the control precision is not high in the case of time-varying and nonlinear hydraulic system during the pitch control of roadheader. The pitch control of roadbeader is realized by controlling the stroke of the hydraulic cylinder. Combining the traditional PID algorithm with fuzzy control and neural network, the accuracy of the stroke control of the hydraulic cylinder can be effectively improved. In order to solve the above problems, a pitch control method for coal mine roadheader based on fuzzy neural network PID is proposed. By analyzing the kinematic relationship of the support part of the roadheader, the mathematical relationship between the pitch angle and the hydraulic cylinder of the support part is obtained. The working principle of the pitch control hydraulic system of the roadheader is introduced, and the hydraulic system and its transfer function model are established. The method combines fuzzy control with neural networks to form a fuzzy neural network. The method optimizes PID control parameters by using the fuzzy neural network. The method combines the mathematical model of the support mechanism and the transfer function model of the hydraulic system to establish a fuzzy neural network PID control model for the pitch angle of the roadheader. It achieves automatic and precise control of the pitch mechanism of the coal mine roadheader. This method can make the pitch mechanism of the roadheader reach the preset position more quickly and accurately, solving the time-varying and nonlinear problems in the pitch control of roadheader. The simulation results show that the fuzzy neural network PID control algorithm reduces tracking errors by 69.34% and 74.49% respectively compared to fuzzy PID and PID control algorithms. The method simulates the pitch control of coal mine roadheaders under sudden and following working conditions through hydraulic cylinder displacement control. The results show that compared with fuzzy PID and PID control algorithms, the fuzzy neural network PID control algorithm has the smallest pitch control tracking error, shortens the average response time to position signals by 27.22% and 50.33% respectively, and has better dynamic control performance.
-
0. 引言
煤炭是我国的主要能源和重要战略资源。目前我国煤矿智能化水平较低,井下作业人员仍然较多、安全风险大,加快煤炭企业对煤矿机器人的研发和应用是破解当前安全生产矛盾、实现减人增效的重要途径和现实需求。对于各类煤矿移动机器人,精确定位与地图构建是其推广应用亟待实现的共性关键技术[1]。同步定位与地图构建(Simultaneous Localization and Mapping,SLAM)具有在定位的同时实时重构场景模型的功能,为辅助驾驶、自主导航等提供定位建图功能,近10 a来得到了长足的发展并已逐渐走向工业应用领域。
在煤矿井下SLAM方法方面,国外较早开展了相关研究,但主要采用手持设备[2]、移动小车[3]、探测机器人[4]、矿车[5]搭载激光雷达及惯性测量单元(Inertial Measurement Unit,IMU)等传感器进行环境建模。D.Tardioli等[6]分析了其所在课题组多年来在地下隧道机器人领域开展的相关研究进展,主要探讨了基于激光雷达、视觉相机、里程计等传感器的SLAM方法在隧道环境的应用。美国国防部高级研究计划局(Defense Advanced Research Projects Agency,DARPA)于2018—2021年举办的SubT赛事面向城市地下空间、隧道和天然洞穴环境下的机器人应用,相关研究针对光照变化[7]、场景退化[8]及巷道特征利用[9]等地下场景SLAM中的典型问题提出了新的解决方案。国内方面,马宏伟等[10]提出了基于深度相机进行井下定位建图实现移动机器人自主导航的方案。陈先中等[11]提出了一种煤矿地下毫米波雷达点云成像、结合深度学习处理稀疏点云的SLAM方法构想。杨健健等[12]研究了基于Hector−SLAM的掘进机器人巷道环境感知方法。Li Menggang等[13]提出了一种基于激光雷达的NDT(正态分布变换)−Graph SLAM方法,利用平面约束减轻地图漂移,用于煤矿救援机器人的高效建图任务。高士岗等[14]在国家能源神东煤炭榆家梁煤矿初步应用了搭载Exscan激光扫描仪的巡检机器人,基于SLAM技术构建了动态数字化工作面,三维点云模型实测精度为0.2 m。总体来看,针对煤矿机器人SLAM的研究集中于解决激光雷达、相机、毫米波雷达等传感器在特定场景、特定条件下的初步应用和性能改善问题,对井下多种条件耦合的复杂环境长期定位建模技术仍有待进一步攻关。
在三维激光SLAM研究方面,以迭代最近点(Iterative Closest Point,ICP)为代表的扫描匹配方法研究较早[15]。6D SLAM[16]的前端利用大量原始点云进行ICP配准,后端使用全局松弛化,实现了非实时的高精地图构建。与使用全部点云不同,为了减少计算量、提高算法精度,另一类基于特征的方法被提出。Zhang Ji等[17]提出了一种激光里程计与建图(LiDAR Odometry and Mapping, LOAM)方法,通过计算曲率分割边点和面点,构建点−线和点−面优化问题,是目前诸多激光里程计采用的方法,但由于缺乏回环检测,LOAM方法在长距离应用中仍然有较大的漂移。Shan Tixiao等[18]基于LOAM方法提出LeGO−LOAM(Light Weight and Ground−optimized LOAM)方法,采用了与LOAM方法同样的特征提取方法,通过深度图像分割剔除地面的干扰。J.Weingartent等[19]和A.J.B.Trevor等[20]均提出构建平面特征进行数据关联,后端分别基于扩展卡尔曼滤波(Extended Kalman Filter,EKF)和图优化构建SLAM。此类方法必须要求环境中有大量可提取的平面,形成稀疏平面特征地图。
在激光−惯性融合定位建图方面,目前主要分为松耦合和紧耦合2种方式。松耦合通过构建惯性导航独立进行状态传播,利用激光进行观测更新。紧耦合方法近年来逐渐得到关注。Ye Haoyang等[21]提出了一种雷达惯性里程计与建图(Lidar Inertial Odometry and Mapping,LIO−mapping)方法,使用面特征构建相邻帧之间的点−面约束,同时构建IMU预积分约束,基于因子图联合优化所有帧间观测,在特征较多的大场景条件下难以满足实时运行需求。Shan Tixiao等[22]提出了一种基于平滑建图的雷达惯性里程计(Lidar Inertial Odometry via Smoothing and Mapping,LIO−SAM)方法,前端同样采用LOAM进行特征提取,后端利用因子图框架融合IMU,取得了较好的效果。除了优化方案,滤波方法也被用于激光惯性传感器融合中。Qin Chao 等[23]提出的用于鲁棒和高效导航的雷达惯性状态估计器(Lidar−Inertial State Estimator for Robust and Efficient Navigation,LINS)和Xu Wei等[24]提出的快速雷达惯性里程计(Fast Lidar Inertial Odometry, Fast−LIO)系列方法,基于迭代扩展卡尔曼滤波(Iterative Extended Kalman Filter,IEKF)实现激光和IMU紧耦合,在观测更新时采用了类似优化方法的迭代过程,但没有使用历史数据,在复杂场景中的精度和鲁棒性较差。杨林等[25]提出了激光惯性融合SLAM方法,前端利用IEKF设计了激光惯性里程计,通过构建关键帧在后端采用位姿图优化进行联合优化,降低了系统累计误差。
煤矿井下机器人SLAM是当前研究热点,但针对提高激光SLAM在井下复杂条件下的精度、鲁棒性的研究仍然不足;传统激光SLAM方法在此类环境下存在累计误差迅速增大、旋转过程鲁棒性差、特征关联错误率高等问题;现有激光−惯性融合的定位建图紧耦合融合机制仍需进一步探讨,以期进一步提高激光−惯性融合SLAM方法对煤矿井下复杂环境的适应能力。为此,本文提出了一种煤矿机器人LiDAR/IMU紧耦合SLAM方法(Lidar−Inertial SLAM,LI−SLAM方法)。基于点−线和点−面扫描匹配构建激光相对位姿约束,融合IMU预积分因子约束和回环因子约束,利用因子图优化实现了LiDAR/IMU紧耦合的SLAM。构建了雷达相对位姿因子约束模型,解析推导了雷达相对位姿约束的残差、雅可比和协方差矩阵;基于因子图模型,构建了激光相对位姿约束与IMU预积分约束的紧耦合数据融合机制,提升了对煤矿井下颠簸路面、机器人剧烈旋转等复杂工况的适应能力。该方法可帮助煤矿移动机器人在井下复杂地形与环境工况中实现鲁棒精确的定位建图,为煤矿移动机器人自主导航提供关键技术支撑。
1. LI−SLAM框架
LI−SLAM框架如图1所示。激光约束构建阶段包括5个步骤:① 雷达数据预处理:利用IMU的短时状态估计传播运动状态,校正时段内采集的雷达线扫运动畸变,进行区域滤波等预处理过程。② 特征提取:提取点云中的边和面特征。③ 关键帧与子图构建:提取关键帧并构建局部子图。④ 特征关联:利用特征关联和扫描匹配计算相对位姿变换。⑤ 雷达相对位姿因子构建:基于帧到子图的扫描匹配建立局部雷达相对位姿因子。完成激光约束构建后,执行激光−惯性紧耦合融合机制:与同时段内惯性预积分因子在滑动窗口中进行联合优化,实现状态估计,利用雷达相对位姿因子限制惯导零偏等参数的漂移,实现紧耦合的联合状态估计,同时利用子图关键帧构建全局地图。迭代执行上述过程,可实现在线激光−惯性紧耦合SLAM。
定义惯导坐标系B,雷达坐标系L,激光惯性里程计坐标系O。LI−SLAM的因子图模型如图2所示。待估计的变量
$ {\boldsymbol{x}}_{{B_i}}^O $ 是优化窗口内所有的IMU相对于局部坐标系下的位置p、速度v、姿态q、加速度零偏ba(a为加速度)、角速度零偏bω(ω为角速度)及从惯导到雷达坐标系间的外参${{\boldsymbol{T}}_{\boldsymbol{B}}^{\boldsymbol{L}}} $ 。从i,i+1,…,j时刻所有的待估计变量$ {\boldsymbol{x}}_{{B_i}}^O $ ,$ \;{\boldsymbol{x}}_{{B_{i + 1}}}^O $ ,…,$ {\boldsymbol{x}}_{{B_j}}^O $ 组成的集合为$ \;\chi $ :$$ \chi = \left[ {\begin{array}{*{20}{c}} {{\boldsymbol{x}}{{_{{B_i}}^{O\,{{\rm{T}}}}}}\;\;{\boldsymbol{x}}{{_{{B_{i + 1}}}^{O\,{{\rm{T}}}}}}}& \cdots &{{\boldsymbol{x}}{{_{{B_j}}^{O\,{{\rm{T}}}}}}}&{{\boldsymbol{T}}_B^L} \end{array}} \right] $$ (1) 优化窗中第i时刻对应的状态变量和外参分别表示为
$$ {\boldsymbol{x}}_{{B_i}}^O = {\left[ {\begin{array}{*{20}{c}} {{\boldsymbol{p}}{{_{{B_i}}^{O\,{{\rm{T}}}}}}}&{{\boldsymbol{v}}{{_{{B_i}}^{O\,{{\rm{T}}}}}}}&{{\boldsymbol{q}}{{_{{B_i}}^{O\,{{\rm{T}}}}}}}&{{\boldsymbol{b}}_{{a_i}}^{\rm{T}}}&{{\boldsymbol{b}}_{{g_i}}^{\rm{T}}} \end{array}} \right]^{\rm{T}}} $$ (2) $$ {\boldsymbol{T}}_B^L = {\left[ {\begin{array}{*{20}{c}} {{\boldsymbol{p}}{{_B^{L{{\rm{T}}}}}}}&{{\boldsymbol{q}}{{_B^{L{{\rm{T}}}}}} } \end{array}} \right]^{\rm{T}}} $$ (3) 采用子图进行扫描匹配过程。在局部优化过程中,仅滑动窗口以内的各类因子参与优化过程,窗口外的状态利用边缘化技术固定。基于因子图模型的优化目标函数为
$$ {\boldsymbol{\hat \chi }} = \mathop {\arg \min }\limits_{\boldsymbol{\chi }} \left\{ {{\phi _{{\rm{prior}}}}({\boldsymbol{\chi }}) + {\phi _L}({\boldsymbol{\chi }}) + {\phi _B}({\boldsymbol{\chi }})} \right\} $$ (4) 式中:
$ \;{\boldsymbol{\hat \chi }} $ 为优化求解后观测变量组成的集合;$ {\phi _{{\rm{prior}}}}({\boldsymbol{\chi }}) $ 为先验项约束;$ {\phi _L}({\boldsymbol{\chi }}) $ 为雷达相对位姿约束;$ {\phi _B}({\boldsymbol{\chi }}) $ 为惯性预积分约束。2. 激光约束构建
2.1 雷达数据预处理
点云畸变在机械式激光雷达高速运动问题中是必须解决的问题。现有手段通常基于匀速运动假设,利用线性插值重新部署激光点云的位置。这种方法对于剧烈的非线性运动适应性差。本文使用高频IMU进行状态传播,以恢复每个点的运动。设定第i时刻和第j时刻对应的雷达观测帧为Si和Sj。对第Si帧与第Sj帧之间的IMU观测进行积分,作为激光扫描匹配的初值。相邻采样时刻间认为加速度和角速度未发生变化,加速度、角速度零偏是固定值。IMU加速度观测值为
$ {{\boldsymbol{\hat a}}_t} $ ,角速度为$ {{\boldsymbol{\hat \omega }}_t} $ ,从t(t=i,i+1,…, j)时刻传播的状态包括位置$ {\boldsymbol{p}}_{{B_j}}^O $ 、速度$ {\boldsymbol{v}}_{{B_j}}^O $ 、方向$ {\boldsymbol{q}}_{{B_j}}^O $ 。$$ \left\{ \begin{array}{l} \begin{gathered} {\boldsymbol{p}}_{{B_j}}^O = {\boldsymbol{p}}_{{B_i}}^O + \sum\limits_{t = i}^{j - 1} {\left( {{\boldsymbol{v}}_{{B_t}}^W\Delta t + \frac{1}{2}\left( {{\boldsymbol{R}}_t^W({{{\boldsymbol{\hat a}}}_t} - {{\boldsymbol{b}}_{{{\boldsymbol{a}}_t}}}) - {{\boldsymbol{g}}^O}} \right)\Delta {t^2}} \right)} \\ {\boldsymbol{v}}_{{B_j}}^O = {\boldsymbol{v}}_{{B_i}}^O + \sum\limits_{t = i}^{j - 1} {\left( {{\boldsymbol{R}}_t^O({{{\boldsymbol{\hat a}}}_t} - {{\boldsymbol{b}}_{{{\boldsymbol{a}}_t}}}) - {{\boldsymbol{g}}^O}} \right)} \Delta t \\ {\boldsymbol{q}}_{{B_j}}^O = {\boldsymbol{q}}_{{B_j}}^O \otimes \prod\limits_{t = i}^{j - 1} {\left( {\left[ {\begin{array}{*{20}{c}} {\dfrac{1}{2}({{{\boldsymbol{\hat \omega }}}_t} - {{\boldsymbol{b}}_{{{\boldsymbol{\omega}} _t}}})\Delta t} \\ 1 \end{array}} \right]} \right)} \\ \end{gathered} \end{array} \right. $$ (5) 式中:
${{{\boldsymbol{v}}}}_{{B_t}}^W$ 为t时刻世界坐标系下的速度;$ {{\boldsymbol{g}}^O} $ 为里程计坐标系下的重力加速度;$ {\boldsymbol{R}}_t^W $ 和$ {\boldsymbol{R}}_t^O $ 为t时刻相对于世界坐标系和里程计坐标系的旋转矩阵。利用在线估计获得的IMU与雷达之间的外参变换
${\boldsymbol{T}}_B^L $ ,计算当前雷达的位置和速度。以各点时间戳将观测帧Sj通过线性插值重设位置,统一到Sj末端时刻的基坐标系,完成点云畸变的校正。利用区域滤波提取雷达测距有效范围内的点云参与后续特征提取、关键帧建立与状态估计。2.2 特征提取
采用LOAM[17]的特征提取方法设计点云粗糙度指标作为线特征和面特征的区分方式。设定某个雷达扫描周期K内点云为PK,点云PK中2个点
$ ({P_A},{P_U}) \in {P_K} $ ,在雷达坐标系下的位置坐标可以表示为$ ({\boldsymbol{X}}_{K,A}^L,{\boldsymbol{X}}_{K,U}^L) $ 。s为点云PK中点PA所在的行中的点组成的连续点集,则点PA所在局部曲面的粗糙度为$$ C = \frac{1}{{\left| s \right| \left\| {{\boldsymbol{X}}_{K,A}^L} \right\|}}\left\| {\sum\limits_{U \in s,U \ne A} {({\boldsymbol{X}}_{K,A}^L - {\boldsymbol{X}}_{K,U}^L)} } \right\| $$ (6) 式中A,U为点云中的2个点对应的序号。
根据以下规则提取特征:① 边缘点:取粗糙度C最大的某些个点排序即为曲率较大的边缘点。② 平面点:取粗糙度C最大的某些个点排序即为曲率较小的平面点。③ 为了使点云特征在雷达坐标系下的各个区域分布尽可能均匀,将每个扫描帧等分为若干区域,每个区域提取2个边点和4个平面点。
2.3 关键帧与子图构建
为处理高频激光观测数据,采用关键帧进行特征匹配和雷达约束的构建。预设最小平移距离阈值、最小旋转角度阈值,若达到其中一个条件,则设置当前帧为关键帧。关键帧位姿变化计算公式可以参考文献[13]。
在前端匹配中,采用帧到子图的方式进行扫描匹配,组合固定数量的关键帧形成子图。设关键帧k的边和面特征集合为 Fek和 Fhk(e,h分别表示边和面,下同),n个关键帧组成的特征集合为{
${{{F}}_{k - n}},{{{F}}_{k - n + 1}} \cdots ,{{{F}}_k}$ }$,{{{F}}_k} = {{F}}_{\rm{e}}^k \cup {{F}}_{\rm{h}}^k$ 。利用各关键帧的变换可以将特征转换到局部里程计坐标系O下。转换后,拼接关键帧,组成局部子图$ {{{M}}_k} $ ,其中包含变换到里程计坐标系后的边和面两类特征,${{{M}}_k} = \{ {{M}}_k^{\rm{e }}\cup {{M}}_k^{\rm{h}}\}$ 。采用下采样方法剔除同一个体素栅格中的重复特征,避免冗余的特征匹配。${{M}}_k^{\rm{e}}$ 和${{M}}_k^{\rm{h}}$ 的下采样精度根据环境特点、计算量按照经验设置,此处设为0.1 m和0.2 m,子图内关键帧数量为n=20。2.4 帧到子图的特征关联与扫描匹配
新获得的雷达扫描帧
${{{F}}_{k + 1}} \left( {{{{F}}_{k + 1}} = {{F}}_{k + 1}^{\rm{e}} \cup {{F}}_{k + 1}^{\rm{h}}} \right)$ 是基于雷达坐标系,需要将当前帧特征从雷达坐标系L转换到里程计坐标系O下。利用IMU预测的机器人运动状态${{{\overline {\boldsymbol{T}}}}_{k + 1}}$ ,可以获得转换后的特征集合${{{F}'}_{{{k}} + 1}}$ 。特征关联即寻找特征集合${{{F}'}_{{{k}} + 1}}$ 中边缘特征${{F}}_{k + 1}^{{\rm{e}}'}$ 和面特征${{F}}_{k + 1}^{{\rm{h}}'}$ 在局部子图${{{M}}_k} = \{ {{M}}_k^{\rm{e}} \cup {{M}}_k^{\rm{h}}\}$ 中的关联特征。边缘点和平面点的特征关联方法如图3所示。图3(a)中,对于任意边缘特征I,基于kd-tree寻找${{M}}_k^{\rm{e}}$ 中最近的点A、${{M}}_k^{\rm{e }}$ 中与A最近且与以上2个点不共线的点U,则(A,U)为特征I的关联边特征点。图3(b)中,可以在${{M}}_k^{\rm{h}}$ 中找到与平面特征I最近的A、${{M}}_k^{\rm{h}}$ 中与A最近的2个不共线的点V和U,即为面特征的关联面特征点(A,V,U)。至此,构建点到线的距离
$ {d_{\rm{e}}} $ :$$ {d_{\rm{e}}} = \frac{{\left| {\left( {{\boldsymbol{X}}_{k + 1,I}^{\rm{e}} - {\boldsymbol{X}}_{k,A}^{\rm{e}}} \right) \times \left( {{\boldsymbol{X}}_{k + 1,I}^{\rm{e}} - {\boldsymbol{X}}_{k,U}^{\rm{e}}} \right)} \right|}}{{\left| {{\boldsymbol{X}}_{k,A}^{\rm{e}} - {\boldsymbol{X}}_{k,U}^{\rm{e}}} \right|}} $$ (7) 式中:
${\boldsymbol{X}}_{k + 1,I}^{\rm{e}}$ 为关键帧k+1对应的边特征I的坐标;${\boldsymbol{X}}_{k,A}^{\rm{e}}$ ,${\boldsymbol{X}}_{k,U}^{\rm{e}}$ 为关键帧k对应的面特征A和U的坐标。构建点到面的距离dh:
$$ {d_{\rm{h}}} = \frac{{\left| {\left( {{\boldsymbol{X}}_{k + 1,I}^{\rm{h}} - {\boldsymbol{X}}_{k,A}^{\rm{h}}} \right) \cdot \left( {{\boldsymbol{X}}_{k,A}^{\rm{h}} - {\boldsymbol{X}}_{k,U}^{\rm{h}}} \right) \times \left( {{\boldsymbol{X}}_{k,A}^{\rm{h}} - {\boldsymbol{X}}_{k,V}^{\rm{h}}} \right)} \right|}}{{\left| {\left( {{\boldsymbol{X}}_{k,A}^{\rm{h}} - {\boldsymbol{X}}_{k,U}^{\rm{h}}} \right) \times \left( {{\boldsymbol{X}}_{k,A}^{\rm{h}} - {\boldsymbol{X}}_{k,V}^{\rm{h}}} \right)} \right|}} $$ (8) 式中:
${\boldsymbol{X}}_{k + 1,I}^{\rm{h}}$ 为关键帧k+1对应的面特征I的坐标;${\boldsymbol{X}}_{k,A}^{\rm{h}}$ ,${\boldsymbol{X}}_{k,U}^{\rm{h}}$ ,${\boldsymbol{X}}_{k,V}^{\rm{h}}$ 为关键帧k对应的面特征A,U,V对应的坐标。利用帧与子图中的点到线的距离
$ {d_{\rm{e}}} $ 与点到面的距离dh,可以组成距离矩阵d,基于扫描匹配方法,可以建立以下代价函数$ f\left( {{{{{\overline {\boldsymbol{T}}}}}_{k + 1}}} \right) $ 来求解帧到子图的最优位姿变换关系$ {{{\overline {\boldsymbol{T}}}}_{k + 1}} $ :$$ \left\{ \begin{array}{l} f\left( {{{{{\overline {\boldsymbol{T}}}}}_{k + 1}}} \right) = {\boldsymbol{d}} \\ {\boldsymbol{d}} = \left[ {\begin{array}{*{20}{c}} {{d_{\rm{e}}}} \\ {{d_{\rm{h}}}} \end{array}} \right] \end{array} \right. $$ (9) 利用列文伯格马切尔特(LM)方法可以构建以下优化方程来求解帧间位姿的变换:
$$ \left\{ \begin{array}{l} \min \dfrac{1}{2}\left\| {f({{{{\overline {\boldsymbol{T}}}}}_{k + 1}}) + {\boldsymbol{J}}{{({{{{\overline {\boldsymbol{T}}}}}_{k + 1}})}^{\rm{T}}}\Delta {{{{\overline {\boldsymbol{T}}}}}_{k + 1}}} \right\| \\ {\rm{s.t.}}\;\;{\left\| {{\boldsymbol{D}}\Delta {{{{\overline {\boldsymbol{T}}}}}_{k + 1}} < \mu } \right\|_2} \end{array} \right. $$ (10) 式中:J为雅可比矩阵,
${\boldsymbol{J}} = {{\partial f} \mathord{\left/ {\vphantom {{\partial f} {\partial {{\boldsymbol{T}}_{k + 1}}}}} \right. } {\partial {{\boldsymbol{T}}_{k + 1}}}}$ ;D为系数矩阵;$ \mu $ 为信赖区域半径。迭代推导出最优估计:
$$ {{\boldsymbol{T}}_{k + 1}} \leftarrow {{\boldsymbol{T}}_{k + 1}} - {\left( {{{\boldsymbol{J}}^{\rm{T}}}{\boldsymbol{J}} + \lambda {\rm{diag}}\left( {{{\boldsymbol{J}}^{\rm{T}}}{\boldsymbol{J}}} \right)} \right)^{ - 1}}{{\boldsymbol{J}}^{\rm{T}}}{\boldsymbol{d}} $$ (11) 式中
$ \lambda $ 为阻尼因子。采用以上列文伯格马切尔特优化方法收敛后可以得到当前位姿估计值
${{\boldsymbol{T}}_{k + 1}} = {{{\overline {\boldsymbol{T}}}}_{k + 1}}$ 。位姿Tk和Tk+1之间的相对位姿变换为$$ \Delta {{\boldsymbol{T}}_{k,k + 1}} = {\boldsymbol{T}}_k^{ - 1}{{\boldsymbol{T}}_{k + 1}} $$ (12) 2.5 相对位姿因子构建
利用雷达相对位姿构建关键帧位姿之间的约束关系,在滑窗中联合惯性预积分一起参与优化。相邻位姿间的优化目标项为
$$ {\phi _L}({\boldsymbol{x}}) = \frac{1}{2}\left\| {{r_L}({{\boldsymbol{x}}_i},{{\boldsymbol{x}}_j})} \right\|_{\boldsymbol{\varSigma }}^2 $$ (13) 式中:x为待优化状态变量;rL为相对位姿因子残差;xi和xj分别为i和j时刻对应的运动状态;
${\boldsymbol{ \varSigma }} $ 为约束不确定度的协方差矩阵,${\boldsymbol{ \varSigma }}\in {\boldsymbol{R}}^{6\text{}\times 6} $ ,R为旋转矩阵。由于估计过程是基于IMU坐标系,相对位姿观测需要转换到LiDAR坐标系:
$$ {\boldsymbol{T}}_{{L_j}}^{{L_i}} = {\boldsymbol{T}}_B^L{\boldsymbol{T}}_{{B_i}}^{{O^{ - 1}}}{\boldsymbol{T}}_{{B_j}}^O{\boldsymbol{T}}_B^{{L^{ - 1}}} $$ (14) 式中:
$ {\boldsymbol{T}}_{{L_j}}^{{L_i}} $ 为从j时刻变换到i时刻雷达坐标系下的位姿变换矩阵;$ {\boldsymbol{T}}_{{B_i}}^O $ 和$ {\boldsymbol{T}}_{{B_j}}^O $ 为对应i和j时刻IMU坐标系相对于里程计坐标系的位姿变换矩阵。2.5.1 残差计算
从i时刻关键帧到j时刻关键帧之间的位姿变换的观测和期望之间的残差可以表示为
$$ {{{r}}_L}({{{{\boldsymbol{z}}}}}_{{B_j}}^{{B_i}},{\boldsymbol{\chi }}) = {{\overline {\boldsymbol{T}}}}{_{{L_j}}^{{{L_i}}^{ - 1}}}{\boldsymbol{T}}_B^L{\boldsymbol{T}}{_{{B_i}}^{O^{ - 1}}}{\boldsymbol{T}}_{{B_j}}^O{\boldsymbol{T}}{_B^{L^{ - 1}}} $$ (15) 式中
${{{r}}_L}({\boldsymbol{z}}_{{B_j}}^{{B_i}},{\boldsymbol{\chi }})$ 为雷达坐标系下相对于观测$ {\boldsymbol{z}}_{{B_j}}^{{B_i}} $ 和待估计变量$ {\boldsymbol{\chi }} $ 的残差,$ {\boldsymbol{z}}_{{B_j}}^{{B_i}} $ 为从j时刻到i时刻IMU坐标系下的观测。推导可得相对位姿因子残差的旋转部分
$ {{\boldsymbol{r}}_{\boldsymbol{\theta }}} $ 和平移部分${{\boldsymbol{r}}}_{{\boldsymbol{\xi}} }$ :$$ \left\{ \begin{array}{l} {{\boldsymbol{r}}_{\boldsymbol{\theta }}} = {{\overline {\boldsymbol{R}}}}{_{{L_j}}^{{{L_i}}^{ - 1}}}{\boldsymbol{R}}_B^L{\boldsymbol{R}}{_{{B_i}}^{O^{ - 1}}}{\boldsymbol{R}}_{{B_j}}^O{\boldsymbol{R}}{_B^{L^{ - 1}}} \\ {{\boldsymbol{r}}_{\boldsymbol{\xi}} } = {{\overline {\boldsymbol{R}}}}{_{{L_j}}^{{{L_i}}^{ - 1}}}\left[ {{\boldsymbol{R}}_B^L{{\boldsymbol{R}}_{{{B_i}}}^{O^{ - 1}}}\left( {{\boldsymbol{p}}_{{B_j}}^O - {\boldsymbol{p}}_{{B_i}}^O - {\boldsymbol{R}}_{{B_j}}^O{{\boldsymbol{R}}_{B}^{L^{ - 1}}}{\boldsymbol{p}}_B^L} \right) + {\boldsymbol{p}}_B^L - {\boldsymbol{\bar p}}_{{L_j}}^{{L_i}}} \right] \end{array} \right. $$ (16) 式中:
${{\overline {\boldsymbol{R}}}}_{{L_j}}^{{L_i}}$ 和${{\overline {\boldsymbol{p}}}}_{{L_j}}^{{L_i}}$ 分别为雷达坐标系下从j时刻到i时刻观测的旋转矩阵和平移矩阵;$ {\boldsymbol{R}}_B^L $ ,$ {\boldsymbol{p}}_B^L $ 分别为从IMU坐标系到雷达坐标系的外参的旋转矩阵和平移矩阵;$ {\boldsymbol{R}}_{{B_i}}^O $ ,$ {\boldsymbol{R}}_{{B_j}}^O $ 为i时刻和j时刻对应的IMU相对于里程计坐标系下的旋转矩阵;$ {\boldsymbol{p}}_{{B_j}}^O $ ,$ {\boldsymbol{p}}_{{B_i}}^O $ 为j时刻和i时刻IMU在里程计坐标系下的平移矩阵。2.5.2 雅可比矩阵计算
(1) 向量空间:平移残差向量在向量空间参数块的雅可比矩阵为
$$ \left\{ \begin{array}{l} \begin{gathered} \frac{{\partial {{\boldsymbol{r}}_{\boldsymbol{\xi}} }}}{{\partial {\boldsymbol{p}}_{{B_i}}^o}} = - {{\overline {\boldsymbol{R}}}}{_{{L_j}}^{{{L_i}}^{\rm{T}}}}{\boldsymbol{R}}_B^L{\boldsymbol{R}}{_{{B_i}}^{O^{\rm{T}}}} \\ \frac{{\partial {{\boldsymbol{r}}_{\boldsymbol{\xi}} }}}{{\partial {\boldsymbol{p}}_{{B_j}}^o}} = {{\overline {\boldsymbol{R}}}}{_{{L_j}}^{{{L_i}}^{\rm{T}}}}{\boldsymbol{R}}_B^L{\boldsymbol{R}}{_{{B_j}}^{O^{\rm{T}}}} \\ \frac{{\partial {{\boldsymbol{r}}_{\boldsymbol{\xi}} }}}{{\partial {\boldsymbol{p}}_B^L}} = - {{\overline {\boldsymbol{R}}}}{_{{L_j}}^{{{L_i}}^{\rm{T}}}}{\boldsymbol{R}}_B^L{\boldsymbol{R}}{_{{B_i}}^{O^{\rm{T}}}}{\boldsymbol{R}}_{{B_j}}^O{\boldsymbol{R}}{_B^{L^{\rm{T}}}} + {\boldsymbol{\overline R}}{_{{L_j}}^{{{L_i}}^{\rm{T}}}} \\ \end{gathered} \end{array} \right. $$ (17) (2) 流形空间:平移与旋转残差向量在流形空间参数块的雅可比为
$$ \left\{ \begin{array}{l} \begin{split} & \frac{{\partial {{\boldsymbol{r}}_{\boldsymbol{\xi}} }}}{{\partial \delta {\boldsymbol{\theta }}_{{B_i}}^O}} = {{\overline {\boldsymbol{R}}}}{_{{L_j}}^{{{L_i}}^{\rm{T}}}}{\boldsymbol{R}}_B^L{\left\lfloor {{{\boldsymbol{R}}_{{{B_i}}}^{O^{\rm{T}}}}\left( {{\boldsymbol{p}}_{{B_j}}^O - {\boldsymbol{p}}_{{B_i}}^O - {\boldsymbol{R}}_{{B_j}}^O{{\boldsymbol{R}}_{B}^{L^{\rm{T}}}}{\boldsymbol{p}}_B^L} \right)} \right\rfloor _ \times } \\ & \frac{{\partial {{\boldsymbol{r}}_{\boldsymbol{\xi}} }}}{{\partial \delta {\boldsymbol{\theta }}_{{B_j}}^O}} = {{\overline {\boldsymbol{R}}}}{_{{L_j}}^{{{L_i}}^{\rm{T}}}}{\boldsymbol{R}}_B^L{\boldsymbol{R}}{_{{B_i}}^{O^{\rm{T}}}}{\boldsymbol{R}}_{{B_j}}^O{\left\lfloor {{{\boldsymbol{R}}_{B}^{L^{ - 1}}}{\boldsymbol{p}}_B^L} \right\rfloor _ \times } \\ & \frac{{\partial {{\boldsymbol{r}}_{\boldsymbol{\xi}} }}}{{\partial \delta {\boldsymbol{\theta }}_B^L}} = {{\overline {\boldsymbol{R}}}}{_{{L_j}}^{{{L_i}}^{\rm{T}}}}{\boldsymbol{R}}_B^L\left( {{{\left\lfloor {{{\boldsymbol{R}}_{{{B_i}}}^{O^{\rm{T}}}}{\boldsymbol{R}}_{{B_j}}^O{{\boldsymbol{R}}_{B}^{L^{\rm{T}}}}{\boldsymbol{p}}_B^L} \right\rfloor }_ \times } - {{\boldsymbol{R}}_{{{B_i}}}^{O^{\rm{T}}}}{\boldsymbol{R}}_{{B_j}}^O{{\left\lfloor {{{\boldsymbol{R}}_{B}^{L^{\rm{T}}}}{\boldsymbol{p}}_B^L} \right\rfloor }_ \times }} \right) \\ & \frac{{\partial {{\boldsymbol{r}}_\theta }}}{{\partial \delta {\boldsymbol{\theta }}_{{B_i}}^O}} = {{\overline {\boldsymbol{R}}}}{_{{L_j}}^{{{L_i}}^{\rm{T}}}}{\boldsymbol{R}}_B^L{\left\lfloor {{{\boldsymbol{R}}_{{{B_i}}}^{O^{\rm{T}}}}{\boldsymbol{R}}_{{B_j}}^O{{\boldsymbol{R}}_{B}^{L^{\rm{T}}}}} \right\rfloor _ \times } \\ & \frac{{\partial {{\boldsymbol{r}}_\theta }}}{{\partial \delta {\boldsymbol{\theta }}_{{B_j}}^O}} = - {{\overline {\boldsymbol{R}}}}{_{{L_j}}^{{{L_i}}^{\rm{T}}}}{\boldsymbol{R}}_B^L{\boldsymbol{R}}{_{{B_i}}^{O^{\rm{T}}}}{\boldsymbol{R}}_{{B_j}}^O{\left\lfloor {{{\boldsymbol{R}}_{B}^{L^{\rm{T}}}}} \right\rfloor _ \times } \\ & \frac{{\partial {{\boldsymbol{r}}_\theta }}}{{\partial \delta {\boldsymbol{\theta }}_B^L}} = {{\overline {\boldsymbol{R}}}}{_{{L_j}}^{{{L_i}}^{\rm{T}}}}{\boldsymbol{R}}_B^L\left( {{{\boldsymbol{R}}_{{{B_i}}}^{O^{\rm{T}}}}{\boldsymbol{R}}_{{B_j}}^O{{\left\lfloor {{{\boldsymbol{R}}_{B}^{L^{\rm{T}}}}} \right\rfloor }_ \times } - {{\left\lfloor {{{\boldsymbol{R}}_{{{B_i}}}^{O^{\rm{T}}}}{\boldsymbol{R}}_{{B_j}}^O{{\boldsymbol{R}}_{B}^{L^{\rm{T}}}}} \right\rfloor }_ \times }} \right) \end{split} \end{array} \right. $$ (18) 式中:
$ {\boldsymbol{\theta }}_{{B_i}}^O $ ,$ {\boldsymbol{\theta }}_{{B_j}}^O $ 分别为i时刻和j时刻的旋转矩阵的四元素表示形式;$ {\boldsymbol{\theta }}_B^L $ 为从IMU坐标系到雷达坐标系的外参旋转部分的四元数表示形式。2.5.3 协方差计算
由于前端配准采用了扫描到子图的方法,因此可通过计算所有匹配成功特征点来计算整体扫描匹配的协方差。计算当前扫描帧c的特征点u的坐标
$X_u^c $ 在子图w中对应特征点u的坐标Xuw:$$ {\boldsymbol{X}}_u^w = {\boldsymbol{R}}{_{{L_c}}^{{{L_w}}^{\rm{T}}}}\left( {{\boldsymbol{X}}_u^c - {\boldsymbol{p}}_{{L_c}}^{{L_w}}} \right) $$ (19) 式中
${\boldsymbol{R}}_{{L_c}}^{{L_w}}$ 和${\boldsymbol{p}}_{{L_c}}^{{L_w}}$ 为扫描帧c到子图w计算的相对变换。所有匹配成功的雷达特征计算的协方差矩阵H为
$$ {\boldsymbol{H}} = \left[ {\begin{array}{*{20}{c}} {{{\left\lfloor {{\boldsymbol{X}}_u^c} \right\rfloor }_ \times }}&{ - {\boldsymbol{R}}_{{L_c}}^{{L_w}}} \end{array}} \right] $$ (20) 定义噪声矩阵
$ {\boldsymbol{\varLambda }} $ 为$$ {\boldsymbol{\varLambda }} = \left[ {\begin{array}{*{20}{c}} {2\sigma _x^2}&{}&{} \\ {}&{2\sigma _y^2}&{} \\ {}&{}&{2\sigma _{\textit{z}}^2} \end{array}} \right] $$ (21) 式中
$ {\sigma _x} $ ,$ {\sigma _y} $ ,$ {\sigma _{\textit{z}}} $ 为x,y,z方向的的噪声西格玛值,受到2个点Xuw和Xuc噪声的影响,参考激光雷达的噪声参数,设置噪声典型值为5 cm。协方差矩阵对应的信息矩阵
$ {\boldsymbol{\varOmega }} $ 为$$ {\boldsymbol{\varOmega }} \leftarrow {\boldsymbol{\varOmega }} + {{\boldsymbol{H}}^{\rm{T}}}{{\boldsymbol{\varLambda }}^{ - 1}}{\boldsymbol{H}} $$ (22) 利用式(22)从初始时刻的06×6矩阵开始迭代计算信息矩阵,随着运动距离增加,信息矩阵的不确定度逐渐增长。
3. IMU预积分约束构建
IMU预积分作为一种高频惯性观测数据处理方法,在视觉惯性里程计、激光惯性里程计中逐渐得到应用,其核心思想是将相邻关键帧之间的大量惯性观测进行集成,在关键帧的局部惯性坐标系下开始积分,获得独立的相对运动约束,与其他约束联合优化,实现完整的状态估计。预积分原理如图4所示,横坐标代表时间戳。预积分过程就是将关键帧So、Si和Sj之间的高频IMU观测独立建立为IMU运动约束。本文采用视觉惯性导航系统(Visual Inertial Navigation System,VINS)的预积分方法对IMU进行积分约束,预积分观测方程、残差、雅可比矩阵和协方差矩阵推导参考文献[26]。
4. 回环检测约束构建
回环检测优化对于获得全局一致的地图有很大作用。本文采用文献[13]提出的回环检测方法,通过以下3个步骤实现回环检测与约束构建。
(1) 里程判断:当新的关键帧建立后,首先搜索因子图并找到与新关键帧位姿在欧氏距离上接近的关键帧。小于设定距离阈值的历史关键帧将被作为回环候选关键帧。
(2) 形貌相似度判断:利用ICP扫描匹配方法匹配当前关键帧的点云,与回环候选关键帧前后设定的某些帧构成局部子图。利用最近点对的均方根最小距离计算拟合率,与设定阈值比较来确定是否为真实回环关键帧。
(3) 回环因子构建:利用以上双重判断选择出回环关键帧后,通过式(12)计算相对变换因子,并将相对变换因子添加到全局因子图中。
5. 实验分析
5.1 野外颠簸路面试验
为了验证LI−SLAM方法在颠簸路面和复杂场景的精度与鲁棒性,基于轮式移动机器人试验平台在野外开展了试验。机器人搭载VLP−16 LiDAR,MTi−G710 IMU。轮式移动机器人搭载的运算平台配置为i7−8700k,32G内存。基于轮式移动机器人的试验平台和3个试验场地如图5所示。
5.1.1 精度分析
为了分析LI−SLAM方法的优越性,将其与当前最优的激光算法LOAM方法、LINS方法、LIO−mapping方法进行了对比。几种方法的轨迹对比及其对齐到卫星地图上的结果如图6−图8所示。可看出:在所有试验场地中,LI−SLAM方法和LOAM方法的地图一致性最好,与真实路线基本吻合,LI−SLAM方法对旋转有更佳的适应能力。LIO−mapping方法无法实时运行,在0.5倍速下可以获得完整轨迹,但在初始运动阶段出现了较大程度的方向偏移,初始化过程容易失败。LINS方法由于仅利用了最新的观测信息,在复杂地形下出现了漂移。
不同方法在机器人起点、终点间测量距离与真值的误差见表1,测量距离通过图上的坐标差获得,距离真值可以通过全站仪等测量和计算获得。从表1可看出:LI−SLAM方法在试验场地1、试验场地2中误差最小,在试验场地3中与LOAM方法的误差接近,均明显优于其他方法。
表 1 不同方法起点、终点间距离与真值的误差Table 1. The distance and truth value error between the starting point and the end point of different methods试验场地 行程/m 时间/s 误差/m LIO−mapping×0.5 LINS LOAM LI−SLAM 1 336 854 8.867 − 4.809 0.233 2 197 477 2.788 − 0.9859 0.4 3 963 1433 29.217 − 15.904 17.204 5.1.2 运算速度分析
LI−SLAM方法各个模块耗时的均值和最大值见表2,可看出回环检测模块耗时最长,其次是建图模块。实际应用中,回环检测和地图构建模块本身不需要实时,可以根据环境和需求人为设定。其余模块均满足10 Hz雷达采样频率的实时运行需求。
表 2 野外试验时LI−SLAM方法中各模块耗时均值与最大值Table 2. The average and maximum time consumption of each module of LI-SLAM method in field testsms 模块 耗时均值 耗时最大值 去畸变 1.889 7.466 特征提取 2.658 13.332 回环检测 85.820 915.839 地图构建 49.322 169.238 优化 0.153 156.012 5.1.3 回环检测效果分析
在试验场地1中机器人起始和终止位置发生的回环优化前后的效果如图9所示。图9(a)中回环尚未发生,在位置2的树木和道路出现明显的模糊,图9(b)中回环优化后树木辨别更加清晰,获得了全局一致性的地图,证明了回环检测的有效性。
5.2 地下车库少结构环境试验
为了验证LI−SLAM方法在少结构的井底车场等环境的应用效果,采用自主研发的CUMTV−C煤矿救援机器人平台[27]进行了地下车库的模拟试验,如图10所示,在机器人上方水平和倾斜30°各部署1个16线激光雷达,分别用于定位和构建稠密地图。机器人搭载Xsens MTi−G−710 IMU,用于惯性数据测量。
LI−SLAM方法的建模效果如图11所示,绿色到红色渐变的球形表示机器人的关键帧位姿,球形之间连线为相对位姿约束与回环约束。从图11可看出,LI−SLAM方法具有较好的建模精度,局部精细化程度高。
不同方法的运动轨迹如图12所示,由于地面较为平坦,LINS、LOAM、LI−SLAM方法均获得了平滑的运动轨迹,LIO−Mapping方法在初始化时出现了偏移,表明该方法在缺乏足够运动激励时难以获得鲁棒的状态估计。
不同方法起点、终点间距离的误差见表3,可看出LOAM和LI−SLAM方法的误差最小,证明了紧耦合方法可以提升定位精度。
表 3 不同方法起点、终点间距离的误差Table 3. The distance error between the starting point and the end point of different methods行程/m 时间/s 误差/m LIO−mappinsg LINS LOAM LI−SLAM 168.9 549.7 1.227 0.181 0.070 0.126 LI−SLAM方法各模块耗时均值与最大值见表4,可以看出:去畸变、特征提取、优化模块均可满足10 Hz运行条件,回环检测与地图构建模块无高频运行需求。
表 4 地下车库试验时LI−SLAM方法中各模块耗时均值与最大值Table 4. The average and maximum time consumption of each module in LI-SLAM method in underground garage testsms 模块 耗时均值 耗时最大值 去畸变 2.319 10.885 特征提取 3.380 9.034 回环检测 44.092 359.736 地图构建 28.115 171.516 优化 0.078 79.906 5.3 煤矿井下现场工业性测试
2020年11月2日,在晋能控股山西煤业股份有限公司塔山煤矿开展了煤矿救援机器人工业性验证试验,如图13所示。采用自主研发的CUMTV−C煤矿救援机器人平台进行试验[27],机器人实时运行LI−SLAM方法,通过无线信号传输(带宽大于120 Mbit/s)实时显示构建的地图,用于人员遥控和自主导航。
机器人在行走过程遇到了各类复杂工况,包括大量水汽、斜坡、岔口、颠簸路面等,如图14所示。搭载LI−SLAM方法的煤矿救援机器人在各类地形环境中均可稳定运行,说明LI−SLAM方法满足鲁棒性、实时性需求。
LI−SLAM方法实际运行中的一组建模结果如图15所示。机器人行驶巷道直线距离为273 m,将地图上的距离作为测量值,利用全站仪测量获得人工标志点的定位结果之间的距离作为真值进行对比,分析30组距离结果表明,平均误差小于15 cm,具有较高的定位和建模精度,基本满足煤矿移动机器人的定位建模精度需求。
6. 结论
(1) 提出了一种基于因子图优化框架的LiDAR和IMU紧耦合SLAM(LI−SLAM)方法,基于点−线和点−面扫描匹配构建激光相对位姿约束,融合IMU预积分因子约束和回环因子约束,利用因子图优化实现了LiDAR/IMU紧耦合的SLAM。
(2) 详细分析了激光约束的构建过程,设计了点云去畸变、特征提取、关键帧与子图构建、特征关联策略,解析推导了雷达相对位姿因子的残差、雅可比与协方差矩阵。
(3) 设计了雷达相对位姿因子、惯性预积分因子、回环检测因子的代价函数,构建了激光惯性里程计因子图紧耦合模型,并设计了无约束优化目标函数。
(4) 在野外颠簸路面、地下车库进行了大量测试,在真实煤矿井下现场进行了工业性试验。结果表明:与LOAM方法、LINS方法、LIO−mapping方法相比,LI−SLAM方法在精度、鲁棒性方面表现更佳,对于颠簸、少结构、水汽等复杂煤矿井下环境的移动机器人精确定位与地图构建有更好的适用性,具有较高的定位和建模精度,基本满足煤矿移动机器人的定位与建模精度需求,对井下各类移动机器人的定位建图与自主导航具有较强的指导意义和参考价值。
-
表 1 模糊控制规则
Table 1 Fuzzy control rule
$\Delta e$ e NB NM NS ZO PS PM PB NB PB/NS/PS PB/NB/NS PM/NM/NB PM/NM/NB PS/NS/NB ZO/ZO/NM ZO/ZO/PS NM PB/NB/PS PB/NB/NS PM/NM/NB PS/NS/NM PS/NS/NM ZO/ZO/NS NS/ZO/ZO NS PM/NB/ZO PM/NM/NS PM/NS/NM PS/NS/NM ZO/ZO/NS NS/PS/NS NS/PS/ZO ZO PM/NM/ZO PM/NM/NS PS/NS/NS ZO/ZO/NS NS/PS/NS NM/PM/NS NM/PM/ZO PS PS/NM/ZO PS/NS/ZO ZO/ZO/ZO NS/PS/ZO NS/PS/ZO NM/PM/ZO NM/PB/ZO PM PS/ZO/PB ZO/ZO/NS NS/PS/PS NM/PS/PS NM/PM/PS NM/PB/PS NB/PB/PB PB ZO/ZO/PB ZO/ZO/PM NM/PS/PM NM/PM/PM NM/PM/PS NB/PB/PS NB/PB/PB 表 2 液压系统主要参数
Table 2 Main parameters of hydraulic system
参数 值 质量/mg 1.5×104 比例阀流量放大系数 1.4×10−4 比例阀流量压力系数 2.4×10−4 液压缸等效容积/mm3 5×105 液压缸截面积/mm2 2450 有效体积弹性模量/Pa 7×108 冲程长度/mm 400 溢流阀开启压力/(kN·m−2) 2×104 液压油密度/(kg·m−3) 850 表 3 2种不同信号跟踪结果
Table 3 Tracking results of two different signals
工况环境 控制算法 跟踪误差/mm 正弦信号 模糊神经网络PID 0.003 2 模糊PID 0.015 0 PID 0.015 8 方波信号 模糊神经网络PID 0.004 0 模糊PID 0.010 0 PID 0.013 0 表 4 方波信号响应时间及跟踪误差
Table 4 Response time and tracking error of square wave signal
控制算法 时间段/s 响应时间/s 跟踪误差/mm PID 0~10 2.21 1.0 10~20 1.97 20~30 2.12 模糊PID 0~10 1.40 0.5 10~20 1.47 20~30 1.43 模糊神经网络PID 0~10 1.06 0.2 10~20 1.02 20~30 1.05 表 5 正弦信号动态性能对比
Table 5 Comparison of dynamic performance of sinusoidal signals
控制算法 滞后时间/s 峰值误差/mm PID 0.36 0.5 模糊PID 0.27 0.3 模糊神经网络PID 0.07 0.1 -
[1] 葛世荣,郝尚清,张世洪,等. 我国智能化采煤技术现状及待突破关键技术[J]. 煤炭科学技术,2020,48(7):28-46. GE Shirong,HAO Shangqing,ZHANG Shihong,et al. Status of intelligent coal mining technology and potential key technologies in China[J]. Coal Science and Technology,2020,48(7):28-46.
[2] 臧富雨,王凯硕,吉晓冬,等. 悬臂式掘进机俯仰角调控系统仿真研究[J]. 工矿自动化,2019,45(5):62-67. ZANG Fuyu,WANG Kaishuo,JI Xiaodong,et al. Simulation research on pitch angle control system of boom roadheader[J]. Industry and Mine Automation,2019,45(5):62-67.
[3] 张晓光,杨悦,孙彦景,等. 基于多频连续波相位差测距的掘进机位姿识别方法[J]. 煤炭学报,2020,45(6):2056-2064. ZHANG Xiaoguang,YANG Yue,SUN Yanjing,et al. Pose recognition method of roadheader based on multifrequency continuous-wave phase-difference ranging[J]. Journal of China Coal Society,2020,45(6):2056-2064.
[4] ZHANG Minjun,LYU Fuyan,FU Shichen,et al. Study on the pitch angle control of a robotized hydraulic drive roadheader using different control methods[J]. Journal of Mechanical Science and Technology,2018,32(10):4893-4901. DOI: 10.1007/s12206-018-0937-7
[5] 张敏骏,吉晓冬,李旭,等. 掘进机姿态调整模型辨识方法与精准控制[J]. 西安交通大学学报,2021,55(6):9-17. ZHANG Minjun,JI Xiaodong,LI Xu,et al. Method of model identification and precise control for tunnel boring machine body posture adjustment[J]. Journal of Xi'an Jiaotong University,2021,55(6):9-17.
[6] 王东杰,王鹏江,李悦,等. 掘进机截割臂自适应截割控制策略研究[J]. 中国机械工程,2022,33(20):2492-2501. WANG Dongjie,WANG Pengjiang,LI Yue,et al. Research on adaptive cutting control strategy of roadheader cutting arms[J]. China Mechanical Engineering,2022,33(20):2492-2501.
[7] 张旭辉,石硕,杨红强,等. 悬臂式掘进机自主调速截割控制系统[J]. 工矿自动化,2023,49(1):80-89. ZHANG Xuhui,SHI Shuo,YANG Hongqiang,et al. Boom-type roadheader autonomous speed regulation cutting control system[J]. Journal of Mine Automation,2023,49(1):80-89.
[8] 刘志森. 悬臂式掘进机截割部恒功率调速控制系统研究[D]. 阜新:辽宁工程技术大学,2012. LIU Zhisen. Research on constant power speed regulation control system for cutting part of cantilever roadheader[D]. Fuxin:Liaoning Technical University,2012.
[9] 王苏彧,田劼,吴淼. 纵轴式掘进机截割轨迹规划及边界控制方法研究[J]. 煤炭科学技术,2016,44(4):89-94,118. WANG Suyu,TIAN Jie,WU Miao. Study on cutting trace planning of longitudinal roadheader and boundary control method[J]. Coal Science and Technology,2016,44(4):89-94,118.
[10] 王鹏江,沈阳,宗凯,等. 结合LSTM深度学习和模糊推理控制的巷道掘进机智能联合截割策略与方法 [J/OL]. 煤炭学报:1-14[2024-06-21]. https://doi.org/ 10.13225/j.cnki.jccs.2023.1612. WANG Pengjiang,SHEN Yang,ZONG Kai,et al. Intelligent joint cutting strategy and method of roadheader combined with LSTM deep learning and fuzzy reasoning control[J/OL]. Journal of China Coal Society:1-14[2024-06-21]. https://doi.org/10.13225/ j.cnki.jccs.2023.1612.
[11] CHELUSZKA P,SOBOTA P,GŁUSZEK G. Studies of behaviour of the automatic control system of roadheader cutting heads movement[J]. MATEC Web of Conferences,2019,252. DOI: 10.1051/matecconf/201925209002.
[12] 郭伦锋,郭一楠,蒋康庆,等. 掘进机姿态参数测量及解算方法[J]. 工矿自动化,2021,47(12):46-54. GUO Lunfeng,GUO Yinan,JIANG Kangqing,et al. Measurement and calculation method of attitude parameters of roadheader[J]. Industry and Mine Automation,2021,47(12):46-54.
[13] 费烨,孙波,林闯. EBZ160悬臂式掘进机液压系统设计[J]. 液压与气动,2015,39(2):103-106. DOI: 10.11832/j.issn.1000-4858.2015.02.026 FEI Ye,SUN Bo,LIN Chuang. The design for hydraulic system of EBZ160 cantilever tunneling machine[J]. Chinese Hydraulics & Pneumatics,2015,39(2):103-106. DOI: 10.11832/j.issn.1000-4858.2015.02.026
[14] 王志武. 掘进机行走机构液压系统液压冲击的分析与处理[J]. 机床与液压,2013,41(4):108-109. DOI: 10.3969/j.issn.1001-3881.2013.04.032 WANG Zhiwu. Analysis and treatment of hydraulic shock in hydraulic system of roadheader traveling mechanism[J]. Machine Tool & Hydraulics,2013,41(4):108-109. DOI: 10.3969/j.issn.1001-3881.2013.04.032
[15] 陶建峰,刘成良. 全断面岩石隧道掘进机液压技术研究现状[J]. 液压与气动,2015,39(6):1-5,12. DOI: 10.11832/j.issn.1000-4858.2015.06.001 TAO Jianfeng,LIU Chengliang. Review of technical research for hydraulic system of tunnel boring machine[J]. Chinese Hydraulics & Pneumatics,2015,39(6):1-5,12. DOI: 10.11832/j.issn.1000-4858.2015.06.001
[16] 张敏骏. 悬臂式掘进机自主纠偏与位姿控制研究[D]. 北京:中国矿业大学(北京),2019. ZHANG Minjun. Research on autonomous correction and pose control of cantilever tunneling machine[D]. Beijing:China University of Mining & Technology-Beijing,2019.
[17] 张建广. 悬臂式掘进机自适应截割控制系统研究[J]. 煤炭科学技术,2016,44(2):148-152. ZHANG Jianguang. Study on adaptive cutting control system of boom-type roadheader[J]. Coal Science and Technology,2016,44(2):148-152.
[18] 雷孟宇,张旭辉,杨文娟,等. 煤矿掘进装备视觉位姿检测与控制研究现状与趋势[J]. 煤炭学报,2021,46(增刊2):1135-1148. LEI Mengyu,ZHANG Xuhui,YANG Wenjuan,et al. Research status and trend of visual pose detection and control of coal mine tunneling equipment[J]. Journal of China Coal Society,2021,46(S2):1135-1148.
[19] 范子彦,李立君,李宇航,等. 油茶果采摘机阀控液压马达模糊神经网络PID控制[J]. 液压与气动,2021,45(11):76-85. DOI: 10.11832/j.issn.1000-4858.2021.11.011 FAN Ziyan,LI Lijun,LI Yuhang,et al. Fuzzy neural network PID control of valve-controlled hydraulic motor for camellia fruit picking machine[J]. Chinese Hydraulics & Pneumatics,2021,45(11):76-85. DOI: 10.11832/j.issn.1000-4858.2021.11.011
[20] 敖邦乾,姜孝均,董泽芳,等. 基于神经网络−PID控制的水面无人艇控制系统设计[J]. 控制工程,2024,31(7):1178-1184. AO Bangqian,JIANG Xiaojun,DONG Zefang,et al. Design of unmanned surface vehicle control system based on neural network-PID control[J]. Control Engineering of China,2024,31(7):1178-1184.
[21] DHIMISH M,HOLMES V,MEHRDADI B,et al. Comparing Mamdani Sugeno fuzzy logic and RBF ANN network for PV fault detection[J]. Renewable Energy,2018,117:257-274. DOI: 10.1016/j.renene.2017.10.066
-
期刊类型引用(12)
1. 赵亚东,马腾飞,思旺斗,王猛. 煤矿井下移动机器人同步定位关键技术研究. 煤矿机械. 2024(02): 48-51 . 百度学术
2. 司垒,王忠宾,魏东,顾进恒,闫海峰,谭超,朱远胜. 基于IMU-LiDAR紧耦合的煤矿防冲钻孔机器人定位导航方法. 煤炭学报. 2024(04): 2179-2194 . 百度学术
3. 高毅楠,姚顽强,蔺小虎,郑俊良,马柏林,冯玮,高康洲. 煤矿井下多重约束的视觉SLAM关键帧选取方法. 煤炭学报. 2024(S1): 472-482 . 百度学术
4. 刘敬东,李旭,于凤启,苟丙荣,贺国庆,巩泽文. 激光SLAM技术在巷道精细建模的应用研究. 煤矿机械. 2024(10): 199-202 . 百度学术
5. 黄晨烜,常健,王雷. 基于激光雷达的井下带式输送机边缘提取方法. 工矿自动化. 2024(09): 115-123 . 本站查看
6. 胡青松,李敬雯,张元生,李世银,孙彦景. 面向矿井无人驾驶的IMU与激光雷达融合SLAM技术. 工矿自动化. 2024(10): 21-28 . 本站查看
7. 崔邵云,鲍久圣,胡德平,袁晓明,张可琨,阴妍,王茂森,朱晨钟. SLAM技术及其在矿山无人驾驶领域的研究现状与发展趋势. 工矿自动化. 2024(10): 38-52 . 本站查看
8. 马亮,高亮,廉博翔,张琦,蔺小虎,姜之跃. 基于已知点约束的高精度煤矿巷道三维点云建模方法. 工矿自动化. 2024(11): 78-83+151 . 本站查看
9. 夏建超,周亮亮,陈仁. 恶劣环境下钢包脱挂钩状态自动识别技术研究. 重型机械. 2023(04): 62-67 . 百度学术
10. 高海跃,王凯,王保兵,王丹丹. 基于全局点云地图的煤矿井下无人机定位方法. 工矿自动化. 2023(08): 81-87+133 . 本站查看
11. 程健,李昊,马昆,刘斌,孙大智,马永壮,殷罡,王广福,李和平. 矿井视觉计算体系架构与关键技术. 煤炭科学技术. 2023(09): 202-218 . 百度学术
12. 李少安,刘欣,郭长鑫,王博,丁浩然,李晓健. 基于激光雷达自主定位导航的多功能机器人. 无线互联科技. 2023(17): 54-57 . 百度学术
其他类型引用(6)