[0001] 本发明涉及定位技术领域，尤其涉及一种基于RTK和IMU的无人驾驶装载机的高精度定位方法。

[0002]  高精度的定位技术是实现无人驾驶工程车辆的关键技术之一，常用的定位技术包括GNSS卫星定位、无线通信定位、IMU惯性测量单元定位以及RTK实时动态差分定位等，其中，实时动态差分定位(Real Time Kinematic, RTK)技术具有定位精度高、成本低、覆盖范围较广、实时性强的特点，也是目前使用最广泛的高精度定位方法。

[0003] 在实际应用中，实时动态差分(RTK)定位技术对工作环境要求较高，其定位精度和稳定性会受到卫星信号质量、通信数据链、大气条件等若干复杂因素的影响，环境适应性较差，难以应对复杂的工况环境。同时，一般RTK定位系统的数据输出频率最大仅有20Hz，在无人驾驶装载机进行转弯或在崎岖地面上运动时，无法快速准确地更新车辆位姿和速度信息，进而对无人驾驶控制系统的性能和稳定性造成负面影响。

[0004] 有资料显示，目前的RTK与IMU组合定位方法大多将定位模型平面化，在此基础构建高维状态方程，并进行数据融合与滤波，以实现优于原有的单一定位方案的二维定位效果。但在装载机的工作环境下，可能存在着起伏复杂的地面状况，进而要求了适用的定位方法不能简单地将场地环境平面化，导致已有的定位方法不能完全满足装载机工作环境下的定位需求。同时，已有的RTK与IMU组合定位方法大多采用高维状态方程来处理，在误差测量和修正方式上较为复杂，导致难以实现快速应用。

[0005] 中国专利文献CN116878503A公开了一种“基于GPS和IMU姿态传递匹配的改进IMU‑RTK松组合导航方法及系统”。采用了在传统的IMU‑RTK松组合基础上进行改进，通过将IMU坐标系与载体坐标系之间的安装误差角加入状态向量中进行滤波估计，在不明显提高传统松组合算法基础上获得了准确的IMU安装误差角估计结果，进而提高组合导航系统的定位精度。上述技术方案将场地环境平面化，缺少考虑起伏复杂的地面状况。

[0031] 为使本发明的目的、技术方案以及优点更加清楚明白，下面通过实施例，并结合附图，对本发明的技术方案作进一步详细说明，应当理解的是，此处所描述的具体实施方式仅是本发明的一种最佳实施例，仅用以解释本发明，并不限定本发明的保护范围，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

[0032] 在更加详细地讨论示例性实施例之前，应当提到的是，一些示例性实施例被描述成作为流程图描绘的处理或方法。虽然流程图将各项操作(或步骤)描述成顺序的处理，但是其中的许多操作(或步骤)可以被并行地、并发地或者同时实施。此外，各项操作的顺序可以被重新安排。当其操作完成时所述处理可以被终止，但是还可以具有未包括在附图中的附加步骤；所述处理可以对应于方法、函数、规程、子例程、子程序等等。

[0033] 将RTK定位技术与IMU定位技术进行结合可以改善如上问题带来的影响，研究者们围绕如上解决方向进行了许多研究，如王德俊于2019年SAECCE中国汽车工程学会年会暨展览会上提出了一种“基于车载IMU与差分RTK系统组合定位技术的研究与实现”，在该文献资料中提出了一种IMU/GPS/轮速计的组合定位系统，该系统采用直接法滤波，构建了8维的状态方程来实现对车辆经纬度、航向角和东北向速度的精确测量与定位。专利号CN116878503A也提出了一种基于GPS和IMU姿态传递匹配的改进IMU‑RTK松组合导航方法，该专利在IMU‑RTK松组合基础模型的基础上进行改进，将惯性导航系统坐标系与车辆载体坐标系之间的安装误差角加入状态向量中，形成18维的卡尔曼滤波状态向量，并同时考虑了安装误差角和解算误差角来估计姿态结果与位置结果。

[0034] 下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明。

[0035] 实施例：本实施例的一种基于RTK和IMU的无人驾驶装载机的高精度定位方法，如图2所示，包括以下步骤：S1初始化组合定位的状态量并获取初始状态量。

[0036] 状态量初始化包括:S1.1由RTK定位系统获取装载机初始位置，具体过程表示为:

[0037] 式中，Pos表示装载机车辆所处的位置；X、Y、Z分别表示RTK定位系统解算出的装载机车辆三维位置坐标。

[0038]  S1.2由装载机车辆搭载的两个RTK定位移动站经差分后，得到装载机车辆的偏航角姿态，记为YawRTK。

[0039] 同时，由IMU惯性测量单元获取装载机初始姿态，具体过程表示为:

[0040] 式中，Euler表示装载机车辆的当前姿态欧拉角；Roll、Pitch、Yaw分别表示IMU惯性测量单元解算的装载机车辆横滚角、俯仰角及航向角。

[0041]  S2进入动态定位状态，采集RTK实时动态差分定位数据和IMU数据。采集RTK实时动态差分定位数据和IMU数据并得到车辆动态位置、姿态等信息，具体过程包括：连续采集RTK定位数据和IMU惯性测量单元的数据，并保存当前时刻的采样数据和上一时刻的采样数据。在一次采样中应包括的数据有：RTK定位系统得到的三维位置坐标数据、车载双RTK移动站差分后得到的航向角数据、IMU惯性测量单元解算得到的姿态数据。

[0042]  S3根据当前时刻与上一时刻的状态观测量，推算装载机车辆运动变化信息。推算装载机车辆速度、姿态变化角速度等信息，具体过程表示为:

[0043]

[0044] 式中, 、 分别表示当前采样时刻的速度和姿态变化角速度信息，分别表示x、y、z三个轴向的速度估计值， 分别表示姿态的欧拉
角角速度估计值； 分别表示当前采样时刻的三维位置坐标和上一采样时刻的三维位置坐标； 分别表示当前采样时刻的车辆姿态角度和上一采样时刻
的车辆姿态欧拉角数据，姿态数据由IMU惯性测量单元解算获得。

[0045]  S4将状态量重组，构建各自的位置和速度组合的二维卡尔曼滤波器以及各自的角度和角速度组合的二维卡尔曼滤波器，预测装载机车辆的当前时刻状态量，并根据当前预测量与实际观测量的偏差，对组合定位滤波参数进行更新，对实际观测量进行滤波修正。

[0046] 具体过程包括：S4.1:将状态量重组。对于前述步骤获得的位置和速度状态量，以三维坐标轴x、y、z为基准，分别将各自坐标轴的位置坐标和速度合成一个二维向量，具体表现形式如：

[0047]

[0048]

[0049] 对于前述步骤获得的横滚角及横滚角角速度、俯仰角及俯仰角角速度，各自组成的二维向量形式如下：

[0050]

[0051] 对于前述步骤中，由RTK定位得到的航向角，及由IMU测量值估计得到的航向角角速速度，两者组合成一个二维向量，表现形式如：

[0052] S4.2:针对上一步骤获得的二维向量，将A、B、C分为一类，构建各自的位置和速度组合的二维卡尔曼滤波器；将D、E、F分为一类，构建各自的角度和角速度组合的二维卡尔曼滤波器。

[0053]  S4.3:使用二维卡尔曼滤波器对装载机车辆的三维位置和姿态状态进行预测，并根据当前预测量与实际观测量的偏差，对实际观测量进行滤波修正。

[0054]  S5修正观测到的装载机车辆位置和姿态后，输出定位结果。输出定位结果，包括三维位置坐标X、Y、Z和姿态欧拉角Roll、Pitch、Yaw。

[0055] 实施例1在本实施案例中，使用的RTK差分定位参考基站的型号为千寻ZED‑F9P RTK GNSS基站模块，使用的两个车载RTK差分定位移动站型号为千寻ZED‑F9P RTK GNSS移动站模块，且车载的两个RTK移动站在装载机车体上呈一前一后的分布，以支持双RTK移动站差分获取车辆航向角数据。由如上型号的基站和移动站构成的RTK定位系统支持多频段GNSS卫星定位，定位数据的最大输出刷新速率为20Hz，静止时的水平定位精度可达0.01M+1PPM，对速度的测量精度误差在0.05M/s以内。

[0056] 本实施案例中使用的车载IMU模块型号为北微BW‑IMU55，它是一款低成本的车载六轴IMU惯性测量单元，它的陀螺仪角速度分辨率可达0.01°每秒，加速度计分辨率可达0.01mg，具备一般车载级别IMU的测量精度和零偏稳定性，最大数据输出速率为100Hz。

[0057] 本实施案例基于一种装载机，其中车载的两个RTK移动站安装位置在水平距离上间距为3.8米，垂直距离上间距为0.8米，分别安装在驾驶室顶端和车位发动机上方，且与车辆头尾方向处于同一直线上。作业环境包括露天散货堆积区域、土方开挖施工场地及气模棚内煤料堆积区域等，均满足GNSS卫星定位信号良好的条件，且RTK基站均设立在空旷室外地面或建筑顶部等开阔空间处。

[0058]  采集和推算步骤完成后，结合本实施案例中的装载机车辆情况对获取的初始姿态信息进行判断。若装载机车辆位于良好平整地面上启动，则横滚角Roll数值应处于[‑1.5°, 1.5°]范围内，俯仰角Pitch数值应处于[‑3.0°, 3.0°]范围内，否则应对IMU传感器和RTK定位系统的工作状态进行检查；若装载机车辆位于作业区内启动时，考虑到作业环境的地形环境，横滚角Roll数值应处于[‑5.0°, 5.0°]范围内，俯仰角Pitch数值应处于[‑
11.0°, 11.0°]范围内，若超出此范围，则检查装载机车辆状态和定位传感器工作状态是否正常。确保车辆姿态处于正常范围内，且初始姿态数据检查通过后，继续进行步骤S2。

[0059] 在步骤S2中，采集RTK实时动态差分定位数据和IMU数据并得到车辆动态位置、姿态等信息，过程包括：连续采集RTK定位数据和IMU惯性测量单元的数据，保存当前时刻的采样数据和上一时刻的采样数据，具体过程表示为：

[0060]

[0061]

[0062]

[0063] 式中， 分别表示当前采样时刻的三维位置坐标和上一采样时刻的三维位置坐标采样值； 分别表示当前采样时刻的车辆姿态角度和上一采样时刻的车辆姿态角度采样值。

[0064] 结合附图2，在所述步骤S3中，通过对前述步骤获取的IMU数据和RTK定位数据进行差分，进而推算出装载机车辆的速度和姿态变化角速度信息，具体过程表示为：

[0065]

[0066] 式中, 、 分别表示当前采样时刻的速度和姿态变化角速度信息，分别表示x、y、z三个轴向的速度估计值， 分别表示姿态的欧拉
角角速度估计值； 分别表示当前采样时刻的三维位置坐标和上一采样时刻的三维位置坐标采样值， 分别表示当前采样时刻的车辆姿态角度和上一采
样时刻的车辆姿态角度采样值。

[0067] 所述步骤S4中，对状态量进行卡尔曼滤波并输出定位结果，具体过程包括：S4.1:将状态量重新提取并组合。结合附图2，对于前述步骤获得的位置和速度状态量，以三维坐标轴x、y、z为基准，分别将各自坐标轴的位置坐标和速度合成一个二维向量，将状态量重组。对于前述步骤获得的位置和速度状态量，以三维坐标轴x、y、z为基准，分别将各自坐标轴的位置坐标和速度合成一个二维向量，具体表现形式如：

[0068]

[0069]

[0070] 结合附图2，对于前述步骤获得的横滚角及横滚角角速度、俯仰角及俯仰角角速度，各自组成的二维向量形式如下：

[0071]

[0072] 结合附图2，航向角Yaw的当前角度数据由车载RTK移动站间差分取得，而对应航向角的角速度数据，则由两次采样间IMU解算出的航向角角度数据进行差分估计后取得，最后将当前航向角度数据和估计出的航向角角速度数据组合成一个二维向量，表现形式如：

[0073] 式中， 为RTK定位系统差分获取的航向角角度数据； 为两次采样间IMU解算的航向角的变化量，用以近似估计航向角角速度。

[0074]  S4.2:针对上一步骤获得的二维向量，将向量A、B、C分为一类，构建各自的位置和速度组合的二维卡尔曼滤波器；将向量D、E、F分为一类，构建各自的角度和角速度组合的二维卡尔曼滤波器。

[0075]  S4.3:使用二维卡尔曼滤波器对装载机车辆的三维位置和姿态数据进行预测，并根据预测量与实际观测量的偏差，对观测量进行滤波修正后，输出定位结果，包括三维位置坐标X、Y、Z和姿态欧拉角Roll、Pitch、Yaw。

[0076] 在GNSS卫星定位信号良好的情况，按照本实施案例对装载机在室外土质地面环境中进行定位测试，与单一RTK定位系统进行三维位置坐标和航向角估计值的对比，实车定位结果表明本发明的定位精度和响应速度均明显优于单一RTK定位系统的效果。在实车静止时的水平定位误差可控制在2厘米以内，高度定位误差可控制在4cm以内，航向角误差控制在2°以内；实车作业运动时，水平定位误差控制在5厘米以内，高度定位误差控制在10cm以内，航向角误差在约5°以内。

[0077]  本发明提供的实施案例可以应用在无人驾驶装载机的作业场地定位，通过对场地原始定位数据的采集和评估，来调整卡尔曼滤波器进行数据融合的参数，以适应不同作业场地的环境特点并实现高精度的实时定位。

[0078] 实施例2本实施案例基于一种推煤机，车载的两个RTK移动站安装位置在水平距离上间距为3.2米，垂直高度差为0.15米，分别安装在车体前后两端，且位于车头方向所在直线上。推煤机工作环境为气模棚内煤料堆积区，作业时使用铲斗对煤堆进行推整，并使用履带来适应煤堆地形上的移动作业，作业环境满足GNSS卫星定位信号良好的条件，且RTK基站均设立在空旷室外地面或建筑顶部等开阔空间处。

[0079]  本实施案例中使用的RTK差分定位参考基站的型号为千寻ZED‑F9P RTK GNSS基站模块，使用的两个车载RTK差分定位移动站型号为千寻ZED‑F9P RTK GNSS移动站模块，且车载的两个RTK移动站在推煤机车体上呈一前一后的分布，以支持双RTK移动站差分获取车辆航向角数据。由如上型号的基站和移动站构成的RTK定位系统支持多频段GNSS卫星定位，定位数据的最大输出刷新速率为20Hz，静止时的水平定位精度可达0.01M+1PPM，对速度的测量精度误差在0.05M/s以内。推煤机车载IMU模块型号为北微BW‑IMU55，它是一款低成本的车载六轴IMU惯性测量单元，它的陀螺仪角速度分辨率可达0.01°每秒，加速度计分辨率可达0.01mg，具备一般车载级别IMU的测量精度和零偏稳定性，最大数据输出速率为100Hz。

[0080]  在GNSS卫星定位信号良好的情况下，按照本实施案例对推煤机在气模棚内煤料堆作业环境中进行定位测试。由于推煤机作业过程和路径的特点所致，车辆会经常在煤堆散料区域和平整地面间来回运动，车辆的俯仰角和横滚角姿态变化较为频繁，而航向角姿态往往变化缓慢，且车辆运动往往处于低速状态，可据此调整二维卡尔曼滤波器的参数，适当提高对位置预测值和航向角姿态预测值的置信度，并降低对相应观测值的置信度，以适应推煤机的作业特点，降低定位和姿态误差。在实际作业测试中，水平定位误差可控制在2厘米以内，高度定位误差可控制在5cm以内，航向角误差控制在约1.5°内。测试结果表明，本发明提供的组合定位方法明显优于单一的RTK定位系统或IMU定位系统，在有效提升定位数据输出频率和平滑度的同时，显著缩减了定位误差并提升了响应速度。

[0081] 实施例3本实施案例基于一种挖掘机，车载的两个RTK移动站安装位置在水平距离上间距为3.2米，垂直距离上间距为1米，分别安装在驾驶室顶部和后部配置上方，两移动站间朝向与车辆动臂方向存在一定夹角。挖掘机工作环境为气模棚内煤料堆积区，作业时使用挖斗对煤堆进行爬取和堆放作业，并使用履带来适应煤堆地形上的移动作业，作业环境满足GNSS卫星定位信号良好的条件，且RTK基站均设立在空旷室外地面或建筑顶部等开阔空间处。

[0082]  本实施案例中使用的RTK差分定位参考基站的型号为千寻ZED‑F9P RTK GNSS基站模块，使用的两个车载RTK差分定位移动站型号为千寻ZED‑F9P RTK GNSS移动站模块，以支持双RTK移动站差分和修正后获取车辆航向角数据。由如上型号的基站和移动站构成的RTK定位系统支持多频段GNSS卫星定位，定位数据的最大输出刷新速率为20Hz，静止时的水平定位精度可达0.01M+1PPM，对速度的测量精度误差在0.05M/s以内。挖掘机车载IMU模块型号为N100，它是一款车载九轴IMU惯性测量单元，静态角度分辨率可达0.05°RMS，动态角度分辨率可达0.1°RMS，加速度计分辨率小于0.5mg，陀螺仪零偏稳定性为5°/hr，完全满足车载IMU的规格精度，最大数据输出速率为400Hz。

[0083] 本实施案例基于一种挖掘机，车载的两个RTK移动站安装位置在水平距离上间距为3.2米，垂直距离上间距为1米，分别安装在驾驶室顶部和后部配置上方，两移动站间朝向与车辆动臂方向存在一定夹角。挖掘机工作环境为气模棚内煤料堆积区，作业时使用挖斗对煤堆进行爬取和堆放作业，并使用履带来适应煤堆地形上的移动作业，作业环境满足GNSS卫星定位信号良好的条件，且RTK基站均设立在空旷室外地面或建筑顶部等开阔空间处。

[0084]  本实施案例中使用的RTK差分定位参考基站的型号为千寻ZED‑F9P RTK GNSS基站模块，使用的两个车载RTK差分定位移动站型号为千寻ZED‑F9P RTK GNSS移动站模块，以支持双RTK移动站差分和修正后获取车辆航向角数据。由如上型号的基站和移动站构成的RTK定位系统支持多频段GNSS卫星定位，定位数据的最大输出刷新速率为20Hz，静止时的水平定位精度可达0.01M+1PPM，对速度的测量精度误差在0.05M/s以内。挖掘机车载IMU模块型号为N100，它是一款车载九轴IMU惯性测量单元，静态角度分辨率可达0.05°RMS，动态角度分辨率可达0.1°RMS，加速度计分辨率小于0.5mg，陀螺仪零偏稳定性为5°/hr，完全满足车载IMU的规格精度，最大数据输出速率为400Hz。

[0085]  本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器(ROM，Read Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁盘或光盘等。

[0086] 本文中所描述的具体实施例仅仅是对本发明精神作举例说明。以上实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出多个变形和改进。因此，本发明专利的保护范围应以所附权利要求为准。