一种考虑ICR纵向偏差补偿的水下履带采矿车轨迹跟踪算法

一种考虑 ICR 纵向偏差补偿的水下履带采矿车轨迹跟踪算法 A trajectory tracking algorithm for underwater tracked mining vehicles considering longitudinal deviation compensation 1 first-author 1 2 1 2 1 2 corresp 胡满江，博士，教授，从事智能网联车辆研究；E-mail：manjiang_h@hnu.edu.cn 胡满江，博士，教授，从事智能网联车辆研究；E-mail：manjiang_h@hnu.edu.cn manjiang_h@hnu.edu.cn 1 2 湖南大学 汽车车身先进设计制造国家重点实验室，湖南 长沙，410082 湖南大学 汽车车身先进设计制造国家重点实验室，湖南 长沙，410082 State Key Laboratory of Advanced Design and Manufacturing for Vehicle Body, Hunan University, Changsha 410082, China State Key Laboratory of Advanced Design and Manufacturing for Vehicle Body, Hunan University, Changsha 410082, China 湖南大学无锡智能控制研究院，江苏 无锡，214072 湖南大学无锡智能控制研究院，江苏 无锡，214072 Wuxi Intelligent Control Research Institute of Hunan University, Wuxi 214072, China Wuxi Intelligent Control Research Institute of Hunan University, Wuxi 214072, China 伍锦花 水下履带采矿车在多金属结核矿区的稀软底质上进行采集作业时，受水动力的影响其瞬时转向中心(ICR)纵向偏移后与几何中心不再重合，故难以基于精准的运动学模型设计水下履带采矿车的轨迹跟踪算法。因而，本文提出一种基于履带车运动学模型的分散式轨迹跟踪算法以实现履带采矿车高精度路径跟踪控制。首先，通过引入虚拟转角设计水下履带采矿车的Stanley横向控制律；其次，利用无迹卡尔曼(UKF)滤波法估计的ICR纵向偏移作为纵向误差的补偿，以此设计水下履带采矿车的纵向控制律；最后，基于运动学模型计算水下履带采矿车的期望驱动轮转速。研究结果表明：引入纵向偏差补偿后的分散式轨迹跟踪算法可以精准地跟踪预设轨迹。 Subjected to hydrodynamic force, there is a longitudinal position offset between instantaneous center of rotation(ICR) and geometric center of an underwater tracked mining vehicle when it tracks the acquisition trajectory on soft substrates in polymetallic nodule mining areas. So, it is a challenge to design a trajectory tracking algorithm for an underwater tracked vehicle based on accurate kinematic model. Therefore, a decentralized trajectory tracking algorithm based on a kinematic model of tracked vehicles was proposed to realize trajectory tracking control of crawler mining vehicle with high precision. Firstly, a virtual wheel angle was introduced to design the Stanley lateral control law of under tracked mining vehicle. Then, the longitudinal offset of ICR estimated by the improved unscented kalman filter(UKF) was used as the compensation of the longitudinal error, so as to design the longitudinal control law. Finally, the expected driving wheel speed of the underwater tracked vehicle was calculated based on the kinematic model. The co-simulation results show that the decentralized trajectory tracking algorithm with the introduction of longitudinal deviation compensation can accurately track the preset trajectory. 深海采矿装备 水下履带采矿车 轨迹跟踪 参数估计 无迹卡尔曼滤波 deep sea mining equipment under tracked mining vehicle trajectory tracking parameter estimation unscented Kalman filter 随着人类社会的不断发展，陆地的矿产资源正在逐渐枯竭，而海底存在较多可开采的矿物资源，如多金属结核、富钴结壳和多金属硫化物 [ 1 1 ] 。多金属结核含铜、镍、钴等多种金属元素，极具商业开采价值 [ 2 2 - 4 4 2-4 ] 。在我国的深海采矿系统中，水下履带采矿车是重要的组成部分，可以在多金属结核矿区的稀软地形上进行采集作业 [ 5 5 - 7 7 5-7 ] 。 当前履带车辆的控制算法已有较多研究成果。以电机转矩作为控制输入，基于动力学模型的线性反馈，陆地履带车的预测控制和鲁棒控制等算法已被证明可以取得理想的控制效果 [ 8 8 - 9 9 8-9 ] 。与陆地履带车辆不同，由于多金属结核矿区底质的力学特性不一致 [ 10 10 - 11 11 10-11 ] ，难以建立水下履带采矿车精准的动力学模型，因此，水下履带采矿车不适合采用基于动力学模型的设计控制算法。 基于运动学模型设计的模型预测控制算法已被应用于履带车的轨迹跟踪 [ 12 12 - 13 13 12-13 ] ，能精准地跟踪预设轨迹。但是模型预测控制在求解优化问题时，存在在线计算时间过长、CPU占有率过高的缺点。为此，孙浩等 [ 14 14 ] 提出了一种加速求解线性二次规划优化问题的方法，但是受到求解库的限制，模型预测控制的优化问题在实际工程应用中往往存在无解的现象。DAI等 [ 15 15 - 16 16 15-16 ] 将模糊PID算法应用于水下履带采矿车的轨迹跟踪，该条件下水下履带采矿车能够跟踪预设轨迹，但是轨迹跟踪精度有待提高。 在基于运动学模型设计算法时，履带与地面存在不规则的打滑现象，故其瞬时转向中心(instantaneous center of rotation，ICR)的位置与几何中心便不再重合，因此，无法获得精准的运动学模型。熊光明等 [ 17 17 ] 提出了基于Levenberg-Marquardt算法迭代估计ICR位置的方法。QIN等 [ 18 18 ] 提出了一种双层自适应无迹卡尔曼滤波，根据观测位置变化量和预测位置变化量来估计ICR位置。这两种方法均能精确地估计ICR的位置，从而得到履带车辆精准的运动学模型。 本文基于经典Stanley横向控制算法的非线性反馈思想 [ 19 19 ] ，通过引入水下履带采矿车期望的虚拟转角，提出水下履带采矿车的横向控制算法；考虑到水下履带采矿车在稀软底质上行走时受到水阻力的影响，其ICR会发生不可忽视的纵向偏移，本文通过改进的UKF算法估计水下履带采矿车ICR的纵向偏移，并将其作为纵向误差的补偿，提出水下履带采矿车的纵向控制算法；基于运动学模型，将横纵向算法得到的水下履带采矿车的期望角速度和线速度转变为驱动轮转速，以实现预设轨迹的精准跟踪。 1 水下履带采矿车运动学建模 1 多金属结核矿区表层为稀软底质，含水量高，承载力弱，表层易流动且伴随有海底不规则洋流的影响，作业环境极其复杂，履带与底质的不规则打滑现象会导致ICR的纵向偏移，此时，传统的不考虑ICR的运动学模型将会失真，因此，本文采用基于ICR的水下履带采矿车运动学模型，如 图1 图1 所示。 图1 图1 中， <math id="M1"><msub><mrow><mi>v</mi></mrow><mrow><mi>F</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682015&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682025&type= 3.21733332 3.72533321 和 <math id="M2"><msub><mrow><mi>v</mi></mrow><mrow><mi>E</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682017&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682016&type= 3.21733332 3.72533321 分别为高速侧履带 <math id="M3"><mi>F</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682019&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682018&type= 2.62466669 2.62466669 点和低速侧履带 <math id="M4"><mi>E</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682021&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682020&type= 2.45533323 2.62466669 点的牵连速度； <math id="M5"><msub><mrow><mi>v</mi></mrow><mrow><mi>o</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682038&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682022&type= 2.96333337 3.72533321 和 <math id="M6"><msub><mrow><mi>v</mi></mrow><mrow><mi>i</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682040&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682039&type= 2.45533323 3.72533321 分别为高速侧履带和低速侧履带的接地端速度； <math id="M7"><mfenced separators="|"><mrow><msub><mrow><mi>x</mi></mrow><mrow><mi>o</mi></mrow></msub><mo>,</mo><msub><mrow><mi>y</mi></mrow><mrow><mi>o</mi></mrow></msub></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682042&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682041&type= 10.75266743 4.74133301 和 <math id="M8"><mfenced separators="|"><mrow><msub><mrow><mi>x</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>,</mo><msub><mrow><mi>y</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682044&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682043&type= 9.73666668 4.74133301 分别为高速侧履带和低速侧履带接地端的ICR的坐标值； <math id="M9"><mfenced separators="|"><mrow><msub><mrow><mi>x</mi></mrow><mrow><mi>c</mi></mrow></msub><mo>,</mo><msub><mrow><mi>y</mi></mrow><mrow><mi>c</mi></mrow></msub></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682046&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682045&type= 10.49866676 4.74133301 为水下履带采矿车的ICR的坐标值； <math id="M10"><mi>B</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682048&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682047&type= 2.37066650 2.62466669 为履带板间的距离； <math id="M11"><mi>v</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682236&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682235&type= 1.69333339 2.62466669 为多金属结核采矿车的线速度； <math id="M12"><mi>ω</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682052&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682051&type= 2.62466669 2.62466669 为水下履带采矿车的角速度； <math id="M13"><msub><mrow><mi>v</mi></mrow><mrow><mi>x</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682054&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682053&type= 2.87866688 3.72533321 和 <math id="M14"><msub><mrow><mi>v</mi></mrow><mrow><mi>y</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682056&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682055&type= 2.87866688 3.72533321 为水下履带采矿车的纵向和横向速度； <math id="M15"><mi>φ</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682058&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682057&type= 2.11666679 3.38666677 为水下履带采矿车的航向角。 水下履带采矿车履带的接地端绕其ICR旋转， <math id="M16"><mi>F</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682084&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682083&type= 2.62466669 2.62466669 点和 <math id="M17"><mi>E</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682086&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682085&type= 2.45533323 2.62466669 点的绝对速度 <math id="M18"><msub><mrow><mi>V</mi></mrow><mrow><mi>F</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682032&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682031&type= 3.80999994 3.72533321 和 <math id="M19"><msub><mrow><mi>V</mi></mrow><mrow><mi>E</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682034&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682033&type= 3.80999994 3.72533321 如下： <math display="block" id="M20"><mfenced open="{" close="" separators="|"><mrow><mtable><mtr><mtd><msub><mrow><mi>V</mi></mrow><mrow><mi>F</mi></mrow></msub><mo>=</mo><mfenced separators="|"><mrow><mo>-</mo><msub><mrow><mi>y</mi></mrow><mrow><mi>o</mi></mrow></msub><mo>+</mo><mi>B</mi><mo>/</mo><mn mathvariant="normal">2</mn></mrow></mfenced><mi>ω</mi></mtd></mtr><mtr><mtd><msub><mrow><mi>V</mi></mrow><mrow><mi>E</mi></mrow></msub><mo>=</mo><mo>-</mo><mfenced separators="|"><mrow><msub><mrow><mi>y</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>+</mo><mi>B</mi><mo>/</mo><mn mathvariant="normal">2</mn></mrow></mfenced><mi>ω</mi></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682036&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682035&type= 29.20999908 12.36133385 根据刚体运动学原理， <math id="M21"><mi>F</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682084&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682083&type= 2.62466669 2.62466669 点和 <math id="M22"><mi>E</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682086&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682085&type= 2.45533323 2.62466669 点的绝对速度满足如下关系： <math display="block" id="M23"><mfenced open="{" close="" separators="|"><mrow><mtable><mtr><mtd><msub><mrow><mi>V</mi></mrow><mrow><mi>F</mi></mrow></msub><mo>=</mo><msub><mrow><mi>v</mi></mrow><mrow><mi>F</mi></mrow></msub><mo>-</mo><msub><mrow><mi>v</mi></mrow><mrow><mi>o</mi></mrow></msub></mtd></mtr><mtr><mtd><msub><mrow><mi>V</mi></mrow><mrow><mi>E</mi></mrow></msub><mo>=</mo><msub><mrow><mi>v</mi></mrow><mrow><mi>E</mi></mrow></msub><mo>-</mo><msub><mrow><mi>v</mi></mrow><mrow><mi>i</mi></mrow></msub></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682072&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682071&type= 18.03400040 8.21266651 其中： <math id="M24"><msub><mrow><mi>v</mi></mrow><mrow><mi>o</mi></mrow></msub><mo>=</mo><msub><mrow><mi>ω</mi></mrow><mrow><mi>o</mi></mrow></msub><msub><mrow><mi>r</mi></mrow><mrow><mi>d</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682074&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682073&type= 13.20800018 3.72533321 ， <math id="M25"><msub><mrow><mi>v</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>=</mo><msub><mrow><mi>ω</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>r</mi></mrow><mrow><mi>d</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682076&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682075&type= 12.19200039 3.72533321 ； <math id="M26"><msub><mrow><mi>r</mi></mrow><mrow><mi>d</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682078&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682077&type= 2.79399991 3.72533321 为驱动轮半径； <math id="M27"><msub><mrow><mi>ω</mi></mrow><mrow><mi>o</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682080&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682079&type= 3.97933316 3.72533321 和 <math id="M28"><msub><mrow><mi>ω</mi></mrow><mrow><mi>i</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682082&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682081&type= 3.47133350 3.72533321 分别为高速侧履带和低速侧履带的驱动轮转速。 水下履带采矿车绕ICR旋转，故 <math id="M29"><mi>F</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682084&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682083&type= 2.62466669 2.62466669 点和 <math id="M30"><mi>E</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682086&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682085&type= 2.45533323 2.62466669 点的牵连速度如下： <math display="block" id="M31"><mfenced open="{" close="" separators="|"><mrow><mtable><mtr><mtd><msub><mrow><mi>v</mi></mrow><mrow><mi>F</mi></mrow></msub><mo>=</mo><msub><mrow><mi>v</mi></mrow><mrow><msup><mrow><mi>c</mi></mrow><mrow><mi>'</mi></mrow></msup></mrow></msub><mo>+</mo><mfrac><mrow><mi>B</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></mfrac><mi>ω</mi><mo>=</mo><msub><mrow><mi>v</mi></mrow><mrow><mi>x</mi></mrow></msub><mo>+</mo><mfrac><mrow><mi>B</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></mfrac><mi>ω</mi></mtd></mtr><mtr><mtd><msub><mrow><mi>v</mi></mrow><mrow><mi>E</mi></mrow></msub><mo>=</mo><msub><mrow><mi>v</mi></mrow><mrow><msup><mrow><mi>c</mi></mrow><mrow><mi>'</mi></mrow></msup></mrow></msub><mo>-</mo><mfrac><mrow><mi>B</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></mfrac><mi>ω</mi><mo>=</mo><msub><mrow><mi>v</mi></mrow><mrow><mi>x</mi></mrow></msub><mo>-</mo><mfrac><mrow><mi>B</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></mfrac><mi>ω</mi></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682088&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682087&type= 38.09999847 17.35666656 其中： <math id="M32"><msub><mrow><mi>v</mi></mrow><mrow><msup><mrow><mi>c</mi></mrow><mrow><mi>'</mi></mrow></msup></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682062&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682061&type= 3.21733332 3.47133350 为水下履带采矿车几何中心 <math id="M33"><mi>c</mi><mi mathvariant="normal">'</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682064&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682063&type= 2.45533323 2.62466669 的线速度。 由刚体运动学理论可知，水下履带采矿车ICR的坐标满足如下关系： <math display="block" id="M34"><mfenced open="{" close="" separators="|"><mrow><mtable columnalign="left"><mtr><mtd><msub><mrow><mi>y</mi></mrow><mrow><mi>c</mi></mrow></msub><mo>=</mo><mfrac><mrow><msub><mrow><mi>v</mi></mrow><mrow><mi>x</mi></mrow></msub></mrow><mrow><mi>ω</mi></mrow></mfrac></mtd></mtr><mtr><mtd><msub><mrow><mi>x</mi></mrow><mrow><mi>c</mi></mrow></msub><mo>=</mo><msub><mrow><mi>x</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>=</mo><msub><mrow><mi>x</mi></mrow><mrow><mi>o</mi></mrow></msub><mo>=</mo><mfrac><mrow><msub><mrow><mi>v</mi></mrow><mrow><mi>y</mi></mrow></msub></mrow><mrow><mi>ω</mi></mrow></mfrac></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682089&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682065&type= 24.21466637 18.11866570 联立式( 1 1 )~( 3 3 )可得： <math display="block" id="M35"><mfenced open="{" close="" separators="|"><mrow><mtable><mtr><mtd><msub><mrow><mi>y</mi></mrow><mrow><mi>o</mi></mrow></msub><mo>=</mo><mfrac><mrow><msub><mrow><mi>v</mi></mrow><mrow><mi>o</mi></mrow></msub><mo>-</mo><msub><mrow><mi>v</mi></mrow><mrow><mi>x</mi></mrow></msub></mrow><mrow><mi>ω</mi></mrow></mfrac></mtd></mtr><mtr><mtd><msub><mrow><mi>y</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>=</mo><mfrac><mrow><msub><mrow><mi>v</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>-</mo><msub><mrow><mi>v</mi></mrow><mrow><mi>x</mi></mrow></msub></mrow><mrow><mi>ω</mi></mrow></mfrac></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682066&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682090&type= 18.62666702 17.61066628 联立 式(4) 式(4) 和(5)可得： <math display="block" id="M36"><mfenced open="{" close="" separators="|"><mrow><mtable columnalign="left"><mtr><mtd><msub><mrow><mi>v</mi></mrow><mrow><mi>x</mi></mrow></msub><mo>=</mo><mfrac><mrow><msub><mrow><mi>y</mi></mrow><mrow><mi>o</mi></mrow></msub><msub><mrow><mi>v</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>-</mo><msub><mrow><mi>y</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>v</mi></mrow><mrow><mi>o</mi></mrow></msub></mrow><mrow><msub><mrow><mi>y</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>-</mo><msub><mrow><mi>y</mi></mrow><mrow><mi>o</mi></mrow></msub></mrow></mfrac></mtd></mtr><mtr><mtd><msub><mrow><mi>v</mi></mrow><mrow><mi>y</mi></mrow></msub><mo>=</mo><mfrac><mrow><msub><mrow><mi>v</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>-</mo><msub><mrow><mi>v</mi></mrow><mrow><mi>o</mi></mrow></msub></mrow><mrow><msub><mrow><mi>y</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>-</mo><msub><mrow><mi>y</mi></mrow><mrow><mi>o</mi></mrow></msub></mrow></mfrac><msub><mrow><mi>x</mi></mrow><mrow><mi>c</mi></mrow></msub></mtd></mtr><mtr><mtd><mi>ω</mi><mo>=</mo><mfrac><mrow><msub><mrow><mi>v</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>-</mo><msub><mrow><mi>v</mi></mrow><mrow><mi>o</mi></mrow></msub></mrow><mrow><msub><mrow><mi>y</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>-</mo><msub><mrow><mi>y</mi></mrow><mrow><mi>o</mi></mrow></msub></mrow></mfrac></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682098&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682067&type= 24.04533195 29.71800041 水下履带采矿车的运动学微分方程可由下式所示： <math display="block" id="M37"><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><mover accent="true"><mi>X</mi><mo>˙</mo></mover></mtd></mtr><mtr><mtd><mover accent="true"><mi>Y</mi><mo>˙</mo></mover></mtd></mtr><mtr><mtd><mover accent="true"><mi>φ</mi><mo>˙</mo></mover></mtd></mtr></mtable></mrow></mfenced><mo>=</mo><mfenced open="[" close="]" separators="|"><mrow><mo>-</mo><mtable><mtr><mtd><mi mathvariant="normal">s</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi>φ</mi></mtd><mtd><mi mathvariant="normal">c</mi><mi mathvariant="normal">o</mi><mi mathvariant="normal">s</mi><mi>φ</mi></mtd><mtd><mn mathvariant="normal">0</mn></mtd></mtr><mtr><mtd><mi mathvariant="normal">c</mi><mi mathvariant="normal">o</mi><mi mathvariant="normal">s</mi><mi>φ</mi></mtd><mtd><mi mathvariant="normal">s</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi>φ</mi></mtd><mtd><mn mathvariant="normal">0</mn></mtd></mtr><mtr><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">1</mn></mtd></mtr></mtable></mrow></mfenced><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><mfrac><mrow><msub><mrow><mi>y</mi></mrow><mrow><mi>o</mi></mrow></msub><msub><mrow><mi>v</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>-</mo><msub><mrow><mi>y</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>v</mi></mrow><mrow><mi>o</mi></mrow></msub></mrow><mrow><msub><mrow><mi>y</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>-</mo><msub><mrow><mi>y</mi></mrow><mrow><mi>o</mi></mrow></msub></mrow></mfrac></mtd></mtr><mtr><mtd><mfrac><mrow><msub><mrow><mi>v</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>-</mo><msub><mrow><mi>v</mi></mrow><mrow><mi>o</mi></mrow></msub></mrow><mrow><msub><mrow><mi>y</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>-</mo><msub><mrow><mi>y</mi></mrow><mrow><mi>o</mi></mrow></msub></mrow></mfrac><msub><mrow><mi>x</mi></mrow><mrow><mi>c</mi></mrow></msub></mtd></mtr><mtr><mtd><mfrac><mrow><msub><mrow><mi>v</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>-</mo><msub><mrow><mi>v</mi></mrow><mrow><mi>o</mi></mrow></msub></mrow><mrow><msub><mrow><mi>y</mi></mrow><mrow><mi>i</mi></mrow></msub><mo>-</mo><msub><mrow><mi>y</mi></mrow><mrow><mi>o</mi></mrow></msub></mrow></mfrac></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682092&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682091&type= 58.25066376 29.71800041 2 纵向偏移补偿的轨迹跟踪算法 1 2.1 　基于 UKF 的 ICR 滑动参数估计算法 2 假设水下履带采矿车的滑动参数 <math id="M38"><mi mathvariant="bold-italic">r</mi><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced><mo>=</mo><mfenced open="[" close="" separators="|"><mrow><msub><mrow><mi>y</mi></mrow><mrow><mi>o</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682099&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682093&type= 21.75933456 4.74133301 ； <math id="M39"><msub><mrow><mi>y</mi></mrow><mrow><mi>i</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682101&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682100&type= 7.95866632 3.89466691 ； <math id="M40"><mfenced open="" close="]" separators="|"><mrow><msub><mrow><mi>x</mi></mrow><mrow><mi>c</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682095&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682094&type= 9.65200043 4.74133301 。由下式计算Sigma点的预测权重： <math display="block" id="M41"><mfenced open="{" close="" separators="|"><mrow><mtable columnalign="left"><mtr><mtd><msubsup><mrow><mi>ω</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow><mrow><mi>m</mi></mrow></msubsup><mo>=</mo><mi>λ</mi><mo>/</mo><mfenced separators="|"><mrow><mi>n</mi><mo>+</mo><mi>λ</mi></mrow></mfenced></mtd></mtr><mtr><mtd><msubsup><mrow><mi>ω</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow><mrow><mi>c</mi></mrow></msubsup><mo>=</mo><mi>λ</mi><mo>/</mo><mfenced separators="|"><mrow><mi>n</mi><mo>+</mo><mi>λ</mi></mrow></mfenced><mo>+</mo><mn mathvariant="normal">1</mn><mo>-</mo><msup><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup><mo>+</mo><mi>β</mi></mtd></mtr><mtr><mtd><msubsup><mrow><mi>ω</mi></mrow><mrow><mi>i</mi></mrow><mrow><mi>c</mi></mrow></msubsup><mo>=</mo><msubsup><mrow><mi>ω</mi></mrow><mrow><mi>i</mi></mrow><mrow><mi>m</mi></mrow></msubsup><mo>=</mo><mn mathvariant="normal">1</mn><mo>/</mo><mn mathvariant="normal">2</mn><mfenced separators="|"><mrow><mi>n</mi><mo>+</mo><mi>λ</mi></mrow></mfenced></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682102&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682096&type= 39.45466614 17.10266685 其中： <math id="M42"><mi>λ</mi><mo>=</mo><msup><mrow><mi>α</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup><mfenced separators="|"><mrow><mi>n</mi><mo>+</mo><mi>κ</mi></mrow></mfenced><mo>-</mo><mi>n</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682104&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682103&type= 24.29933357 3.89466691 ； <math id="M43"><mi>β</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682106&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682105&type= 2.11666679 3.38666677 为非负常数，根据经验公式取 <math id="M44"><mi>β</mi><mo>=</mo><mn mathvariant="normal">2</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682108&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682107&type= 7.28133297 3.38666677 ； <math id="M45"><mi>α</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682110&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682109&type= 1.94733346 2.62466669 的取值范围为 <math id="M46"><msup><mrow><mi>e</mi></mrow><mrow><mo>-</mo><mn mathvariant="normal">4</mn></mrow></msup><mo>≤</mo><mi>α</mi><mo>≤</mo><mn mathvariant="normal">1</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682112&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682111&type= 14.64733315 2.96333337 ，根据经验公式取 <math id="M47"><mi>α</mi><mo>=</mo><mn mathvariant="normal">1</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682114&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682113&type= 7.11199999 2.62466669 ； <math id="M48"><mi>κ</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682116&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682115&type= 1.86266661 2.62466669 为常数，取 <math id="M49"><mi>κ</mi><mo>=</mo><mo>-</mo><mn mathvariant="normal">1</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682118&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682117&type= 9.05933285 2.62466669 ； <math id="M50"><mi>n</mi><mo>=</mo><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">k</mi><mo stretchy="false">(</mo><mi mathvariant="bold-italic">P</mi><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682120&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682119&type= 16.51000023 3.38666677 为协方差矩阵 <math id="M51"><mi mathvariant="bold-italic">P</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682122&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682121&type= 2.53999996 2.62466669 的秩。 通过无迹变换计算 <math id="M52"><mi>k</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682124&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682123&type= 1.86266661 2.62466669 时刻Sigma点的坐标值： <math display="block" id="M53"><msub><mrow><mi mathvariant="bold-italic">ξ</mi></mrow><mrow><mi>i</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced><mo>=</mo><mfenced open="{" close="" separators="|"><mrow><mtable columnalign="left"><mtr><mtd><mover accent="true"><mi mathvariant="bold-italic">r</mi><mo>^</mo></mover><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced><mo>,</mo><mi>i</mi><mo>=</mo><mn mathvariant="normal">0</mn></mtd></mtr><mtr><mtd><mover accent="true"><mi mathvariant="bold-italic">r</mi><mo>^</mo></mover><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced><mo>+</mo><mroot><mrow><mfenced separators="|"><mrow><mi>n</mi><mo>+</mo><mi>λ</mi></mrow></mfenced><mi mathvariant="bold-italic">P</mi><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced></mrow><mrow/></mroot><mo>,</mo><mi>i</mi><mo>=</mo><mn mathvariant="normal">1,2</mn><mo>,</mo><mo>⋯</mo><mo>,</mo><mi>n</mi></mtd></mtr><mtr><mtd><mover accent="true"><mi mathvariant="bold-italic">r</mi><mo>^</mo></mover><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced><mo>-</mo><mroot><mrow><mfenced separators="|"><mrow><mi>n</mi><mo>+</mo><mi>λ</mi></mrow></mfenced><mi mathvariant="bold-italic">P</mi><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced></mrow><mrow/></mroot><mo>,</mo><mi>i</mi><mo>=</mo><mi>n</mi><mo>+</mo><mn mathvariant="normal">1</mn><mo>,</mo><mi>n</mi><mo>+</mo><mn mathvariant="normal">2</mn><mo>,</mo><mo>⋯</mo><mo>,</mo><mn mathvariant="normal">2</mn><mi>n</mi></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682126&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682125&type= 79.75599670 19.72733307 根据 式(7) 式(7) 的微分方程，预测每个Sigma点在一个采样周期内位置的变化量，并基于 k 时刻计算 <math id="M54"><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682128&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682127&type= 6.85799980 2.62466669 时刻的位置变化量均值 <math id="M55"><mover accent="true"><mi>η</mi><mo>¯</mo></mover><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn><mo stretchy="false">|</mo><mi>k</mi></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682130&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682129&type= 14.81666565 4.57200003 和协方差 <math id="M56"><mi>P</mi><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn><mo stretchy="false">|</mo><mi>k</mi></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682143&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682097&type= 15.49400043 4.57200003 ： <math display="block" id="M57"><mover accent="true"><mi mathvariant="bold-italic">η</mi><mo>¯</mo></mover><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn><mo stretchy="false">|</mo><mi>k</mi></mrow></mfenced><mo>=</mo><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>i</mi><mo>=</mo><mn mathvariant="normal">0</mn></mrow><mrow><mn mathvariant="normal">2</mn><mi>n</mi></mrow></munderover></mstyle><mrow><mfenced open="[" close="]" separators="|"><mrow><msubsup><mrow><mi>ω</mi></mrow><mrow><mi>i</mi></mrow><mrow><mi>m</mi></mrow></msubsup><msub><mrow><mi mathvariant="bold-italic">ξ</mi></mrow><mrow><mi>i</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn><mo stretchy="false">|</mo><mi>k</mi></mrow></mfenced></mrow></mfenced></mrow><mo>=</mo><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>i</mi><mo>=</mo><mn mathvariant="normal">0</mn></mrow><mrow><mn mathvariant="normal">2</mn><mi>n</mi></mrow></munderover></mstyle><mrow><msubsup><mrow><mi>ω</mi></mrow><mrow><mi>i</mi></mrow><mrow><mi>m</mi></mrow></msubsup><mi>f</mi><mfenced separators="|"><mrow><msub><mrow><mi mathvariant="bold-italic">ξ</mi></mrow><mrow><mi>i</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced></mrow></mfenced></mrow></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682145&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682144&type= 73.82933807 8.72066593 <math display="block" id="M58"><mi mathvariant="bold-italic">P</mi><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn><mo stretchy="false">|</mo><mi>k</mi></mrow></mfenced><mo>=</mo><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>i</mi><mo>=</mo><mn mathvariant="normal">0</mn></mrow><mrow><mn mathvariant="normal">2</mn><mi>n</mi></mrow></munderover></mstyle><mrow><mfenced open="[" close="" separators="|"><mrow><msubsup><mrow><mi>ω</mi></mrow><mrow><mi>i</mi></mrow><mrow><mi>c</mi></mrow></msubsup><mfenced separators="|"><mrow><msub><mrow><mi mathvariant="bold-italic">ξ</mi></mrow><mrow><mi>i</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn><mo stretchy="false">|</mo><mi>k</mi></mrow></mfenced><mo>-</mo><mover accent="true"><mi mathvariant="bold-italic">η</mi><mo>¯</mo></mover><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn><mo stretchy="false">|</mo><mi>k</mi></mrow></mfenced></mrow></mfenced><mo>×</mo></mrow></mfenced></mrow></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682131&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682146&type= 70.18866730 8.72066593 <math display="block" id="M59"><mfenced open="" close="]" separators="|"><mrow><msup><mrow><mfenced separators="|"><mrow><msub><mrow><mi mathvariant="bold-italic">ξ</mi></mrow><mrow><mi>i</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn><mo stretchy="false">|</mo><mi>k</mi></mrow></mfenced><mo>-</mo><mover accent="true"><mi mathvariant="bold-italic">η</mi><mo>¯</mo></mover><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn><mo stretchy="false">|</mo><mi>k</mi></mrow></mfenced></mrow></mfenced></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup></mrow></mfenced><mo>+</mo><mi mathvariant="bold-italic">Q</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682147&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682132&type= 47.15933609 7.61999989 其中： <math id="M60"><mi mathvariant="bold-italic">Q</mi><mo>∈</mo><msup><mrow><mi mathvariant="bold">R</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682149&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682148&type= 10.58333302 3.64066648 ，为过程噪声的协方差矩阵； <math id="M61"><mi mathvariant="bold">R</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682151&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682150&type= 2.70933342 2.62466669 为实数集； <math id="M62"><msubsup><mrow><mi>ω</mi></mrow><mrow><mi>i</mi></mrow><mrow><mi>c</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682153&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682152&type= 3.80999994 4.06400013 和 <math id="M63"><msubsup><mrow><mi>ω</mi></mrow><mrow><mi>i</mi></mrow><mrow><mi>m</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682155&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682154&type= 4.40266657 4.06400013 为Sigma点的权重。 传统的UKF通过观测方程计算每个Sigma点的观测值 [ 20 20 ] ，本文提出的UKF可以自定义观测步长 <math id="M64"><mi>m</mi><mfenced separators="|"><mrow><mi>m</mi><mo>></mo><mn mathvariant="normal">2</mn><mi>n</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682157&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682156&type= 20.57399940 3.72533321 ，记录当前时刻 <math id="M65"><mi>k</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682159&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682158&type= 1.86266661 2.62466669 的惯性导航传感器的观测位姿信息 <math id="M66"><mi mathvariant="bold-italic">z</mi><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced><mo>=</mo><msup><mrow><mfenced open="[" close="]" separators="|"><mrow><mi>X</mi><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced><mo>,</mo><mi>Y</mi><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced><mo>,</mo><mi>φ</mi><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced></mrow></mfenced></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682161&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682160&type= 44.87333298 5.33400011 ，并根据历史时刻的观测位姿信息 <math id="M67"><mi mathvariant="bold-italic">z</mi><mfenced separators="|"><mrow><mi>k</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682163&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682162&type= 11.59933376 3.72533321 ， <math id="M68"><mi mathvariant="bold-italic">z</mi><mfenced separators="|"><mrow><mi>k</mi><mo>-</mo><mn mathvariant="normal">2</mn></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682165&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682164&type= 11.93799973 3.72533321 ， <math id="M69"><mo>⋯</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682167&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682166&type= 3.72533321 1.18533325 ， <math id="M70"><mi mathvariant="bold-italic">z</mi><mfenced separators="|"><mrow><mi>k</mi><mo>-</mo><mi>m</mi></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682169&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682168&type= 12.78466606 3.72533321 计算一个采样周期内位置的观测变化量 <math id="M71"><msub><mrow><mi mathvariant="bold-italic">Z</mi></mrow><mrow><mi>i</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682171&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682170&type= 8.38199997 3.89466691 。 <math display="block" id="M72"><msub><mrow><mi mathvariant="bold-italic">Z</mi></mrow><mrow><mi>i</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced><mo>=</mo><mi mathvariant="bold-italic">z</mi><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mi>i</mi></mrow></mfenced><mo>-</mo><mi mathvariant="bold-italic">z</mi><msup><mrow><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mi>i</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></mfenced></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup><mo>,</mo><mi>i</mi><mo>=</mo><mn mathvariant="normal">0,1</mn><mo>,</mo><mo>⋯</mo><mo>,</mo><mi>m</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682173&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682172&type= 65.27799988 4.65666676 与传统的UKF不同，观测值的权重由无迹变换得到，本文UKF的 <math id="M73"><msub><mrow><mi mathvariant="bold-italic">Z</mi></mrow><mrow><mi>i</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682175&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682174&type= 8.38199997 3.89466691 观测变化量的权重 <math id="M74"><msubsup><mrow><mi>ω</mi></mrow><mrow><mi>i</mi></mrow><mrow><mi>z</mi></mrow></msubsup><mo>∈</mo><mo stretchy="false">(</mo><mn mathvariant="normal">0,1</mn><mo stretchy="false">)</mo><mtext> </mtext><mo stretchy="false">(</mo><mi>i</mi><mo>=</mo><mn mathvariant="normal">0,1</mn><mo>,</mo><mo>⋯</mo><mo>,</mo><mi>m</mi><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682177&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682176&type= 34.45933533 4.82600021 可根据实际情况以及计算观测变化量 <math id="M75"><mover accent="true"><mi mathvariant="bold-italic">Z</mi><mo>¯</mo></mover><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682179&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682178&type= 8.21266651 3.89466691 的均值、协方差 <math id="M76"><msub><mrow><mi mathvariant="bold-italic">P</mi></mrow><mrow><mi>η</mi><mi>z</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682181&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682180&type= 9.73666668 4.31799984 和 <math id="M77"><msub><mrow><mi mathvariant="bold-italic">P</mi></mrow><mrow><mi>z</mi><mi>z</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682183&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682182&type= 9.48266602 3.89466691 进行标定。 <math display="block" id="M78"><mover accent="true"><mi mathvariant="bold-italic">Z</mi><mo>¯</mo></mover><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced><mo>=</mo><mfenced open="[" close="]" separators="|"><mrow><msubsup><mrow><mi>ω</mi></mrow><mrow><mi>i</mi></mrow><mrow><mi>z</mi></mrow></msubsup><msub><mrow><mi mathvariant="bold-italic">Z</mi></mrow><mrow><mi>i</mi></mrow></msub><mo stretchy="false">(</mo><mi>k</mi><mo stretchy="false">)</mo></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682134&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682184&type= 27.68600082 4.74133301 <math display="block" id="M79"><msub><mrow><mi mathvariant="bold-italic">P</mi></mrow><mrow><mi>η</mi><mi>z</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced><mo>=</mo><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>i</mi><mo>=</mo><mn mathvariant="normal">0</mn></mrow><mrow><mn mathvariant="normal">2</mn><mi>n</mi></mrow></munderover></mstyle><mrow><mfenced open="[" close="]" separators="|"><mrow><mfenced separators="|"><mrow><msub><mrow><mi mathvariant="bold-italic">ξ</mi></mrow><mrow><mi>i</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn><mo stretchy="false">|</mo><mi>k</mi></mrow></mfenced><mo>-</mo><mover accent="true"><mi mathvariant="bold-italic">η</mi><mo>¯</mo></mover><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn><mo stretchy="false">|</mo><mi>k</mi></mrow></mfenced></mrow></mfenced><msup><mrow><mfenced separators="|"><mrow><msub><mrow><mi mathvariant="bold-italic">Z</mi></mrow><mrow><mi>i</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced><mo>-</mo><mover accent="true"><mi mathvariant="bold-italic">Z</mi><mo>¯</mo></mover><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced></mrow></mfenced></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup></mrow></mfenced></mrow></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682186&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682185&type= 81.53399658 8.63599968 <math display="block" id="M80"><msub><mrow><mi mathvariant="bold-italic">P</mi></mrow><mrow><mi>z</mi><mi>z</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced><mo>=</mo><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>i</mi><mo>=</mo><mn mathvariant="normal">0</mn></mrow><mrow><mi>m</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></munderover></mstyle><mrow><mfenced open="[" close="]" separators="|"><mrow><mfenced separators="|"><mrow><msub><mrow><mi mathvariant="bold-italic">Z</mi></mrow><mrow><mi>i</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced><mo>-</mo><mover accent="true"><mi mathvariant="bold-italic">Z</mi><mo>¯</mo></mover><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced></mrow></mfenced><msup><mrow><mfenced separators="|"><mrow><msub><mrow><mi mathvariant="bold-italic">Z</mi></mrow><mrow><mi>i</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced><mo>-</mo><mover accent="true"><mi mathvariant="bold-italic">Z</mi><mo>¯</mo></mover><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced></mrow></mfenced></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup></mrow></mfenced><mo>+</mo><mi>R</mi></mrow></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682188&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682187&type= 78.40132904 8.72066593 其中： <math id="M81"><mi mathvariant="bold-italic">R</mi><mo>∈</mo><msup><mrow><mi mathvariant="bold">R</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682190&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682189&type= 10.49866676 2.96333337 ，为测量噪声的协方差矩阵。 计算UKF的增益 <math id="M82"><mi mathvariant="bold-italic">K</mi><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682135&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682191&type= 12.95400047 3.72533321 ，并更新滑动参数 <math id="M83"><mi mathvariant="bold-italic">r</mi><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682137&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682192&type= 11.59933376 3.72533321 和协方差矩阵 <math id="M84"><mi mathvariant="bold-italic">P</mi><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682194&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682138&type= 12.61533356 3.72533321 ： <math display="block" id="M85"><mi mathvariant="bold-italic">K</mi><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced><mo>=</mo><msub><mrow><mi mathvariant="bold-italic">P</mi></mrow><mrow><mi>η</mi><mi>z</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced><msubsup><mrow><mi mathvariant="bold-italic">P</mi></mrow><mrow><mi>z</mi><mi>z</mi></mrow><mrow><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msubsup><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682198&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682196&type= 37.76133347 4.40266657 <math display="block" id="M86"><mi mathvariant="bold-italic">P</mi><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced><mo>=</mo><mi mathvariant="bold-italic">P</mi><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn><mo stretchy="false">|</mo><mi>k</mi></mrow></mfenced><mo>+</mo><mi mathvariant="bold-italic">K</mi><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced><msub><mrow><mi mathvariant="bold-italic">P</mi></mrow><mrow><mi>z</mi><mi>z</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced><msup><mrow><mi mathvariant="bold-italic">K</mi></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682141&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682200&type= 73.99867249 4.57200003 <math display="block" id="M87"><mi mathvariant="bold-italic">r</mi><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced><mo>=</mo><mover accent="true"><mi mathvariant="bold-italic">r</mi><mo>^</mo></mover><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced><mo>+</mo><mi mathvariant="bold-italic">K</mi><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced><mfenced separators="|"><mrow><msub><mrow><mi mathvariant="bold-italic">Z</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced><mo>-</mo><mover accent="true"><mi mathvariant="bold-italic">η</mi><mo>¯</mo></mover><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn><mo stretchy="false">|</mo><mi>k</mi></mrow></mfenced></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682142&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682202&type= 71.03533173 5.24933338 2.2 　水下履带采矿车轨迹跟踪算法 2 水下履带采矿车的横纵向偏差如 图2 图2 所示，其中载体坐标系为 <math id="M88"><mi>c</mi><mi>'</mi><mi>x</mi><mi>y</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682207&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682204&type= 5.75733376 3.38666677 ，大地坐标系为 <math id="M89"><mi>O</mi><mi>X</mi><mi>Y</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682210&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682209&type= 7.45066643 2.62466669 ； <math id="M90"><msub><mrow><mi>e</mi></mrow><mrow><mi>x</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682220&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682211&type= 2.87866688 3.72533321 为载体坐标系下的纵向误差； <math id="M91"><msub><mrow><mi>e</mi></mrow><mrow><mi>y</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682223&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682222&type= 2.87866688 3.72533321 为载体坐标系下的横向误差； <math id="M92"><mfenced separators="|"><mrow><msub><mrow><mi>X</mi></mrow><mrow><mi mathvariant="normal">r</mi></mrow></msub><mo>,</mo><msub><mrow><mi>Y</mi></mrow><mrow><mi mathvariant="normal">r</mi></mrow></msub></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682216&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682214&type= 11.09133339 4.65666676 为当前时刻大地坐标系的参考轨迹点； <math id="M93"><msub><mrow><mi>v</mi></mrow><mrow><mi mathvariant="normal">r</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682225&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682217&type= 2.62466669 3.72533321 为当前时刻的参考线速度； <math id="M94"><mi>v</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682236&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682235&type= 1.69333339 2.62466669 为当前时刻的水下履带采矿车线速度； <math id="M95"><msub><mrow><mi>R</mi></mrow><mrow><mi mathvariant="normal">t</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682229&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682237&type= 3.21733332 3.72533321 为轨迹的曲率半径； <math id="M96"><msub><mrow><mi>x</mi></mrow><mrow><mi>c</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682232&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682231&type= 2.96333337 3.72533321 为ICR的纵向偏移量； <math id="M97"><msub><mrow><mi>φ</mi></mrow><mrow><mi mathvariant="normal">e</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682251&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682249&type= 3.21733332 4.48733330 为航向角偏差； <math id="M98"><msub><mrow><mi>δ</mi></mrow><mrow><mi mathvariant="normal">e</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682240&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682252&type= 2.87866688 3.72533321 为横向偏差； <math id="M99"><mi>δ</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682243&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682242&type= 1.77800000 2.62466669 为期望的虚拟转角。 在横向控制中，由水下履带采矿车的虚拟转角 <math id="M100"><mi>δ</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682243&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682242&type= 1.77800000 2.62466669 代替Stanley算法中的前轮偏角 [ 19 19 ] ， <math id="M101"><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682340&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682339&type= 6.85799980 2.62466669 时刻期望的虚拟转角 <math id="M102"><mi>δ</mi><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682270&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682269&type= 11.76866722 3.72533321 由下式计算： <math display="block" id="M103"><mi>δ</mi><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced><mo>=</mo><msub><mrow><mi>δ</mi></mrow><mrow><mi mathvariant="normal">e</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced><mo>+</mo><msub><mrow><mi>φ</mi></mrow><mrow><mi mathvariant="normal">e</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682247&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682246&type= 36.32199860 3.89466691 其中： <math id="M104"><msub><mrow><mi>δ</mi></mrow><mrow><mi mathvariant="normal">e</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced><mo>=</mo><mi mathvariant="normal">a</mi><mi mathvariant="normal">r</mi><mi mathvariant="normal">c</mi><mi mathvariant="normal">t</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mfenced separators="|"><mrow><msub><mrow><mi>k</mi></mrow><mrow><mi>δ</mi></mrow></msub><mi>*</mi><msub><mrow><mi>e</mi></mrow><mrow><mi>y</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced><mo>/</mo><mi>v</mi><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682276&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682274&type= 46.31266785 5.67266655 ； <math id="M105"><msub><mrow><mi>k</mi></mrow><mrow><mi>δ</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682281&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682279&type= 2.87866688 3.72533321 为比例系数。 采用比例微分法由下式计算得到 <math id="M106"><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682340&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682339&type= 6.85799980 2.62466669 时刻期望的水下履带采矿车角速度 <math id="M107"><mi>ω</mi><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682288&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682287&type= 12.53066730 3.72533321 ： <math display="block" id="M108"><mi>ω</mi><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced><mo>=</mo><msub><mrow><mi>k</mi></mrow><mrow><mi mathvariant="normal">p</mi></mrow></msub><mi>δ</mi><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced><mo>+</mo><msub><mrow><mi>k</mi></mrow><mrow><mi mathvariant="normal">d</mi></mrow></msub><mfrac><mrow><mi>δ</mi><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced><mo>-</mo><mi>δ</mi><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced></mrow><mrow><mi>T</mi></mrow></mfrac></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682294&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682277&type= 62.73799896 7.61999989 其中： <math id="M109"><msub><mrow><mi>k</mi></mrow><mrow><mi mathvariant="normal">p</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682290&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682295&type= 2.96333337 3.72533321 和 <math id="M110"><msub><mrow><mi>k</mi></mrow><mrow><mi mathvariant="normal">d</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682308&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682291&type= 2.96333337 3.72533321 为比例系数； <math id="M111"><mi>T</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682311&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682310&type= 2.45533323 2.62466669 为采样周期。 由于瞬时转向中心存在纵向偏移，水下履带采矿车的瞬时转向中心与几何中心并不重合，故在纵向控制中，将改进UKF估计得到的 <math id="M112"><msub><mrow><mi>x</mi></mrow><mrow><mi mathvariant="normal">c</mi></mrow></msub><mfenced separators="|"><mrow><mi>t</mi></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682314&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682312&type= 7.36600018 3.89466691 作为纵向偏差 <math id="M113"><msub><mrow><mi>e</mi></mrow><mrow><mi>x</mi></mrow></msub><mfenced separators="|"><mrow><mi>t</mi></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682304&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682302&type= 7.19666624 3.89466691 的补偿，然后得到实际的纵向偏差 <math id="M114"><msub><mrow><mi>e</mi></mrow><mrow><mi mathvariant="normal">L</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682323&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682306&type= 3.21733332 3.72533321 ，由下式计算得到 <math id="M115"><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682327&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682325&type= 6.85799980 2.62466669 时刻水下履带采矿车期望的线速度 <math id="M116"><mi>v</mi><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682315&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682329&type= 11.76866722 3.72533321 ： <math display="block" id="M117"><mi>v</mi><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced><mo>=</mo><msub><mrow><mi>k</mi></mrow><mrow><mi mathvariant="normal">L</mi></mrow></msub><msub><mrow><mi>e</mi></mrow><mrow><mi mathvariant="normal">L</mi></mrow></msub><mo>+</mo><msub><mrow><mi>k</mi></mrow><mrow><mi mathvariant="normal">v</mi></mrow></msub><msub><mrow><mi>e</mi></mrow><mrow><mi mathvariant="normal">v</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682317&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682331&type= 31.75000000 3.89466691 其中： <math id="M118"><msub><mrow><mi>e</mi></mrow><mrow><mi mathvariant="normal">L</mi></mrow></msub><mo>=</mo><msub><mrow><mi>e</mi></mrow><mrow><mi>x</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced><mo>-</mo><msub><mrow><mi>x</mi></mrow><mrow><mi mathvariant="normal">c</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682335&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682334&type= 26.50066566 3.89466691 ； <math id="M119"><msub><mrow><mi>e</mi></mrow><mrow><mi mathvariant="normal">v</mi></mrow></msub><mo>=</mo><msub><mrow><mi>v</mi></mrow><mrow><mi mathvariant="normal">r</mi></mrow></msub><mo>-</mo><mi>v</mi><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682319&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682337&type= 19.30400085 3.89466691 ； <math id="M120"><msub><mrow><mi>k</mi></mrow><mrow><mi mathvariant="normal">L</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682338&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682321&type= 3.21733332 3.72533321 和 <math id="M121"><msub><mrow><mi>k</mi></mrow><mrow><mi mathvariant="normal">v</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682356&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682353&type= 2.96333337 3.72533321 为比例系数。 由 图1 图1 可知，水下履带采矿车满足如下的运动学关系： <math display="block" id="M122"><msup><mrow><mi>v</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup><mo>=</mo><msubsup><mrow><mi>v</mi></mrow><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup><mo>+</mo><msubsup><mrow><mi>v</mi></mrow><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup><mo>=</mo><msubsup><mrow><mi>v</mi></mrow><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup><mo>+</mo><msup><mrow><mfenced separators="|"><mrow><mi>ω</mi><msub><mrow><mi>x</mi></mrow><mrow><mi mathvariant="normal">c</mi></mrow></msub></mrow></mfenced></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682360&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682358&type= 35.64466476 5.58799982 为此，由下式计算 <math id="M123"><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682340&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682339&type= 6.85799980 2.62466669 时刻水下履带采矿车的驱动轮转速： <math display="block" id="M124"><mfenced open="{" close="" separators="|"><mrow><mtable><mtr><mtd><msub><mrow><mi>ω</mi></mrow><mrow><mi>o</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced><mo>=</mo><mfenced separators="|"><mrow><mn mathvariant="normal">2</mn><msub><mrow><mi>v</mi></mrow><mrow><mi>x</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced><mo>+</mo><mi>B</mi><mi>ω</mi><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced></mrow></mfenced><mo>/</mo><msub><mrow><mi>r</mi></mrow><mrow><mi mathvariant="normal">d</mi></mrow></msub></mtd></mtr><mtr><mtd><msub><mrow><mi>ω</mi></mrow><mrow><mi>i</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced><mo>=</mo><mfenced separators="|"><mrow><mn mathvariant="normal">2</mn><msub><mrow><mi>v</mi></mrow><mrow><mi>x</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced><mo>-</mo><mi>B</mi><mi>ω</mi><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced></mrow></mfenced><mo>/</mo><msub><mrow><mi>r</mi></mrow><mrow><mi mathvariant="normal">d</mi></mrow></msub></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682344&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682343&type= 60.87533569 12.36133385 其中： <math id="M125"><msub><mrow><mi>v</mi></mrow><mrow><mi>x</mi></mrow></msub><mfenced separators="|"><mrow><mi>k</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682366&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682364&type= 12.53066730 3.89466691 由 式(22) 式(22) 计算得到。 3 仿真分析 1 3.1 　仿真环境及参数设置 2 本文基于RecurDyn搭建水下履带采矿车的底盘机械模型，在Simulink软件中模拟水下履带采矿车的控制系统，用于验证所提出的轨迹跟踪算法。 RecurDyn搭建的水下履带采矿车的底盘模型如 图3 图3 所示，模型参数如 表1 表1 所示。 模型参数 参数值 长/m <math id="M126"><mn>2.7</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682376&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682375&type= 4.23333359 2.37066650 宽/m <math id="M127"><mn>2.4</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682383&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682377&type= 4.23333359 2.37066650 <math id="M128"><mi mathvariant="normal">高</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682400&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682380&type= 3.38666677 3.13266683 /m <math id="M129"><mn>0.9</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682384&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682403&type= 4.23333359 2.37066650 水下质量/kg <math id="M130"><mn>95</mn><mtext> </mtext><mn>430</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682387&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682406&type= 9.05933285 2.45533323 迎水面积/ <math id="M131"><msup><mrow><mi mathvariant="normal">m</mi></mrow><mrow><mn>2</mn></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682409&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682389&type= 3.89466691 2.62466669 <math id="M132"><mn>1.5</mn><mtext> </mtext></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682394&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682392&type= 4.91066647 2.45533323 为了模拟水下履带采矿车的作业环境，通过在RecurDyn中设置履带与接触面的力学参数，以模拟水下履带采矿车在多金属结核矿区作业时的稀软底质；并在RecurDyn中添加外部负载力以模拟水下履带采矿车车受到的水阻力，其中水阻力 <math id="M133"><msub><mrow><mi>F</mi></mrow><mrow><mi mathvariant="normal">w</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682415&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682428&type= 4.14866638 3.72533321 由下式计算： <math display="block" id="M134"><msub><mrow><mi>F</mi></mrow><mrow><mi mathvariant="normal">w</mi></mrow></msub><mo>=</mo><mfrac><mrow><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">2</mn></mrow></mfrac><msub><mrow><mi>C</mi></mrow><mrow><mi mathvariant="normal">d</mi></mrow></msub><msub><mrow><mi>ρ</mi></mrow><mrow><mi mathvariant="normal">w</mi></mrow></msub><mi>A</mi><mfenced open="|" close="|" separators="|"><mrow><msub><mrow><mi>v</mi></mrow><mrow><mi mathvariant="normal">w</mi></mrow></msub><mo>-</mo><mi>v</mi></mrow></mfenced><mfenced separators="|"><mrow><msub><mrow><mi>v</mi></mrow><mrow><mi mathvariant="normal">w</mi></mrow></msub><mo>-</mo><mi>v</mi></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682435&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682417&type= 45.04266739 7.19666624 其中： <math id="M135"><msub><mrow><mi>C</mi></mrow><mrow><mi mathvariant="normal">d</mi></mrow></msub><mo>=</mo><mn mathvariant="normal">2</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682437&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682419&type= 8.97466660 3.72533321 ，为水阻力系数； <math id="M136"><msub><mrow><mi>ρ</mi></mrow><mrow><mi mathvariant="normal">w</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682441&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682439&type= 3.89466691 4.48733330 为水密度， <math id="M137"><mi mathvariant="normal">k</mi><mi mathvariant="normal">g</mi><mo>/</mo><msup><mrow><mi mathvariant="normal">m</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682424&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682423&type= 8.89000034 3.72533321 ； <math id="M138"><msub><mrow><mi>v</mi></mrow><mrow><mi mathvariant="normal">w</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682426&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682446&type= 3.47133350 3.72533321 为水流速度， <math id="M139"><mi mathvariant="normal">m</mi><mo>/</mo><mi mathvariant="normal">s</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682459&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682448&type= 5.33400011 2.62466669 ； <math id="M140"><mi>A</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682450&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682461&type= 2.45533323 2.62466669 为迎水面积， <math id="M141"><msup><mrow><mi mathvariant="normal">m</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682465&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682464&type= 4.14866638 2.87866688 。 在仿真分析中，水流速度 <math id="M142"><msub><mrow><mi>v</mi></mrow><mrow><mi mathvariant="normal">w</mi></mrow></msub><mo>=</mo><mn mathvariant="normal">1</mn><mtext> </mtext><mi mathvariant="normal">m</mi><mo>/</mo><mi mathvariant="normal">s</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682468&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682466&type= 14.73200035 3.80999994 ，水密度 <math id="M143"><msub><mrow><mi>ρ</mi></mrow><mrow><mi mathvariant="normal">w</mi></mrow></msub><mo>=</mo><mn mathvariant="normal">1</mn><mtext> </mtext><mn mathvariant="normal">037</mn><mtext> </mtext><mi mathvariant="normal">k</mi><mi mathvariant="normal">g</mi><mo>/</mo><msup><mrow><mi mathvariant="normal">m</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682472&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682470&type= 24.97666740 4.82600021 ，水下履带采矿车的参考线速度 <math id="M144"><msub><mrow><mi>v</mi></mrow><mrow><mi mathvariant="normal">r</mi></mrow></msub><mo>=</mo><mn mathvariant="normal">0.5</mn><mtext> </mtext><mi mathvariant="normal">m</mi><mo>/</mo><mi mathvariant="normal">s</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682475&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682457&type= 16.59466553 3.80999994 。根据调试经验，测量噪声的协方差矩阵 <math id="M145"><mi mathvariant="bold-italic">R</mi><mo>=</mo><mi mathvariant="normal">d</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">g</mi><mo stretchy="false">(</mo><msup><mrow><mn mathvariant="normal">10</mn></mrow><mrow><mo>-</mo><mn mathvariant="normal">3</mn></mrow></msup><mo>,</mo><msup><mrow><mn mathvariant="normal">10</mn></mrow><mrow><mo>-</mo><mn mathvariant="normal">3</mn></mrow></msup><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682479&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682488&type= 28.53266525 3.72533321 ，过程噪声的协方差矩阵 <math id="M146"><mi mathvariant="bold-italic">Q</mi><mo>=</mo><mi mathvariant="normal">d</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">g</mi><mo stretchy="false">(</mo><msup><mrow><mn mathvariant="normal">10</mn></mrow><mrow><mo>-</mo><mn mathvariant="normal">4</mn></mrow></msup><mo>,</mo><msup><mrow><mn mathvariant="normal">10</mn></mrow><mrow><mo>-</mo><mn mathvariant="normal">4</mn></mrow></msup><mo>,</mo><msup><mrow><mn mathvariant="normal">10</mn></mrow><mrow><mo>-</mo><mn mathvariant="normal">4</mn></mrow></msup><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682484&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682481&type= 36.40666580 3.72533321 3.2 　仿真结果与分析 2 本文分别以2种直线圆弧拼接的采集工况为例进行仿真实验。 3.2.1　工况 <math id="M147"><mi mathvariant="normal">Ι</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682495&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682492&type= 1.26999998 2.62466669 的仿真结果 3 引入纵向偏差补偿和未引入纵向偏差补偿的轨迹跟踪效果对比如 图4 图4 所示。由轨迹跟踪结果可知，本文提出的轨迹跟踪方法可以有效地跟踪工况 <math id="M148"><mi mathvariant="normal">Ι</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682503&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682497&type= 1.26999998 2.62466669 中水下履带采矿车的轨迹。 横向偏差、纵向偏差和航向角偏差对比如 图5 图5 所示。由仿真结果可知，引入纵向偏差补偿后，纵向偏差降低了约0.05 m，降幅显著；同时，最大航向角偏差由约5.5°降低到了3.5°。 (a) 轨迹跟踪横向偏差；(b) 轨迹跟踪纵向偏差； (c) 轨迹跟踪航向角偏差 引入纵向偏差补偿后，轨迹跟踪过程中水下履带采矿车的速度变化如 图6 图6 所示。由仿真结果可知，水下履带采矿车的速度变化平稳，轨迹跟踪时的平顺性较好。 UKF估计的ICR纵向偏移如 图7 图7 所示，在约70 <math id="M155"><mi mathvariant="normal">s</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682566&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682563&type= 1.52400005 2.62466669 后，ICR的纵向偏移为固定值0.093 m，由此可知UKF估计的IRC纵向偏移是可收敛的。 3.2.2　工况 <math id="M158"><mi mathvariant="normal">Ι</mi><mi mathvariant="normal">Ι</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682598&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682583&type= 2.53999996 2.62466669 的仿真结果 3 工况 <math id="M159"><mi mathvariant="normal">Ι</mi><mi mathvariant="normal">Ι</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682686&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682685&type= 2.53999996 2.62466669 的水下履带采矿车轨迹跟踪结果如 图8 图8 所示，由 图8 图8 可见：本文提出的轨迹跟踪方法同样可以有效跟踪预设轨迹。 工况 <math id="M162"><mi mathvariant="normal">Ι</mi><mi mathvariant="normal">Ι</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682686&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682685&type= 2.53999996 2.62466669 的轨迹跟踪偏差如 图9 图9 所示。由 图9 图9 可见，通过引入纵向偏差补偿，基本消除了纵向偏差，未引入纵向偏差的纵向偏差最大可达约0.251 m； 其中横向偏差也较小，最大只有约0.021 m；同时在跟踪圆弧轨迹时航向角偏差也减少了约2°。 (a) 轨迹跟踪横向偏差；(b) 轨迹跟踪纵向偏差； (c) 轨迹跟踪航向角偏差 工况 <math id="M165"><mi mathvariant="normal">Ι</mi><mi mathvariant="normal">Ι</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682686&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682685&type= 2.53999996 2.62466669 中，轨迹跟踪时的水下履带采矿车速度变化如 图10 图10 所示，可见水下履带采矿车的速度变化同样较平稳，轨迹跟踪时的平顺性较好。 工况 <math id="M168"><mi mathvariant="normal">Ι</mi><mi mathvariant="normal">Ι</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682686&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682685&type= 2.53999996 2.62466669 的ICR纵向偏移如 图11 图11 所示。由 图11 图11 可见：67 <math id="M169"><mi mathvariant="normal">s</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682688&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=43682687&type= 1.52400005 2.62466669 后ICR的纵向偏移为固定值0.143 m，因此，UKF估计的ICR纵向偏移同样是收敛的。 4 结论 1 1) 基于考虑瞬时转移中心(ICR)的履带车运动学模型，提出了一种分散式的水下履带采矿车采矿车的轨迹跟踪算法。 2) 在纵向控制中，通过改进的UKF算法估计ICR的纵向偏移，将ICR的纵向偏移作为纵向偏差的补偿，以此计算得到水下履带采矿车期望的线速度。在横向控制中，基于横向偏差计算水下履带采矿车期望的虚拟转角，以此计算得到水下履带采矿车期望的角速度。最后，将线速度和角速度转变为履带驱动轮的转速作为控制输入。仿真结果表明，水下履带采矿车可以平顺并精准地跟踪预设轨迹，通过引入纵向偏差补偿可以降低轨迹跟踪时的纵向偏差和航向角偏差。 参考文献 李艳 , 贺加贝 . 海底多金属硫化物集矿过程固液两相流数值模拟研究 [J]. 中南大学学报(自然科学版) . 2022 , 53 ( 2 ): 461 - 470 . 海底多金属硫化物集矿过程固液两相流数值模拟研究 LI Yan , HE Jiabei . Numerical simulation of solid-liquid two-phase flow of the gathering process of seafloor massive sulfide [J]. Journal of Central South University(Science and Technolegy) . 2022 , 53 ( 2 ): 461 - 470 . Numerical simulation of solid-liquid two-phase flow of the gathering process of seafloor massive sulfide 康娅娟 , 王长伟 , 刘少军 , 等 . 深海多金属结核商业开采水下垂直提升方案 [J]. 中国有色金属学报 . 2021 , 31 ( 10 ): 2938 - 2952 . 深海多金属结核商业开采水下垂直提升方案 KANG Yajuan , WANG Changwei , LIU Shaojun . et al . Development history and prospect of deep sea polymetallic nodules mining technology [J]. The Chinese Journal of Nonferrous Metals , 2021 , 31 ( 10 ): 2848 - 2860 . Development history and prospect of deep sea polymetallic nodules mining technology 刘少军 , 胡建华 , 戴瑜 , 等 . 深海多金属硫化物集矿罩的流场分析和参数优化 [J]. 中南大学学报(自然科学版) . 2017 , 48 ( 5 ): 1198 - 1203 . 深海多金属硫化物集矿罩的流场分析和参数优化 LIU Shaojun , HU Jianhua , DAI Yu , et.al . Flow field analysis and parameter optimization of deep-sea collector of SMS [J]. Journal of Central South University(Science and Technolegy) . 2017 , 48 ( 5 ): 1198 - 1203 . Flow field analysis and parameter optimization of deep-sea collector of SMS 徐海良 , 彭能 , 杨放琼 . 避障模式下深海采矿系统动力学分析 [J]. 中南大学学报(自然科学版) . 2020 , 51 ( 3 ): 641 - 649 . 避障模式下深海采矿系统动力学分析 XU Hailiang , PENG Neng , YANG Fangqiong . Dynamic analysis of deep sea mining system in obstacle avoidance mode [J]. Journal of Central South University(Science and Technolegy) . 2020 , 51 ( 3 ): 641 - 649 . Dynamic analysis of deep sea mining system in obstacle avoidance mode 戴瑜 , 张晏仰 , 卞家能 , 等 . 海底采矿作业车螺旋桨推进器水动力特性研究 [J]. 中国有色金属学报 . 2021 , 31 ( 10 ): 2771 - 2781 . 海底采矿作业车螺旋桨推进器水动力特性研究 DAI Yu , ZHANG Yanyang , BIAN Jianeng , et al . Hydrodynamic characteristics of propeller of subsea mining vehicle [J]. The Chinese Journal of Nonferrous Metals , 2021 , 31 ( 10 ): 2771 - 2781 . Hydrodynamic characteristics of propeller of subsea mining vehicle 黄鑫 , 戴瑜 , 苏乔 , 等 . 深海采矿机器人入水冲击流场仿真及试验验证 [J]. 中国有色金属学报 . 2021 , 31 ( 10 ): 2782 - 2794 . 深海采矿机器人入水冲击流场仿真及试验验证 HUANG Xin , DAI Yu , SU Qiao , et al . Simulation and experimental verification of water impact flow field of deep ocean mining vehicle entering water [J]. The Chinese Journal of Nonferrous Metals , 2021 , 31 ( 10 ): 2782 - 2794 . Simulation and experimental verification of water impact flow field of deep ocean mining vehicle entering water 戴瑜 , 张健 , 张滔 , 等 . 基于多体动力学模型集成的深海采矿系统联动仿真 [J]. 机械工程学报 . 2017 , 53 ( 4 ): 155 - 160 . 基于多体动力学模型集成的深海采矿系统联动仿真 DAI Yu , ZHANG Jian , ZHANG Tao , 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 . Motion simulation of the deep ocean mining system based on its integrated multi-body dynamic model 李睿 , 项昌乐 , 王超 , 等 . 自动驾驶履带车辆鲁棒自适应轨迹跟踪控制方法 [J]. 兵工学报 . 2021 , 42 ( 6 ): 1128 - 1137 . 自动驾驶履带车辆鲁棒自适应轨迹跟踪控制方法 LI Rui , XIANG Changle , WANG Chao , et al . Robust adaptive trajectory tracking control approach for autonomous tracked vehicles [J]. Acta Armamentarii , 2021 , 42 ( 6 ): 1128 - 1137 . Robust adaptive trajectory tracking control approach for autonomous tracked vehicles 张杰 , 马晓军 , 刘春光 , 等 . 双侧独立电驱动履带车辆反馈线性化解耦与预测行驶控制 [J]. 兵工学报 . 2021 , 42 ( 4 ): 697 - 705 . 双侧独立电驱动履带车辆反馈线性化解耦与预测行驶控制 ZHANG Jie , MA Xiaojun , LIU Chunguang , et al . Feedback linearization decoupling and predictive driving control for dual independent electric drive tracked vehicle [J]. Acta Armamentarii , 2021 , 42 ( 4 ): 697 - 705 . Feedback linearization decoupling and predictive driving control for dual independent electric drive tracked vehicle DAI Yu , XUE Cong , SU Qiao , et al . Numerical analysis on hydrodynamic characteristics of a deep-sea mining vehicle under three typical motions [J]. Ocean Engineering , 2021 , 235 : 109446 . Numerical analysis on hydrodynamic characteristics of a deep-sea mining vehicle under three typical motions 陈昱衡 , 吴鸿云 , 边有钢 . 基于AUPF算法的水下履带车动力学参数估计 [J]. 湖南大学学报(自然科学版) . 2022 , 49 ( 8 ): 29 - 35 . 基于AUPF算法的水下履带车动力学参数估计 CHEN Yuheng , WU Hongyun , BIAN Yougang . Estimation of motion parameters of a underwater track mining vehicle based on adaptive unscented particle filter algorithm [J]. Journal of Hunan University(Natural Science) . 2022 , 49 ( 8 ): 29 - 35 . Estimation of motion parameters of a underwater track mining vehicle based on adaptive unscented particle filter algorithm 白国星 , 刘丽 , 孟宇 , 等 . 基于非线性模型预测控制的移动机器人实时路径跟踪 [J]. 农业机械学报 . 2020 , 51 ( 9 ): 47 - 52 . 基于非线性模型预测控制的移动机器人实时路径跟踪 BAI Guoxing , LIU Li , MENG Yu , et al . Real-time path tracking of mobile robot based on nonlinear model predictive control [J]. Transactions of the Chinese Society for Agricultural Machinery , 2020 , 51 ( 9 ): 47 - 52 . Real-time path tracking of mobile robot based on nonlinear model predictive control 陈昱衡 , 吴鸿云 , 边有钢 . 基于滑模预测控制的海底采矿车轨迹跟踪算法 [J]. 信息与控制 , 2022 , 51 ( 1 ): 119 - 128 . 基于滑模预测控制的海底采矿车轨迹跟踪算法 CHEN Yuheng , WU Hongyun , BIAN Yougang . A sliding mode predictive control-based trajectory tracking algorithm for a seabed mining vehicle [J]. Information and Control , 2022 , 51 ( 1 ): 119 - 128 . A sliding mode predictive control-based trajectory tracking algorithm for a seabed mining vehicle 孙浩 , 杜煜 , 卜德旭 , 等 . 汽车轨迹跟踪模型预测控制的加速求解方法 [J]. 湖南大学学报(自然科学版) . 2020 , 47 ( 10 ): 19 - 25 . 汽车轨迹跟踪模型预测控制的加速求解方法 SUN Hao , DU Yu , BU Dexu , et al . Accelerated solution method for vehicle trajectory tracking based on model predictive control [J]. Journal of Hunan University(Natural Sciences) , 2020 , 47 ( 10 ): 19 - 25 . Accelerated solution method for vehicle trajectory tracking based on model predictive control DAI Yu , SU Qiao , ZHANG Yanyang . A new dynamic model and trajectory tracking control strategy for deep ocean mining vehicle [J]. Ocean Engineering , 2020 , 216 : 108162 . A new dynamic model and trajectory tracking control strategy for deep ocean mining vehicle DAI Yu , XUE Cong , SU Qiao . An integrated dynamic model and optimized fuzzy controller for path tracking of deep-sea mining vehicle [J]. Journal of Marine Science and Engineering , 2021 , 9 ( 3 ): 249 . An integrated dynamic model and optimized fuzzy controller for path tracking of deep-sea mining vehicle 熊光明 , 鲁浩 , 郭孔辉 , 等 . 基于滑动参数实时估计的履带车辆运行轨迹预测方法研究 [J]. 兵工学报 . 2017 , 38 ( 3 ): 600 - 607 . 基于滑动参数实时估计的履带车辆运行轨迹预测方法研究 XIONG Guangming , LU Hao , GUO Konghui , et al . Research on trajectory prediction of tracked vehicles based on real time slip estimation [J]. Acta Armamentarii , 2017 , 38 ( 3 ): 600 - 607 . Research on trajectory prediction of tracked vehicles based on real time slip estimation QIN Zhaobo , CHEN Liang , FAN Jinging , 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 . An improved real-time slip model identification method for autonomous tracked vehicles using forward trajectory prediction compensation THRUN S , MONTEMERLO M , DAHLKAMP H , et al . Stanley: the robot that won the DARPA Grand Challenge [J]. Journal of Robotic Systems , 2006 , 23 ( 9 ): 661 - 692 . Stanley: the robot that won the DARPA Grand Challenge CUI Qingjia , DING Rongjun , ZHOU Bing , et al . Path-tracking of an autonomous vehicle via model predictive control and nonlinear filtering [J]. Proceedings of the Institution of Mechanical Engineers Part D: Journal of Automobile Engineering , 2018 , 232 ( 9 ): 1237 - 1252 . Path-tracking of an autonomous vehicle via model predictive control and nonlinear filtering 10.11817/j.issn.1672-7207.2023.04.012 TP249 A 1672-7207（2023）04-1336-08 陈昱衡, 边有钢, 崔庆佳, 等. 一种考虑ICR纵向偏差补偿的水下履带采矿车轨迹跟踪算法[J]. 中南大学学报(自然科学版), 2023, 54(4): 1336-1343. 陈昱衡, 边有钢, 崔庆佳, 等. 一种考虑ICR纵向偏差补偿的水下履带采矿车轨迹跟踪算法[J]. 中南大学学报(自然科学版), 2023, 54(4): 1336-1343. CHEN Yuheng, BIAN Yougang, CUI Qingjia, et al. A trajectory tracking algorithm for underwater tracked mining vehicles considering longitudinal deviation compensation[J]. Journal of Central South University(Science and Technology), 2023, 54(4): 1336-1343. CHEN Yuheng, BIAN Yougang, CUI Qingjia, et al. A trajectory tracking algorithm for underwater tracked mining vehicles considering longitudinal deviation compensation[J]. Journal of Central South University(Science and Technology), 2023, 54(4): 1336-1343. 国家重点研发计划(2021YFC2801700)；湖南省自然科学基金资助项目(2021JC0010, 2021JJ40086)；湖南省科技重大专项(2020GK1025)；湖南省科技创新计划资助项目(2020RC20260) Project(2021YFC2801700) supported by the National Key R&D Program of China; Projects(2021JC0010, 2021JJ40086) supported by the Natural Science Foundation of Hunan Province; Project(2020GK1025) supported by the Major Science and Technology Program of Hunan Province; Project(2020RC20260) supported by the Science and Technology Innovation Program of Hunan Province 2022-06-19 2022-08-21 2023-04-26 2023-05-22 37692584 中南大学学报(自然科学版) 4 54