一种无人矿卡货箱电控举升与自动驾驶的联合标定方法与流程

本发明涉及无人驾驶矿用卡车，具体地说，涉及一种无人矿卡货箱电控举升与自动驾驶的联合标定方法。

背景技术：

1、无人驾驶矿用卡车在露天矿山作业中，需要频繁执行装载、运输和卸载作业。卸载作业时，车辆停靠于卸料点，电控举升系统控制货箱举升至一定角度，使物料在重力作用下滑落。卸载完成后，货箱回落至初始位置，车辆驶离。

2、在这一作业流程中，电控举升系统与自动驾驶系统之间的协同工作精度直接影响卸载效果。现有技术存在以下不足：

3、现有标定技术多针对激光雷达、摄像头、惯性测量单元等感知传感器之间的外参标定，典型的如多激光雷达与毫米波雷达的联合标定、gnss增强的多传感器在线时空标定等。这些方案虽然能够提升感知系统自身的精度，但并未将电控举升系统纳入统一的坐标系管理框架。

4、在实际工程中，电控举升系统自身存在多种误差源：举升角度传感器存在零点偏移和增益误差，导致角度测量值与实际值之间存在偏差；货箱后铰点的实际安装位置与设计值存在偏差；举升旋转轴与车辆坐标系的轴线之间存在安装夹角。这些传感器误差和安装几何误差直接影响举升控制精度，进而导致卸载位置偏差、物料残留等问题。

5、然而，现有技术缺乏对上述举升机构物理参数的系统化标定方法。具体而言，现有方案既未将举升系统作为独立坐标系进行空间建模，也未利用自动驾驶系统已有的高精度感知传感器（如激光雷达）作为外部测量基准，来对举升系统的传感器误差参数和安装几何参数进行联合求解。这种“重感知轻执行”的现状，使得举升系统的控制精度受限于出厂时的装配精度和传感器初始校准状态，难以在车辆全生命周期内保持稳定。

技术实现思路

1、针对上述现有技术，本发明要解决的技术问题是：现有技术中缺乏对电控举升系统进行独立坐标系建模和物理参数系统化标定。

2、为解决上述问题，本发明提供了一种无人矿卡货箱电控举升与自动驾驶的联合标定方法，包括以下步骤：

3、s1：建立世界坐标系、车辆坐标系和举升坐标系，并完成三个坐标系之间的初始对齐；其中，举升坐标系以货箱后铰点中心为原点，轴与货箱底板平行且指向货箱前端，轴指向货箱左侧，轴垂直于货箱底板向上；将货箱完全落下至锁止状态时，举升坐标系与车辆坐标系平行，初始转换矩阵为单位矩阵；

4、s2：对自动驾驶系统和电控举升系统进行时间同步，在预设标定条件下，同步采集自动驾驶系统的位姿感知数据和电控举升系统的动作状态数据；

5、s3：从后向激光雷达点云数据中提取货箱上的多个特征点，特征点在举升坐标系中的坐标通过货箱设计数据或离线测量预先获得；建立货箱上任一点在世界坐标系、车辆坐标系和举升坐标系之间的转换数学模型：

6、；

7、其中，为货箱上一点在世界坐标系中的坐标，为世界坐标系到车辆坐标系的转换矩阵，为举升坐标系到车辆坐标系的转换矩阵且为举升角度的函数，为点在举升坐标系中的坐标；

8、基于同步数据和特征点求解联合标定参数；联合标定参数包括：举升角度传感器的零点偏移量、增益系数、货箱后铰点在车辆坐标系中的坐标以及举升旋转轴与车辆轴的安装夹角；

9、s4：将求解得到的联合标定参数代入转换数学模型，对标定结果进行精度验证，若验证合格则保存并生效标定参数。

10、上述技术方案为本发明的基础方案。通过定义独立的举升坐标系并建立三坐标系统一转换模型，首次将举升机构物理参数（传感器零偏、增益、铰点坐标、安装夹角）作为系统化的联合标定对象，以自动驾驶系统的激光雷达点云数据为外部测量基准，采用非线性优化方法实现多参数的联合精确求解。标定后的参数直接校准了举升系统的控制精度，解决了现有技术中举升机构缺乏系统化几何标定的问题。

11、进一步地，步骤s1具体包括：

12、定义世界坐标系，原点为矿山全局基准点，轴指向东，轴指向北，轴垂直向上；

13、定义车辆坐标系，原点为车辆后轴中心，轴指向车辆正前方，轴指向车辆左侧，轴垂直向上；

14、利用自动驾驶系统的rtk-gnss和imu完成世界坐标系与车辆坐标系的初始转换，得到转换矩阵。

15、进一步地，步骤s2中，时间同步采用ptp精确时间协议，使两个系统的数据时间戳误差小于1ms；预设标定条件为：在平坦坚硬的水平标定场地，车辆空载，发动机怠速运行，液压系统油温达到40℃～60℃；同步采集的数据包括：自动驾驶系统输出的车辆位姿数据、imu加速度和角速度数据、后向激光雷达点云数据，以及电控举升系统输出的举升角度传感器数据、举升油缸无杆腔压力数据、液压泵转速数据。

16、进一步地，步骤s2的数据采集方案为：控制车辆依次停靠在至少3个不同的标定位置，每个位置记录自动驾驶系统输出的车辆初始位姿数据；在每个标定位置，控制货箱从0°到最大举升角度以0.5°/s的速度匀速举升，每0.1°触发一次数据采集；重复上述步骤至少3次，获得多组独立的同步标定数据集。多位置标定保证了空间激励的充分性，密采样保证了角度分辨率，多次重复保证了统计可靠性。

17、进一步地，步骤s3中，求解联合标定参数采用非线性优化算法，目标函数为：

18、；

19、其中，为后向激光雷达测量得到的货箱上特征点在世界坐标系中的坐标，特征点在举升坐标系中的坐标通过货箱设计数据或离线测量预先获得；为举升坐标系到车辆坐标系的转换矩阵；非线性优化算法包括列文伯格-马夸尔特算法。由于特征点坐标为已知量，激光雷达测量值为已知量，由rtk-gnss和imu获得，中含有的待优化参数（、、铰点坐标、安装夹角）可通过最小化上述目标函数唯一确定。

20、进一步地，步骤s4的精度验证具体包括：将求解得到的联合标定参数代入转换数学模型；控制车辆在标定场地进行至少10次随机卸载作业；测量每次作业的实际卸载位置与目标卸载位置的平面偏差和高度偏差，以及货箱举升角度的实际值与模型计算值的偏差；当所有偏差均满足预设的精度阈值时，判定标定合格；预设的精度阈值为：卸载位置平面偏差≤±5cm，卸载高度偏差≤±3cm，举升角度测量偏差≤±0.1°。平面偏差和高度偏差共同验证坐标转换的整体精度，角度偏差直接验证传感器校准效果，三个指标互为补充。

21、在上述基础标定方案完成之后，本发明进一步提供以下延伸优化方案：

22、动态耦合模型在线辨识与前馈补偿

23、基础标定方案解决了举升系统自身的几何参数标定问题，但在实际卸载作业中，货箱举升过程会引起显著的车体姿态变化——装载后的矿卡总质量可达数十吨甚至上百吨，重心的大幅转移导致底盘悬架压缩和车身俯仰/侧倾。若对此不加补偿，自动驾驶系统的定位数据将产生偏差。

24、为此，所述方法还包括动态耦合模型建立与前馈补偿步骤：在无人矿卡进行装载并执行举升作业过程中，同步记录货箱的实时举升角度、角速度、举升油缸压力以及自动驾驶系统的组合惯导输出的车体俯仰角和/或侧倾角；以货箱的实时举升角度和根据油缸压力估计的有效载荷质量为输入，以车体俯仰角和/或侧倾角为输出，采用递推最小二乘法或高斯过程回归在线辨识举升动作与车体姿态之间的动态耦合模型；将动态耦合模型存储为补偿表或补偿函数，在自动驾驶系统进行路径规划和感知定位时，对因举升引起的车体姿态变化进行前馈补偿。

25、区别于出厂预设的静态模型，在线辨识意味着模型能够随着车辆的实际运行持续更新，自适应机械磨损、载荷变化等时变因素，确保补偿效果在全生命周期内保持稳定。

26、底盘形变预补偿与实时安全校验

27、动态耦合模型不仅可用于定位补偿，还可进一步用于举升过程的安全控制。现有技术通常在举升前检测地面水平度或车身初始倾角，或在举升中监测姿态角是否超出安全阈值——这类方案属于被动式监测，只有当姿态角已经达到危险值时才触发保护动作。

28、为此，所述方法还包括底盘形变预补偿与实时安全校验步骤：在举升指令执行之前，自动驾驶系统根据标定获得的转换数学模型及当前货箱倾角，利用动态耦合模型预估举升过程中底盘悬架压缩量和/或车身俯仰角变化量，向底盘线控系统发送前馈补偿指令，用于预先调整车辆空气悬架的刚度和/或四轮扭矩分配；在货箱举升过程中，举升系统通过倾角传感器和行程传感器实时采集当前货箱的实际倾角与实际油缸行程，并以不大于50毫秒的周期反馈给自动驾驶系统；自动驾驶系统根据实际倾角、由油缸压力估计的当前有效载荷质量以及动态耦合模型计算预期车体姿态变化量，并将预期车体姿态变化量与经过低通滤波去噪后的惯性测量单元采集到的实际车体姿态变化量进行比对校验；若校验差值超出预设安全阈值，则自动驾驶系统向举升系统发送限速或迫降指令。

29、对imu实测数据施加低通滤波，可有效滤除路面颠簸和液压系统振动引入的高频噪声，避免因瞬态误差导致误触发限速或迫降。上述技术方案形成了“事前预补偿—事中高频校验—异常即时响应”的完整安全闭环。由于动态耦合模型同时考虑了举升角度和有效载荷质量两个核心变量，校验结果准确可靠，有效避免了重载时误报和特定工况下漏报的问题。

30、在线自标定与自适应补偿

31、基础标定方案在出厂或定期维护时执行，但在长期运行中，传感器漂移、机械磨损、环境温度变化等因素会导致标定参数逐渐退化。

32、为此，所述方法还包括在线自标定与自适应补偿步骤：当满足以下任一预设触发条件时，系统自动触发在线自标定：(1)车辆累计运行时间超过100小时；(2)车辆更换了举升角度传感器或液压系统核心部件；(3)环境温度相对于上次标定时的温度变化超过预设阈值；(4)标定精度验证结果超过预设的精度阈值；利用车辆正常卸载作业过程中采集的符合条件的数据，采用增量式卡尔曼滤波算法对标定参数进行迭代更新；当参数变化量小于预设阈值时，保存并生效更新后的标定参数。采用增量式卡尔曼滤波算法，计算量小，无需存储全部历史数据，适合在车载嵌入式计算平台上实时运行。

33、在车辆出厂前，分别在空载、半载、满载三种典型工况下进行标定，建立负载-标定参数映射数据库；作业过程中，通过举升油缸压力实时计算车辆载重，根据载重自动调用对应的标定参数和控制参数。

34、在举升角度传感器和液压系统中安装温度传感器，建立温度-传感器漂移模型，根据实时温度自动修正举升角度传感器的输出值；当环境温度低于-20℃或高于40℃时，无论是否满足上述触发条件，均强制将在线自标定的触发间隔缩短至24小时。上述温度变化触发的是单次即时标定（应对温度骤变），极端环境强制缩短的是周期性标定间隔（应对持续极端工况），两者并行不悖且后者优先级更高。

35、卸载作业全流程协同控制

36、基于上述标定结果，可进一步实现卸载作业的全流程协同控制。所述方法还包括以下至少一项卸载作业协同控制步骤：

37、(a)根据卸载点的三维地形数据和物料堆积情况，自动规划最优的车辆停靠位置、姿态以及货箱举升轨迹；通过部署于车尾的激光雷达对不同货箱倾角下的货箱进行点云扫描，拟合生成对应当前货箱倾角的货箱动态包络多边形；将货箱动态包络多边形转换至车辆底盘坐标系，并进一步映射至大地坐标系；将映射后的动态包络空间与高精地图进行碰撞检测，并根据检测结果判断是否允许执行举升动作。该方案利用标定获得的精确坐标转换关系，将货箱包络准确映射至大地坐标系，为举升前的安全决策提供可靠依据，有效解决了卸料刮碰问题。

38、(b)在举升过程中，根据实时举升角度动态调整车辆位置：当货箱举升角度小于第一角度阈值时，车辆保持静止；当货箱举升角度在第一角度阈值与第二角度阈值之间时，车辆以预设低速向前移动，移动距离与举升角度增量成正比，且车辆移动累计距离不超过预设最大值；当货箱举升角度大于等于第二角度阈值时，车辆停止移动，货箱继续举升至最大角度并保持预设时间；实时监测车辆的侧倾角和俯仰角，当侧倾角或俯仰角超过预设安全阈值时，立即停止举升并自动调整车辆姿态至安全范围。累计距离上限的设置为车辆移动提供了硬性熔断机制，杜绝因控制异常导致车辆无限前移的隐患。该方案通过举升-位移协同控制，解决了物料局部堆积问题，使物料沿卸料区域均匀铺展。

39、(c)在货箱下降过程中，当货箱下降至第一角度区间时，控制货箱以第一速度下降；当货箱下降至第二角度区间且识别到有物料残留时，控制货箱以第二速度下降，第二速度低于第一速度；当货箱下降至接近底盘位置时，控制货箱以第三速度下降，第三速度低于第二速度。该方案通过分段速度控制，在物料残留时降低下降速度以减少冲击，在接近底盘时进一步减速以确保平稳锁止，有效解决了重载液压冲击问题。

40、与现有技术相比，本发明具有以下有益效果：

41、1.首次将电控举升系统纳入独立的坐标系管理框架，以货箱后铰点中心为原点建立举升坐标系，通过三坐标系统一转换数学模型，实现了对举升角度传感器零偏、增益、铰点安装位置、旋转轴安装夹角等物理参数的精确标定，填补了现有技术中缺乏举升机构几何参数系统化标定方法的空白。

42、2.以激光雷达点云数据为外部测量基准，通过非线性优化联合求解多个标定参数，标定精度高、鲁棒性强，标定后卸载位置平面偏差可达±5cm以内，举升角度测量偏差可达±0.1°以内。

43、3.在基础标定之上，进一步设计的在线辨识举升动作与车体姿态的动态耦合模型，能够自适应车辆机械磨损、载荷变化等时变因素，用于自动驾驶定位的前馈补偿，解决了出厂预设模型随时间退化的问题，显著提升作业过程中定位精度的长期稳定性。

44、4.构建了基于“模型计算车体姿态变化量 vs imu实测车体姿态变化量”的高频比对校验机制，能够主动预测安全风险并在毫秒级周期内提前干预，相比现有技术中被动式阈值监测，安全性显著提升。

45、5.设计了多种触发条件的在线自标定机制，结合负载自适应调用和极端环境强制缩短标定间隔等策略，确保了标定参数在全生命周期和各类恶劣工况下的持续有效性。

46、6.将标定结果延伸应用于卸载作业的全流程协同控制，形成了从标定到控制的完整技术闭环。