CN116360253A - 一种港内智能转运车自适应箱重控制方法 - Google Patents

Abstract

本发明公开一种港内智能转运车自适应箱重控制方法，包括自适应箱重功能交互流程的建立和验证，主要为：结合港区任务运行与智驾车辆的状态，基于Stateflow有限状态机建立自适应箱重功能交互流程；交互流程中，建立智驾车辆动力学模型，并基于扩展卡尔曼滤波进行整车质量的估计；进而搭建自适应箱重控制方法流程的仿真模型，基于实际运营数据在仿真模型中验证，该方法的交互流程能够快速、高效地适应装卸箱作业场景；对外部调度系统的依赖仅限于需要获取集装箱状态是否发生变化，降低整体系统的复杂程度；且将集装箱质量问题的处理集中在智驾系统控制模块中，增强智驾系统的功能完备性和整体性，提升测试问题排查与运营作业的效率。

Description

一种港内智能转运车自适应箱重控制方法

技术领域

本发明属于智能驾驶技术领域，具体涉及到一种港内智能转运车自适应箱重控制方法。

背景技术

现有的港内集装箱智能转运车辆在装载不同质量的集装箱后，自动驾驶系统为保证其仍具有稳定的行驶状态，通常采用两种方案：一种是借助港区自身在集装箱吊具四角上安装的称重传感器，将质量信息传输至其任务调度系统，自动驾驶系统再通过与港区内任务调度系统建立通信机制，获取任务调度系统下发的装载集装箱的质量信息，并传输至整车自动驾驶控制模块，完成对车辆的行驶状态控制；另一种是在集装箱智能转运车辆上安装箱重质量传感器，由箱重质量传感器实时采集车辆装载的集装箱质量，并通过控制器局域网络接入整车通信总线，从而将集装箱质量信息传递至整车自动驾驶控制模块。

集装箱智能转运车辆的自动驾驶系统中，负责与港区内任务调度系统建立通信的模块为任务管理模块，任务管理模块与上游的港区调度系统在通信上面主要进行超文本传输协议通信，即作为服务端接收来自任务调度系统的命令下发，同时也作为客户端发送命令、反馈数据给任务调度系统。

在自动驾驶系统中，任务管理模块与地图定位、规划控制等其他智驾模块的通信方式为机器人操作系统的通信机制。任务管理模块通过话题的订阅与发布形式，与其他智驾模块之间建立数据的传递与交互关系；在进行装卸箱任务中，当智能转运车辆行驶到位并完成装卸箱任务后，上游调度系统会下发所装载集装箱质量的HTTP数据信息，任务管理模块接收到HTTP数据后进行解析，再以话题形式直接发布于下游规划控制模块，保证车辆的纵向行驶状态。

然而，上述这种通过与港区调度系统进行交互获取装载集装箱质量信息的方式虽然开发量少，但也存在不足之处，包括：

(1)需要港区内吊具安装质量传感器，或通过其他方式获取到装吊的集装箱质量信息，加大了港区的运营成本，同时依赖调度系统下发质量信息的可靠性、准确性。港区任务调度系统下发的质量值是岸桥吊完成抓放箱后的测量值，如果这个测量值比较粗略，存在不确定的误差，误差可能过大或过小，调度系统不能保证测量质量值的准确可靠，也会对智驾系统造成影响。

(2)易受到通信链路稳定性影响。该方案结合了港区任务调度系统、车辆任务管理模块、规划控制模块之间的实时通信，整体链路较长，中间环节易受影响。如果其中某一模块的通信质量收到影响，集装箱质量信息无法正常传递至后续模块，从而会造成行驶状态的不稳定。

(3)质量信息只能表示单箱质量状态。智能转运车在港区内与港口业务领域深度融合，多任务实时切换，可以转运的箱型包括20尺、40尺、45尺箱。调度系统下发的质量信息只能表示当前任务结束后放置在转运车辆上的集装箱质量，若在不同箱区贝位装吊双20尺的集装箱时，就无法直接使用该信息表示装载在车辆上的集装箱总质量。

(4)测试问题的排查效率低。当测试任务中出现与集装箱质量有关的问题时，需要调动相关模块共同排查，涉及港区调度系统、任务管理模块，规划控制模块、通信链路等，问题排查效率低，占用人员时间成本高。

(5)多车多任务时造成调度系统冗余。港区内智能转运车辆按照任务运行，不同的任务执行不同的行驶状态。所有智能转运车辆由调度系统派发任务，调度系统需要对每一台车辆进行配置并派发质量信息，当参与任务的车辆数量足够多时，会使得调度系统复杂冗余，缺少灵活性。

另外一种在智能转运车辆上安装箱重质量传感器，实时监测装载集装箱质量并传输至智能转运车智驾系统，与从上游任务调度系统获取质量信息相比，可以弥补装载双箱时的质量信息问题，交互链路也缩短为传感器直接接入整车智驾通信网络，不与上游任务调度系统进行交互，通信受到其他因素影响的风险降低，也降低了任务调度系统的冗余程度和排查测试问题的难度。

但加装集装箱质量传感器会不可避免地提升车辆的开发成本。当智能转运车在全国各个港区大批量地生产和参与运营，对于集装箱质量传感器费用投入是一笔不可忽视的开销。以德国HBM柱式传感器C16IC3/60t数字称重传感器为例，该传感器适用于轨道衡、汽车衡和工业集装箱的水平测量，市场单价在4400元/个。传感器的内部电路板和其他零件的更换及相关操作需要由专业工程师或技术人员进行，需要专业人员进行定期维修保养，在部分组件中包含敏感部件，可能会被静电损坏，后期也需要投入人员成本。

在集装箱吊具安装称重传感器与任务调度系统通信和在智能转运车辆上安装集装箱质量传感器并接入智驾通信网络的两种方案，在基本满足目标需求的同时都有较为明显的缺点与不足。

发明内容

针对上述问题，本发明的主要目的在于设计一种港内智能转运车自适应箱重控制方法,解决现有技术中获取集装箱装载直流时质量参数以及整体通信链路的不稳定性、易造成系统冗余、问题排查效率低，或者在智能转运车辆使用质量传感器带来的高昂的设备成本和人员成本投入的技术问题。

为了实现上述目的本发明采用如下技术方案：

一种港内智能转运车自适应箱重控制方法，该方法包括自适应箱重功能交互流程的建立和验证，具体步骤如下：

步骤1：结合港区任务运行与智驾车辆的状态，基于Stateflow有限状态机建立自适应箱重功能交互流程；

步骤2：在交互流程中，建立智驾车辆动力学模型，并基于扩展卡尔曼滤波进行整车质量的估计；

步骤3：通过智驾车辆动力学模型的建立与基于扩展卡尔曼滤波的整车质量估计方法，搭建自适应箱重控制方法流程的仿真模型，并基于智驾车辆实际运营数据回灌在仿真模型中进行验证。

作为本发明的进一步描述，所述智驾车辆的自动驾驶系统包括控制模块、任务管理模块、规划模块；

步骤1中，自适应箱重功能的有限状态机，为智驾车辆在装箱状态变化的任务中，控制模块与其他智驾车辆自动驾驶系统各模块之间的交互流程。

作为本发明的进一步描述，具体的自适应箱重功能交互流程为，智驾车辆的控制模块在上游任务管理模块下发的任务、规划模块下发的动作指令下行驶运作，智驾车辆到达下一次的装卸箱贝位点，反馈到上游规划模块完成下发的动作指令，规划模块向任务管理模块反馈任务结束，即完成一次装卸箱任务。

作为本发明的进一步描述，具体的自适应箱重功能交互流程，包括如下步骤：

S1：任务管理模块在完成智驾车辆装箱或卸箱任务后，告知下游智驾车辆的控制模块当前装箱状态是否发生变化；

若没有变化，则存储和使用上一次的箱重信息值，若有变化，则在S2中重置为初始状态；

S2：依据任务管理模块，判断智驾车辆上是否存在集装箱；

若无集装箱，则进入计算参数处理状态，若有集装箱，则进入准备驶离的等待状态；

S3：在智驾车辆准备驶离等待状态中，等待接收规划模块下发的起步请求指令；

当规划模块下发了起步请求指令时，则进入智驾车辆驱动扭矩输出状态，输出驱动扭矩到纵向控制逻辑，并驱动智驾车辆起步行驶；

S4：车辆起步后，进入完成起步状态，并在此状态中对智驾车辆的整车质量进行估计计算，并进行质量参数的处理；

S5：将处理后的质量参数进行存储和输出到纵向控制逻辑，控制智驾车辆按照质量参数计算输出扭矩，完成车辆的纵向控制。

作为本发明的进一步描述，步骤2中，智驾车辆动力学模型包括智驾车辆的纵向动力学模型，在智驾车辆的行驶过程中，纵向动力学模型的建立为：

F_t＝F_f+F_w+F_i+F_j

其中，F_f智驾车辆行驶的滚动阻力，F_w为智驾车辆行驶迎风面阻力，F_i为智驾车辆沿坡面所受到的坡度阻力，F_j为智驾车辆加速时虚拟化出的一个加速阻力，F_t为地面对智驾车辆的驱动力。

作为本发明的进一步描述，纵向动力学模型建立过程中，各分力的表示公式如下：

地面对智驾车辆的驱动力：

其中，T_t为车辆输出的总驱动扭矩，i_o、i_g为减速机构的传动比，η为动力系统的机械传递效率，r为车辆轮胎行驶半径；

智驾车辆行驶的滚动阻力：

F_f＝fGcosα

其中，f为车辆滚动系数，G为整车重量，α为道路坡度角；

智驾车辆行驶迎风面阻力：

其中，C_D为迎风阻力系数，A为迎风面积，v为实际车速，ρ为空气密度；

智驾车辆沿坡面所受到的坡度阻力：

F_i＝Gsinα

其中，G为整车重量，α为道路坡度角；

智驾车辆受到的加速阻力：

其中，δ为旋转质量换算系数，m为整车质量；

则智驾车辆纵向动力学模型的建立为：

作为本发明的进一步描述，扩展卡尔曼滤波通过将非线性系统在其在当前时刻附近进行泰勒展开，去除高阶项，得到非线性系统的近似线性关系，根据线性关系通过卡尔曼滤波进行状态空间的估计；

卡尔曼滤波对上一个时刻系统状态的估计值和当前时刻实际测量的观测值，对线性系统当前时刻的状态值进行滤波估计，具体包括预测与分析两个阶段：

预测阶段，依据上一时刻系统状态值对下一时刻的系统状态进行预估；

分析阶段，基于实际测量到的观测值，利用最小方差估计方法对系统当前状态进行矫正分析，得出当前时刻状态空间参数的估计值。

作为本发明的进一步描述，基于扩展卡尔曼滤波整车质量的估计方式为，根据智驾车辆的纵向动力学模型，建立系统的状态空间模型，状态空间方程的线性动态系统：

式中，k为离散时间；X(k)为系统在k时刻的状态；Y(k)为系统对应状态的观测信号；U(k)为系统的输入控制信号；W(k)为输入信号的白噪声；V(k)为观测信号的噪声；A为状态转移矩阵；B为输入控制矩阵；T为噪声驱动矩阵；C为观测矩阵；D为输出控制矩阵；

系统的状态变量关系为：

X＝[v,m,i]

其中，v为车速，m为整车质量，i为道路坡度，均为状态变量。

作为本发明的进一步描述，结合智驾车辆的纵向动力学模型，完成对系统状态空间参数的估计，具体如下：

在智驾车辆装卸箱后起步时，连续系统方程为：

其中，g为重力加速度；

采用前向差分法将连续系统方程进行离散化处理，离散时间状态的空间方程为：

在空间方程中输入信号白噪声和观测信号的噪声，以及对应方差分别为Q和R的不相关高斯白噪声，则系统的状态方程为：

系统的观测量方程为：

则整理后的系统的状态空间方程为：

其中，W为信号白噪声，V为观测信号的噪声，且W和V均值为0，初始状态值与W、V不相关；

基于状态空间方程，进行状态更新预测过程，更新方程为：

其中，

为上一时刻的状态变量最优估计值；

为状态变量的先验估计值；

P(k|k-1)为先验误差协方差；

P(k-1|k-1)为上一时刻的误差协方差；

过程方程向量函数对状态变量求偏导数得到的雅可比矩阵；

此处：

观测更新方程为：

其中，K(k)即为卡尔曼增益；

为状态变量的后验估计值；P(k|k)为误差后验协方差。

作为本发明的进一步描述，步骤3中，通过智驾车辆动力学模型的建立与基于扩展卡尔曼滤波的整车质量估计方法，并借助Stateflow搭建自适应箱重控制流程的状态机，即仿真模型；

基于智驾车辆装载不同质量的集装箱，通过车载的数据监控采集系统采集CAN数据，回灌到搭建的仿真模型中，对自适应箱重控制方法进行离线验证。

相对于现有技术，本发明的技术效果为：

本发明提供了一种港内智能转运车自适应箱重控制方法，在智驾车辆装卸箱时，基于MATLAB/Simulink/Stateflow的自适应箱重状态交互流程能够快速、高效地适应装卸箱作业场景；自适应箱重状态机交互流程中，对外部调度系统的依赖仅限于需要获取智驾车辆上的集装箱状态是否发生变化，降低了整体系统的复杂程度；另外，将集装箱质量问题的处理集中在智驾系统控制模块中，增强了智驾系统的功能完备性和整体性，同时也提升了测试问题排查与运营作业的效率。

附图说明

图1为本发明自适应箱重功能状态交互示意图；

图2为本发明中智驾车辆纵向动力学模型示意图；

图3为本发明的卡尔曼滤波计算过程示意图。

具体实施方式

下面结合附图对本发明进行详细描述：

一种港内智能转运车自适应箱重控制方法，参考图1-3所示，该方法包括自适应箱重功能交互流程的建立和验证，具体步骤如下：

步骤1：结合港区任务运行与智驾车辆的状态，基于Stateflow有限状态机建立自适应箱重功能交互流程；

步骤2：在交互流程中，建立智驾车辆动力学模型，并基于扩展卡尔曼滤波进行整车质量的估计；

步骤3：通过智驾车辆动力学模型的建立与基于扩展卡尔曼滤波的整车质量估计方法，搭建自适应箱重控制方法流程的仿真模型，并基于智驾车辆实际运营数据回灌在仿真模型中进行验证。

具体的，在本实施例公开的自适应箱重控制方法中，智驾系统控制模块的主要功能是接收上游任务管理、规划等模块下发的状态信息，参考轨迹和行驶参数等，通过综合处理与计算，得出控制量下发到智驾车辆横纵向执行器，如电动助力转向系统(ElectricPower Steering，EPS)、整车控制单元(Vehicle Control Unit,VCU)、电子制动系统(ElectronicBrakeSystem，EBS)和车身控制模块(Body Control Module,BCM)，使车辆达到期望的行驶状态。

相关车辆信息如车速、加速度、档位和转矩等，均为智驾车辆在装卸箱过程中需要使用到的重要参数值，控制模块可以直接从整车CAN通信网络中交互获取得到，从而简化与上游模块的交互。

本实施例，针对上述自适应箱重控制方法的步骤中公开的内容进行具体分析，分析内容如下：

1.自适应箱重功能状态机

智驾车辆的自动驾驶系统包括控制模块、任务管理模块、规划模块；

自适应箱重功能的有限状态机，为智驾车辆在装箱状态变化的任务中，控制模块内部是如何与其他智驾车辆的任务管理模块、规划模块之间进行交互的具体流程，如图1所示。

具体的自适应箱重功能交互流程为，智驾车辆的控制模块在上游任务管理模块下发的任务、规划模块下发的动作指令下行驶运作，智驾车辆到达下一次的装卸箱贝位点，反馈到上游规划模块完成下发的动作指令，规划模块向任务管理模块反馈任务结束，即完成一次装卸箱任务，并重复以上流程。

更进一步的，具体的自适应箱重功能交互流程，包括如下步骤：

S1：任务管理模块在完成智驾车辆装箱或卸箱任务后，告知下游智驾车辆的控制模块当前装箱状态是否发生变化；若没有变化，则存储和使用上一次的箱重信息值，若有变化，则在S2中重置为初始状态；

S2：依据任务管理模块，再判断智驾车辆上是否存在集装箱；若无集装箱，则直接进入计算参数处理状态，若有集装箱，则进入准备驶离的等待状态；

S3：在智驾车辆准备驶离等待状态中，等待接收规划模块下发的起步请求指令；当规划模块下发了起步请求指令时，则进入智驾车辆驱动扭矩输出状态，输出驱动扭矩到纵向控制逻辑，并驱动智驾车辆起步行驶；

S4：车辆起步后，进入完成起步状态，并在此状态中对智驾车辆的整车质量进行估计计算，并进行质量参数的处理；

S5：将处理后的质量参数进行存储和输出到纵向控制逻辑，控制智驾车辆按照质量参数计算输出扭矩，完成车辆的纵向控制。

在S4中，对智驾车辆的整车质量进行估计计算后，通过质量参数的处理和调整，减少计算结果因其他重要参数变化受到的影响，并保持计算结果在合理范围内。

2.车辆纵向动力学模型建立

智驾车辆动力学模型包括智驾车辆的纵向动力学模型，如图2所示，在智驾车辆的行驶过程中，纵向动力学模型的建立为：

F_t＝F_f+F_w+F_i+F_j

其中，F_f为智驾车辆行驶的滚动阻力，F_w为智驾车辆行驶迎风面阻力，F_i为智驾车辆沿坡面所受到的坡度阻力，F_j为智驾车辆加速时虚拟化出的一个加速阻力，F_t为地面对智驾车辆的驱动力。

具体的，纵向动力学模型建立过程中，各分力的表示公式如下：

地面对智驾车辆的驱动力可用如下公式表示：

其中，T_t为车辆输出的总驱动扭矩，i_o、i_g为减速机构的传动比，η为动力系统的机械传递效率，r为车辆轮胎行驶半径。

智驾车辆行驶所受的滚动阻力可用如下公式表示：

F_f＝fGcosα

其中，f为车辆滚动系数，G为整车重量，α为道路坡度角。

智驾车辆行驶迎风面阻力：

其中，C_D为迎风阻力系数，A为迎风面积，v为实际车速，ρ为空气密度。

智驾车辆沿坡面所受到的坡度阻力可用如下公式表示：

F_i＝Gsinα

其中，G为整车重量，α为道路坡度角。

智驾车辆受到的加速阻力可用如下公式表示：

其中，δ为旋转质量换算系数，m为整车质量。

综合上述公式，则智驾车辆纵向动力学模型的建立为：

至此，完成了智驾车辆纵向动力学模型的建立，上述公式较全面地公开了智驾车辆纵向运动的形式特性，大部分参数为车辆自身固有特性，并且部分参数可以通过手册查阅直接获取，而且针对智驾车辆在港区内运营的场景特点可以做进一步简化处理。

3.基于扩展卡尔曼滤波的整车质量估计方法

(1)线性卡尔曼滤波

卡尔曼滤波(KalmanFilter，KF)是20世纪60年代由美国电气工程师Rudolf EmilKalman提出的一种时域方法。卡尔曼滤波可以根据线性系统的状态空间方程，通过测量系统中掺有噪声和干扰的输入输出信号值，从而对系统的状态进行最优估计，即为是一种信号滤波过程。

卡尔曼滤波的基本原理是，结合上一个时刻对系统状态的估计值和当前时刻实际测量的观测值，对线性系统当前时刻的状态值进行滤波估计，包括预测与分析两个阶段。

在预测阶段，依据上一时刻系统状态值对下一时刻的系统状态进行预估；

在分析阶段，考虑实际测量到的观测值，利用最小方差估计(Minimum VarianceEstimation，MVE)方法对系统当前状态进行矫正分析，得出当前时刻状态空间参数的估计值。

随着时间推移，下一时刻的预测阶段利用当前时刻的状态估计值，分析阶段利用实际观测到的测量值，即可对下一时刻的状态进行估计，以此类推下去，即可实现时域上系统状态的估计。

考虑具有如下状态空间方程的线性动态系统：

式中，k为离散时间；X(k)为系统在k时刻的状态；Y(k)为系统对应状态的观测信号；U(k)为系统的输入控制信号；W(k)为输入信号的白噪声；V(k)为观测信号的噪声；A为状态转移矩阵；B为输入控制矩阵；T为噪声驱动矩阵；C为观测矩阵；D为输出控制矩阵。

通常情况下，假设W(k)和V(k)都是均值为0、方差分别为Q和R的不相关高斯白噪声(WhiteGaussian Noise)，且初始状态值

与W(k)、V(k)不相关，设初始状态为：

在预测阶段，借由当前时刻状态的估计值

单步预测下一时刻的状态预测值/>

同时对单步的协方差矩阵P(k+1|k)进行预测，预测方程式为：

状态预测：

协方差矩阵预测：

P(k+1|k)＝AP(k|k)A^T+TQT^T

在分析矫正阶段，结合实际测量得到的观测量Y(k+1)和滤波增益矩阵K(k+1)，对预测状态值

进行更新，即可得到下一时刻的状态估计值/>

同时更新协方差矩阵P(k+1|k+1)，更新方程为：

状态更新：

滤波增益矩阵：

K(k+1)＝P(k+1|k)C^T[CP(k+1|k)C^T+R]^-1

协方差矩阵更新：

P(k+1|k+1)＝[I_n-K(k+1)C]P(k+1|k)

由上述方程可以看出，在一个单步预测周期中，卡尔曼滤波有两个主要信息更新阶段：状态更新和观测更新，如图3所示。

状态预测方程解释了利用k时刻的状态来预测k+1时刻状态的方法，协方差矩阵公式对该单步预测方法的质量做了定量分析。两个方程中仅使用了与系统特性相关的参数，如状态转移矩阵A、噪声驱动矩阵T、输入噪声方差Q。从时间角度看，两个方程式将时间从k时刻推进到k+1时刻，表述了卡尔曼滤波的时间更新过程。

更新方程计算时间更新过程的修正量，通过更新质量的优劣(P(k+1|k))、观测信息的质量优劣(R)、观测量与状态值、控制量之间的关系(C、D)以及具体观测量Y(k)来确定修正量，以保证合理准确地使用观测量Y(k)，所以该过程是卡尔曼滤波的观测更新阶段。

卡尔曼滤波的计算是时域内的递推过程，是一个不断地预测+修正的过程，方程求解时无需大量数据存储，当观测到新数据值即可计算新滤波值，非常适用于需要芯片计算实时处理的系统，因此卡尔曼滤波在数据通信、规划导航、精确制导与工业控制等多领域得到了广泛的应用。

(2)扩展卡尔曼滤波

卡尔曼滤波适用于线性系统的状态估计，对于非线性系统，使用较为广泛的是扩展卡尔曼滤波(ExtendedKalmanFilter，EKF)。

扩展卡尔曼滤波通过将非线性系统在其当前时刻附近进行泰勒展开，去除高阶项，得到非线性系统的近似线性关系。再利用线性化之后的状态方程和预测方程进行状态估计，计算流程与线性系统的卡尔曼滤波相同。

(3)算法实现

基于扩展卡尔曼滤波整车质量的估计方式具体为，根据智驾车辆的纵向动力学模型，建立系统的状态空间模型，状态变量选取为车速v、整车质量m和道路坡度i，则系统的状态向量为：

X＝[v,m,i]

智驾车辆装卸箱后起步时，整车质量m可以看成是常量，且港区内因堆叠集装箱几乎不会剧烈的道路坡度变化，因此整车质量m和道路坡度i均可认为在当前状态下其变化率近似于0，因此结合动力学模型有：

其中，g为重力加速度；

上述方程为连续系统方程，在工程实践中通常使用的是离散时间状态空间方程，因此需要对上述连续系统方程进行离散化处理，为计算和简化方便，本实施例采用前向差分法将连续系统方程进行离散化处理，离散时间状态的空间方程为：

/>

假设在空间方程中输入信号白噪声W和观测信号的噪声V都是均值为0，以及方差分别为Q和R的不相关高斯白噪声，且初始状态值

与W、V不相关，则系统的状态方程为：

系统的观测量方程为：

则整理后的系统的状态空间表达式为：

基于状态空间方程，进行状态更新预测过程，更新方程为：

其中，

为上一时刻的状态变量最优估计值；

为状态变量的先验估计值；

P(k|k-1)为先验误差协方差；

P(k-1|k-1)为上一时刻的误差协方差；

过程方程向量函数对状态变量求偏导数得到的雅可比矩阵。

此处：

观测更新方程为：

其中，K(k)即为卡尔曼增益；

为状态变量的后验估计值；P(k|k)为误差后验协方差。/>

观测更新方程计算时间更新过程的修正量，通过更新质量的优劣、观测信息的质量优劣、观测量与状态值、控制量之间的关系以及具体观测量来确定修正量，以保证合理准确地使用观测量。

通过上述公开的内容完成整车质量估计算法的实现和对系统状态空间参数的估计。

3.仿真验证

完成智驾车辆动力学模型的建立与基于扩展卡尔曼滤波的整车质量估计方法的实现后，可在MATLAB/Simulink中搭建算法实现策略，并借助Stateflow搭建自适应箱重控制流程的状态机，即仿真模型；然后可以从实车上采集实际运营数据，智驾车辆按照实际运营或测试情况，装载不同质量的集装箱，通过车载的数据监控采集系统采集CAN数据，回灌到搭建的仿真模型中，即可对自适应箱重控制方法进行离线验证。

本实施例的技术方案相对于现有技术的技术方案进行具体分析：

1.现有的港区内智驾车辆装载的集装箱质量的获取，依赖调度系统下发质量信息的可靠性、准确性。港区任务调度系统下发的质量值是岸桥吊完成抓放集装箱后的测量值，如果此测量值比较粗略，存在不确定的误差，误差可能过大或过小，调度系统不能保证测量质量值的准确性和可靠性，也会对智驾系统造成影响；同时，从调度系统获取测量的集装箱质量信息也受通信链路不稳定性的影响，频繁交互也会有一定的时间成本。

本发明针对港区智能车辆的运营特点，提出的方案可以解决在智驾车辆装卸箱任务完成后无法快速、准确获取装载总质量从而影响智驾车辆快速驶离的问题，设计开发基于MATLAB/Simulink/Stateflow的自适应箱重状态交互流程能够快速、高效地适应装卸箱作业场景；自适应箱重状态机交互流程中，对外部调度系统的依赖仅限于需要获取智驾车辆上的集装箱状态是否发生变化这一状态参数而无需其他信息，降低了整体系统的复杂程度；将集装箱质量问题的处理集中在智驾系统控制模块中，增强了智驾系统的功能完备性和整体性，同时也提升了测试问题排查与运营作业的效率。

2.大多数情况下，若智驾车辆没有安装坡度传感器获取准确稳定的坡度信息，在使用卡尔曼方法对整车质量参数估计的同时，也会对道路坡度参数进行估计。

本发明的技术方案中借助智驾车辆搭载的惯导系统，可以直接获取智驾车辆所处道路的精确坡度值，并将坡度信息作为系统的观测变量，从而提升利用扩展卡尔曼方法对智驾车辆整车质量的估计精度。

通过上述公开的内容，本发明相对于现有技术的优势包括：

1.减少了智驾系统对外部模块的依赖，提高智驾系统的稳定性、完备性；

2.降低了多车多任务运营时，智驾系统与调度系统之间冗余度；

3.降低了测试问题排查难度，提升问题定位效率；

4.提升了智驾车辆整车估计质量参数的准确性，同时也提升了智驾系统在多场景下的自适应性；

5.降低了前后期的大量物力、人力等成本投入。

还需要说明的是，本发明还包括另外一种实施例，在另外一种实施例中，利用最小二乘法对整车质量进行估计，然后通过纵向动力学模型直接计算整车质量，具体实施方式公开如下：

1.利用最小二乘法对整车质量进行估计

对于智驾车辆整车质量的估计还包括最小二乘法。

最小二乘法，又称最小平方法，是一种数学优化方法。它通过真实目标对象与拟合目标对象的差，即最小化误差的平方和寻找数据的最佳函数匹配。利用最小二乘法可以简便地求得未知的数据，并使得这些求得的数据与实际数据之间误差的平方和为最小。选择最佳拟合曲线的标准可以确定为使总的拟合误差(即总残差)达到最小。

最小二乘法的一般模型表达式如下：

y＝θ₁x+θ₂x²+θ₃x³+...+θ_nxⁿ

其中，θ_n(n＝0，1，2，…)是待定系数，x为n阶的多项式变量；

真实值

与拟合值y_i的误差e_i：

误差的平方和Q为：

假设有m个样本，对这些样本有：

简化为：

X·θ＝Y

其中，

则：

该函数是关于θ的多元二次函数方程组，对其求偏导：

即：

2.纵向动力学模型直接计算整车质量

智驾车辆在起步阶段时速度低，而且行驶阻力较小，在该阶段估计车辆质量的准确性会比较高；并且智驾车辆完成起步后，质量基本不会发生变化，因此在车辆起步阶段可直接进行质量计算，其原理仍然是车辆纵向动力学模型：

由于在智驾车辆即将起步的过程中，车速极低，可以忽略迎风阻力部分的影响；通常情况下，车辆起步时坡度较小，将道路坡度做近似处理，即cosα≈1，sinα≈i；智驾车辆的纵向加速度参数可以直接从车辆总线上获取。则上述公式将变为：

依据上式，可以在智驾车辆起步的时刻直接计算得出整车质量。

本实施例的方法比较简单且容易实现，但计算结果十分依赖各个相关参数的准确性，且相关的参数较多，信号的质量也参差不齐。本实施例的方法虽然同样可以得出整车质量，但与实际值可能会存在不稳定的较大误差，每次计算的结果都有可能不同，因此也需要针对计算结果再次进行处理。

以上实施例仅用以说明本发明的技术方案而非限制，本领域普通技术人员对本发明的技术方案所做的其他修改或者等同替换，只要不脱离本发明技术方案的精神和范围，均应涵盖在本发明的权利要求范围当中。

Claims (10)

1.一种港内智能转运车自适应箱重控制方法，其特征在于：该方法包括自适应箱重功能交互流程的建立和验证，具体步骤如下：

步骤1：结合港区任务运行与智驾车辆的状态，基于Stateflow有限状态机建立自适应箱重功能交互流程；

步骤2：在交互流程中，建立智驾车辆动力学模型，并基于扩展卡尔曼滤波进行整车质量的估计；

步骤3：通过智驾车辆动力学模型的建立与基于扩展卡尔曼滤波的整车质量估计方法，搭建自适应箱重控制方法流程的仿真模型，并基于智驾车辆实际运营数据回灌在仿真模型中进行验证。

2.根据权利要求1所述的一种港内智能转运车自适应箱重控制方法，其特征在于：所述智驾车辆的自动驾驶系统包括控制模块、任务管理模块、规划模块；

步骤1中，自适应箱重功能的有限状态机，为智驾车辆在装箱状态变化的任务中，控制模块与其他智驾车辆自动驾驶系统各模块之间的交互流程。

3.根据权利要求2所述的一种港内智能转运车自适应箱重控制方法，其特征在于：具体的自适应箱重功能交互流程为，智驾车辆的控制模块在上游任务管理模块下发的任务、规划模块下发的动作指令下行驶运作，智驾车辆到达下一次的装卸箱贝位点，反馈到上游规划模块完成下发的动作指令，规划模块向任务管理模块反馈任务结束，即完成一次装卸箱任务。

4.根据权利要求3所述的一种港内智能转运车自适应箱重控制方法，其特征在于：具体的自适应箱重功能交互流程，包括如下步骤：

S1：任务管理模块在完成智驾车辆装箱或卸箱任务后，告知下游智驾车辆的控制模块当前装箱状态是否发生变化；

若没有变化，则存储和使用上一次的箱重信息值，若有变化，则在S2中重置为初始状态；

S2：依据任务管理模块，判断智驾车辆上是否存在集装箱；

若无集装箱，则进入计算参数处理状态，若有集装箱，则进入准备驶离的等待状态；

S3：在智驾车辆准备驶离等待状态中，等待接收规划模块下发的起步请求指令；

当规划模块下发了起步请求指令时，则进入智驾车辆驱动扭矩输出状态，输出驱动扭矩到纵向控制逻辑，并驱动智驾车辆起步行驶；

S4：车辆起步后，进入完成起步状态，并在此状态中对智驾车辆的整车质量进行估计计算，并进行质量参数的处理；

S5：将处理后的质量参数进行存储和输出到纵向控制逻辑，控制智驾车辆按照质量参数计算输出扭矩，完成车辆的纵向控制。

5.根据权利要求1所述的一种港内智能转运车自适应箱重控制方法，其特征在于：步骤2中，智驾车辆动力学模型包括智驾车辆的纵向动力学模型，在智驾车辆的行驶过程中，纵向动力学模型的建立为：

F_t＝F_f+F_w+F_i+F_j

其中，F_f智驾车辆行驶的滚动阻力，F_w为智驾车辆行驶迎风面阻力，F_i为智驾车辆沿坡面所受到的坡度阻力，F_j为智驾车辆加速时虚拟化出的一个加速阻力，F_t为地面对智驾车辆的驱动力。

6.根据权利要求5所述的一种港内智能转运车自适应箱重控制方法，其特征在于：纵向动力学模型建立过程中，各分力的表示公式如下：

地面对智驾车辆的驱动力：

其中，T_t为车辆输出的总驱动扭矩，i_o、i_g为减速机构的传动比，η为动力系统的机械传递效率，r为车辆轮胎行驶半径；

智驾车辆行驶的滚动阻力：

F_f＝fGcosα

其中，f为车辆滚动系数，G为整车重量，α为道路坡度角；

智驾车辆行驶迎风面阻力：

其中，C_D为迎风阻力系数，A为迎风面积，v为实际车速，ρ为空气密度；

智驾车辆沿坡面所受到的坡度阻力：

F_i＝Gsinα

其中，G为整车重量，α为道路坡度角；

智驾车辆受到的加速阻力：

其中，δ为旋转质量换算系数，m为整车质量；

则智驾车辆纵向动力学模型的建立为：

7.根据权利要求1所述的一种港内智能转运车自适应箱重控制方法，其特征在于：扩展卡尔曼滤波通过将非线性系统在其在当前时刻附近进行泰勒展开，去除高阶项，得到非线性系统的近似线性关系，根据线性关系通过卡尔曼滤波进行状态空间的估计；

卡尔曼滤波对上一个时刻系统状态的估计值和当前时刻实际测量的观测值，对线性系统当前时刻的状态值进行滤波估计，具体包括预测与分析两个阶段：

预测阶段，依据上一时刻系统状态值对下一时刻的系统状态进行预估；

分析阶段，基于实际测量到的观测值，利用最小方差估计方法对系统当前状态进行矫正分析，得出当前时刻状态空间参数的估计值。

8.根据权利要求7所述的一种港内智能转运车自适应箱重控制方法，其特征在于：基于扩展卡尔曼滤波整车质量的估计方式为，根据智驾车辆的纵向动力学模型，建立系统的状态空间模型，状态空间方程的线性动态系统：

式中，k为离散时间；X(k)为系统在k时刻的状态；Y(k)为系统对应状态的观测信号；U(k)为系统的输入控制信号；W(k)为输入信号的白噪声；V(k)为观测信号的噪声；A为状态转移矩阵；B为输入控制矩阵；T为噪声驱动矩阵；C为观测矩阵；D为输出控制矩阵；

系统的状态变量关系为：

X＝[v,m,i]

其中，v为车速，m为整车质量，i为道路坡度，均为状态变量。

9.根据权利要求8所述的一种港内智能转运车自适应箱重控制方法，其特征在于：结合智驾车辆的纵向动力学模型，完成对系统状态空间参数的估计，具体如下：

在智驾车辆装卸箱后起步时，连续系统方程为：

其中，g为重力加速度；

采用前向差分法将连续系统方程进行离散化处理，离散时间状态的空间方程为：

在空间方程中输入信号白噪声和观测信号的噪声，以及对应方差分别为Q和R的不相关高斯白噪声，则系统的状态方程为：

系统的观测量方程为：

则整理后的系统的状态空间方程为：

其中，W为信号白噪声，V为观测信号的噪声，且W和V均值为0，初始状态值与W、V不相关；

基于状态空间方程，进行状态更新预测过程，更新方程为：

其中，

为上一时刻的状态变量最优估计值；

为状态变量的先验估计值；

P(k|k-1)为先验误差协方差；

P(k-1|k-1)为上一时刻的误差协方差；

过程方程向量函数对状态变量求偏导数得到的雅可比矩阵；

此处：

观测更新方程为：

其中，K(k)即为卡尔曼增益；

为状态变量的后验估计值；P(k|k)为误差后验协方差。

10.根据权利要求1所述的一种港内智能转运车自适应箱重控制方法，其特征在于：步骤3中，通过智驾车辆动力学模型的建立与基于扩展卡尔曼滤波的整车质量估计方法，并借助Stateflow搭建自适应箱重控制流程的状态机，即仿真模型；

基于智驾车辆装载不同质量的集装箱，通过车载的数据监控采集系统采集CAN数据，回灌到搭建的仿真模型中，对自适应箱重控制方法进行离线验证。

Images