基于AUPF算法的水下履带车动力学参数估计

陈昱衡 吴鸿云 边有钢

摘要：针对多金属结核采矿车在稀软底质行驶作业时有效驱动轮半径和履带打滑率等动力学参数难确定的问题，基于多金属结核采矿车的牵引力分析和液压驱动系统的负载特性分析，建立用于多金属结核采矿车动力学参数估计的高阶非线性系统模型.针对基于高斯模型的无迹卡尔曼滤波（Unscented Kalman Filter，UKF）算法无法在非线性系统取得较高估计精度的问题，提出利用基于蒙特卡洛采样原理的自适应无迹粒子滤波（Adaptive Unscented Particle Filter，AUPF）算法进行动力学参数测算方案，通过自适应无迹卡尔曼滤波（Adaptive Unscented Kalman Filter，AUKF）改善粒子滤波（Particle Filter，PF）的概率密度函数，解决PF容易发散和UKF估计精度不高的问题.实验结果表明，AUPF算法得到的多金属结核采矿车的动力学参数误差均小于最大允许误差，满足精准在线测算的性能需求.

关键词：深海采矿装备；多金属结核采矿车；参数估计；动力学分析；自适应无迹粒子滤波

中图分类号：TP249文献标志码：A

Estimation of Motion Parameters of a Underwater Track Mining Vehicle Based on Adaptive Unscented Particle Filter Algorithm

CHEN Yuheng1，WU Hongyun2，3，BIAN Yougang1

（1. State Key Laboratory of Advanced Design and Manufacturing for Vehicle Body，Hunan University，Changsha 410082，China；2. Changsha Institute of Mining Research Co.，Ltd.，Changsha 410012，China；3. National Engineering Research Center for Metal Mining，Changsha 410012，China）

Abstract：Aiming at the difficulty in determining the motion parameters such as effective drive wheels and track slip rate of a polymetallic nodule mining vehicle，based on the modeling of traction force and load characteristics of tracks，a higher-order nonlinear system for estimating the motion parameters of a polymetallic nodule mining vehicle is proposed. To address the problem that the UKF based on Gaussian model cannot achieve high estimation accuracy in high-order nonlinear systems，an adaptive traceless particle filtering algorithm （AUPF）based on Monte Carlo sampling principle is proposed，and the adaptive traceless Kalman filter （AUKF）is used to improve the Particle Filter （PF）by refining the probability density function，which solves the shortcomings that the PF is easy to diverge and theestimation accuracy of UKF is low. The experimental results show that the AUPF algorithm can achieve an accurate estimation of the motion parameters of polymetallic nodule mining vehicles，and there is an important engineering application value.

Key words：deep sea mining equipment；polymetallic nodule mining vehicle；parameter estimation；dynamics analysis；adaptive unscented particle filter

位于4～6 km水深的深海底，其表層沉积物中以半埋藏状态覆存着大量的多金属结核矿产资源，因其富含镒、铜、钻、镍等稀贵金属元素，极具商业开采价值.20世纪70年代，以美国为首的西方发达国家率先开始深海采矿技术研究，完成了不同程度的试验，催发了一轮深海采矿技术研究热潮[1].

在深海采矿作业时，采矿车沿着预设轨迹进行自行式采集结核，需要在线获取采矿车的动力学参数.然而，多金属结核矿区表层含水量高，承载力弱，易流动且伴随有海底不规则洋流的影响，导致多金属结核采矿车易发生履带沉陷和履带不规则打滑的现象[2]，故履带沉陷时采矿车的有效驱动轮半径应考虑履带板厚度和未陷入底质的驱动轮部分.若忽略驱动轮半径变化时履带打滑的动态响应，可能会对控制性能产生负面影响.因此，精准的履带打滑率和有效驱动轮半径等运动学参数估计显得尤为重要.

为解决上述问题，文献［3-6］基于RecurDyn参数化建模技术，构建了多刚体深海采矿系统，完成了联动仿真分析，并对多金属结核采矿车的直行、差速转向、爬坡和越障等工况时的机械特性进行了仿真分析，并基于模型车进行试验以验证方案的可行性，为深海采矿系统的整体集成设计、性能预测及作业操控提供了参考，同时为多金属结核采矿车整车机械结构的优化设计提供了理论支撑[7-9].

无迹卡尔曼滤波（Unscented Kalman Filter，UKF）算法通过无迹变换（Unscented Transformation，UT）将非线性函数模拟成真实分布的近似高斯分布，具有3阶的泰勒精度，是一种易实现的参数估计算法［10-12］.周兵等[13]基于UKF对非线性轮胎模型中的路面附着系数进行了在线估计，提高了自动驾驶车辆的主动转向控制性能.Qin等[14]基于瞬时转向中心变化的履带车运动学模型，提出了一种改进的UKF算法，实现了陆地自行式履带车辆履带打滑率的精准在线估计.

本文从多金属结核采矿车的受力分析出发，建立底盘液压驱动系统与履带负载特性的动力学关系，提出了多金属结核采矿车的有效驱动轮半径和履带打滑率等动力学参数的状态空间模型.针对UKF算法无法在高非线性模型中得到较高估计精度的问题[15-16]，采用基于蒙特卡洛采样原理的自适应无迹粒子滤波（Adaptive Unscented Particle Filter，AUPF）算法进行多金属结核采矿车的动力学参数在线测算，利用自适应无迹卡尔曼滤波（Adaptive Unscented Kalman Filter，AUKF）算法改进概率密度函数以指导粒子滤波（particle filter，PF）进行重要性采样，克服了粒子滤波易发散的缺点［17］.试验结果表明，所提出的AUPF算法可实现多金属结核采矿车的动力学参数的精准在线测算.

1多金属结核采矿车参数估计建模

1.1多金属结核采矿车受力分析

与陆地履带车不同，多金属结核采矿车在多金属结核矿区稀软的底质上行驶，为了提高多金属结核采矿车的抓地能力，行走履带采用三角履齿.多金属结核采矿车如图1所示，三角履齿如图2所示.

在计算采矿车履带产生的牵引力F时，不仅需要考虑履带对底质产生的剪切力F₁，还需要考虑履齿剪切底质产生的压力F₂.F₁和F₂分别见式（1）和式（2）.

式中：A为履带的接地面积，m²；W为车身正压力，N；c为底质内聚压力，kPa；φ为底质内摩擦角参数，（°）；l为履带接地长度，m；i为履带打滑率；k为底质水平剪切模变数，m.

式中：b₂为履齿宽度，m；γ_s为底质比重，kg/m³；h_b为履齿高度，m；N_φ=tan²（45°+0.5φ）为底质流值；n₂为履齿个数.

牵引力F为：

1.2多金属结核采矿车动力学参数在线测算模型

多金属采矿车在含水量高的稀软底质进行自行式作业时，底盘会发生沉陷现象，故实际的履带有效驱动轮半径r_a为：

r_a=r+r₁-r₂（4）

式中：r₁為履带板厚度和履齿高度的和，m；r₂为测高声呐测得的履带沉陷，m；r为理论驱动轮半径，m.

多金属采矿车左右履带驱动采用两独立的闭式液压回路，即采用两电动机分别驱动两变量泵，再经过定量马达传递到左右履带，马达直接驱动采矿车行走.其中液压马达的系统压力p_l与马达的驱动力矩T_q呈线性关系，如式（5）所示.

多金属结核采矿车为低速匀速行驶，故在建模时可假设马达的负载力矩T_L等于驱动力矩T_q：

T_q=T_L（6）

驱动轮由液压马达直驱，故驱动轮转速ω等于液压马达转速ω_m：

ω=ω_m（7）

联立式（5）～式（7）负载力矩T_L与驱动轮转速ω的方程，如式（8）所示.

同时驱动力矩T_q满足：

T_q=r_aF=r_a（F₁+F₂）（9）

为了简化运算，基于文献[18]，对式（1）进行简化，得

F₁=（Ac+Wtanφ）（e^-illk-0.03）（10）

联立式（2）式、式（4）、式（8）～式（10），可得到履带有效驱动轮半径和履带打滑率的关系.

通过离散化式（4）和式（10），定义如式（12）所示的状态空间模型.

状态量为：

X（k）=[x₁（k）；x₂（k）；x₃（k）]=[i（k）；r_a（k）；θ_m（k）]

控制量为：

观测量为：

Y（k+1）=[i（k+1）；r_a（k+1）]

式中：T为采样时间，s；L为每个采样时间T内的履带行走长度，m.

2自適应无迹粒子滤波算法

粒子滤波算法不依赖高斯模型，基于蒙特卡罗采样原理，可应用于高线性系统的参数估计.但是PF算法存在滤波易发散的缺点，通常采用UKF算法指导粒子重采样以改进PF算法.本文采用AUKF指导粒子重采样的AUPF进行参数估计.AUPF的算法流程如表1所示.

AUKF算法的具体步骤如下：

1）初始化.

2）Sigma点权重计算.

式中：λ=n（α²-1），α是控制Sigma点分布的常数，其取值范围为e^-4≤α≤1；β为非负常数，将高阶成分的信息转换到方差中，取β=2.

3）Sigma点选择和估计值更新.在UT中，随机变量由一组最小的样本点表示，称为Sigma点.计算这些点在非线性系统模型传播时的后验平均值和协方差.Sigma点由式（16）计算.

4）状态更新.

粒子集合X_AUPF（k）由式（23）计算.

式中：n_rand为0～1的随机数.

粒子权重计算具体步骤如下：

1）计算观测值Y_AUPF（k）.

Y_AUPF（k）=H（X_AUPF（k））（24）

2）计算粒子权重并归一化.

通过设定权重阈值进行重采样并更新状态与基本PF算法一致，本文不再赘述，见参考文献[10].

基于张滔等人[9]的研究，式（1）、式（2）中多金属结核底质土力学特性的参数值和采矿车动力学参数测算模型的参数如表2所示.

3动力学参数在线测算方法实验验证

为了验证本文提出的动力学参数测算算法能否精准测算多金属结核采矿车的履带打滑率和有效驱动轮半径，定义如下性能需求：有效驱动轮最大误差不超过5 mm，履带打滑率的最大误差不超过5%.以上述多金属结核采矿车为实验对象，在深海采矿实验水池进行行走实验，实验水池长60 m，宽50 m，深5 m，实验室水池如图3所示.

水池底质为河泥以模拟多金属结核矿区的稀软地形，水深保证可以淹没整个多金属结核采矿车.多金属结核采矿车通过起重机下放至水池中，由惯性导航单元和多普勒测速组成的水下组合导航系统使采矿车完成如图4所示的行走路线.

测控主系统为美国National Instruments公司推出的Compact RIO嵌入式测控平台及软件.车载本地测控站采集并记录采矿车上各传感器检测参数，多金属结核采矿车搭载多普勒测速仪、光纤陀螺、履带驱动电机编码器、测高声呐等传感器实时获取采

车载本地测控站记录所测算的外侧履带打滑率i_o、内侧履带打滑率i_i、外侧有效驱动轮半径r_o和内侧有效驱动轮半径r_i的实验数据分别如表3和图5～图8所示.同时实际的驱动轮半径通过式（4）计算，实际打滑率由式（26）计算.

式中：v_a为履带实际速度，由式（13）计算.

由实验结果可知，有效驱动轮半径的误差在3 mm以内，履带打滑率的最大误差不超过5%，有效驱动轮半径的误差均方差小于0.5，履带打滑率误差均方差小于1.5.

误差的产生可能是由于实验室模拟底质的力学特性与真实底质的力学特性有一定的差距，故状态空间模型中的底质力学特性与实验水池模拟底质的力学特性存在不匹配的情况.但是有效驱动轮半径的误差和履带打滑误差都小于最大误差，且均方差均较低.通过AUPF算法可以得到精度较高的多金属结核采矿车动力学参数，同时说明AUPF算法对模型的不匹配情况也具有较好的适应性.

4总结

针对多金属结核采矿车在海底行驶作业时，履带打滑率和有效驱动轮半径等动力学参数难以确定的问题，本文通过分析多金属结核采矿车的履带牵引力和履带的液压负载特性，建立了多金属结核采矿车履带打滑率和有效驱动轮半径等动力学参数在线测算的状态空间模型.针对动力学参数测算状态空间模型非线性程度较高的问题，提出了利用AUPF算法进行多金属结核采矿车动力学参数测算的方案.实验结果表明，有效驱动轮半径的误差控制在3 mm以内，打滑率的最大误差不超过5%，满足动力学参数测算的精度要求，验证了所提方案的可行性，具有重要的工程应用价值.

参考文献

[1]陈铭，冷静.深海大型爬行机器人研究现状[J].海洋工程，2020，38（5）：156-168.

CHEN M，LENG J. The development of deep-sea large-scale crawling robots [J]. The Ocean Engineering，2020，38（5）：156168. （In Chinese）

[2] DAI Y，XUE C，SU Q，et al. Numerical analysis on hydrodynamic characteristics of a deep-sea mining vehicle under three typical motions [J]. Ocean Engineering，2021，235：109446.

[3] DAI Y，ZHANG Y Y，LIX Y. Numerical and experimental investigations on pipeline internal solid-liquid mixed fluid for deep oceanmining[J].Ocean Engineering，2021，220：108411.

[4] DAI Y，ZHU X，ZHOU H B，et al. Trajectory tracking control for seafloor tracked vehicle by adaptive neural-fuzzy inference system algorithm [J]. International Journal of Computers （Communications & Control，2018，13（4）：465-476.

[5]戴瑜，张健，张滔，等.基于多体动力学模型集成的深海采矿系统联动仿真[J].机械工程学报，2017，53（4）：155-160.

DAI Y，ZHANG J，ZHANG T，et al. Motion simulation of the deep ocean mining system based on its integrated multi-body dynamic model[J]. Journal of Mechanical Engineering，2017，53 （4）：155-160.（In Chinese）

[6]刘少军，李渊文，胡小舟.基于DEM-CFD颗粒体积分数对深海扬矿电泵工作性能的影响[J].机械工程学报，2020，56 （10）：257-264.

LIU S J，LI Y W，HU X Z. Effect of particle volume fraction on the performance of deep-sea mining electric lifting pump based on DEM-CFD[J]. Journal of Mechanical Engineering，2020，56 （10）：257-264.（In Chinese）

[7] DAI Y，SU Q，ZHANG Y Y.A new dynamic model and trajectory tracking control strategy for deep ocean mining vehicle[J]. Ocean Engineering，2020，216：108162.

[8] DAI Y，YIN W W，MA F Y.Nonlinear multi-body dynamic modeling and coordinated motion control simulation of deep-sea mining system[J].IEEE Access，2019，7：86242-86251.

[9]张滔，戴瑜，刘少军，等.深海履带式集矿机多体动力学建模与行走性能仿真分析[J].机械工程学报，2015，51（6）：173-180.

ZHANG T，DAI Y，LIU S J，et al. Multi-body dynamic modeling and mobility simulation analysis of deep ocean tracked miner[J]. Journal of Mechanical Engineering，2015，51（6）：173-180.（In Chinese）

[10]黃小平，王岩，缪鹏程.粒子滤波原理及应用：MATLAB仿真[M].北京：电子工业出版社，2017：30-45.

HUANG X P，WANG Y，MIAO P C. Principle and application of particle filter：MATLAB simulation [M]. Beijing：Publishing House of Electronics Industry，2017：30-45.（In Chinese）

[11]周兵，田晨，宋义彤，等.基于路面附着系数估计的AFS控制策略研究[J].湖南大学学报（自然科学版），2017，44（4）：16-22.

ZHOU B，TIAN C，SONG Y T，et al. Control strategy of AFS based on estimation of tire-road friction coefficient[J]. Journal of Hunan University（Natural Sciences），2017，44（4）：16-22.（In Chinese）

[12]郭玉荣，龙沐恩.基于改进IMK恢复力模型的钢筋混凝土柱参数识别与应用[J].湖南大学学报（自然科学版），2021，48（1）：126-134.

GUO Y R，LONG M E. Parameter identification and application of reinforced concrete column based on modified ibarra-medina- krawinkler hysteretic model[J]. Journal of Hunan University （Natural Sciences），2021，48（1）：126-134.（In Chinese）

[13]周兵，邱香，吴晓建，等.基于UKF车辆状态及路面附着系数估计的AFS控制[J].湖南大学学报（自然科学版），2019，46 （8）：1-8.

ZHOU B，QIU X，WU X J，et al.AFS control based on estimation of vehicle state and road coefficient using UKF method[J]. Journal of Hunan University（Natural Sciences），2019，46（8）：1-8. （In Chinese）

[14] QIN Z B，CHEN L，FAN J J，et al. An improved real-time slip model identification method for autonomous tracked vehicles using forward trajectory prediction compensation[J]. IEEE Transactions on Instrumentation and Measurement，2021，70：1-12.

[15] LIU F X，LI X Y，YUAN S H，et al. Slip-aware motion estimation for off-road mobile robots via multi-innovation unscented Kalman filter[J].IEEE Access，2020，8：43482-43496.

[16] LIU Z，CHAN S C. Adaptive fading Bayesian unscented Kalman filter and smoother for state estimation of unmanned aircraft sys- tems[J].IEEE Access，2020，8：119470-119486.

[17]黃小平，王岩，缪鹏程.目标定位跟踪原理及应用：MATLAB仿真[M].北京：电子工业出版社，2018：55-75.

HUANG X P，WANG Y，MIAO P C. Principle and application of target location and tracking：MATLAB simulation[M]. Beijing：Publishing House of Electronics Industry，2018：55-75.（In Chinese）

[18]吴鸿云.集矿机牵引性能若干影响因素研究[D].长沙：中南大学，2010.

WU H Y. Research on some faetors on driving performance of seabed traeked vehicle on seabed soft sediments [D]. Changsha：Central South University，2010.（In Chinese）