CN113914406B - 一种电动装载机路径跟踪系统及控制方法 - Google Patents

Abstract

本发明提出一种电动装载机路径跟踪系统及控制方法，该系统包括：导航设备，用于提供位置信息；拉线位移传感器，用于提供实时转向角度；行走电机，用于驱动装载机前进或后退，并提供当前行走电机转速；转向机构，用于控制装载机左右转向；上位机控制系统，用于根据位置信息、实时转向角度和当前行走电机转速生成控制命令；整车控制器，用于接收所述上位机控制系统的控制命令，并生成相应的控制信号以通过行走电机控制装载机行走、通过转向机构控制装载机转向。该电动装载机路径跟踪控制系统及方法可以使电动装载机在无驾驶员的情况下沿着目标轨迹实时、精准、稳定、安全的行走。

Description

一种电动装载机路径跟踪系统及控制方法

技术领域

本发明属于路径跟踪技术领域，具体涉及一种电动装载机路径跟踪系统及 控制方法。

背景技术

未来智慧矿山、智慧港口将得到大力发展，其工作量大、24小时不停机的 特性决定了未来市场对装载机提出了更高的技术要求。无人化智能装载机可以 很好的解决上述问题，可以在特定的工作区域内24小时不停机、高效率的进行 铲装作业。

电动装载机路径跟踪控制方法是电动装载机无人化、智能化的关键部分， 因此，研究和开发出适宜的电动装载机路径跟踪控制系统及方法具有重要的意 义。

发明内容

本发明的目的在于克服现有技术中的不足，提供一种电动装载机路径跟踪 系统及控制方法，可以实时的调整装载机的航向角，使装载机对目标路径进行 精准的跟踪。

为达到上述目的，本发明是采用下述技术方案实现的：

本发明提供一种电动装载机路径跟踪系统，包括：

导航设备，用于提供位置信息；

拉线位移传感器，用于提供实时转向角度；

行走电机，用于驱动装载机前进或后退，并提供当前行走电机转速；

转向电磁阀，包括左转向电磁阀和右转向电磁阀，用于控制装载机左右转 向；

液压电机，用于给转向电磁阀输出液压动力；

上位机控制系统，分别与所述导航设备、拉线位移传感器、整车控制器、 行走电机和液压电机相连接，从而获取位置信息、实时转向角度和当前行走电 机转速；所述上位机控制系统根据获取的信号进行算法和逻辑处理，生成控制 命令并输出给整车控制器；

整车控制器，分别与所述上位机控制系统、行走电机、液压电机、左转向 电磁阀和右转向电磁阀相连接，用于接收所述上位机控制系统的控制命令，并 生成相应的控制信号以控制行走电机前进或后退、控制左转向电磁阀驱动装载 机进行左转向、控制右转向电磁阀驱动装载机进行右转向以及控制液压电机给 转向电磁阀输出液压动力。

电动装载机属于前后中心铰接式车体，分为可相对转动的前车体和后车体。

进一步的，所述整车控制器通过CAN总线或硬接线与行走电机、液压电机、 左转向电磁阀和右转向电磁阀相连接。

进一步的，所述导航设备安装在驾驶室顶部，为RTK惯性导航设备；

所述拉线位移传感器一端安装在前车架上，另一端安装在后车架上。

第二方面，本发明提供一种电动装载机路径跟踪控制方法，电动装载机属 于前后中心铰接式车体，分为可相对转动的前车体和后车体，包括以下步骤：

获取导航设备的位置信息和目标轨迹信息；

获取实时转向角度、当前行走电机转速和装载机几何结构信息；

根据获取的导航设备的位置信息、实时转向角度以及装载机几何结构信息， 经前、后车体坐标系控制方法，获得车体位置信息，所述车体位置信息包括前 桥中点车体坐标系和前车体航向角以及后桥中点车体坐标系和后车体航向角；

根据所述实时转向角度以及车体位置信息与目标轨迹信息的横向偏差、航 向角偏差，生成用于控制液压电机和转向电磁阀进行实时转向的控制命令；

根据所述当前行走电机转速以及车体位置信息与目标轨迹的纵向偏差，生 成用于控制装载机前进或后退或制动的控制命令。

进一步的，所述导航设备的位置信息包括导航设备的经纬度和航向角；

所述装载机几何结构信息包括导航设备距前后车体铰接中点的距离、铰接 中点到前桥中点的距离以及铰接中点到后桥中点的距离；

所述目标轨迹信息包括目标轨迹和规划的目标轨迹全局坐标系；目标轨迹 包括多个序列点组成的曲线或直线，每个序列点包含该点的目标轨迹全局坐标 系中的位置以及该点的航向角；

所述前、后车体坐标系控制方法的步骤包括：

将导航设备的经纬度转换为笛卡尔全局坐标系；

根据实时转向角度和航向角得到装载机的前车体航向角和后车体航向角；

将笛卡尔全局坐标系结合前、后车体航向角以及导航设备距前后车体铰接 中点的距离，使笛卡尔全局坐标系转换到前后车体铰接中点上；

通过铰接中点到前桥中点的距离、铰接中点到后桥中点的距离以及前车体 航向角和后车体航向角，将笛卡尔全局坐标系分别转移到前桥中点和后桥中点 上，形成前桥中点全局坐标系和后桥中点全局坐标系；

将规划的目标轨迹全局坐标系分别转移到前桥中点、后桥中点上，将全局 坐标系转换为车体坐标系，当装载机前进时输出前桥中点车体坐标系和前车体 航向角、装载机后退时输出后桥中点车体坐标系和后车体航向角。

进一步的，生成用于控制液压电机和转向电磁阀进行实时转向的控制命令 的方法包括以下步骤：

根据装载机当前位置寻找当前车体位置距离目标轨迹的最近点作为此次轨 迹跟踪的起点，将目标轨迹可视化为由多个序列点组成的路径，从最近点到目 标轨迹的终点进行遍历；

当装载机前进时以前桥中点车体坐标系和前车体航向角为当前车体位置， 当装载机后退时以后桥中点车体坐标系和后车体航向角为当前车体位置；

判断目标轨迹序列点间隔是否小于或者等于装载机前进的预瞄距离，如果 是，那么下一个序列点为装载机的下一个前进点；如果否，那么将目标轨迹终 点作为装载机的下一前进点；沿着目标轨迹行驶过程中输出纵向预瞄点、横向 预瞄点、航向角预瞄点；

对横向预瞄点和航向角预瞄点进行加权处理；将加权后的横向预瞄点与航 向角预瞄点相加作为车体轨迹跟踪过程中的总偏差；

对总偏差和总偏差增量进行模糊化处理，获得输入参数；将输入参数输入 PID算法得到为保持装载机精准跟踪的目标转向角度；

根据目标转向角度和实时转向角度生成控制命令。

进一步的，根据目标转向角度和实时转向角度生成控制命令的方法包括：

判断目标转向角度是否等于实时转向角度；

若等于，发送装载机不转向的控制命令至整车控制器，使左、右转向电磁 阀控制信号为0，液压电机转速为怠速；

若目标转向角度小于装载机实际转向角度，发送装载机左转向的控制命令 至整车控制器，使整车控制器生成左转向电磁阀控制信号以及相应的液压电机 转速；若目标转向角度大于装载机实际转向角度，则发送装载机右转向的控制 命令至整车控制器，使整车控制器生成右转向电磁阀控制信号以及相应的液压 电机转速。

进一步的，生成用于控制装载机前进或后退或制动的控制命令的方法步骤 如下：

判断是否具有工作指令，如果为否，则发送无任何扭矩输出的控制命令至 整车控制器；

若具有工作指令，判断装载机是否前进，如果为是，则根据设定的装载机 前进目标驱动转速和当前行走电机转速，得到前进转速偏差；

如果具有工作指令且装载机后退，则根据设定的装载机后退目标驱动转速 和当前行走电机转速，得到后退转速偏差；

将装载机驱动转速偏差和偏差增量进行模糊化处理，得到输入参数；

根据输入参数进行PID算法处理，得到行走电机驱动扭矩。

进一步的，所述路径跟踪控制方法还包括：

判断装载机距离目标轨迹终点距离是否小于设定的制动距离；

如果装载机距离目标轨迹终点大于设定的制动距离，则输出相应的行走电 机行走扭矩的控制命令；

如果装载机距离目标轨迹终点小于设定的制动距离，则根据设定的电机制 动的目标转速和当前行走电机转速，得到制动转速偏差，再对制动转速偏差、 转速偏差增量进行模糊处理和PID处理，输出相应的行走电机制动扭矩的控制 命令。

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

1.本发明提出的前、后车体坐标系转换控制方法精准的将导航设备输出的经 纬度经过车体的几何关系最终转换为装载机前进时的前桥中点车体坐标系，装 载机后退时的后桥中点车体坐标系，可以提高路径跟踪的鲁棒性和精准度；

2.本发明提出的同步转向控制方法，可以实时调整装载机的转向电磁阀 PWM信号和液压电机转速，从而控制装载机沿着目标轨迹进行稳定、精准的路 径跟踪；

3.本发明提出的驱动控制方法，可以控制装载机在任何路况下以一个稳定 的速度进行前进、后退、转向、铲装，在装载机即将到达目标路径终点时可以 控制装载机进行精准稳定的制动。

附图说明

图1是本发明电动装载机路径跟踪系统框架图；

图2是本发明上位机控制系统结构示意图；

图3是本发明前、后车体坐标系控制方法流程图；

图4是本发明转向同步控制方法流程图；

图5是本发明驱动控制方法流程图；

图6是本发明的转向角度计算示意图；

图7是本发明的车体航向角在π1/2到π3/2之间后车体航向角计算示意图；

图8是本发明的车体航向角在π3/2到2π之间后车体航向角计算示意图；

图9是本发明的前车体航向角计算示意图；

图10是本发明的前桥中点全局坐标系和后桥中点全局坐标系转换示意图；

图11是本发明的车体坐标系转换示意图。

具体实施方式

下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明 本发明的技术方案，而不能以此来限制本发明的保护范围。

实施例一：

本实施例提供一种电动装载机路径跟踪系统，如图1所示，包括导航设备、 拉线位移传感器、上位机控制系统、整车控制器、行走电机、左转向电磁阀、 右转向电磁阀和液压电机。导航设备获取的位置信息、拉线位移传感器获取的 的装载机转向角度信息、行走电机的当前转速信息都输出给上位机系统，上位 机系统将控制指令发送给整车控制器，整车控制器根据指令控制左转向电磁阀、 右转向电磁阀、行走电机、液压电机。

电动装载机为中心铰接式结构，前车架和后车架相铰接，可以相对转动。

具体的，上位机控制系统分别与安装在驾驶室顶部的导航设备、安装在前 后车架的拉线位移传感器、整车控制器、行走电机和液压电机相连接，从而获 取导航设备输出的装载机实时位置信息、拉线位移传感器的实时转向角度、行 走电机输出的当前行走电机转速；所述上位机控制系统根据获取的信号进行算 法和逻辑处理，生成控制命令并输出给所述整车控制器。

拉线位移传感器是一个，一端安装在前车架上，另一端安装在后车架上， 中间通过金属丝线连接，左右转向时金属丝线长度会进行变化。

整车控制器分别与所述行走电机、液压电机、左转向电磁阀和右转向电磁 阀相连接，用于接收所述上位机控制系统的控制命令，并生成相应的控制信号 控制行走电机前进或后退、控制左转向电磁阀驱动装载机进行左转向、控制右 转向电磁阀驱动装载机进行右转向、控制液压电机给转向电磁阀输出液压动力。

所述整车控制器通过CAN总线或硬接线与行走电机、液压电机、左转向电 磁阀和右转向电磁阀相连接。CAN总线能够实现控制器与行走电机、液压电机的 信号连接，稳定方便易实现。导航设备安装在驾驶室顶部，为RTK惯性导航设 备。

实施原理：上位机控制系统获取RTK惯性导航设备输出的位置信息、拉线 位移传感器的实时转向角度、行走电机状态信息、液压电机状态信息；上位机 控制系统根据获取的信号进行算法和逻辑处理，将控制命令输出给整车控制器； 整车控制器通过CAN总线或硬接线将相应的控制命令输出给行走电机、液压电 机、左转向电磁阀、右转向电磁阀。上位机控制系统包括前、后车体坐标系控 制方法、转向同步控制方法、驱动控制方法，所述的前、后车体坐标系控制方 法是将导航设备输出的经纬度、航向角以及目标轨迹坐标系结合装载机车体几 何关系经过算法处理最终转换为前桥中点车体坐标系和前车体航向角、后桥中 点车体坐标系和后车体航向角；所述的转向同步控制方法是根据装载机当前位 置与目标轨迹位置的横向偏差和航向角偏差结合模糊算法与PID算法，共同控 制转向电磁阀和液压电机；所述的驱动控制方法为模糊算法与PID算法相结合 根据装载机当前位置和目标轨迹位置的纵向偏差，控制装载机前进或后退或制 动。该电动装载机路径跟踪控制系统及方法可以使电动装载机在无驾驶员的情 况下沿着目标轨迹实时、精准、稳定、安全的行走。

实施例二：

本实施例提供电动装载机路径跟踪控制方法，基于实施例二所述的装置， 电动装载机为中心铰接式结构，前车架和后车架相铰接，可以相对转动，如图1 所示，包括如下步骤：

步骤一、上位机控制系统获取RTK惯性导航设备的位置信息、拉线位移传 感器的实时转向角度、行走电机的状态信号、液压电机的状态信号；

获取目标轨迹信息，主要包括目标轨迹和目标轨迹全局坐标系，目标轨迹 是路径规划系统中得到的，根据起点位置、终点位置、以及沿途障碍物位置， 通过目标轨迹算法生成的目标轨迹，获取的目标轨迹是由N个点组成的曲线或 直线，每个点包含目标轨迹全局坐标系该点的x轴位置y轴位置以及该点的航 向角，根据工作场景可以是直线可以是曲线；其中，目标轨迹算法也可以称作 路径规划算法，主要是将现有的ReadSheep算法和A_Star算法相结合，再进行 优化得到混合A_Star算法；

步骤二、上位机控制系统根据获取的信号进行算法和逻辑处理，将控制命 令输出给整车控制器；具体包括：

根据获取的实时位置信号以及装载机几何结构，经前、后车体坐标系控制 方法，最终转换为前桥中点车体坐标系和前车体航向角、后桥中点车体坐标系 和后车体航向角；

根据获取的装载机实时转向角度以及装载机当前位置与目标轨迹位置的横 向偏差、航向角偏差，经转向同步控制方法，最终控制液压电机和转向电磁阀 进行实时转向；

根据获取的行走电机转速信号以及装载机当前位置与目标轨迹的纵向偏差， 经驱动控制算法，最终控制装载机前进或后退或制动；

步骤三、整车控制器通过CAN总线或硬接线将相应的控制命令输出给行走 电机、液压电机、左转向电磁阀、右转向电磁阀。

具体的，前、后车体坐标系控制方法；如图3所示具体实施方式如下：

步骤一、首先将驾驶室上安装的RTK惯性导航设备输出的经纬度转换为笛 卡尔全局坐标系X、Y；

步骤二、用拉线位移传感器计算装载机的转向角度γ，通过转向角度和导航 设备输出的航向角θ可以得到我们所需要的装载机前车体航向角θ₁和后车体航 向角度θ₂；

(1)拉线位移传感器一端安装在前车架上，另一端安装在后车架上，中间 通过金属丝线连接，左右转向时金属丝线长度会进行变化，如图6所示，AC为 前车架拉线位移传感器距铰接中心的距离；AB为后车架拉线位移传感器距铰接 中心的距离；BC为装载机摆正时拉线位移传感器的长度；DA为装载机右转时 拉线传感器的长度；α为AC与AB的夹角；β为AC与CB的夹角；μ为CB与 AB的夹角；δ为AD与AB的夹角；γ为装载机的转向角度。

δ＝α+γ

装载机的转向角度γ＝δ+μ+β-π

(2)如图7-8所示，θ为导航设备输出的航向角、后车体航向角为θ₂

当π1/2＜θ＜π3/2；

当π3/2＜θ＜2π；

(3)如图9所示，前车体航向角θ₁，γ为转向角度，θ₁＝θ₂-γ。

步骤三、如图10所示，将车体导航设备输出的全局坐标系结合前、后车体 航向角以及导航设备距前、后车体铰接中点的距离，将导航设备输出的全局坐 标系转移到前、后车体铰接中点上，再通过铰接中点到前桥中点的距离、后桥 中点的距离以及前、后车体航向角，将坐标系转移到前桥中点和后桥中点上， 形成前桥中点全局坐标系和后桥中点全局坐标系；

其中，O为前后车体铰接中点，A、B分别为前、后桥中点，P₁、P₂分别为 RTK惯性导航设备的1号天线、2号天线，其中RTK输出的坐标系信息为2号 天线的坐标系，C为天线之间的中点，γ为装载机的转向角度，θ₁、θ₂分别为前 后车架的航向角。

前桥中点A的坐标系为：

后桥中点B的坐标系为：

x_A，y_A为前桥中点坐标系的坐标点，x_B，y_B为后桥中点坐标系的坐标点,

为导航设备2号天线的坐标点。

步骤四、如图11所示，将规划的目标轨迹全局坐标系转移到前桥中点、后 桥中点上，并将转移后的全局坐标系转换为车体坐标系，装载机前进时输出前 桥中点车体坐标系、装载机后退时输出后桥中点车体坐标系。

全局坐标系XOY，航向角θ，车体坐标系X₁OY₁：

车体坐标系转换公式：

/>

则,

图中，x₁,y₁为装载机的车体坐标系坐标，x,y为装载机的全局坐标系坐标。

所述的转向同步控制方法；如图4所示具体实施方式如下：

步骤一、寻找车体当前位置距离目标轨迹的最近点a作为此次轨迹跟踪的 起点，将目标轨迹可视化位N个点组成的路径，从a点到目标轨迹终点b进行 遍历；

步骤二、判断目标轨迹序列点间隔是否小于或者等于装载机前进的预瞄距 离，如果是，那么a+1点为装载机的下一个前进点；如果否，那么b点作为装载 机的下一前进点；装载机在沿着目标轨迹行驶过程中输出纵向预瞄点X_Preview (纵向预瞄点表示装载机前进的或后退的下一个点的距离)、横向预瞄点 Y_Preview(横向预瞄点表示装载机距离目标轨迹的横向偏差)、航向角预瞄点 θ_Preview(航向角预瞄点表示前车体或后车体与目标轨迹航向角的偏差)；

步骤三、对横向预瞄点Y_Preview和航向角预瞄点θ_Preview进行加权处理； 例如横向预瞄点大了，那么就乘0.8输出，横向预瞄点少了就乘0.3输出；航向角 预瞄点也类似的处理，将横向预瞄点处理后的值和航向角预瞄点处理后的值相 加进行输出；

步骤四、将加权后的横向预瞄点与航向角预瞄点相加作为装载机轨迹跟踪 过程中的总偏差Erro；

步骤五、总偏差增量是指单位时间内总偏差的变化量，在程序中实现的， 将总偏差输入给Simulink中的一个功能模块即可得到偏差增量，对Erro和Erro增 量进行模糊化处理，输出比例常数K_p、积分常数K_i、微分常数K_d；

模糊化处理的算法可以采用输入点隶属度模糊算法，模糊处理输出的三个 常数与PID结合起来称为模糊PID，具体实现方式可以采用的是Simulink软件里面 的模糊算法模块，将模糊算法模块的隶属度函数和模糊规则进行配置和调试， 该模块会自动进行模糊化处理，生成kp比例常数，ki积分常数，kd微分常数，这 三个常数作用于PID算法，作为PID算法的输入；

步骤六、结合PID算法得到为保持装载机精准跟踪的目标转向角度；

步骤七、判断目标转向角度是否等于装载机实际转向角度，等于时装载机 不转向，左、右转向电磁阀控制信号为0，液压电机转速为怠速；

步骤八、判断目标转向角度是否小于装载机实际转向角度，小于时装载机 左转向，输出左转向电磁阀PWM控制信号、相应的液压电机转速500rmp/min；

步骤九、判断目标转向角度是否大于装载机实际转向角度，大于时装载机 右转向，输出右转向电磁阀PWM控制信号、相应的液压电机转速500rmp/min；

所述的驱动控制方法；如图5所示具体实施方式如下：

步骤一、判断装载机是否为驱动模式，如果为否，则行走电机无任何扭矩 输出，装载机保持不动；驱动模式是指，上位机系统是否接收到来自其他调度 系统或根据目标轨迹生成的工作指令，比如目标轨迹信息，有工作指令为驱动 模式，无工作指令为静止模式；

步骤二、装载机为驱动模式时，判断装载机是否前进，如果为是，则根据 设定的装载机前进目标驱动转速m和当前行走电机转速p，得到前进转速偏差 Erro₁＝m-p；

步骤三、如果装载机为驱动模式且装载机后退，则根据设定的装载机后退 目标驱动转速n和当前行走电机转速p，得到后退转速偏差Erro₂＝n-p；

步骤四、将装载机驱动的转速偏差和偏差增量进行模糊处理，得到比例常 数K_p、积分常数K_i、微分常数K_d；

步骤五、再对比例常数K_p、积分常数K_i、微分常数K_d进行PID算法处理， 得到行走电机驱动扭矩j；

步骤六、判断装载机距离目标轨迹终点距离是否小于设定的制动距离1.5米， 如果为否，则输出行走电机驱动扭矩j，驱动装载机进行前进或后退；

步骤七、如果装载机距离目标轨迹终点小于设定的制动距离1.5米，则根据 设定的电机制动目标转速0和当前行走电机转速p，得到转速偏差Erro＝0-p，再 对Erro进行PID算法处理，输出装载机制动扭矩i，控制装载机进行稳定制动；

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

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

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

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

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

Claims (9)

1.一种电动装载机路径跟踪系统，其特征在于，包括：

导航设备，用于提供位置信息；

拉线位移传感器，用于提供装载机的实时转向角度；

行走电机，用于驱动装载机前进或后退，并提供当前行走电机转速；

转向机构，用于控制装载机左右转向；

上位机控制系统，用于根据位置信息、实时转向角度和当前行走电机转速生成控制命令；

整车控制器，用于接收所述控制命令，并生成相应的控制信号以通过行走电机控制装载机行走、通过转向机构控制装载机转向；

所述拉线位移传感器一端安装在前车架上，另一端安装在后车架上；

所述系统基于的电动装载机为前后中心铰接式车体，分为可相对转动的前车体和后车体；

所述上位机系统通过安装在装载机上的导航设备获取装载机的经纬度和航向角，再将经纬度转换为笛卡尔坐标系，再结合装载机的导航设备安装位置以及装载机的几何机构关系，转换为装载机驱动桥的前桥中点全局坐标系和后桥中点全局坐标系，再和目标轨迹坐标系相结合，得到车辆当前位置与目标轨迹的基于全局坐标系的横向偏差和纵向偏差，再以车体自身为参考系转为相对于车体的横向偏差、纵向偏差；通过导航设备获取的航向角再结合装载机的铰接点、前后车体几何关系，转换为前车体航向角和后车体航向角，将转换来的前车体航向角和后车体航向角与目标轨迹的航向角相结合得到相对于前车体的航向角偏差和相对于后车体的航向角偏差；当车辆前进时实时调用前桥中点相对于车体的横向偏差、纵向偏差以及相对于前车体的航向角偏差，当车辆后退时实时调用后桥中点相对于车体的横向偏差、纵向偏差以及相对于后车体的航向角偏差，横向误差和航向角误差通过加权处理控制车辆左右转向，纵向误差控制车辆驱动、制动。

2.根据权利要求1所述的电动装载机路径跟踪系统，其特征在于，所述整车控制器通过CAN总线或硬接线与行走电机、转向机构信号连接。

3.根据权利要求1所述的电动装载机路径跟踪系统，其特征在于，所述导航设备安装在驾驶室顶部，为RTK惯性导航设备。

4.一种电动装载机路径跟踪控制方法，其特征在于，包括以下步骤：

获取导航设备的位置信息和目标轨迹信息；

获取装载机的实时转向角度、当前行走电机转速和装载机几何结构信息；

根据获取的导航设备的位置信息、实时转向角度以及装载机几何结构信息，获得车体坐标信息和车体航向角；

根据所述车体航向角与目标轨迹信息生成航向角偏差；

根据所述车体坐标信息与目标轨迹信息生成横向偏差和纵向偏差；

根据实时转向角度、横向偏差、航向角偏差生成用于控制转向机构进行实时转向的控制命令；

根据当前行走电机转速和纵向偏差，生成用于控制装载机行走或制动的控制命令；

所述方法基于的电动装载机为前后中心铰接式车体，分为可相对转动的前车体和后车体；

根据获取的导航设备的位置信息、实时转向角度以及装载机几何结构信息，获得车体坐标信息和车体航向角，包括：

通过安装的装载机上的导航设备获取装载机的经纬度和航向角，再将经纬度转换为笛卡尔坐标系，再结合装载机的导航设备安装位置以及装载机的几何机构关系，转换为装载机驱动桥的前桥中点全局坐标系和后桥中点全局坐标系，通过导航设备获取的航向角再结合装载机的铰接点以及前后车体几何关系，转换为前车体航向角和后车体航向角；

根据所述车体坐标信息与目标轨迹信息生成横向偏差和纵向偏差、航向角偏差，包括：

转换为装载机驱动桥的前桥中点全局坐标系和后桥中点全局坐标系，再和目标轨迹坐标系相结合，得到车辆当前位置与目标轨迹的基于全局坐标系的横向偏差和纵向偏差，再以车体自身为参考系转为相对于车体的横向偏差、纵向偏差；将转换来的前车体航向角和后车体航向角与目标轨迹的航向角相结合得到相对于前车体的航向角偏差和相对于后车体的航向角偏差；当车辆前进时实时调用前桥中点相对于车体的横向偏差、纵向偏差以及相对于前车体的航向角偏差，当车辆后退时实时调用后桥中点相对于车体的横向偏差、纵向偏差以及相对于后车体的航向角偏差，横向误差和航向角误差通过加权处理控制车辆左右转向，纵向误差控制车辆驱动、制动。

5.根据权利要求4所述的电动装载机路径跟踪控制方法，其特征在于，导航设备的位置信息包括导航设备的经纬度和航向角；

所述装载机几何结构信息包括导航设备与车体的位置关系；

获得车体坐标信息和车体航向角的方法的步骤包括：

根据导航设备与车体的位置关系，将导航设备的经纬度转换为车体坐标信息；

根据导航设备与车体的位置关系以及实时转向角度，将导航设备的航向角转换为车体航向角。

6.根据权利要求5所述的电动装载机路径跟踪控制方法，其特征在于，目标轨迹信息包括多个序列点组成的曲线或直线，每个序列点包含该点的坐标信息以及该点的航向角；

生成用于控制转向机构进行实时转向的控制命令的方法包括以下步骤：

根据车体坐标信息寻找当前车体距离目标轨迹的最近序列点作为此次轨迹跟踪的起点，从最近点到目标轨迹的终点进行遍历；

判断目标轨迹序列点间隔是否小于或者等于装载机前进的预瞄距离，如果是，那么将下一个序列点作为装载机的下一个前进点；如果否，那么将目标轨迹终点作为装载机的下一前进点；

向装载机的下一前进点行进过程中生成横向偏差、航向角偏差；对横向偏差和航向角偏差进行加权处理；将加权后的横向偏差与航向角偏差相加，得到车体轨迹跟踪过程中的总偏差和总偏差增量；

对总偏差和总偏差增量进行模糊化处理，获得输入参数；

将输入参数输入给PID算法，得到为保持装载机精准跟踪的目标转向角度；

根据目标转向角度和实时转向角度生成控制命令。

7.根据权利要求6所述的电动装载机路径跟踪控制方法，其特征在于，根据目标转向角度和实时转向角度生成控制命令的方法包括：

判断目标转向角度是否等于实时转向角度；

若等于，则生成装载机不转向的控制命令；

若目标转向角度小于实时转向角度，则生成装载机左转向的控制命令；

若目标转向角度大于实时转向角度，则生成装载机右转向的控制命令。

8.根据权利要求4所述的电动装载机路径跟踪控制方法，其特征在于，生成用于控制装载机行走或制动的控制命令的方法步骤如下：

判断是否获取到目标轨迹信息，如果为否，则生成制动的控制命令；

若获取到目标轨迹信息，则根据设定的装载机前进目标驱动转速和当前行走电机转速，得到转速偏差；

将所述转速偏差和偏差增量进行模糊化处理，得到输入参数；

根据输入参数进行PID算法处理，输出相应的行走电机行走扭矩的控制命令。

9.根据权利要求4所述的电动装载机路径跟踪控制方法，其特征在于，所述路径跟踪控制方法还包括：

判断装载机距离目标轨迹终点距离是否小于设定的制动距离；

如果装载机距离目标轨迹终点大于设定的制动距离，则输出相应的行走电机行走扭矩的控制命令；

如果装载机距离目标轨迹终点小于设定的制动距离，则根据设定的电机制动的目标转速和当前行走电机转速，得到制动转速偏差，再对制动转速偏差、转速偏差增量进行模糊处理和PID处理，输出相应的行走电机制动扭矩的控制命令。

Images