CN109469328A - 臂架末端位置控制方法及系统及包含该系统的工程机械 - Google Patents

Abstract

本发明实施例提供一种臂架末端位置控制方法、系统及工程机械，该方法包括：接收用于操控臂架末端位置的操控机构所指示的方向及幅度；接收臂架末端的实际速度及位置；根据所述方向及幅度以及所述臂架末端的实际速度及实际位置，确定所述臂架末端的目标位置；根据所述臂架末端的目标位置，反解出一组目标关节位置；计算一组目标关节速度；控制所述臂架的驱动机构，以使得所述臂架的所述一个或多个关节满足各自的目标关节位置及目标关节速度。通过该方案，可使得操作人员直接控制臂架末端位置，而无需经由对臂架每一关节进行单独操控而控制臂架末端位置。

Description

臂架末端位置控制方法及系统及包含该系统的工程机械

技术领域

本发明涉及工程机械领域，具体地涉及臂架末端位置控制方法、臂架末端位置控制系统及包含该臂架末端位置控制系统的工程机械。

背景技术

工程机械行业内的很多工程机械涉及到对臂架的控制，该臂架一般包含一个或多个关节，而目前对多关节臂架操控的常规方法是通过遥控器分别控制每个关节动作，使得臂架末端逐渐接近目标位置。

随着关节数量增多，操作过程繁琐，对操作人员熟练程度和经验水平要求也较高。研究出一种可使得操作人员可直接控制臂架末端位置的控制方法和系统具有重要意义。

发明内容

本发明实施例的目的是提供一种臂架末端位置控制方法、臂架末端位置控制系统及包含该臂架末端位置控制系统的工程机械，其可使得操作人员直接控制臂架末端位置，而无需经由对臂架每一关节进行单独操控而控制臂架末端位置。

为了实现上述目的，本发明实施例提供一种臂架末端位置控制方法，该臂架包含一个或多个关节，该方法包括：接收用于操控所述臂架末端位置的操控机构所指示的方向及幅度；接收所述臂架末端的实际速度及实际位置；根据所述方向及幅度以及所述臂架末端的实际速度及实际位置，确定所述臂架末端的目标位置；以及根据所述臂架末端的目标位置，反解出一组目标关节位置，该组目标关节位置包含所述臂架末端运行至所述目标位置时，所述臂架的所述一个或多个关节各自的目标关节位置；针对所述臂架的所述一个或多个关节中的每一关节，执行以下操作以得出一组目标关节速度，其中该组目标关节速度包含所述臂架末端运行至所述目标位置时，所述臂架的所述一个或多个关节各自的目标关节速度：确定该关节的实际关节位置及实际关节速度，以作为该关节的关节空间轨迹的起点的关节位置及关节速度；根据该关节的实际关节位置及实际关节速度以及所述目标关节位置，确定所述臂架末端运行至所述目标关节位置时的目标关节速度，并将该目标关节速度及所述目标关节位置作为所述关节空间轨迹的终点的关节位置及关节速度；控制所述臂架的驱动机构，以使得所述臂架的所述一个或多个关节满足各自的目标关节位置及目标关节速度。

可选的，所述确定所述臂架末端运行至所述目标位置时的目标关节速度包括：根据所述关节的实际关节位置以及目标关节位置，确定所述关节在当前轨迹规划周期内的期望平均运动速度；以及根据该期望平均运动速度及所述关节在当前轨迹规划周期开始时的实际关节速度，确定所述关节的所述目标关节速度。

可选的，所述目标关节速度被确定为：

w_end＝2w_avg-w_init，

w_avg＝(q_end–q_init)/T1,

其中，w_end为所述目标关节速度，w_avg为所述关节在当前轨迹规划周期内的期望平均运动速度，w_init为所述关节在当前轨迹规划周期开始时的实际关节速度，q_end及q_init分别所述关节的目标关节位置以及实际关节位置，T1为所述当前轨迹规划周期的长度。

可选的，该方法还包括在所述关节空间轨迹的所述起点与所述终点之间插补M-1个关节空间轨迹点，每一轨迹点涉及该轨迹点的关节位置及关节速度，其中所述M为大于或等于1的整数。

可选的，所述控制所述臂架的驱动机构包括：根据所述关节的关节空间轨迹的所述插补点以及所述终点，生成M组指令，其中每组指令与一轨迹点相对应，该组指令包括所述驱动机构的行程及速度以及控制流经该驱动机构的液压油的流量及方向的比例阀的阀控量；从所述M组指令中选取与当前指令更新周期相对应的指令，并根据该指令控制所述比例阀。

可选的，所述阀控量在通过在预先标定的驱动机构速度与阀控量之间的对应关系表内查找与所述驱动机构速度相对应的阀控量而被确定的。

可选的，所述比例阀带有CAN通信接口，该比例阀可通过CAN通信接口来接收针对该比例阀的控制操作。

可选的，根据所述方向及幅度以及所述臂架末端的实际速度及实际位置确定所述臂架末端的目标位置包括：根据所述方向及幅度以及所述臂架末端的实际速度及实际位置、以及加速度上限约束和/或速度上限约束，确定所述臂架末端在所述加速度上限约束和/或速度上限约束下的期望运动速度；根据所述操控机构指示所述方向及幅度的持续时间，对所述期望运动速度进行积分，得出所述臂架末端的位移增量；以及根据所述位移增量及所述臂架末端的所述实际位置，确定所述臂架末端的目标位置。

可选的，所述加速度上限约束和/或速度上限约束根据以下一者或多者而被确定：臂架应用场景的工作需求、臂架参数、驱动所述臂架的液压系统的流量情况、所述臂架接近工作空间边缘的程度、以及臂架关节接近极限位置的程度。

相应地，本发明另一实施例还提供一种臂架末端位置控制系统，该臂架包含一个或多个关节，该系统包括：操纵机构，用于在用户的操纵下输出方向及幅度；检测机构，用于检测所述臂架末端的实际速度及实际位置；驱动机构，用于驱动所述臂架运动；控制器，被配置为用于执行以下操作：接收用于操控所述臂架末端位置的操控机构所指示的方向及幅度；接收所述臂架末端的实际速度及实际位置；根据所述方向及幅度以及所述臂架末端的实际速度及实际位置，确定所述臂架末端的目标位置；以及根据所述臂架末端的目标位置，反解出一组目标关节位置，该组目标关节位置包含所述臂架末端运行至所述目标位置时，所述臂架的所述一个或多个关节各自的目标关节位置；针对所述臂架的所述一个或多个关节中的每一关节，执行以下操作以得出一组目标关节速度，其中该组目标关节速度包含所述臂架末端运行至所述目标位置时，所述臂架的所述一个或多个关节各自的目标关节速度：确定该关节的实际关节位置及实际关节速度，以作为该关节的关节空间轨迹的起点的关节位置及关节速度；及根据该关节的实际关节位置及实际关节速度以及所述目标关节位置，确定所述臂架末端运行至所述目标关节位置时的目标关节速度，并将该目标关节速度及所述目标关节位置作为所述关节空间轨迹的终点的关节位置及关节速度；控制所述臂架的驱动机构，以使得所述臂架的所述一个或多个关节满足各自的目标关节位置及目标关节速度。

可选的，所述确定所述臂架末端运行至所述目标位置时的目标关节速度包括：根据所述关节的实际关节位置以及目标关节位置，确定所述关节在当前轨迹规划周期内的期望平均运动速度；以及根据该期望平均运动速度及所述关节在当前轨迹规划周期开始时的实际关节速度，确定所述关节的所述目标关节速度。

可选的，所述目标关节速度被确定为：

w_end＝2w_avg-w_init，

w_avg＝(q_end–q_init)/T1,

其中，w_end为所述目标关节速度，w_avg为所述关节在当前轨迹规划周期内的期望平均运动速度，w_init为所述关节在当前轨迹规划周期开始时的实际关节速度，q_end及q_init分别所述关节的目标关节位置以及实际关节位置，T1为所述当前轨迹规划周期的长度。

可选的，所述控制器还用于在所述关节空间轨迹的所述起点与所述终点之间插补M-1个关节空间轨迹点，每一轨迹点涉及该轨迹点的关节位置及关节速度，其中所述M为大于或等于1的整数。

可选的，所述控制所述臂架的驱动机构包括：根据所述关节的关节空间轨迹的所述插补点以及所述终点，生成M组指令，其中每组指令与一轨迹点相对应，该组指令包括所述驱动机构的行程及速度以及控制流经该驱动机构的液压油的流量及方向的比例阀的阀控量；从所述M组指令中选取与当前指令更新周期相对应的指令，并根据该指令控制所述比例阀。

可选的，所述阀控量在通过在预先标定的驱动机构速度与阀控量之间的对应关系表内查找与所述驱动机构速度相对应的阀控量而被确定的。

可选的，所述比例阀带有CAN通信接口，该比例阀可通过CAN通信接口来接收针对该比例阀的控制操作。

可选的，根据所述方向及幅度以及所述臂架末端的实际速度及实际位置确定所述臂架末端的目标位置包括：根据所述方向及幅度以及所述臂架末端的实际速度及实际位置、以及加速度上限约束和/或速度上限约束，确定所述臂架末端在所述加速度上限约束和/或速度上限约束下的期望运动速度；根据所述操控机构指示所述方向及幅度的持续时间，对所述期望运动速度进行积分，得出所述臂架末端的位移增量；以及根据所述位移增量及所述臂架末端的所述实际位置，确定所述臂架末端的目标位置。

可选的，所述加速度上限约束和/或速度上限约束根据以下一者或多者而被确定：臂架应用场景的工作需求、臂架参数、驱动所述臂架的液压系统的流量情况、所述臂架接近工作空间边缘的程度、以及臂架关节接近极限位置的程度。

相应地，本发明另一实施例还提供一种工程机械，该工程机械包含上述臂架末端位置控制系统。

相应地，本发明另一实施例还提供一种机器可读存储介质，该机器可读存储介质上存储有指令，该指令用于使得机器执行上述臂架末端位置控制方法。

通过上述技术方案，操作人员可直接控制臂架末端位置，而无需经由对臂架每一关节进行单独操控而控制臂架末端位置，减小了劳动强度和操作复杂性，也降低了对操作人员的经验水平要求。另外，臂架末端直接控制能避免单关节控制时反复的位置试凑，能更快地将臂架末端运动到预期位置，有利于提高工作效率。

本发明实施例的其它特征和优点将在随后的具体实施方式部分予以详细说明。

附图说明

附图是用来提供对本发明实施例的进一步理解，并且构成说明书的一部分，与下面的具体实施方式一起用于解释本发明实施例，但并不构成对本发明实施例的限制。在附图中：

图1为本发明实施例提供的臂架末端位置控制系统的结构示意图；

图2为臂架末端的实际位置及实际速度的确定方法流程图；

图3为本发明实施例提供的臂架末端位置控制系统的具体结构示意图；

图4A及4B为本发明实施例提供的臂架末端位置控制方法的流程图；

图5为指令队列的示意图；

图6为在加速度/速度上限约束下的期望臂架末端运动速度；

图7A及7B为比例阀的控制指令的生成过程示意图；

图8为本发明一实施例提供的臂架关节控制的流程图；

图9为本发明另一实施例提供的臂架关节控制的流程图；

图10为臂架关节速度与阀控量的关系示意图；以及

图11为根据指令控制比例阀的控制流程图。

具体实施方式

以下结合附图对本发明实施例的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本发明实施例，并不用于限制本发明实施例。

图1为本发明实施例提供的臂架末端位置控制系统的结构示意图。如图1所示，本发明一实施例提供一种臂架末端位置控制系统，该系统包括：操纵机构，用于在用户的操纵下输出方向及幅度；检测机构，用于检测所述臂架末端的实际速度及实际位置；驱动机构，用于驱动所述臂架运动；控制器，被配置为用于执行以下操作：接收用于操控所述臂架末端位置的操控机构所指示的方向及幅度；接收所述臂架末端的实际速度及实际位置；根据所述方向及幅度及其施加时长以及所述臂架末端的实际速度，确定所述臂架末端的目标位置；以及控制所述驱动机构，以使得所述臂架末端运动至所述目标位置。

其中，所述操纵机构可为遥控器，该控器上安装了摇杆，操作人员可通过以不同的幅度和方向拨动摇杆产生臂架末端运动指令，臂架末端运动的方向与摇杆被拨动的方向有关，臂架末端运动速度的大小与摇杆被拨动的幅度有关；设置摇杆的数量至少可以支持操作人员设置期望的臂架末端沿X，Y，Z轴三个维度中的至少两个维度运动的方向和速度指令。所述控制器可配备有遥控接收器，遥控接收器通过无线通信接收遥控器发出的遥控信息号。所述操纵机构还可为其他用于操控臂架动作的指令输入设备，诸如机械操控杆、触摸屏幕上的模拟操控杆等等。

所述检测机构可为直接测所述臂架末端的实际速度及实际位置的传感器，也可为检测所述臂架各关节的关节变量(诸如，关节位置、关节速度等)的传感器，之后可根据该关节变量来推导出臂架末端的实际位置及实际速度。如图2所示，可根据安装于各个关节处的传感器读数，获取实际关节位置，这可通过各传感器采样值经过处理和运算得到。之后，通过实际关节变量可以推导算出臂架末端相对臂架基座的位置，将一个笛卡尔空间坐标系原点附着在臂架基座合适位置，则臂架末端的位置可以表示为在该坐标系下的X，Y，Z轴坐标。另外，通过对实际关节位置进行微分运算，可得出实际关节速度，之后根据通过综合考虑各个关节的实际关节位置及实际关节速度，推算出臂架末端的实际速度。

另外，需要说明的是，本公开内所提及的“检测机构”或者“传感器”对于位置和/或速度的检测既可通过实际的传感器硬件(其专用于检测位置和/或速度)来实现，亦可通过根据所检测的信息进行换算而得出所需的信息，从而实现对位置和/或速度的检测。例如，可通过对位置信息进行时间微分处理而得出相应的速度信息，亦可通过对速度信息进行时间积分处理而得出相应的位置信息，这些均应处于本案所欲保护的技术方案的保护范围之内。

所述驱动机构可包含用于驱动臂架各个关节的油缸设备和/或用于驱动臂架转台的液压马达，所述油缸设备可包括可通过油缸伸缩改变铰接的臂架结构之间的夹角的油缸设备，还可包括可通过油缸伸缩改变臂架结构的长短的油缸设备。与该驱动机构相连的比例阀可控制该驱动机构的速度(例如，转动速度、伸缩速度)及行程(例如，油缸运动行程或液压马达转动角度)。

所述控制器可为执行所有计算、控制操作的设备，该控制器可以是通用处理器、专用处理器、常规处理器、数字信号处理器(DSP，Digital Signal Processing)、多个微处理器、与DSP核心关联的一个或多个微处理器、控制器、微控制器、专用集成电路(ASIC，Application Specific Integrated Circuit)、现场可编程门阵列(FPGA，Field-Programmable Gate Array)电路、其他任何类型的集成电路(IC，Integrated Circuit)、状态机等等。以下将对该控制器所执行的操作进行详细描述。

图3为本发明实施例提供的臂架末端位置控制系统的具体结构示意图。如图3所示，臂架末端位置控制系统的构成和连接关系如图所示，其中工控机、遥控接收器和带有CAN接口的电液比例阀通过CAN总线与PLC连接；遥控器和遥控接收器通过无线通信连接；电液比例阀组通过液压管路与油缸和液压马达连接，工作时通过油缸或液压马达驱动臂架运动。

系统各组成部分简介及主要功能如下：

(1)、PLC，接收工控机下发的油缸长度、转台角度、油缸速度、液压马达速度、各比例阀阀口开度参考值等指令；接收各传感器输出的信号，并通过这些信号计算得到臂架当前的状态信息；根据上述指令和状态信息计算得到当前各比例阀的控制指令，控制指令包括阀口开度和方向指令，并将控制指令通过CAN总线发送给比例阀组。

(2)、工控机，用于接收CAN总线上的遥控指令CAN报文和传感器输出的位移、角度、速度等信息；也可通过工控机自身的人机接口接收操作人员的操控指令；根据传感器信息和臂架参数并根据正运动学算法计算得到臂架末端的当前位置；运行逆运动学算法和轨迹规划算法；通过CAN总线下发油缸长度、转台角度、油缸速度、液压马达速度、各比例阀阀口开度参考值等指令。

(3)、遥控器，遥控器上安装了摇杆，操作人员可通过以不同的幅度和方向拨动摇杆产生臂架末端运动指令，末端运动的方向与摇杆被拨动的方向有关，末端运动速度的大小与摇杆被拨动的幅度有关；设置摇杆的数量至少可以支持操作人员设置期望的臂架末端沿X，Y，Z轴三个维度中的至少两个维度运动的方向和速度指令。

(4)、遥控接收器，遥控接收器通过无线通信接收遥控器发出的遥控信息号，并将这些遥控信息通过CAN通信转发给PLC。

(5)、传感器，传感器包括位移传感器、角度传感器和角度编码器等测量位移和角度的传感器，传感器的输出信息用于计算臂架的位置和姿态，传感器通过CAN总线或输出电压/电流信号给PLC传递传感器输出信息。

(6)、比例阀组，该比例阀组由多个带CAN通信接口的电液比例阀组成，可以通过CAN通信控制每个比例阀的液压流量和方向。电液比例阀出油口通过液压管路连接到各油缸设备和各液压马达设备。与常规采用模拟量或电气信号直接控制的电液比例阀相比，使用CAN总线的比例阀能减少连接线束以及连接器数量，抗干扰能力也更强，具有更高的可靠性。

(7)、液压马达设备，液压马达为转台转动和/或油缸设备提供驱动力。

(8)、油缸设备，部分油缸设备通过油缸伸缩改变铰接的臂架结构之间的夹角，其他油缸设备通过油缸伸缩改变臂架结构的长短。

需要说明的是，在该图3所示的臂架末端位置控制系统的具体结构示意图，其所示出的结构仅为示例性的结构，并不对本申请的方案造成限制，例如工控机及PLC可统一为由图1所示的控制器来实施。

图4A及4B为本发明实施例提供的臂架末端位置控制方法的流程图。图4A示出了臂架末端位置控制方法所涉及的步骤，图4B示出了臂架末端位置控制方法所涉及的参量及其计算过程。

本发明所提供的臂架末端控制系统的程序架构可包含执行周期一长一短的两种循环，下文中将周期较长的循环称为轨迹规划循环(或者轨迹规划周期)，将周期较短的循环称为指令更新循环(或者指令更新周期)。轨迹规划循环的执行周期为T_l，其可规划出臂架的运行轨迹，该运行轨迹内包含数个轨迹点，该臂架轨迹规划循环的执行周期即为控制臂架运行完整个规划出的运行轨迹所需的时间。指令更新循环执行周期为T_s，T_l是T_s的M倍，该M与所述运行轨迹内包含的轨迹点的数量有关(例如，该指令的执行主要是使得臂架从所述臂架运行轨迹内的一轨迹点运行至下一轨迹点，该指令更新循环执行周期即为控制臂架从一轨迹点运行至下一轨迹点所需的时间，当然也可以说是该M决定了在轨迹规划时应规划出多少个轨迹点)，也可以与CAN总线的信号发送频率有关(例如，CAN总线每100ms发送一次信息，则指令更新信号执行周期可为100ms，如果该周期对于臂架的运动过短，那么相邻两个指令所指示的内容可能是相同的)，本领域技术人员可根据需要来进行设定。关于长短周期，长周期可被分为M个短周期，在每个短周期内发出一组指令，短周期的号为队列编号，依据该队列编号对应不同的短周期。长周期的长短由设计者给定，由于CAN总线每100ms发送一次信息，则以100ms为短周期长短，长周期大于该值，最好是整数倍。

需要说明的是，虽然本文中的是以轨迹规划周期均包含M个短周期为例进行说明的，但在实际操作过程中，每个轨迹规划周期不一定要长度相等，即M是可变，而且每一指令更新周期也可未必长度相同。

每个轨迹规划循环会基于遥控器指令和当前臂架状态进行一次轨迹规划，产生M个能使臂架运动状态趋向遥控器指令的关节空间轨迹点(每个轨迹点包含若干个关节的关节位置及关节速度)，并计算出与每个轨迹点对应的一组指令，这组指令包括驱动机构行程(例如，油缸行程、转台角度)、驱动机构速度(例如，油缸速度、转台转速)以及比例阀的阀控量。

由于每次轨迹规划会产生M个关节空间轨迹点，所以每个轨迹规划循环也会计算得到M组指令，这些指令组成一个指令队列。每个短循环周期则会从轨迹规划循环中计算生成的指令队列中依次取出与当前周期对应的那组指令，并通过CAN通信将指令发送给PLC。

轨迹规划循环程序的流程图如图4A所示，指令队列示意图如图5所示。指令更新循环会从指令队列中取出当前周期对应的一组指令，基于这组指令得到控制环的参考指令。具体如何根据该指令来控制比例阀，将在以下参考图8进行说明。

以下按照图4A所示的步骤对整个流程进行说明。

1、实际臂架关节变量计算

为了计算臂架末端位置，必须先得到当前时刻各个臂架关节变量的实际值，这可通过各传感器采样值经过处理和运算得到。

2、实际臂架末端位置计算

通过实际关节变量可以推导算出臂架末端相对臂架基座的位置，将一个笛卡尔空间坐标系原点附着在臂架基座合适位置，则臂架末端的位置可以表示为在该坐标系下的X，Y，Z轴坐标。

3、目标臂架末端位置计算

可以将当前末端实际位置叠加期望的位移增量作为目标末端位置。

每个轨迹规划循环开始时，会获取遥控器摇杆拨动的方向和幅值，通过方向和幅值计算出操作人员期望的末端沿沿X，Y，Z轴三个维度运动的方向和速度。传统方案中计算位移增量时通常简单采用如下方法：位移增量＝遥控器速度指令×轨迹规划执行周期。这种方案的缺点有：在遥控器速度指令变化较大时容易导致臂架运动不平稳；在先后连续执行不同的规划轨迹时，也无法流畅地衔接；在臂架运动到可工作空间边缘或者关节位置接近极限位置时，不能对运动速度和加速度做出限制。

本方案使用经过加速度上限约束和速度上限约束后的期望末端速度的积分来计算位移增量，并最终基于这一增量来计算规划轨迹终点的末端目标XYZ轴位置。

首先，确定期望臂架末端速度，其计算过程如图6所示，其中点划线表示的V_cmd为用户通过摇杆设置的速度指令，图中，在[t₀,t₀+2T_l)时间段内，V_cmd为V₁；在[t₀+2T_l,t₀+3T_l)时间段内，V_cmd为V₂。在t₀和t₀+2T_l时刻，遥控指令V_cmd分别有一次跳变，但用于轨迹规划算法的速度会受到加速度上限和速度上限的限制。

图6中，V_lmt即为被加速度和速度上限约束后的期望臂架末端速度。

时间段[t₀,t₀+2T_l)体现了加速度上限的限制，图中AB段和BC段的斜率代表了当前的加速度上限，受此加速度上限限制，[t₀,t₀+2T_l)时间段的期望末端速度在AC段按加速度上限斜率上升，在CD段的C点才增长到遥控器设置的速度V₁，C点后的期望臂架末端速度才等于遥控器设置的速度。

时间段[t₀+2T_l,t₀+3T_l)除了体现了加速度上限的限制作用，还体现了速度上限的限制作用。虽然在该时间段靠后部分，如仅考虑加速度限制时，臂架末端运动速度允许达到遥控指令速度V₂，但由于V₂的数值超过当前允许的最大速度V_max，期望末端速度会限制在最大允许速度V_max，而不会增长到V₂。

加速度上限和速度上限的设置可以基于如下因素的一点或多点：臂架应用场景的工作需求、臂架参数、液压系统流量情况、臂架接近工作空间边缘的程度、臂架关节接近极限位置的程度等。

可以通过对当前轨迹规划周期内受限后的期望速度V_lmt进行积分得到对应的某一轴上的位移增量。计算公式如下：

这一方法的优点在于，当遥控器指令有很大突变时，末端预期速度会按允许的最大加速度上升(或下降)，防止臂架运动速度出现较大变化导致臂架震荡或者出现液压系统流量不足。同时也能在接近工作空间边缘或关节接近极限位置时限制关节运动速度，能更好的保护臂架并避免出现异常。

需要说明的是，虽然上述参照图6同时介绍了加速度和速度上限约束，但也可单独应用其中的一者。

分别对X，Y，Z轴的受限期望末端速度进行积分可得到末端在X，Y，Z轴三个维度上的位移增量。臂架末端在X，Y，Z轴上的目标位置即可通过臂架末端当前实际位置叠加对应的位移增量得到。

4、轨迹规划终点关节位置确定

确定好轨迹规划的末端目标X，Y，Z轴位置后，即可通过计算反解出一组目标关节位置组合，当臂架的各关节运动到这组关节位置组合时，能保证臂架末端处在目标位置。

若存在多组关节位置组合均能使末端达到指定位置，则要设定优化目标，并使用优化方法找出最优的一组关节位置组合。反解和优选出的这组关节位置组合即为轨迹规划终点关节位置。

5、轨迹规划终点关节速度的确定

轨迹规划时，需要确定本次轨迹规划循环结束时的关节速度。常规方法是通过臂架末端速度来反解出关节速度，这种方法计算量极大，往往不能满足实时轨迹规划的实时性要求。

本方案基于合理的假设和近似，给出了一种简单而有效的轨迹终点关节速度确定方法。

定义某关节在当前轨迹规划周期内的期望平均运动速度为w_avg＝(q_end–q_init)/T1。其中q_init为轨迹规划周期开始时测算得到的该关节的实际位置，q_end为估计规划周期结束时期望的关节位置，注意q_end已在上述第4部分计算得到，为已知量，故可以计算得w_avg。

设轨迹规划终点的关节速度为w_end。假设关节速度在规划周期内以恒定加速度线性变化，这样的假设和近似在规划周期的周期不太长时是合理的，由此可得w_end的计算表达式为w_end＝2w_avg-w_init。其中，w_init为规划周期开始时测算出的关节运动速度的实际值。

通过此方式来确定轨迹规划终点关节速度，而不需要从臂架末端运动速度反解出关节运动速度，计算量大大减小，因而可以缩短采样并响应遥控器指令的周期，从而提高对遥控器指令的响应速度，提升操控体验。

6、关节空间轨迹插补

前述步骤计算出了关节空间轨迹终点的目标关节位置和速度，为了实现更加平滑的臂架运动，还需要在轨迹的起点和终点间插补M-1个关节空间轨迹点，其中所述M为大于或等于1的整数。本方案关节空间轨迹插补采用三阶多项式插补算法，设在规划时间段内，某关节变量与时间的关系满足如下三阶多项式：

q(t)＝a₀+a₁t+a₂t²+a₃t³

为了使臂架关节运动到规划轨迹的终点时，关节速度和位置满足之前步骤(上述第4部分和上述第5部分)给出的期望目标，多项式需要满足如下约束条件：

其中，q_init和w_init为该关节的实际位置和实际速度，q_end和w_end为上述第4部分和第5部分分别得到的期望关节位置和期望关节速度。

根据约束条件，可以求解出三阶多项式的系数如下：

得到系数后，取t＝iTs,(i＝1,2,3,…,M-1)，即可通过三阶多项式插补出该关节在轨迹规划周期内的处起点与终点之外的所有轨迹点对应的关节变量取值。

7、指令队列计算

通过轨迹插值，一共得到了M个关节空间轨迹点(不包含起点)及其相关联的关节位置及速度，且每一轨迹点的确定均涉及臂架的各个关节的位置及速度的确定。每个轨迹点的关节位置和速度已经得到，但是因为臂架系统是液压油缸和液压马达驱动的，关节位置不能直接用于对驱动机构进行控制，还需要进一步计算出图5所示的指令队列里的每一组指令。计算过程如图7A及7B所示。

转台角度、油缸行程计算

对于油缸驱动的关节，仅知道关节的角度和位置还不能控制关节的运动，还需要计算得到与关节位置对应的油缸行程。每个轨迹点的关节变量确定好后，即可使用几何方法和其他数学方法唯一确定一组与之对应的油缸行程。对于用于驱动转台转动的液压马达也是如此。

转台转动速度、油缸运动速度计算

转台的转动速度可以通过相邻两个轨迹点转台的角度增量除以周期Ts得到。

对于油缸驱动的关节，油缸在t0+iTs时刻的运动速度v(i)可由该时刻到t0+(i+1)Ts时刻的油缸行程增量除以周期Ts得到，即：v(i)＝(L(i+1)-L(i))/Ts，其中L(i)和L(i+1)分别是t0+iTs时刻和t0+(i+1)Ts时刻的期望油缸行程。

阀控量计算

阀控量决定了电液比例阀阀口开度和出油口出油方向，阀控量与油缸运动速度和液压马达转速具有相关关系。通过预先对驱动机构速度(例如，油缸运动速度或液压马达转速)与阀控量的对应关系进行试验标定，经数据处理后制作成查找表，通过查表和插值来计算与驱动机构运动速度对应的阀控量。通过这一阀控量来控制比例阀，即能使油缸或转台的运动速度接近预期的速度。

经过前述步骤，已得到完整的指令队列。每次指令更新循环都从指令队列里取出当前时刻对应的那组指令，将这组指令作为PLC中控制环算法的参考指令，进行闭环控制。指令更新循环在工控机中运行时，每个循环周期通过CAN通信发送一组指令给PLC。另外，指令更新循环也可以在PLC中运行时，过程如下：在工控机计算得到完整的指令队列后，将队列中的全部指令成批次下发给PLC，PLC程序在每个指令更新循环更新一次指令。

可针对每个驱动机构(例如，油缸或液压马达)配置一个双闭环控制器，用来控制对应油缸或液压马达的运动。多关节臂架可能会有多个油缸和多个液压马达，对应会有多个双闭环控制器。当然，该一个或多个双闭环控制器所执行的操作亦可完全由图1所示的控制器来执行。

图8为本发明一实施例提供的臂架关节控制的流程图。如图8所述，其示出了操控设备、轨迹规划单元、参考指令计算单元、位置控制器、速度控制器、阀控量汇总计算单元、位置反馈计算单元以及速度反馈计算单元。

其中，操控设备用于供用户给出控制臂架移动的指令，在传统单关节控制时，指令通常是期望的关节运动的方向和速度；在采用臂架末端直接控制时，指令通常是期望的臂架末端运动的速度和方向。

所述轨迹规划单元用于根据用户给出的操控指令以及臂架各关节当前的位置，合理规划各关节的运动轨迹，计算给出一系列关节位置和关节速度的目标值。该单元可能包含正向运动学、逆向运动学、关节空间轨迹插值等算法。

所述参考指令计算单元对应于上述指令队列计算部分的“转台角度、油缸行程计算”和“转台转动速度、油缸运动速度计算”，其可将轨迹规划单元给出的关节位置和关节速度目标值换算为便于液压机构控制算法实施的位置参考指令和速度参考指令。例如：对于液压油缸驱动改变角度的臂架关节，将轨迹规划单元给出的关节位置目标值换算为油缸行程参考指令；关节速度目标值相应会换算为油缸移动速度参考指令。

所述位置控制器可根据臂架关节位置参考指令与实际关节位置之间的位置误差计算出对应控制量，该控制量有助于使位置误差趋向零。位置控制器可以是PID控制器或基于PID控制器的改进控制器。

所述速度控制器可根据臂架关节速度参考指令与实际关节速度之间的速度误差计算出对应控制量，该控制量有助于使速度误差趋向零。速度控制器可以是PID控制器或基于PID控制器的改进控制器。

所述阀控量汇总计算单元用于将位置控制器和速度控制器输出的比例阀控制量汇总计算输出最终的比例阀阀控量，该阀控量用于最终控制比例阀出油口方向和开度。该汇总操作例如可对上述比例阀控制量做加和计算。

所述位置反馈计算单元用于对位移传感器输出的位移数据进行滤波、变换等处理，提供位置反馈信号给位置闭环和轨迹规划单元。所述速度反馈计算单元用于对速度传感器输出的位移数据进行滤波、变换等处理，提供速度反馈信号给速度闭环。该速度反馈计算单元也可不用速度传感器，而是通过对来源于位置传感器的位置信号经计算(例如，通过微分计算)得到速度反馈信号，而且优选为通过对来源于位置传感器的位置信号经微分计算得到速度反馈信号，这样可使得速度更能够准确地反映出位置变化情况，提高整体的控制准确性。

所述速度控制器根据臂架关节速度参考指令与实际关节速度之间的速度误差计算出对应控制量的过程中所采用的“臂架关节速度参考指令”内含目标关节速度，该目标关节速度是根据所述臂架关节在当前控制周期开始时的实际关节位置及实际关节速度以及所述目标关节位置而被确定的。该“当前控制周期”可指代上述“轨迹规划周期”，亦可指代上述“指令更新周期”，还可为针对该臂架关节控制过程设定的一专门执行周期。在所述“当前控制周期”指代上述“轨迹规划周期”的情况下，对于“目标关节速度”的确定，可参考上述轨迹规划周期中确定轨迹规划终点关节速度的过程，即，其考虑了所述臂架关节在当前轨迹规划周期开始时的实际关节位置及实际关节速度以及所述目标关节位置；在所述“当前控制周期”指代上述“指令更新周期”的情况下，对于“目标关节速度”的确定，可参考上述轨迹规划周期中确定轨迹规划终点关节速度的过程及后续的关节空间轨迹插补过程。

具体而言，根据所述臂架关节在当前控制周期开始时的实际关节位置、实际关节速度以及所述目标关节位置确定所述目标关节速度包括：根据所述臂架关节在当前控制周期开始时的实际关节位置以及目标关节位置，确定所述关节在当前控制周期内的期望平均运动速度；以及根据该期望平均运动速度及所述臂架关节在当前控制周期开始时的实际关节速度，确定所述关节的所述目标关节速度。

可选的，所述目标关节速度被确定为：

w_end＝2w_avg-w_init，

w_avg＝(q_end–q_init)/T1,

其中，w_end为所述目标关节速度，w_avg为所述臂架关节在当前控制周期内的期望平均运动速度，w_init为所述臂架关节在当前控制周期开始时的实际关节速度，q_end及q_init分别所述臂架关节的目标关节位置以及实际关节位置，T1为所述当前控制周期的长度。

上述“轨迹规划单元”、“参考指令计算单元”、“位置控制器”、“速度控制器”、“阀控量汇总计算单元”、“位置反馈计算单元”以及“速度反馈计算单元”均可作为图1所示的控制器内的功能模块来实现。

现有技术的方案一般将速度环作为位置、速度双闭环的内环，将位置外环的控制器的输出作为速度内环的参考指令。本方案中速度闭环与位置闭环是并联关系，速度闭环的参考指令源于轨迹规划单元(即，根据所述臂架关节在当前控制周期开始时的实际关节位置及实际关节速度以及所述目标关节位置而确定的目标关节速度)，而不是位置闭环，该目标关节速度的确定考虑到了实际关节位置及实际关节速度以及所述目标关节位置。这有助于获得更快的位置跟踪响应速度，能预先“感知”到速度变化的趋势，因而具有更快的响应速度，能更加快速将臂架各关节移动到目标关节位置及目标关节速度。

图9为本发明另一实施例提供的臂架关节控制的流程图。如图10所示，其与图8所示的实施例的不同之处在于，其还增设了一速度前馈计算单元，该速度前馈计算单元可对应与上述指令队列计算部分的“阀控量计算”部分，且也可作为图2所示的控制器内的功能模块来实现。阀控量汇总计算单元可将位置控制器、速度控制器和速度前馈计算单元的输出的比例阀控制量汇总计算输出最终的比例阀阀控量，该阀控量用于最终控制比例阀出油口方向和开度。该汇总操作例如可对上述比例阀控制量做加和计算。为了进一步提高响应速度，除了速度闭环与位置闭环并联，本方案还采用了速度前馈控制。速度前馈计算单元计算并给出这一前馈量，本方案中前馈量计算的结果是根据速度查表得到的与速度对应的比例阀阀控量。

针对臂架中的每个关节，为了制作查找表，需要实测并记录该关节在不同运动速度下对应比例阀的阀控量，正负全范围速度均应测试，速度取点的间隔应合理选择，且在速度正负方向都要实测速度接近0时的阀控量，便于进行比例阀的死区补偿，之后将所测数据制作成查找表。图10为某臂架关节速度与阀控量的关系示意图，横坐标是关节速度(SpeedRef)，纵坐标是阀控量(ValveCtlRef)。为了具有一般性，图中数据经过归一化处理，实际使用时应根据实际参数和实测数据制作查找表。图中的点代表测了数据的速度点及对应的实测阀控量，图中曲线由全部相邻两点的连线组成。查表时，若待查表的速度刚好是测过的点，则直接将该点对应的实测阀控量作为前馈量计算单元的输出结果，若表中没有待查速度，则先判断待查速度落入表中哪个速度区间段，取该区间段首末两点间线性插值的结果作为前馈量计算单元的输出结果。

线性插值计算公式为dout＝(vin-vl)/(vr-vl)*(dr-dl)+dl，其中，dout是插值后计算得出的阀控量，vin是待查表的速度输入，vin落入了(vl,vr)这一速度区间，vl是速度区间左端点的速度，dl是与vl对应的阀控量；vr是速度区间右端点的速度，dr是与vr对应的阀控量。

图10中的局部放大图给出了速度大小接近0时的实测阀控量，注意接近0时，阀控量并不为零，这是为了克服比例阀的死区特性。为避免速度接近0时查出比死区补偿量小的阀控量，对表中速度接近0的两个(正负方向各一个)实测点，将其速度大小调整为尽可能小的值，方向不变。设速度为正时，调整为vmin，速度为负时，调整为-vmin。查表计算前对速度进行检查，如速度大小小于vmin，则正速度输出vmin对应的阀控量，负速度输出-vmin对应的阀控量。

对比普通方案的前馈量计算方法，使用本方案的速度前馈方法有助于补偿比例阀的死区，使臂架运动更稳定。

图11为根据指令控制比例阀的控制流程图。该图11与图9基本上是等效的。如图11所示，油缸行程指令、油缸速度指令和比例阀阀控量指令取自上位机生成的指令队列里的M组指令中的一组，且取该组指令中与所控制油缸对应的3条指令(即，上述油缸行程指令、油缸速度指令和比例阀阀控量)。每个短周期指令更新一次，闭环算法则以更快的频率计算，每个短周期会多次计算得到控制比例阀的控制指令。计算过程如下：其包含两个闭环的控制，即油缸行程控制及油缸速度控制，确保通过该两个闭环控制来实现对比例阀的阀控量的控制，进而使得油缸可达到油缸行程指令及油缸速度指令的要求。

本发明的上述关于臂架关节控制的技术方案采用位置闭环控制、速度闭环控制并联，同时结合速度前馈控制这种复合控制结构，其中速度前馈控制基于预先试验标定好的查找表(该查找表可带有死区补偿功能)，通过查找期望速度对应的阀控量并将阀控量前馈至计算最终阀控量计算单元(即框图中的阀控量汇总计算单元)。对比外环位置环、内环速度环方案，本方案能直接跟踪源于轨迹规划和参考指令计算单元给出的速度指令，能预先“感知”到速度变化的趋势，因而具有更快的响应速度，能更加快速将臂架各关节移动到目标位置。另一方面，对比理论方法计算得到速度前馈量，本方案基于实验实测数据制作的查找表，不存在理论方法中的建模误差、近似误差和非线性系统线性化误差，能更精确的反映实际物理量之间的关系，更具有工程实用性，而且查找表能更精确有效的补偿比例阀的死区特性，使臂架运动更加平稳。

本发明的方案可应用于各类包含多关节臂架的工程机械，诸如混凝土湿喷机、混凝土泵车等等。其可具有以下优点：

1、可以使多关节臂架操作人员简单灵活的直接控制臂架末端移动到预期位置，不需要分别操纵每个关节移动，减小了劳动强度和操作复杂性，也降低了对操作人员的经验水平要求。

2、臂架末端直接控制能避免单关节控制时反复的位置试凑，能更快地将臂架末端移动到预期位置，有利于提高工作效率。

3、使用CAN总线比例阀组，能减少线束和连接器数量，提高可靠性；使用数字量控制比例阀，具有更强的抗干扰能力。

4、本方案不需要从臂架末端运动速度反解出关节运动速度，计算量大大减小，因而可以缩短采样并响应遥控器指令的周期，从而提高对遥控器指令的响应速度，提升操控体验。

5、本发明所给出的轨迹规划方案通过运用加速度上限和/或速度上限约束，可使得臂架在运动过程中能更平稳的过渡和衔接，提升臂架移动的平滑性。

以上结合附图详细描述了本发明实施例的可选实施方式，但是，本发明实施例并不限于上述实施方式中的具体细节，在本发明实施例的技术构思范围内，可以对本发明实施例的技术方案进行多种简单变型，这些简单变型均属于本发明实施例的保护范围。

另外需要说明的是，在上述具体实施方式中所描述的各个具体技术特征，在不矛盾的情况下，可以通过任何合适的方式进行组合。为了避免不必要的重复，本发明实施例对各种可能的组合方式不再另行说明。

本领域技术人员可以理解实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序存储在一个存储介质中，包括若干指令用以使得单片机、芯片或处理器(processor)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、运动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

此外，本发明实施例的各种不同的实施方式之间也可以进行任意组合，只要其不违背本发明实施例的思想，其同样应当视为本发明实施例所公开的内容。

Claims (14)

1.一种臂架末端位置控制方法，该臂架包含一个或多个关节，该方法包括：

接收用于操控所述臂架末端位置的操控机构所指示的方向及幅度；

接收所述臂架末端的实际速度及实际位置；

根据所述方向及幅度以及所述臂架末端的实际速度及实际位置，确定所述臂架末端的目标位置；以及

根据所述臂架末端的目标位置，反解出一组目标关节位置，该组目标关节位置包含所述臂架末端运行至所述目标位置时，所述臂架的所述一个或多个关节各自的目标关节位置；

针对所述臂架的所述一个或多个关节中的每一关节，执行以下操作以得出一组目标关节速度，其中该组目标关节速度包含所述臂架末端运行至所述目标位置时，所述臂架的所述一个或多个关节各自的目标关节速度：

确定该关节的实际关节位置及实际关节速度，以作为该关节的关节空间轨迹的起点的关节位置及关节速度；

根据该关节的实际关节位置及实际关节速度以及所述目标关节位置，确定所述臂架末端运行至所述目标关节位置时的目标关节速度，并将该目标关节速度及所述目标关节位置作为所述关节空间轨迹的终点的关节位置及关节速度；

控制所述臂架的驱动机构，以使得所述臂架的所述一个或多个关节满足各自的目标关节位置及目标关节速度。

2.根据权利要求1所述的臂架末端位置控制方法，其特征在于，所述确定所述臂架末端运行至所述目标位置时的目标关节速度包括：

根据所述关节的实际关节位置以及目标关节位置，确定所述关节在当前轨迹规划周期内的期望平均运动速度；以及

根据该期望平均运动速度及所述关节在当前轨迹规划周期开始时的实际关节速度，确定所述关节的所述目标关节速度。

3.根据权利要求2所述的臂架末端位置控制方法，其特征在于，所述目标关节速度被确定为：

w_end＝2w_avg-w_init，

w_avg＝(q_end–q_init)/T1,

其中，w_end为所述目标关节速度，w_avg为所述关节在当前轨迹规划周期内的期望平均运动速度，w_init为所述关节在当前轨迹规划周期开始时的实际关节速度，q_end及q_init分别所述关节的目标关节位置以及实际关节位置，T1为所述当前轨迹规划周期的长度。

4.根据权利要求1所述的臂架末端位置控制方法，其特征在于，该方法还包括在所述关节空间轨迹的所述起点与所述终点之间插补M-1个关节空间轨迹点，每一轨迹点涉及该轨迹点的关节位置及关节速度，其中所述M为大于或等于1的整数。

5.根据权利要求4所述的臂架末端位置控制方法，其特征在于，所述控制所述臂架的驱动机构包括：

根据所述关节的关节空间轨迹的所述插补点以及所述终点，生成M组指令，其中每组指令与一轨迹点相对应，该组指令包括所述驱动机构的行程及速度以及控制流经该驱动机构的液压油的流量及方向的比例阀的阀控量；

从所述M组指令中选取与当前指令更新周期相对应的指令，并根据该指令控制所述比例阀。

6.根据权利要求1所述的臂架末端位置控制方法，其特征在于，根据所述方向及幅度以及所述臂架末端的实际速度及实际位置确定所述臂架末端的目标位置包括：

根据所述方向及幅度以及所述臂架末端的实际速度及实际位置、以及加速度上限约束和/或速度上限约束，确定所述臂架末端在所述加速度上限约束和/或速度上限约束下的期望运动速度；

根据所述操控机构指示所述方向及幅度的持续时间，对所述期望运动速度进行积分，得出所述臂架末端的位移增量；以及

根据所述位移增量及所述臂架末端的所述实际位置，确定所述臂架末端的目标位置。

7.一种臂架末端位置控制系统，该臂架包含一个或多个关节，其特征在于，该系统包括：

操纵机构，用于在用户的操纵下输出方向及幅度；

检测机构，用于检测所述臂架末端的实际速度及实际位置；

驱动机构，用于驱动所述臂架运动；

控制器，被配置为用于执行以下操作：

接收用于操控所述臂架末端位置的操控机构所指示的方向及幅度；

接收所述臂架末端的实际速度及实际位置；

根据所述方向及幅度以及所述臂架末端的实际速度及实际位置，确定所述臂架末端的目标位置；以及

根据所述臂架末端的目标位置，反解出一组目标关节位置，该组目标关节位置包含所述臂架末端运行至所述目标位置时，所述臂架的所述一个或多个关节各自的目标关节位置；

针对所述臂架的所述一个或多个关节中的每一关节，执行以下操作以得出一组目标关节速度，其中该组目标关节速度包含所述臂架末端运行至所述目标位置时，所述臂架的所述一个或多个关节各自的目标关节速度：

确定该关节的实际关节位置及实际关节速度，以作为该关节的关节空间轨迹的起点的关节位置及关节速度；及

根据该关节的实际关节位置及实际关节速度以及所述目标关节位置，确定所述臂架末端运行至所述目标关节位置时的目标关节速度，并将该目标关节速度及所述目标关节位置作为所述关节空间轨迹的终点的关节位置及关节速度；

控制所述臂架的驱动机构，以使得所述臂架的所述一个或多个关节满足各自的目标关节位置及目标关节速度。

8.根据权利要求7所述的臂架末端位置控制系统，其特征在于，所述确定所述臂架末端运行至所述目标位置时的目标关节速度包括：

根据所述关节的实际关节位置以及目标关节位置，确定所述关节在当前轨迹规划周期内的期望平均运动速度；以及

根据该期望平均运动速度及所述关节在当前轨迹规划周期开始时的实际关节速度，确定所述关节的所述目标关节速度。

9.根据权利要求8所述的臂架末端位置控制系统，其特征在于，所述目标关节速度被确定为：

w_end＝2w_avg-w_init，

w_avg＝(q_end–q_init)/T1,

其中，w_end为所述目标关节速度，w_avg为所述关节在当前轨迹规划周期内的期望平均运动速度，w_init为所述关节在当前轨迹规划周期开始时的实际关节速度，q_end及q_init分别所述关节的目标关节位置以及实际关节位置，T1为所述当前轨迹规划周期的长度。

10.根据权利要求7所述的臂架末端位置控制系统，其特征在于，所述控制器还用于在所述关节空间轨迹的所述起点与所述终点之间插补M-1个关节空间轨迹点，每一轨迹点涉及该轨迹点的关节位置及关节速度，其中所述M为大于或等于1的整数。

11.根据权利要求10所述的臂架末端位置控制系统，其特征在于，所述控制所述臂架的驱动机构包括：

根据所述关节的关节空间轨迹的所述插补点以及所述终点，生成M组指令，其中每组指令与一轨迹点相对应，该组指令包括所述驱动机构的行程及速度以及控制流经该驱动机构的液压油的流量及方向的比例阀的阀控量；

从所述M组指令中选取与当前指令更新周期相对应的指令，并根据该指令控制所述比例阀。

12.根据权利要求7所述的臂架末端位置控制系统，其特征在于，根据所述方向及幅度以及所述臂架末端的实际速度及实际位置确定所述臂架末端的目标位置包括：

根据所述方向及幅度以及所述臂架末端的实际速度及实际位置、以及加速度上限约束和/或速度上限约束，确定所述臂架末端在所述加速度上限约束和/或速度上限约束下的期望运动速度；

根据所述操控机构指示所述方向及幅度的持续时间，对所述期望运动速度进行积分，得出所述臂架末端的位移增量；以及

根据所述位移增量及所述臂架末端的所述实际位置，确定所述臂架末端的目标位置。

13.一种工程机械，其特征在于，该工程机械包含根据权利要求7-12中任一项权利要求所述的臂架末端位置控制系统。

14.一种机器可读存储介质，其特征在于，该机器可读存储介质上存储有指令，该指令用于使得机器执行权利要求1-6中任一项所述的臂架末端位置控制方法。