CN116976104B - 装载机行驶控制方法、装置及系统 - Google Patents

Abstract

本发明公开了一种装载机行驶控制方法、装置及系统，包括将装载机工作装置动力学模型输出的动臂与前车架间铰点处的运动副反力、翻斗缸液压力、动臂缸液压力、转向缸液压力以及轮胎模型输出的表达在轮胎局部坐标系下的纵向轮胎力和侧向轮胎力输入至预先建立好的装载机转向装置动力学模型中，计算得到前车架位置、后车架位置、各轮胎位置；将计算得到的前车架位置、后车架位置以及轮胎位置作为反馈信号，结合闭环控制算法，对装载机的行驶轨迹进行闭环控制。本发明计算效率高，具有非常好的实时性，可实现对装载机的闭环控制，还可用于装载机的HIL测试，提高产品开发效率，缩短产品开发周期，节省研发成本。

Description

装载机行驶控制方法、装置及系统

技术领域

本发明属于装载机控制技术领域，具体涉及一种装载机行驶控制方法、装置及系统。

背景技术

随着客户对产品舒适性、操控性、经济性的要求进一步提升，智能化、数字化发展正成为工程机械行业的重要趋势。无人装载机作为一种典型的工程机械智能化产品，可适应各种恶劣环境，对减轻工作人员的劳动强度，保障职业健康、人身安全等，有着十分关键的作用。

一方面，在无人装载机关键技术中，其中一项是自动转向功能的实现，即对装载行驶轨迹的控制。由于装载机转向时，前、后车架转过的角度影响因素较多，其行驶时的轨迹难以直接测量。

另一方面，现有技术中，对于舒适性、操控性、经济性的优化方案，都是基于现有产品以及尝试的办法去解决，缺少先进的手段去分析预测，精准调试，在优化性能的基础上缩短调试时间。

现有技术中，无人装载机虚拟样机模型的搭建主要通过两种方式，一种是基于ADAMS、RecurDyn等商业多体动力学软件，这类动力学软件具有建模过程直观、计算结果可靠等优点，但其数学模型复杂、计算成本高，不适用于闭环控制、硬件在环测试(HIL)等对仿真实时性要求较高的场合；另一种是采用公式化建模的方式，首先根据实际问题的需要对模型作出简化假设，再使用适当的方法描述各变量间的数学关系，此类方法计算结果精度不高。

发明内容

针对上述问题，本发明提出一种装载机行驶控制方法、装置及系统，不仅计算效率高，而且具有非常好的实时性，可实现对装载机的闭环控制，还可用于装载机的HIL测试，提高产品开发效率，缩短产品开发周期，节省研发成本。

为了实现上述技术目的，达到上述技术效果，本发明通过以下技术方案实现：

第一方面，本发明提供了一种装载机行驶控制方法，包括：

将获取到的翻斗缸长度、动臂缸长度，以及装载机转向装置动力学模型在初始姿态下输出的前车架转动角度，输入至装载机工作装置动力学模型中，计算得到装载机工作装置中各铰点的坐标、动臂与前车架间铰点处的运动副反力、翻斗缸液压力和动臂缸液压力；

根据计算得到的前车架和后车架上各铰点的坐标，计算出转向缸长度，进而获得转向缸液压力；

对装载机转向装置动力学模型在运动姿态下输出的各轮胎与地面接触点的坐标进行微分运算，并通过前车架转角、后车架转角构造坐标变换矩阵，计算得到表达在轮胎局部坐标系下的轮胎速度，结合获取到的作用于各轮胎的垂向载荷输入至轮胎模型中，计算得到纵向轮胎力和侧向轮胎力；

将所述动臂与前车架间铰点处的运动副反力、翻斗缸液压力、动臂缸液压力、纵向轮胎力、侧向轮胎力和转向缸液压力，输入至装载机转向装置动力学模型，计算得到前车架位置、后车架位置和各轮胎位置；

基于所述前车架位置、后车架位置和各轮胎位置对装载机的行驶轨迹进行控制。

可选地，所述装载机转向装置动力学模型包括装载机转向装置运动学约束方程和装载机转向装置动力学方程；

前车架和后车架上各铰点、各轮胎与地面接触点的坐标的计算方法包括：

约定装载机的初始姿态、坐标原点和坐标轴方向，得到各铰点、各构件质心的初始坐标；

根据装载机转向装置的结构，结合装载机转向装置运动学约束方程，得到装载机转向装置动力学方程；

将装载机转向装置动力学方程计算得到的前车架平移加速度、转动加速度和后车架的转动加速度代入装载机转向装置运动学约束方程，计算得到后车架的平移加速度；

以各铰点、各构件质心的初始坐标作为初始条件，对前车架平移加速度、转动加速度，以及后车架平移加速度、转动加速度，做两次积分运算，得到前车架和后车架的质心坐标、相对于初始姿态的转角；

基于前车架和后车架的质心坐标、相对于初始姿态的转角，计算前车架和后车架上各铰点、各轮胎与地面接触点的坐标。

可选地，所述装载机转向装置运动学约束方程的表达式为：

其中，a_x-f、a_z-f为前车架的平动加速度；a_x-r、a_z-r为后车架的平动加速度；ω_f、分别为前车架的转动角速度和转动角加速度；ω_r、分别为后车架的转动角速度和转动角加速度；ψ_f、ψ_r分别为前车架和后车架的转动角度；L_O1R、L_O2R分别为前车架和后车架质心与前、后车架间铰点的距离。

可选地，所述装载机转向装置动力学方程的获取方法包括：

分别获取装载机转向装置在两个坐标轴方向上的力平衡方程、前车架在前、后车架铰点处的力矩平衡方程以及后车架在前、后车架铰点处的力矩平衡方程，具体为：

装载机转向装置在x方向的力平衡：

F_x-I+F_x-fdg+F_x-dbg+(F_x-FL+F_x-FR)cosψ_f+(F_z-FL+F_z-FR)sinψ_f+(F_x-RL+F_x-RR)cosψ_r+(F_z-RL+F_z-RR)sinψ_r＝m_qcja_x-f+m_hcja_x-r

装载机转向装置在z方向的力平衡：

F_z-I+F_z-fdg+F_z-dbg-(F_x-FL+F_x-FR)sinψ_f+(F_z-FL+F_z-FR)cosψ_f-(F_x-RL+F_x-RR)simψ_r+(F_z-RL+F_z-RR)cosψ_r＝m_qcja_z-f+m_hcja_z-r

前车架在前、后车架铰点处的力矩平衡方程：

后车架在前、后车架铰点处的力矩平衡方程：

其中，F_x-I、F_z-I分别为动臂与前车架间铰点处的运动副反力在两个坐标轴方向上的分量；F_x-fdg、F_z-fdg分别为翻斗缸液压力在两个坐标轴方向上的分量；F_x-dbg、F_z-dbg分别为动臂缸液压力在两个坐标轴方向上的分量；F_x-FL、F_z-FL、F_x-FR、F_z-FR、F_x-RL、F_z-RL、F_x-RR、F_z-RR分别为表达在轮胎局部坐标系下左前轮胎、右前轮胎、左后轮胎和右后轮胎的纵向轮胎力和侧向轮胎力；T_sum为动臂与前车架间铰点处的运动副反力、翻斗缸液压力以及动臂缸液压力在前、后车架间铰点处的作用力矩之和；T_s为左、右转向缸在前、后车架间铰点处的作用力矩之和；m_qcj、m_hcj分别为前车架和后车架质量；I_y-qcj、I_y-hcj分别为前车架和后车架在前、后车架间铰点处的转动惯量；L_QfR、L_QrR分别为前桥中心、后桥中心与前、后车架间铰点的距离；W_b为轮距；

将装载机转向装置运动学约束方程代入上述方程中，得到装载机转向装置动力学方程，具体为：

装载机转向装置在x方向的力平衡：

装载机转向装置在z方向的力平衡：

前车架在前、后车架铰点处的力矩平衡方程：

后车架在前、后车架铰点处的力矩平衡方程：

可选地，前车架和后车架上各铰点、各轮胎与地面接触点的坐标计算公式为：

其中，当和分别表示初始姿态和k时刻前车架质心坐标时，为前车架上待求铰点或左前、右前轮胎与地面接触点在初始姿态的坐标，为前车架上待求铰点或左前、右前轮胎与地面接触点在k时刻的坐标，ψ为前车架转角；当和 分别表示初始姿态和k时刻后车架质心坐标时，为后车架上待求铰点或左后、右后轮胎与地面接触点在初始姿态的坐标，为后车架上待求铰点或左后、右后轮胎与地面接触点在k时刻的坐标，ψ为后车架转角。

可选地，所述纵向轮胎力和侧向轮胎力计算方法为：

对所述各轮胎与地面接触点的坐标进行微分运算，得到轮胎在全局坐标下的速度，并通过前车架转角、后车架转角构造坐标变换矩阵，计算得到表达在轮胎局部坐标系下的轮胎速度，轮胎速度的数学表达式具体为：

其中，当ψ为前车架转角时，v_x，g、v_z，g为左前轮胎或右前轮胎在全局坐标下的速度，v_x，r、v_z，r为左前轮胎或右前轮胎在轮胎局部坐标下的速度；当ψ为后车架转角时，v_x，g、v_z，g为左后轮胎或右后轮胎在全局坐标下的速度，v_x，r、v_z，r为左后轮胎或右后轮胎在轮胎局部坐标下的速度，为坐标变换矩阵；

将所述表达在轮胎局部坐标系下的轮胎速度以及称重系统计算出的作用于各轮胎的垂向载荷输入至预先建立好的轮胎模型中，计算得到表达在轮胎局部坐标系下的纵向轮胎力和侧向轮胎力。

可选地，所述装载机行驶控制方法还包括：

获取装载机实际位置；

基于各轮胎与地面接触点的坐标，计算出装载机计算位置；

基于所述装载机实际位置和装载机计算位置，对装载机的行驶轨迹进行控制。

可选地，所述装载机行驶控制方法还包括：

当将所述动臂与前车架间铰点处的运动副反力、翻斗缸液压力、动臂缸液压力、纵向轮胎力、侧向轮胎力和转向缸液压力，输入至装载机转向装置动力学模型后，计算得到转向缸长度；

基于所述转向缸长度对装载机的工作循环的功率、压力冲击进行检测分析。

第二方面，本发明提供了一种装载机行驶控制装置，包括：

第一计算模块，用于将获取到的翻斗缸长度、动臂缸长度，以及装载机转向装置动力学模型在初始姿态下输出的前车架转动角度，输入至装载机工作装置动力学模型中，计算得到装载机工作装置中各铰点的坐标、动臂与前车架间铰点处的运动副反力、翻斗缸液压力和动臂缸液压力；

第二计算模块，用于根据计算得到的前车架和后车架上各铰点的坐标，计算出转向缸长度，进而获得转向缸液压力；

第三计算模块，用于对装载机转向装置动力学模型在运动姿态下输出的各轮胎与地面接触点的坐标进行微分运算，并通过前车架转角、后车架转角构造坐标变换矩阵，计算得到表达在轮胎局部坐标系下的轮胎速度，结合获取到的作用于各轮胎的垂向载荷输入至轮胎模型中，计算得到纵向轮胎力和侧向轮胎力；

第四计算模块，用于将所述动臂与前车架间铰点处的运动副反力、翻斗缸液压力、动臂缸液压力、纵向轮胎力、侧向轮胎力和转向缸液压力，输入至装载机转向装置动力学模型，计算得到前车架位置、后车架位置、各轮胎位置；

控制模块，用于基于所述前车架位置、后车架位置以及各轮胎位置对装载机的行驶轨迹进行控制。

第三方面，本发明提供了一种装载机行驶控制装置，包括控制器单元以及与所述控制器单元相连的第一长度传感器、第二长度传感器和控制器单元；

所述第一长度传感器、第二长度传感器将获取到的翻斗缸长度、动臂缸长度发送至控制器单元；

所述控制器单元包括：

第一计算模块，用于将获取到的翻斗缸长度、动臂缸长度，以及装载机转向装置动力学模型在初始姿态下输出的前车架转动角度，输入至装载机工作装置动力学模型中，计算得到装载机工作装置中各铰点的坐标、动臂与前车架间铰点处的运动副反力、翻斗缸液压力和动臂缸液压力；

第二计算模块，用于根据计算得到的前车架和后车架上各铰点的坐标，计算出转向缸长度，进而获得转向缸液压力；

第三计算模块，用于对装载机转向装置动力学模型在运动姿态下输出的各轮胎与地面接触点的坐标进行微分运算，并通过前车架转角、后车架转角构造坐标变换矩阵，计算得到表达在轮胎局部坐标系下的轮胎速度，结合获取到的作用于各轮胎的垂向载荷输入至轮胎模型中，计算得到纵向轮胎力和侧向轮胎力；

第四计算模块，用于将所述动臂与前车架间铰点处的运动副反力、翻斗缸液压力、动臂缸液压力、纵向轮胎力、侧向轮胎力和转向缸液压力，输入至装载机转向装置动力学模型，计算得到前车架位置、后车架位置、各轮胎位置；

控制模块，用于基于所述前车架位置、后车架位置以及各轮胎位置对装载机的行驶轨迹进行控制。

第四方面，本发明提供了一种装载机行驶控制系统，包括存储介质和处理器；

所述存储介质用于存储指令；

所述处理器用于根据所述指令进行操作以执行根据第一方面中任一项所述的方法。

与现有技术相比，本发明的有益效果：

相比于现有技术，本发明中通过将装载机工作装置动力学模型输出的动臂与前车架间铰点处的运动副反力、翻斗缸液压力、动臂缸液压力和轮胎模型输出的表达在轮胎局部坐标系下的纵向轮胎力和侧向轮胎力以及转向缸液压力输入至预先建立好的转向装置动力学模型中，计算得到前车架位置、后车架位置、各轮胎位置，将计算得到的前、后车架位置以及轮胎位置作为反馈信号，结合闭环控制算法，对装载机的行驶轨迹进行闭环控制，结合PID等控制算法，实现对装载机行驶轨迹的闭环控制。不仅计算效率高，而且具有非常好的实时性，可实现对装载机的闭环控制，还可用于装载机的HIL测试，提高产品开发效率，缩短产品开发周期，节省研发成本。

本发明中还通过将装载机工作装置动力学模型输出的动臂与前车架间铰点处的运动副反力、翻斗缸液压力、动臂缸液压力和轮胎模型输出的表达在轮胎局部坐标系下的纵向轮胎力和侧向轮胎力以及转向缸液压力输入至预先建立好的转向装置动力学模型中，计算得到转向缸长度，基于计算得到的转向缸长度，通过和液压系统模型的数据传递，对整个工作循环的功率、压力冲击进行检测分析，实现精准调试。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图，其中：

图1为现有技术中某一装载机转向装置的结构示意图；

图2为本发明提供的装载机行驶控制方法的示意图；

图3为本发明提供的装载机转向装置动力学模型的建模流程图；

图4为本发明提供的一种简化后的装载机转向装置示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

另外，若本发明实施例中有涉及“第一”、“第二”等的描述，则该“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。另外，各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。

实施例1

本发明实施例中提供了一种装载机行驶控制方法，包括以下步骤：

(1)将获取到的翻斗缸长度、动臂缸长度，以及装载机转向装置动力学模型在初始姿态下输出的前车架转动角度，输入至装载机工作装置动力学模型中，计算得到装载机工作装置中各铰点的坐标、动臂与前车架间铰点处的运动副反力、翻斗缸液压力和动臂缸液压力；本发明实施例中的装载机工作装置动力学模型为现有技术中的装载机工作装置动力学模型，如2010年西安科技大学硕士学位论文《小型装载机工作装置与转向系统的分析与设计》中公开的装载机工作装置动力学模型。

(2)根据计算得到的前车架和后车架上各铰点的坐标，计算出转向缸长度，输入至装载机转向液压系统模型中，计算得到转向缸液压力；所述装载机转向液压系统模型采用现有技术中的即可，本发明并未对装载机转向液压系统模型进行创新；

(3)对装载机转向装置动力学模型在运动姿态下输出的各轮胎与地面接触点的坐标进行微分运算，并通过前车架转角、后车架转角构造坐标变换矩阵，计算得到表达在轮胎局部坐标系下的轮胎速度，结合获取到的作用于各轮胎的垂向载荷输入至轮胎模型中，计算得到纵向轮胎力和侧向轮胎力；

(4)将所述动臂与前车架间铰点处的运动副反力、翻斗缸液压力、动臂缸液压力、纵向轮胎力、侧向轮胎力和转向缸液压力，输入至装载机转向装置动力学模型，计算得到前车架位置、后车架位置和各轮胎位置；

(5)基于所述前车架位置、后车架位置和各轮胎位置对装载机的行驶轨迹进行控制。

在具体实施过程中，步骤(1)-(5)为反复循环执行的步骤，其中，步骤(1)中的前车架转动角度在后续的循环中，是通过装载机转向装置动力学模型在运动姿态下输出。

本发明中通过将装载机工作装置动力学模型输出的动臂与前车架间铰点处的运动副反力、翻斗缸液压力、动臂缸液压力和轮胎模型输出的表达在轮胎局部坐标系下的纵向轮胎力和侧向轮胎力以及转向缸液压力输入至预先建立好的转向装置动力学模型中，计算得到前车架位置、后车架位置、各轮胎位置，将计算得到的前、后车架位置以及轮胎位置作为反馈信号，结合闭环控制算法，对装载机的行驶轨迹进行闭环控制，结合PID等控制算法，实现对装载机行驶轨迹的闭环控制。不仅计算效率高，而且具有非常好的实时性，可实现对装载机的闭环控制，还可用于装载机的HIL测试，提高产品开发效率，缩短产品开发周期，节省研发成本。

在本发明实施例的一种具体实施方式中，所述各铰点、各构件质心的初始坐标的获取方法包括：

对装载机转向装置进行结构简化，生成对应的二维模型；在具体实施过程中，结构简化的原则为：不考虑转向装置俯仰、滚转以及在地面垂直方向上的动态，不考虑轮胎变形的影响；将转向缸视为无质量的构件且不考虑移动副的作用，即将转向缸视为二力杆；所有构件均视为刚体，即不考虑构件在力作用下所产生的形变；

约定装载机的初始姿态、坐标原点和坐标轴方向，结合所述二维模型计算出各铰点、各构件质心的初始坐标；在具体实施过程中，所述的初始姿态、坐标原点和坐标轴的定义可以是任意的。为了方便建模，通常将重力方向作为其中一个坐标轴方向。为便于说明，这里假设转向装置位于xz平面内，即各铰点、各构件质心在y方向的坐标始终为零。

在本发明实施例的一种具体实施方式中，所述装载机转向装置动力学模型包括装载机转向装置运动学约束方程和装载机转向装置动力学方程；

前车架和后车架上各铰点、各轮胎与地面接触点的坐标的计算方法包括：

约定装载机的初始姿态、坐标原点和坐标轴方向，得到各铰点、各构件质心的初始坐标；

根据装载机转向装置的结构，结合装载机转向装置运动学约束方程，得到装载机转向装置动力学方程；

将装载机转向装置动力学方程计算得到的前车架平移加速度、转动加速度和后车架的转动加速度代入装载机转向装置运动学约束方程，计算得到后车架的平移加速度；

以各铰点、各构件质心的初始坐标作为初始条件，对前车架平移加速度、转动加速度，以及后车架平移加速度、转动加速度，做两次积分运算，得到前车架和后车架的质心坐标、相对于初始姿态的转角；

基于前车架和后车架的质心坐标、相对于初始姿态的转角，计算前车架和后车架上各铰点、各轮胎与地面接触点的坐标。

在本发明实施例的一种具体实施方式中，所述装载机转向装置运动学约束方程的表达式为：

其中，a_x-f、a_z-f为前车架的平动加速度；a_x-r、a_z-r为后车架的平动加速度；ω_f、分别为前车架的转动角速度和转动角加速度；ω_r、分别为后车架的转动角速度和转动角加速度；ψ_f、ψ_r分别为前车架和后车架的转动角度；L_O1R、L_O2R分别为前车架和后车架质心与前、后车架间铰点的距离。

在本发明实施例的一种具体实施方式中，所述装载机转向装置动力学方程的获取方法包括：

分别获取装载机转向装置在两个坐标轴方向上的力平衡方程、前车架在前、后车架铰点处的力矩平衡方程以及后车架在前、后车架铰点处的力矩平衡方程，具体为：

装载机转向装置在x方向的力平衡：

F_x-I+F_x-fdg+F_x-dbg+(F_x-FL+F_x-FR)cosψ_f+(F_z-FL+F_z-FR)sinψ_f+(F_x-RL+F_x-RR)cosψ_r+(F_z-RL+F_z-RR)sinψ_r＝m_qcja_x-f+m_hcja_x-r

装载机转向装置在z方向的力平衡：

F_z-I+F_z-fdg+F_z-dbg-(F_x-FL+F_x-FR)sinψ_f+(F_z-FL+F_z-FR)cosψ_f-(F_x-RL+F_x-RR)sinψ_r+(F_z-RL+F_z-RR)cosψ_r＝m_qcja_z-f+m_hcja_z-r

前车架在前、后车架铰点处的力矩平衡方程：

后车架在前、后车架铰点处的力矩平衡方程：

其中，F_x-I、F_z-I分别为动臂与前车架间铰点处的运动副反力在两个坐标轴方向上的分量；F_x-fdg、F_z-fdg分别为翻斗缸液压力在两个坐标轴方向上的分量；F_x-dbg、F_z-dbg分别为动臂缸液压力在两个坐标轴方向上的分量；F_x-FL、F_z-FL、F_x-FR、F_z-FR、F_x-RL、F_z-RL、F_x-RR、F_z-RR分别为表达在轮胎局部坐标系下左前轮胎、右前轮胎、左后轮胎和右后轮胎的纵向轮胎力和侧向轮胎力；T_sum为动臂与前车架间铰点处的运动副反力、翻斗缸液压力以及动臂缸液压力在前、后车架间铰点处的作用力矩之和；T_s为左、右转向缸在前、后车架间铰点处的作用力矩之和；m_qcj、m_hcj分别为前车架和后车架质量；I_y-qcj、I_y-hcj分别为前车架和后车架在前、后车架间铰点处的转动惯量；L_QfR、L_QrR分别为前桥中心、后桥中心与前、后车架间铰点的距离；W_b为轮距；

将装载机转向装置运动学约束方程代入上述方程中，得到装载机转向装置动力学方程，具体为：

装载机转向装置在x方向的力平衡：

装载机转向装置在z方向的力平衡：

前车架在前、后车架铰点处的力矩平衡方程：

后车架在前、后车架铰点处的力矩平衡方程：

在本发明实施例的一种具体实施方式中，前车架和后车架上各铰点、各轮胎与地面接触点的坐标计算公式为：

其中，当和分别表示初始姿态和k时刻前车架质心坐标时，为前车架上待求铰点或左前、右前轮胎与地面接触点在初始姿态的坐标，为前车架上待求铰点或左前、右前轮胎与地面接触点在k时刻的坐标，ψ为前车架转角；当和 分别表示初始姿态和k时刻后车架质心坐标时，为后车架上待求铰点或左后、右后轮胎与地面接触点在初始姿态的坐标，为后车架上待求铰点或左后、右后轮胎与地面接触点在k时刻的坐标，ψ为后车架转角。

在本发明实施例的一种具体实施方式中，所述纵向轮胎力和侧向轮胎力计算方法为：

对所述各轮胎与地面接触点的坐标进行微分运算，得到轮胎在全局坐标下的速度，并通过前车架转角、后车架转角构造坐标变换矩阵，计算得到表达在轮胎局部坐标系下的轮胎速度，轮胎速度的数学表达式具体为：

其中，当ψ为前车架转角时，v_x，g、v_z，g为左前轮胎或右前轮胎在全局坐标下的速度，v_x，r、v_z，r为左前轮胎或右前轮胎在轮胎局部坐标下的速度；当ψ为后车架转角时，v_x，g、v_z，g为左后轮胎或右后轮胎在全局坐标下的速度，v_x，r、v_z，r为左后轮胎或右后轮胎在轮胎局部坐标下的速度，为坐标变换矩阵；

将所述表达在轮胎局部坐标系下的轮胎速度以及称重系统计算出的作用于各轮胎的垂向载荷输入至预先建立好的轮胎模型中，计算得到表达在轮胎局部坐标系下的纵向轮胎力和侧向轮胎力。

在本发明实施例的一种具体实施方式中，所述装载机行驶控制方法还包括：

获取装载机实际位置；

基于各轮胎与地面接触点的坐标，计算出装载机计算位置；

基于所述装载机实际位置和装载机计算位置，对装载机的行驶轨迹进行控制。

在本发明实施例的一种具体实施方式中，所述装载机行驶控制方法还包括：

当将所述动臂与前车架间铰点处的运动副反力、翻斗缸液压力、动臂缸液压力、纵向轮胎力、侧向轮胎力和转向缸液压力，输入至装载机转向装置动力学模型后，计算得到转向缸长度；

基于所述转向缸长度对装载机的工作循环的功率、压力冲击进行检测分析。

下面结合一具体实施方式对本发明实施例中的方法进行详细说明。

如图1所示，装载机包括工作装置、转向装置和轮胎10等，其中工作装置包括铲斗1、拉杆2、摇臂3、动臂4、翻斗缸5、动臂缸6和若干个铰点，转向装置包括前车架7、转向缸8、后车架9和若干个铰点等。铲斗1与动臂4间通过铰点M₁连接在一起；拉杆2的一端通过铰点M₂与铲斗1相连，另一端通过铰点M₃与摇臂3相连；摇臂3通过铰点M₄安装在动臂4上，另一端通过铰点M₅与翻斗缸5相连；翻斗缸5另一端通过铰点M₇安装在前车架7上；动臂缸6一端通过铰点M₆与动臂4相连，另一端通过铰点M₉与前车架7相连；动臂4与前车架7通过铰点M₈相连。

所述装载机行驶控制方法包括以下步骤：

步骤(一)对装载机中的转向装置按如下规则进行简化：

a.不考虑转向装置俯仰、滚转以及在地面垂直方向上的动态，不考虑轮胎变形的影响；

b.将转向缸视为无质量的构件，且不考虑移动副的作用，即：将转向缸视为二力杆；

c.所有构件均视为刚体，即：不考虑构件在力作用下所产生的形变。

然后，设定初始姿态下前、后车架转动角度均为零度，坐标原点与M₁点重合，坐标轴方向如图4所示，其中xoz为全局坐标系；X_FLW_FLY_FL、X_FRW_FRY_FR、X_RLW_RLY_RL和X_RRW_RRY_RR分别为固连在左前轮胎、右前轮胎、左后轮胎、右后轮胎的局部坐标系，W_FL、W_FR、W_RL和W_RR分别表示左前轮胎、右前轮胎、左后轮胎、右后轮胎的接地点；前车架与后车架间通过铰点R连接在一起；左转向缸一端通过铰点A_L与前车架相连，另一端通过铰点B_L与后车架相连；右转向缸一端通过铰点A_R与前车架相连，另一端通过铰点B_R与后车架相连，O₁和O₂分别为前车架和后车架质心。

步骤(二)计算前车架转动角度、后车架转动角度以及各轮胎与地面接触点坐标，具体步骤为：

(1)获取装载机转向装置运动学约束方程，具体为：

其中，a_x-f、a_z-f为前车架的平动加速度；a_x-r、a_z-r为后车架的平动加速度；ω_f、分别为前车架的转动角速度和转动角加速度；ω_r、分别为后车架的转动角速度和转动角加速度；ψ_f、ψ_r分别为前车架和后车架的转动角度；L_O1R、L_O2R分别为前车架和后车架质心与前、后车架间铰点的距离。

(2)根据装载机转向装置的结构，结合装载机转向装置运动学约束方程，推导出装载机转向装置动力学方程，如图3所示，具体为：

首先，分别建立转向装置在两个坐标轴方向上的力平衡方程、前车架在前、后车架铰点处的力矩平衡方程以及后车架在前、后车架铰点处的力矩平衡方程，具体为：

转向装置在x方向的力平衡：

F_x-I+F_x-fdg+F_x-dbg+(F_x-FL+F_x-FR)cosψ_f+(F_z-FL+F_z-FR)sinψ_f+(F_x-RL+F_x-RR)cosψ_r+(F_z-RL+F_z-RR)sinψ_r＝m_qcja_x-f+m_hcja_x-r，

转向装置在z方向的力平衡：

F_z-I+F_z-fdg+F_z-dbg-(F_x-FL+F_x-FR)sinψ_f+(F_z-FL+F_z-FR)cosψ_f-(F_x-RL+F_x-RR)sinψ_r+(F_z-RL+F_z-RR)cosψ_r＝m_qcja_z-f+m_hcja_z-r，

前车架在前、后车架铰点处的力矩平衡方程：

，

后车架在前、后车架铰点处的力矩平衡方程：

其中，F_x-I、F_z-I为动臂与前车架间铰点处的运动副反力在两个坐标轴方向上的分量；F_x-fdg、F_z-fdg为翻斗缸液压力在两个坐标轴方向上的分量；F_x-dbg、F_z-dbg为动臂缸液压力在两个坐标轴方向上的分量；F_x-FL、F_z-FL、F_x-FR、F_z-FR、F_x-RL、F_z-RL、F_x-RR、F_z-RR分别为表达在轮胎局部坐标系下左前轮胎、右前轮胎、左后轮胎和右后轮胎的纵向轮胎力和侧向轮胎力；T_sum为动臂与前车架间铰点处的运动副反力、翻斗缸液压力以及动臂缸液压力在前、后车架间铰点处的作用力矩之和；T_s为左、右转向缸在前、后车架间铰点处的作用力矩之和；m_qcj、m_hcj分别为前车架和后车架质量；I_y-qcj、I_y-hcj分别为前车架和后车架在前、后车架间铰点处的转动惯量；L_QfR、L_QrR分别为前桥中心、后桥中心与前、后车架间铰点的距离；W_b为轮距。

然后，将转向装置运动学约束方程代入上述方程中，得到装载机转向装置动力学方程，具体为：

转向装置在x方向的力平衡：

，

转向装置在z方向的力平衡：

，

前车架在前、后车架铰点处的力矩平衡方程：

，

后车架在前、后车架铰点处的力矩平衡方程：

(3)基于装载机转向装置动力学方程，将求得的前车架平动加速度a_x-f、a_z-f、前车架转动加速度和后车架转动加速度代入运动学约束方程中，计算得到后车架平动加速度a_x-r、a_z-r。

(4)对前车架平动加速度a_x-f、a_z-f、前车架转动加速度后车架转动加速度后车架平动加速度a_x-r、a_z-r进行两次积分运算，得到前车架质心坐标、转动角度和后车架质心坐标、转动角度。

(5)计算前车架和后车架上各铰点、各轮胎与地面接触点的坐标：

其中，当和分别表示初始姿态和k时刻前车架质心坐标时，为前车架上待求铰点或左前、右前轮胎与地面接触点在初始姿态的坐标，为前车架上待求铰点或左前、右前轮胎与地面接触点在k时刻的坐标，ψ为前车架转角；当和 分别表示初始姿态和k时刻后车架质心坐标时，为后车架上待求铰点或左后、右后轮胎与地面接触点在初始姿态的坐标，为后车架上待求铰点或左后、右后轮胎与地面接触点在k时刻的坐标，ψ为后车架转角。

(6)根据前车架和后车架上各铰点坐标，计算出转向缸长度。转向缸设置在前车架和后车架之间。

步骤(三)计算表达在轮胎局部坐标系下的纵向轮胎力和侧向轮胎力，具体步骤为：

(1)对所述各轮胎与地面接触点的坐标进行微分运算，轮胎在全局坐标下的速度，并通过前车架转角、后车架转角构造坐标变换矩阵，计算得到表达在轮胎局部坐标系下的轮胎速度，具体为：

其中，当ψ为前车架转角时，v_x，g、v_z，g为左前轮胎或右前轮胎在全局坐标下的速度，v_x，r、v_z，r为左前轮胎或右前轮胎在局部坐标下的速度；当ψ为后车架转角时，v_x，g、v_z，g为左后轮胎或右后轮胎在全局坐标下的速度，v_x，r、v_z，r为左后轮胎或右后轮胎在局部坐标下的速度，为坐标变换矩阵。

(2)将所述表达在轮胎局部坐标系下的轮胎速度以及作用于各轮胎的垂向载荷输入至预先建立好的轮胎模型中，计算得到表达在轮胎局部坐标系下的纵向轮胎力和侧向轮胎力；其中，所述作用于各轮胎的垂向载荷由称重系统计算出来；所述轮胎模型为现有技术，基于轮胎模型计算得到在轮胎局部坐标系下的纵向轮胎力和侧向轮胎力的过程为现有技术。

步骤(四)获取翻斗缸长度、动臂缸长度和装载机位置；

步骤(五)将获取到的翻斗缸长度、动臂缸长度，以及装载机转向装置动力学模型在初始姿态下输出的前车架转动角度，输入至装载机工作装置动力学模型中，计算得到装载机工作装置中各铰点的坐标、动臂与前车架间铰点处的运动副反力、翻斗缸液压力和动臂缸液压力；

步骤(六)根据计算得到的前车架和后车架上各铰点的坐标，计算出转向缸长度，输入至装载机转向液压系统模型中，计算得到转向缸液压力；

步骤(七)对装载机转向装置动力学模型在运动姿态下输出的各轮胎与地面接触点的坐标进行微分运算，并通过前车架转角、后车架转角构造坐标变换矩阵，计算得到表达在轮胎局部坐标系下的轮胎速度，结合获取到的作用于各轮胎的垂向载荷输入至轮胎模型中，计算得到纵向轮胎力和侧向轮胎力；

步骤(八)将所述动臂与前车架间铰点处的运动副反力、翻斗缸液压力、动臂缸液压力、纵向轮胎力、侧向轮胎力和转向缸液压力，输入至装载机转向装置动力学模型，计算得到前车架位置、后车架位置、各轮胎位置和转向缸长度；

步骤(九)基于所述前车架位置、后车架位置和各轮胎位置对装载机的行驶轨迹进行控制；基于计算得到的转向缸长度，通过和液压系统模型的数据传递，对整个工作循环的功率、压力冲击进行检测分析，实现精准调试。

实施例2

基于与实施例1相同的发明构思，本发明实施例中提供了一种装载机行驶控制装置，包括：

第一计算模块，用于将获取到的翻斗缸长度、动臂缸长度，以及装载机转向装置动力学模型在初始姿态下输出的前车架转动角度，输入至装载机工作装置动力学模型中，计算得到装载机工作装置中各铰点的坐标、动臂与前车架间铰点处的运动副反力、翻斗缸液压力和动臂缸液压力；

第二计算模块，用于根据计算得到的前车架和后车架上各铰点的坐标，计算出转向缸长度，输入至装载机转向液压系统模型中，计算得到转向缸液压力；

第三计算模块，用于对装载机转向装置动力学模型在运动姿态下输出的各轮胎与地面接触点的坐标进行微分运算，并通过前车架转角、后车架转角构造坐标变换矩阵，计算得到表达在轮胎局部坐标系下的轮胎速度，结合获取到的作用于各轮胎的垂向载荷输入至轮胎模型中，计算得到纵向轮胎力和侧向轮胎力；

第四计算模块，用于将所述动臂与前车架间铰点处的运动副反力、翻斗缸液压力、动臂缸液压力、纵向轮胎力、侧向轮胎力和转向缸液压力，输入至装载机转向装置动力学模型，计算得到前车架位置、后车架位置、各轮胎位置；

控制模块，用于基于所述前车架位置、后车架位置以及各轮胎位置对装载机的行驶轨迹进行控制。

在本发明实施例的一种具体实施方式中，所述各铰点、各构件质心的初始坐标的获取方法包括：

对装载机转向装置进行结构简化，生成对应的二维模型；在具体实施过程中，结构简化的原则为：不考虑转向装置俯仰、滚转以及在地面垂直方向上的动态，不考虑轮胎变形的影响，图4为对图1所示结构进行简化后的结果；将转向缸视为无质量的构件且不考虑移动副的作用，即将转向缸视为二力杆；所有构件均视为刚体，即不考虑构件在力作用下所产生的形变；

约定装载机的初始姿态、坐标原点和坐标轴方向，结合所述二维模型计算出各铰点、各构件质心的初始坐标；在具体实施过程中，所述的初始姿态、坐标原点和坐标轴的定义可以是任意的。为了方便建模，通常将重力方向作为其中一个坐标轴方向。为便于说明，这里假设转向装置位于xz平面内，即各铰点、各构件质心在y方向的坐标始终为零。

在本发明实施例的一种具体实施方式中，所述装载机转向装置动力学模型包括装载机转向装置运动学约束方程和装载机转向装置动力学方程；

前车架和后车架上各铰点、各轮胎与地面接触点的坐标的计算方法包括：

约定装载机的初始姿态、坐标原点和坐标轴方向，得到各铰点、各构件质心的初始坐标；

根据装载机转向装置的结构，结合装载机转向装置运动学约束方程，得到装载机转向装置动力学方程；

将装载机转向装置动力学方程计算得到的前车架平移加速度、转动加速度和后车架的转动加速度代入装载机转向装置运动学约束方程，计算得到后车架的平移加速度；

以各铰点、各构件质心的初始坐标作为初始条件，对前车架平移加速度、转动加速度，以及后车架平移加速度、转动加速度，做两次积分运算，得到前车架和后车架的质心坐标、相对于初始姿态的转角；

基于前车架和后车架的质心坐标、相对于初始姿态的转角，计算前车架和后车架上各铰点、各轮胎与地面接触点的坐标。

在本发明实施例的一种具体实施方式中，所述装载机转向装置运动学约束方程的表达式为：

其中，a_x-f、a_z-f为前车架的平动加速度；a_x-r、a_z-r为后车架的平动加速度；ω_f、分别为前车架的转动角速度和转动角加速度；ω_r、分别为后车架的转动角速度和转动角加速度；ψ_f、ψ_r分别为前车架和后车架的转动角度；L_O1R、L_O2R分别为前车架和后车架质心与前、后车架间铰点的距离。

在本发明实施例的一种具体实施方式中，所述装载机转向装置动力学方程的获取方法包括：

分别获取装载机转向装置在两个坐标轴方向上的力平衡方程、前车架在前、后车架铰点处的力矩平衡方程以及后车架在前、后车架铰点处的力矩平衡方程，具体为：

装载机转向装置在x方向的力平衡：

F_x-I+F_x-fdg+F_x-dbg+(F_x-FL+F_x-FR)cosψ_f+(F_z-FL+F_z-FR)sinψf+(F_x-RL+F_x-RR)cosψ_r+(F_z-RL+F_z-RR)sinψ_r＝m_qcja_x-f+m_hcja_x-r

装载机转向装置在z方向的力平衡：

F_z-I+F_z-fdg+F_z-dbg-(F_x-FL+F_x-FR)sinψ_f+(F_z-FL+F_z-FR)cosψ_f-(F_x-RL+F_x-RR)simψr+(F_z-RL+F_z-RR)cosψr＝m_qcja_z-f+m_hcja_z-r

前车架在前、后车架铰点处的力矩平衡方程：

后车架在前、后车架铰点处的力矩平衡方程：

其中，F_x-I、F_z-I分别为动臂与前车架间铰点处的运动副反力在两个坐标轴方向上的分量；F_x-fdg、F_z-fdg分别为翻斗缸液压力在两个坐标轴方向上的分量；F_x-dbg、F_z-dbg分别为动臂缸液压力在两个坐标轴方向上的分量；F_x-FL、F_z-FL、F_x-FR、F_z-FR、F_x-RL、F_z-RL、F_x-RR、F_z-RR分别为表达在轮胎局部坐标系下左前轮胎、右前轮胎、左后轮胎和右后轮胎的纵向轮胎力和侧向轮胎力；T_sum为动臂与前车架间铰点处的运动副反力、翻斗缸液压力以及动臂缸液压力在前、后车架间铰点处的作用力矩之和；T_s为左、右转向缸在前、后车架间铰点处的作用力矩之和；m_qcj、m_hcj分别为前车架和后车架质量；I_y-qcj、I_y-hcj分别为前车架和后车架在前、后车架间铰点处的转动惯量；L_QfR、L_QrR分别为前桥中心、后桥中心与前、后车架间铰点的距离；W_b为轮距；

将装载机转向装置运动学约束方程代入上述方程中，得到装载机转向装置动力学方程，具体为：

装载机转向装置在x方向的力平衡：

装载机转向装置在z方向的力平衡：

前车架在前、后车架铰点处的力矩平衡方程：

后车架在前、后车架铰点处的力矩平衡方程：

在本发明实施例的一种具体实施方式中，前车架和后车架上各铰点、各轮胎与地面接触点的坐标计算公式为：

其中，当和分别表示初始姿态和k时刻前车架质心坐标时，为前车架上待求铰点或左前、右前轮胎与地面接触点在初始姿态的坐标，为前车架上待求铰点或左前、右前轮胎与地面接触点在k时刻的坐标，ψ为前车架转角；当和 分别表示初始姿态和k时刻后车架质心坐标时，为后车架上待求铰点或左后、右后轮胎与地面接触点在初始姿态的坐标，为后车架上待求铰点或左后、右后轮胎与地面接触点在k时刻的坐标，ψ为后车架转角。

在本发明实施例的一种具体实施方式中，所述纵向轮胎力和侧向轮胎力计算方法为：

对所述各轮胎与地面接触点的坐标进行微分运算，得到轮胎在全局坐标下的速度，并通过前车架转角、后车架转角构造坐标变换矩阵，计算得到表达在轮胎局部坐标系下的轮胎速度，轮胎速度的数学表达式具体为：

其中，当ψ为前车架转角时，v_x，g、v_z，g为左前轮胎或右前轮胎在全局坐标下的速度，v_x，r、v_z，r为左前轮胎或右前轮胎在轮胎局部坐标下的速度；当ψ为后车架转角时，v_x，g、v_z，g为左后轮胎或右后轮胎在全局坐标下的速度，v_x，r、v_z，r为左后轮胎或右后轮胎在轮胎局部坐标下的速度，为坐标变换矩阵；

将所述表达在轮胎局部坐标系下的轮胎速度以及称重系统计算出的作用于各轮胎的垂向载荷输入至预先建立好的轮胎模型中，计算得到表达在轮胎局部坐标系下的纵向轮胎力和侧向轮胎力。

在本发明实施例的一种具体实施方式中，所述装载机行驶控制方法还包括：

获取装载机实际位置；

基于各轮胎与地面接触点的坐标，计算出装载机计算位置；

基于所述装载机实际位置和装载机计算位置，对装载机的行驶轨迹进行控制。

在本发明实施例的一种具体实施方式中，所述装载机行驶控制方法还包括：

当将所述动臂与前车架间铰点处的运动副反力、翻斗缸液压力、动臂缸液压力、纵向轮胎力、侧向轮胎力和转向缸液压力，输入至装载机转向装置动力学模型后，计算得到转向缸长度；

基于所述转向缸长度对装载机的工作循环的功率、压力冲击进行检测分析。

实施例3

基于与实施例1相同的发明构思，本发明实施例中提供了一种装载机行驶控制装置，包括控制器单元以及与所述控制器单元相连的第一长度传感器、第二长度传感器和控制器单元；

所述第一长度传感器、第二长度传感器将获取到的翻斗缸长度、动臂缸长度发送至控制器单元；在具体实施过程中，所述第一长度传感器、第二长度传感器可以被统称为获取模块；

所述控制器单元包括：

第一计算模块，用于将获取到的翻斗缸长度、动臂缸长度，以及装载机转向装置动力学模型在初始姿态下输出的前车架转动角度，输入至装载机工作装置动力学模型中，计算得到装载机工作装置中各铰点的坐标、动臂与前车架间铰点处的运动副反力、翻斗缸液压力和动臂缸液压力；

第二计算模块，用于根据计算得到的前车架和后车架上各铰点的坐标，计算出转向缸长度，输入至装载机转向液压系统模型中，计算得到转向缸液压力；

第三计算模块，用于对装载机转向装置动力学模型在运动姿态下输出的各轮胎与地面接触点的坐标进行微分运算，并通过前车架转角、后车架转角构造坐标变换矩阵，计算得到表达在轮胎局部坐标系下的轮胎速度，结合获取到的作用于各轮胎的垂向载荷输入至轮胎模型中，计算得到纵向轮胎力和侧向轮胎力；

第四计算模块，用于将所述动臂与前车架间铰点处的运动副反力、翻斗缸液压力、动臂缸液压力、纵向轮胎力、侧向轮胎力和转向缸液压力，输入至装载机转向装置动力学模型，计算得到前车架位置、后车架位置、各轮胎位置；

控制模块，用于基于所述前车架位置、后车架位置以及各轮胎位置对装载机的行驶轨迹进行控制。

在本发明实施例的一种具体实施方式中，所述装载机行驶控制装置还包括雷达或GPS，所述雷达或GPS用于获取装载机实际位置；

所述控制器单元基于各轮胎与地面接触点的坐标，计算出装载机计算位置；并基于所述装载机实际位置和装载机计算位置，对装载机的行驶轨迹进行控制。在具体实施过程中，由行使轨迹控制模块基于装载机实际位置和装载机计算位置实现对装载机的行驶轨迹进行控制。

在本发明实施例的一种具体实施方式中，所述各铰点、各构件质心的初始坐标的获取方法包括：

对装载机转向装置进行结构简化，生成对应的二维模型；在具体实施过程中，结构简化的原则为：不考虑转向装置俯仰、滚转以及在地面垂直方向上的动态，不考虑轮胎变形的影响，图4为对图1所示结构进行简化后的结果；将转向缸视为无质量的构件且不考虑移动副的作用，即将转向缸视为二力杆；所有构件均视为刚体，即不考虑构件在力作用下所产生的形变；

约定装载机的初始姿态、坐标原点和坐标轴方向，结合所述二维模型计算出各铰点、各构件质心的初始坐标；在具体实施过程中，所述的初始姿态、坐标原点和坐标轴的定义可以是任意的。为了方便建模，通常将重力方向作为其中一个坐标轴方向。为便于说明，这里假设转向装置位于xz平面内，即各铰点、各构件质心在y方向的坐标始终为零。

在本发明实施例的一种具体实施方式中，所述装载机转向装置动力学模型包括装载机转向装置运动学约束方程和装载机转向装置动力学方程；

前车架和后车架上各铰点、各轮胎与地面接触点的坐标的计算方法包括：

约定装载机的初始姿态、坐标原点和坐标轴方向，得到各铰点、各构件质心的初始坐标；

根据装载机转向装置的结构，结合装载机转向装置运动学约束方程，得到装载机转向装置动力学方程；

将装载机转向装置动力学方程计算得到的前车架平移加速度、转动加速度和后车架的转动加速度代入装载机转向装置运动学约束方程，计算得到后车架的平移加速度；

以各铰点、各构件质心的初始坐标作为初始条件，对前车架平移加速度、转动加速度，以及后车架平移加速度、转动加速度，做两次积分运算，得到前车架和后车架的质心坐标、相对于初始姿态的转角；

基于前车架和后车架的质心坐标、相对于初始姿态的转角，计算前车架和后车架上各铰点、各轮胎与地面接触点的坐标。

在本发明实施例的一种具体实施方式中，所述装载机转向装置运动学约束方程的表达式为：

其中，a_x-f、a_z-f为前车架的平动加速度；a_x-r、a_z-r为后车架的平动加速度；ω_f、分别为前车架的转动角速度和转动角加速度；ω_r、分别为后车架的转动角速度和转动角加速度；ψ_f、ψ_r分别为前车架和后车架的转动角度；L_O1R、L_O2R分别为前车架和后车架质心与前、后车架间铰点的距离。

在本发明实施例的一种具体实施方式中，所述装载机转向装置动力学方程的获取方法包括：

分别获取装载机转向装置在两个坐标轴方向上的力平衡方程、前车架在前、后车架铰点处的力矩平衡方程以及后车架在前、后车架铰点处的力矩平衡方程，具体为：

装载机转向装置在x方向的力平衡：

F_x-I+F_x-fdg+F_x-dbg+(F_x-FL+F_x-FR)cosψ_f+(F_z-FL+F_z-FR)sinψ_f+(F_x-RL+F_x-RR)cosψr+(F_z-RL+F_z-RR)sinψ_r＝m_qcja_x-f+m_hcja_x-r

装载机转向装置在z方向的力平衡：

F_z-I+F_z-fdg+F_z-dbg-(F_x-FL+F_x-FR)sinψ_f+(F_z-FL+F_z-FR)cosψ_f-(F_x-RL+F_x-RR)sinψ_r+(F_z-RL+F_z-RR)cosψ_r＝m_qcja_z-f+m_hcja_z-r

前车架在前、后车架铰点处的力矩平衡方程：

后车架在前、后车架铰点处的力矩平衡方程：

其中，F_x-I、F_z-I分别为动臂与前车架间铰点处的运动副反力在两个坐标轴方向上的分量；F_x-fdg、F_z-fdg分别为翻斗缸液压力在两个坐标轴方向上的分量；F_x-dbg、F_z-dbg分别为动臂缸液压力在两个坐标轴方向上的分量；F_x-FL、F_z-FL、F_x-FR、F_z-FR、F_x-RL、F_z-RL、F_x-RR、F_z-RR分别为表达在轮胎局部坐标系下左前轮胎、右前轮胎、左后轮胎和右后轮胎的纵向轮胎力和侧向轮胎力；T_sum为动臂与前车架间铰点处的运动副反力、翻斗缸液压力以及动臂缸液压力在前、后车架间铰点处的作用力矩之和；Ts为左、右转向缸在前、后车架间铰点处的作用力矩之和；m_qcj、m_hcj分别为前车架和后车架质量；I_y-qcj、I_y-hcj分别为前车架和后车架在前、后车架间铰点处的转动惯量；L_QfR、L_QrR分别为前桥中心、后桥中心与前、后车架间铰点的距离；W_b为轮距；

将装载机转向装置运动学约束方程代入上述方程中，得到装载机转向装置动力学方程，具体为：

装载机转向装置在x方向的力平衡：

装载机转向装置在z方向的力平衡：

前车架在前、后车架铰点处的力矩平衡方程：

后车架在前、后车架铰点处的力矩平衡方程：

在本发明实施例的一种具体实施方式中，前车架和后车架上各铰点、各轮胎与地面接触点的坐标计算公式为：

其中，当和分别表示初始姿态和k时刻前车架质心坐标时，为前车架上待求铰点或左前、右前轮胎与地面接触点在初始姿态的坐标，为前车架上待求铰点或左前、右前轮胎与地面接触点在k时刻的坐标，ψ为前车架转角；当和 分别表示初始姿态和k时刻后车架质心坐标时，为后车架上待求铰点或左后、右后轮胎与地面接触点在初始姿态的坐标，为后车架上待求铰点或左后、右后轮胎与地面接触点在k时刻的坐标，ψ为后车架转角。

在本发明实施例的一种具体实施方式中，所述纵向轮胎力和侧向轮胎力计算方法为：

对所述各轮胎与地面接触点的坐标进行微分运算，得到轮胎在全局坐标下的速度，并通过前车架转角、后车架转角构造坐标变换矩阵，计算得到表达在轮胎局部坐标系下的轮胎速度，轮胎速度的数学表达式具体为：

其中，当ψ为前车架转角时，v_x，g、v_z，g为左前轮胎或右前轮胎在全局坐标下的速度，v_x，r、v_z，r为左前轮胎或右前轮胎在轮胎局部坐标下的速度；当ψ为后车架转角时，v_x，g、v_z，g为左后轮胎或右后轮胎在全局坐标下的速度，v_x，r、v_z，r为左后轮胎或右后轮胎在轮胎局部坐标下的速度，为坐标变换矩阵；

将所述表达在轮胎局部坐标系下的轮胎速度以及称重系统计算出的作用于各轮胎的垂向载荷输入至预先建立好的轮胎模型中，计算得到表达在轮胎局部坐标系下的纵向轮胎力和侧向轮胎力。

在本发明实施例的一种具体实施方式中，所述装载机行驶控制装置还包括：

获取模块，用于获取装载机实际位置；

第五计算模块，用于基于各轮胎与地面接触点的坐标，计算出装载机计算位置；

控制模块，用于基于所述装载机实际位置和装载机计算位置，对装载机的行驶轨迹进行控制。

在本发明实施例的一种具体实施方式中，所述装载机行驶控制装置还包括：

第六计算模块，用于当将所述动臂与前车架间铰点处的运动副反力、翻斗缸液压力、动臂缸液压力、纵向轮胎力、侧向轮胎力和转向缸液压力，输入至装载机转向装置动力学模型后，计算得到转向缸长度；

分析模块，用于基于所述转向缸长度对装载机的工作循环的功率、压力冲击进行检测分析。

实施例4

基于与实施例1相同的发明构思，本发明实施例中提供了一种装载机行驶控制系统，包括存储介质和处理器；

所述存储介质用于存储指令；

所述处理器用于根据所述指令进行操作以执行根据实施例1中任一项所述的方法。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上结合附图对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护之内。

以上显示和描述了本发明的基本原理和主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (9)

1.一种装载机行驶控制方法，其特征在于，包括：

将获取到的翻斗缸长度、动臂缸长度，以及装载机转向装置动力学模型在初始姿态下输出的前车架转动角度，输入至装载机工作装置动力学模型中，计算得到装载机工作装置中各铰点的坐标、动臂与前车架间铰点处的运动副反力、翻斗缸液压力和动臂缸液压力；

根据计算得到的前车架和后车架上各铰点的坐标，计算出转向缸长度，进而获得转向缸液压力；

对装载机转向装置动力学模型在运动姿态下输出的各轮胎与地面接触点的坐标进行微分运算，并通过前车架转角、后车架转角构造坐标变换矩阵，计算得到表达在轮胎局部坐标系下的轮胎速度，结合获取到的作用于各轮胎的垂向载荷输入至轮胎模型中，计算得到纵向轮胎力和侧向轮胎力；

将所述动臂与前车架间铰点处的运动副反力、翻斗缸液压力、动臂缸液压力、纵向轮胎力、侧向轮胎力和转向缸液压力，输入至装载机转向装置动力学模型，计算得到前车架位置、后车架位置和各轮胎位置；

基于所述前车架位置、后车架位置和各轮胎位置对装载机的行驶轨迹进行控制；

获取装载机实际位置；

基于各轮胎与地面接触点的坐标，计算出装载机计算位置；

基于所述装载机实际位置和装载机计算位置，对装载机的行驶轨迹进行控制。

2.根据权利要求1所述的一种装载机行驶控制方法，其特征在于：所述装载机转向装置动力学模型包括装载机转向装置运动学约束方程和装载机转向装置动力学方程；

前车架和后车架上各铰点、各轮胎与地面接触点的坐标的计算方法包括：

约定装载机的初始姿态、坐标原点和坐标轴方向，得到各铰点、各构件质心的初始坐标；

根据装载机转向装置的结构，结合装载机转向装置运动学约束方程，得到装载机转向装置动力学方程；

将装载机转向装置动力学方程计算得到的前车架平移加速度、转动加速度和后车架的转动加速度代入装载机转向装置运动学约束方程，计算得到后车架的平移加速度；

以各铰点、各构件质心的初始坐标作为初始条件，对前车架平移加速度、转动加速度，以及后车架平移加速度、转动加速度，做两次积分运算，得到前车架和后车架的质心坐标、相对于初始姿态的转角；

基于前车架和后车架的质心坐标、相对于初始姿态的转角，计算前车架和后车架上各铰点、各轮胎与地面接触点的坐标。

3.根据权利要求2所述的一种装载机行驶控制方法，其特征在于：所述装载机转向装置运动学约束方程的表达式为：

其中，a_x-f、a_z-f为前车架的平动加速度；a_x-r、a_z-r为后车架的平动加速度；ω_f、分别为前车架的转动角速度和转动角加速度；分别为后车架的转动角速度和转动角加速度；ψ_f、ψ_r分别为前车架和后车架的转动角度；L_O1R、L_O2R分别为前车架和后车架质心与前、后车架间铰点的距离。

4.根据权利要求3所述的一种装载机行驶控制方法，其特征在于：所述装载机转向装置动力学方程的获取方法包括：

分别获取装载机转向装置在两个坐标轴方向上的力平衡方程、前车架在前、后车架铰点处的力矩平衡方程以及后车架在前、后车架铰点处的力矩平衡方程，具体为：

装载机转向装置在x方向的力平衡：

F_x-I+F_x-fdg+F_x-dbg+(F_x-FL+F_x-FR)cosψ_f+(F_z-FL+F_z-FR)sinψ_f+(F_x-RL+F_x-RR)cosψ_r+(F_z-RL+F_z-RR)sinψ_r＝m_qcja_x-f+m_hcja_x-r

装载机转向装置在z方向的力平衡：

F_z-I+F_z-fdg+F_z-dbg-(F_x-FL+F_x-FR)sinψ_f+(F_z-FL+F_z-FR)cosψ_f-(F_x-RL+F_x-RR)sinψ_r+(F_z-RL+F_z-RR)cosψ_r＝m_qcja_z-f+m_hcja_z-r

前车架在前、后车架铰点处的力矩平衡方程：

后车架在前、后车架铰点处的力矩平衡方程：

其中，F_x-I、F_z-I分别为动臂与前车架间铰点处的运动副反力在两个坐标轴方向上的分量；F_x-fdg、F_z-fdg分别为翻斗缸液压力在两个坐标轴方向上的分量；F_x-dbg、F_z-dbg分别为动臂缸液压力在两个坐标轴方向上的分量；F_x-FL、F_z-FL、F_x-FR、F_z-FR、F_x-RL、F_z-RL、F_x-RR、F_z-RR分别为表达在轮胎局部坐标系下左前轮胎、右前轮胎、左后轮胎和右后轮胎的纵向轮胎力和侧向轮胎力；T_sum为动臂与前车架间铰点处的运动副反力、翻斗缸液压力以及动臂缸液压力在前、后车架间铰点处的作用力矩之和；T_s为左、右转向缸在前、后车架间铰点处的作用力矩之和；m_qcj、m_hcj分别为前车架和后车架质量；I_y-qcj、I_y-hcj分别为前车架和后车架在前、后车架间铰点处的转动惯量；L_QfR、L_QrR分别为前桥中心、后桥中心与前、后车架间铰点的距离；W_b为轮距；

将装载机转向装置运动学约束方程代入上述方程中，得到装载机转向装置动力学方程，具体为：

装载机转向装置在x方向的力平衡：

装载机转向装置在z方向的力平衡：

前车架在前、后车架铰点处的力矩平衡方程：

后车架在前、后车架铰点处的力矩平衡方程：

5.根据权利要求4所述的一种装载机行驶控制方法，其特征在于：前车架和后车架上各铰点、各轮胎与地面接触点的坐标计算公式为：

其中，当和分别表示初始姿态和k时刻前车架质心坐标时，为前车架上待求铰点或左前、右前轮胎与地面接触点在初始姿态的坐标，为前车架上待求铰点或左前、右前轮胎与地面接触点在k时刻的坐标，ψ为前车架转角；当和分别表示初始姿态和k时刻后车架质心坐标时，为后车架上待求铰点或左后、右后轮胎与地面接触点在初始姿态的坐标，为后车架上待求铰点或左后、右后轮胎与地面接触点在k时刻的坐标，ψ为后车架转角。

6.根据权利要求1所述的一种装载机行驶控制方法，其特征在于：所述纵向轮胎力和侧向轮胎力计算方法为：

对所述各轮胎与地面接触点的坐标进行微分运算，得到轮胎在全局坐标下的速度，并通过前车架转角、后车架转角构造坐标变换矩阵，计算得到表达在轮胎局部坐标系下的轮胎速度，轮胎速度的数学表达式具体为：

其中，当ψ为前车架转角时，v_x,g、v_z,g为左前轮胎或右前轮胎在全局坐标下的速度，v_x,r、v_z,r为左前轮胎或右前轮胎在轮胎局部坐标下的速度；当ψ为后车架转角时，v_x,g、v_z,g为左后轮胎或右后轮胎在全局坐标下的速度，v_x,r、v_z,r为左后轮胎或右后轮胎在轮胎局部坐标下的速度，为坐标变换矩阵；

将所述表达在轮胎局部坐标系下的轮胎速度以及称重系统计算出的作用于各轮胎的垂向载荷输入至预先建立好的轮胎模型中，计算得到表达在轮胎局部坐标系下的纵向轮胎力和侧向轮胎力。

7.根据权利要求1所述的一种装载机行驶控制方法，其特征在于，所述装载机行驶控制方法还包括：

当将所述动臂与前车架间铰点处的运动副反力、翻斗缸液压力、动臂缸液压力、纵向轮胎力、侧向轮胎力和转向缸液压力，输入至装载机转向装置动力学模型后，计算得到转向缸长度；

基于所述转向缸长度对装载机的工作循环的功率、压力冲击进行检测分析。

8.一种装载机行驶控制装置，其特征在于，包括控制器单元以及与所述控制器单元相连的第一长度传感器、第二长度传感器和控制器单元；

所述第一长度传感器、第二长度传感器将获取到的翻斗缸长度、动臂缸长度发送至控制器单元；

所述控制器单元包括：

第一计算模块，用于将获取到的翻斗缸长度、动臂缸长度，以及装载机转向装置动力学模型在初始姿态下输出的前车架转动角度，输入至装载机工作装置动力学模型中，计算得到装载机工作装置中各铰点的坐标、动臂与前车架间铰点处的运动副反力、翻斗缸液压力和动臂缸液压力；

第二计算模块，用于根据计算得到的前车架和后车架上各铰点的坐标，计算出转向缸长度，进而获得转向缸液压力；

第三计算模块，用于对装载机转向装置动力学模型在运动姿态下输出的各轮胎与地面接触点的坐标进行微分运算，并通过前车架转角、后车架转角构造坐标变换矩阵，计算得到表达在轮胎局部坐标系下的轮胎速度，结合获取到的作用于各轮胎的垂向载荷输入至轮胎模型中，计算得到纵向轮胎力和侧向轮胎力；

第四计算模块，用于将所述动臂与前车架间铰点处的运动副反力、翻斗缸液压力、动臂缸液压力、纵向轮胎力、侧向轮胎力和转向缸液压力，输入至装载机转向装置动力学模型，计算得到前车架位置、后车架位置、各轮胎位置；

控制模块，用于基于所述前车架位置、后车架位置以及各轮胎位置对装载机的行驶轨迹进行控制；

所述控制器单元还获取装载机实际位置；基于各轮胎与地面接触点的坐标，计算出装载机计算位置；

基于所述装载机实际位置和装载机计算位置，对装载机的行驶轨迹进行控制。

9.一种装载机行驶控制系统，其特征在于，包括存储介质和处理器；

所述存储介质用于存储指令；

所述处理器用于根据所述指令进行操作以执行根据权利要求1-7中任一项所述的方法。