WO2015109975A1 - 一种混凝土泵车和臂架控制方法 - Google Patents

Abstract

一种臂架控制方法及混凝土泵车，控制方法包括实时获取遥控器动作信号、臂架的各节臂当前状态角度信号及臂架旋转角度信号；对臂架末端移动进行规划。该方法还包括预测控制，根据臂架末端的目标移动方向，实时计算臂架末端点的坐标值，如果臂架末端点偏离了目标移动方向，则对臂架末端进行二次规划，该二次规划输出下一个目标点。该方法在臂架末端偏离目标移动方向的情况下，对臂架末端进行二次规划，降低了臂架伸长和缩短反复交替运作的可能性，具有臂架末端移动平稳的优点。

Description

一种混凝土泵车和臂架控制方法

本申请要求于2014年01月26日提交中国专利局、申请号为201410036995.7、发明名称为“一种混凝土泵车和臂架控制方法”的中国专利申请的优先权，其全部内容通过引用结合在本申请中。

技术领域

本发明涉及工程机械领域，特别涉及一种混凝土泵车和臂架控制方法。

背景技术

混凝土泵车的臂架，一般由4节臂或5节臂或6节臂相互铰接而成，混凝土通过臂架输送到目标浇注点。通常的臂架控制方式是由操作人员使用遥控器对各节臂分别进行姿态调整，使臂架的整体从初始姿态运动到目标姿态，这种方式操作较为复杂，并且效率较低。

针对上述控制方式存在的不足，目前提出的臂架智能控制方法，能根据目标点坐标和各节臂的相关参数，通过运动方程和各种矩阵变换求出各个自由度的值，采用逆向求解的结果会出现多组解。对于多自由度的臂架，要使臂架末端移动到目标点，各节臂之间有不同的姿态，这样就需要确定一组较优解作为实际控制策略。而目前确定一个最优解较为复杂，控制效果不佳。

为了解决上述的技术问题，CN201110197146.6专利公开了“一种工程机械以及机械臂的控制方法与控制装置”，该专利技术每次动作采用两个自由度的机械臂，并遵从节臂位移量最小的原则，通过预先定义的“斜率”，根据控制指令确定需要动作的两个节臂。这样虽然简化了算法，但会导致调节的两个节臂运动突变，臂架运动不连续，臂架稳定性差。另外，如果臂架遇到障碍物，会出现无法控制等问题。而且，各节臂是由液压油缸驱动的，臂架在实际运动过程中，液压油缸伸缩量与各节臂角度变化量是非线性函数关系，即使液压油缸小的位移动作，也会导致节臂角度的大幅变化，造成突变及不稳定性问题。

与此同时，对于大惯性的柔性混凝土泵车臂架控制系统而言，因为各节臂伸长油缸进油量控制精度、臂架旋转角控制精度及臂架运动惯性等客观原因，经常会出现臂架末端偏离目标移动方向。如图1所示，在目标移动AB方向，臂架末端移动趋势是缩短的，在规划臂架末端移动时，都是根据遥控器动作信号开度，确定臂架末端移动的步长，P1和P2为臂架末端移动规划的下一个目标点，如果臂架末端偏离了P1点，到达S1点，臂架会根据原规划方案，从S1点向P2点移动。这样就会使得臂架末端从A点至S1点臂架是缩短的，从S1点到P2点臂架是伸长的，从而造成规划方案与目标移动方向的臂架末端移动趋势相反的情况，使得臂架伸长和缩短反复交替运作，导致臂架末端移动不平稳。

发明内容

有鉴于此，本发明提出一种混凝土泵车和臂架控制方法，以减少臂架运动过程中，臂架伸长和缩短反复交替动作，造成臂架末端移动不平稳的问题。

一方面，本发明提供了一种臂架控制方法，包括实时获取遥控器动作信号、臂架的各节臂当前状态角度信号及臂架旋转角度信号，对臂架末端移动进行规划，并且还包括预测控制，所述预测控制包括：

根据所述臂架末端的目标移动方向，实时计算臂架末端点的坐标值，如果所述臂架末端点偏离了所述目标移动方向，则对所述臂架末端进行二次规划，所述二次规划输出下一个目标点。

进一步地，所述预测控制还包括以下步骤：

根据所述目标移动方向，预测臂架伸长区域和/或臂架缩短区域；

如果所述臂架末端点在所述臂架伸长区域，且偏离了所述目标移动方向，则对所述臂架末端进行使臂架伸长的二次规划；

如果所述臂架末端点在所述臂架缩短区域，且偏离了所述目标移动方向，则对所述臂架末端进行使臂架缩短的二次规划。

进一步地，在所述目标移动方向上找到与臂架旋转中心距离最短的垂直点，所述垂直点至所述目标移动方向的区域为所述臂架伸长区域，相对 侧为所述臂架缩短区域。

进一步地，在所述垂直点周围划定一个区域为臂架伸缩转换区域，如果所述臂架末端点在所述臂架伸缩转换区域，则关闭预测控制。

进一步地，对臂架末端移动规划包括：

步骤1：根据臂架当前位置至下一目标位置建立数学模型，并根据各节臂移动量最小和运动平稳性原则，建立目标约束函数；

步骤2：根据所述遥控器动作信号、臂架的各节臂当前状态角度信号及臂架旋转角度信号，计算获得各节臂收展角度和臂架目标旋转角度；

步骤3：判断所述各节臂收展角度是否超出臂架收展角度范围；

步骤4：如果所有节臂收展角度均超出，停止控制臂架动作；如果部分节臂收展角度超出，返回步骤2重新计算。

进一步地，所述目标约束函数包括如下公式：

OBJ₁＝∑w_i·(θ_i-θ_i-1)²

OBJ₂＝∑w_i·((θ_i-θ_i-1)-(θ_i-1-θ_i-2))²

其中，w_i表示i节臂的权重；θ_i表示i节臂目标角度，θ_i-1表示i节臂当前角度，θ_i-2表示i节臂前一步的角度。

进一步地，如果i节臂被操作手锁定，w_i为0。

进一步地，在所述步骤2中：将空间轨迹规划分成臂架旋转和机械臂平面收展分别计算，并分别获得所述各节臂收展角度和臂架目标旋转角度。

进一步地，所述数学模型如下：

其中，θ_min≤θ_i≤θ_max，θ_i为i节臂与i-1节臂的夹角，θ_min表示i节臂最小收展角度，θ_max表示i节臂最大收展角度，θ₀为旋转角度，l_i为各 节臂长度，x_end,y_end,z_end为末端点坐标，N为臂架的节臂数量。

另外，本发明，还提供了一种混凝土泵车，包括臂架，并采用上述任意一种所述的臂架控制方法。

本发明提供的一种混凝土泵车和臂架控制方法，增加了预测控制程序，根据臂架末端的目标移动方向，实时计算臂架末端点的坐标值，如果臂架末端偏离了目标移动方向，则对臂架末端进行二次规划，该二次规划输出下一个目标点。由于在臂架末端偏离目标移动方向的情况下，对臂架末端进行了二次规划，不再根据原规划的方案控制臂架动作，保证臂架末端点始终朝向目标点移动，降低了臂架伸长和缩短反复交替运作的可能性，使臂架末端动作更平稳。

进一步的技术方案中，根据臂架末端的目标移动方向，预测臂架伸长区域和/或臂架缩短区域，作为对臂架运动规划的限制条件。如果臂架末端点在臂架伸长区域，且偏离了目标移动方向，对臂架末端移动进行使臂架伸长的二次规划；如果臂架末端点在臂架缩短区域，且偏离了目标移动方向，对臂架末端移动进行使臂架缩短的二次规划。根据二次规划的下一个目标点进行臂架末端移动规划计算，得到对应的臂架旋转角度和各个节臂的收展角度，使得臂架规划方案与目标移动方向的臂架移动趋势一致，进一步避免了臂架伸长或缩短反复交替动作，臂架移动平稳。

另外，规划臂架运动过程中，根据各节臂移动量最小和运动平稳性原则，建立目标约束函数，通过目标约束函数，确定一个最优解，实现臂架运动的连续性；同时，提高了臂架运动平稳性和精确性，防止臂架运动突变不稳定的情况发生，并起到减少各节臂移动量及节能减排的作用。

为了简化计算，优选将空间轨迹规划分成臂架旋转和机械臂平面收展分别进行计算，并分别获得各节臂收展角度和臂架目标旋转角度，然后判断是否超出臂架移动范围，实现了臂架智能控制的目的。

为了进一步提高臂架运动的连续性和平稳性能，还综合考虑臂架铰接连杆机构关系，融入运动学、动力学关系，调整轨迹算法，进而控制电流为斜坡函数。为了增大臂架作业范围，防止臂架与障碍物相碰，增加了节臂锁定功能，如果节臂被操作手锁定，节臂的权重为0。操作者可根据当前臂架姿态进行任意节臂的锁定实现目标点的轨迹规划，使得操作更加灵 活，扩大了施工作业范围和臂架规划策略通用性。

附图说明

构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1为现有臂架控制方法示意图；

图2为本发明臂架预测控制方法流程示意图；

图3为本发明臂架控制原理示意图；

图4为本发明臂架末端移动规划流程示意图；

图5为本发明臂架末端移动规划计算流程示意图。

具体实施方式

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本发明。

本发明提供的一种臂架控制方法，包括实时获取遥控器动作信号和臂架的各节臂当前状态角度信号及臂架旋转角度信号，对臂架末端移动进行规划，此外还包括预测控制，该预测控制包括：

根据臂架末端的目标移动方向，实时计算臂架末端点的坐标值，如果臂架末端点偏离了目标移动方向，则对臂架末端进行二次规划，该二次规划输出下一个目标点。

需要说明的是，臂架末端点偏离了目标移动方向是指偏离了目标移动方向一定的范围，因此应理解为可以具有一定误差的偏离，即对于误差范围内的偏离目标移动方向，可以不进行二次规划，超过误差范围就需要进行二次规划。根据控制精度，可以设置合适的误差范围。

本发明由于在臂架末端偏离目标移动方向的情况下，对臂架末端进行了二次规划，即从臂架末端的当前点到目标点重新规划，不再根据原规划的方案控制臂架动作，保证臂架末端点始终朝向目标点移动，降低了臂架伸长和缩短反复交替运作的可能性，使臂架末端动作更平稳。

如图2和图3所示，本发明优选的一种臂架控制方法，包括预测控制，预测控制优选包括：根据臂架末端的目标移动方向AB，预测臂架伸长区域和/或臂架缩短区域；实时计算臂架末端点坐标值，如果臂架末端点在臂架伸长区域，且臂架末端点偏离了目标移动方向AB，对臂架末端进行使臂架伸长的二次规划；如果臂架末端点在臂架缩短区域，且臂架末端点偏离了目标移动方向AB，对臂架末端进行使臂架缩短的二次规划，二次规划输出下一个目标点H1、H2。

需要说明的是，二次规划使臂架伸长及二次规划使臂架缩短均应理解为包括臂架保持不变的情形，即在伸长区域，臂架不会改变伸长的趋势，在缩短区域，臂架不会改变缩短的趋势。因而在臂架伸长区域，不会出现臂架缩短的情况，在臂架缩短区域，不会出现臂架伸长的情况，进一步防止了臂架伸长和缩短反复动作。如果臂架末端点在臂架伸缩转换区域上，关闭预测控制。

通过数学计算方法，预测臂架伸长区域和臂架缩短区域及臂架伸缩转换区域；在目标移动方向上找到与臂架旋转中心距离最短的垂直点Q，垂直点Q至目标移动方向的区域为臂架伸长区域QB，相对侧为臂架缩短区域AQ，在垂直点Q周围划定一个区域为臂架伸缩转换区域。一般以垂直点Q为中心，以500MM为半径画圆，作为臂架伸缩转换区域。在臂架伸缩转换区域不进行预测控制，避免因预测控制功能使臂架不能向目标方向移动的问题。

臂架控制原理如图3所示，获取遥控器动作信号、臂架的各节臂当前状态角度信号及臂架旋转角度信号，在目标移动方向AB上，规划臂架末端点每一步移动的下一个目标点P1、P2、P3、P4、P5、P6，一般P1至P2的步长等于P2至P3的步长，P4至P5的步长等于P5至P6的步长。同时也实时采集遥控器动作信号，根据遥控器动作信号实时更新各个目标点。

假设臂架末端点当前位置A向P1点移动，计算得到臂架末端点当前位置A和下一个目标点P1的坐标值，根据臂架当前位置A至下一个目标点P1对臂架末端移动进行规划计算；获得各节臂收展角度和臂架目标旋转角度(收展角度为各节臂架绕其铰接点转动的角度，臂架目标旋转角度为整个臂架装置绕连接臂架的转台需要旋转的角度)。

由于各节臂伸长油缸进油量控制精度、臂架旋转角控制精度及臂架运动惯性等客观原因，臂架末端点偏离目标移动方向AB，臂架末端点到达S1点。此时，需要判断臂架末端点S1在臂架伸长区域，还是在臂架缩短区域或者臂架转换区域。经过判断臂架末端点S1在臂架缩短区域，就需要对臂架末端的下一个目标点H1进行二次规划，不能依据前一次规划的下一个目标点P2进行移动。否则，臂架会伸长缩短交替动作，造成臂架运动不平稳。在对下一个目标点H1规划时，需要考虑使臂架缩短作为二次规划的限制条件。臂架末端点从A点至S1点，臂架是缩短的，臂架末端点从S1点至H1点，臂架也是缩短的，这样使得臂架末端在臂架缩短区域，臂架一直都是缩短的，臂架运动不平稳。

假设臂架末端点从P4向P5点移动，由于各节臂伸长油缸进油量控制精度、臂架旋转角控制精度及臂架运动惯性等客观原因，臂架末端点偏离目标移动方向AB，臂架末端点到达S2点。此时臂架末端点S2在臂架伸长区域，就需要对臂架末端的下一个目标点H2进行二次规划，不能依据前一次规划的下一个目标点P6进行移动。否则，臂架会伸长缩短交替动作，造成臂架运动不平稳。在对下一个目标点H2规划时，需要考虑使臂架伸长作为二次规划的限制条件。臂架末端点从P4点至S2点，臂架是伸长的，臂架末端点从S2点至H2点，臂架也是伸长的，这样使得臂架末端在臂架伸长区域，臂架一直都是伸长的，臂架运动不平稳。

如图4和图5所示，在规划臂架末端移动时，包括：

步骤1：根据臂架当前位置至下一个目标位置建立数学模型，并根据各节臂移动量最小和运动平稳性原则，建立目标约束函数；

步骤2：根据遥控器动作信号、臂架的各节臂当前状态角度信号、臂架当前旋转角度信号，通过计算获得各节臂收展角度和臂架目标旋转角度；

步骤3：判断各节臂收展角度是否超出臂架收展角度范围；

步骤4：如果所有节臂收展角度均超出臂架收展角度范围，停止控制臂架动作；如果部分节臂收展角度超出臂架收展角度范围，返回步骤2重新计算，否则，控制臂架动作。

具体实施如下：

1)建立坐标系

以混凝土泵车转台与臂架始端节臂铰接处作为坐标原点，以车身的料斗方向为X正半轴，以垂直于车身的方向为Y轴正半轴，根据笛卡尔坐标系右手定则，可确定Z轴。本坐标系是臂架智能运动系统基准坐标系，即对臂架设计的运动规划策略都是以该坐标系作为参照的。

2)通过信号采集装置接收获取遥控器动作信号、臂架的各节臂当前状态角度信号、臂架当前旋转角度信号。遥控器动作信号包括操作手柄的方向信号和速度信号，信号采集装置通过无线信号接收装置接收遥控装置动作信号，通过角度传感器获取臂架的各节臂当前状态角度信号和臂架当前旋转角度信号。

3)臂架空间轨迹规划建立数学模型

设计多自由度臂架运动规划策略之前，首先需要建立系统数学模型。根据臂架结构运动学关系可知，在上述建立的坐标系基础上，数学模型为：

其中，θ_min≤θ_i≤θ_max，θ_i为i节臂与i-1节臂的夹角，θ_min表示i节臂最小收展角度，θ_max表示i节臂最大收展角度；θ₀为旋转角度；l_i为各节臂长度；x_end,y_end,z_end为末端点坐标，N为臂架的节臂数量。

将空间轨迹规划分成臂架旋转和机械臂平面收展两个规划分别进行计算，分别获得各节臂收展角度和臂架目标旋转角度。

若只考虑臂架平面运动则上述数学模型简化为：

4)建立目标约束函数时需要考虑：

其一、从当前位置移动到目标位置，臂架移动尽量小，即节能减排；

其二、臂架平稳移动，避免剧烈的速度变化，特别是反向运动。

OBJ₁＝∑w_i·(θ_i-θ_i-1)²

OBJ₂＝∑w_i·((θ_i-θ_i-1)-(θ_i-1-θ_i-2))²

其中，w_i表示i节臂的权重，θ_i表示i节臂目标角度，θ_i-1表示i节臂当前角度，θ_i-2表示i节臂前一步的角度。

如果i节臂被操作手锁定，i节臂的权重w_i为0。

5)建立带约束的优化问题，将带约束的优化问题转化为非约束的优化问题。

下一步加入惩罚因子，将带约束的优化问题转化为非约束的优化问题

因此只要求得g(θ)的解即可。

如图4所示，具体步骤如下：

第一步：设定初始值，将获得的臂架的各节臂当前状态角度信号设定为初值、臂架当前旋转角度信号设定为初值；

第二步：采用一维线性搜索方法计算步长h或者采用负梯度方法；

第三步，如果，ceq_1、2(x)<ε并且θ_imin≤θ_i≤θ_imax，则退出；

第四步，否则跳转第二步。

6)臂架在运动过程中某节臂被锁定；

在运动过程中，某个节臂若因为人为或者结构原因，导致无法移动，需要在规划策略设计中降低一维或多维自由度。具体地分为两种情况：

其一、若臂架旋转无法满足输入命令要求，则停止运动；

其二、若臂架某个节臂被锁定，则将当前节臂姿态记录下来，通过节臂平面运动规划中，将该i节臂的权重w_i为0。

7)通过计算获得各节臂收展角度和臂架目标旋转角度；

8)判断各节臂收展角度是否超出臂架收展角度范围；如果所有节臂收展角度均超出，停止控制臂架动作；如果部分节臂收展角度超出，返回步骤2重新计算，否则，控制臂架动作。

9)在步骤4中：根据各节臂角度与驱动各节臂动作的液压油缸伸缩量函数关系，需要充分考虑结构运动学和动力学，计算得到液压油缸伸缩量，向控制液压油缸的电磁换向阀输入控制电流，控制电流是一个斜坡函数，而不是矩形函数，确保臂架动作连续性和平顺性。

10)如果遥控器停止动作，停止臂架动作；如果遥控器继续动作，返回步骤1。

本发明提供的一种臂架控制方法，根据各节臂移动量最小和运动平稳性原则，建立目标约束函数；通过目标约束函数，确定一个最优解，实现臂架运动的连续性；同时，提高了臂架运动平稳性和精确控制，防止臂架运动突变不稳定的情况发生，减少各节臂移动量，起到节能减排的作用。

另外，为了简化计算，将空间轨迹规划分成臂架旋转和机械臂平面收展分别进行计算，并分别获得各节臂收展角度和臂架目标旋转角度，然后判断是否超出臂架移动范围，防止与障碍物相碰，实现了臂架智能控制的目的。

为了进一步提高臂架运动的连续性和平稳性能，还综合考虑臂架铰接连杆机构关系，融入运动学、动力学关系，调整轨迹算法，控制电流是一个斜坡函数。

为了增大臂架作业范围，防止臂架与障碍物相碰，增加了节臂锁定功能，如果i节臂被操作手锁定，i节臂的权重w_i为0，这样操作者可根据当 前臂架姿态进行任意节臂的锁定，实现目标点的轨迹规划，使得操作更加灵活，扩大了施工作业范围和臂架规划策略通用性。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1. 一种臂架控制方法，包括实时获取遥控器动作信号、臂架的各节臂当前状态角度信号及臂架旋转角度信号，对臂架末端移动进行规划，其特征在于，还包括预测控制，所述预测控制包括：

   根据所述臂架末端的目标移动方向，实时计算臂架末端点的坐标值，如果所述臂架末端点偏离了所述目标移动方向，则对所述臂架末端进行二次规划，所述二次规划输出下一个目标点。
2. 根据权利要求1所述的臂架控制方法，其特征在于，所述预测控制还包括以下步骤：

   根据所述目标移动方向，预测臂架伸长区域和/或臂架缩短区域；

   如果所述臂架末端点在所述臂架伸长区域，且偏离了所述目标移动方向，则对所述臂架末端进行使臂架伸长的二次规划；

   如果所述臂架末端点在所述臂架缩短区域，且偏离了所述目标移动方向，则对所述臂架末端进行使臂架缩短的二次规划。
3. 根据权利要求2所述的臂架控制方法，其特征在于，在所述目标移动方向上找到与臂架旋转中心距离最短的垂直点，所述垂直点至所述目标移动方向的区域为所述臂架伸长区域，相对侧为所述臂架缩短区域。
4. 根据权利要求3所述的臂架控制方法，其特征在于，在所述垂直点周围划定一个区域为臂架伸缩转换区域，如果所述臂架末端点在所述臂架伸缩转换区域，则关闭预测控制。
5. 根据权利要求1-4任一项所述的臂架控制方法，其特征在于，对臂架末端移动规划包括：

   步骤1：根据臂架当前位置至下一目标位置建立数学模型，并根据各节臂移动量最小和运动平稳性原则，建立目标约束函数；

   步骤2：根据所述遥控器动作信号、臂架的各节臂当前状态角度信号及臂架旋转角度信号，计算获得各节臂收展角度和臂架目标旋转角度；

   步骤3：判断所述各节臂收展角度是否超出臂架收展角度范围；

   步骤4：如果所有节臂收展角度均超出，停止控制臂架动作；如果部分节臂收展角度超出，返回步骤2重新计算。
6. 根据权利要求5所述的臂架控制方法，其特征在于，所述目标约束函数包括如下公式：

   OBJ₁＝∑w_i·(θ_i-θ_i-1)²

   OBJ₂＝∑w_i·((θ_i-θ_i-1)-(θ_i-1-θ_i-2))²

   其中，w_i表示i节臂的权重；θ_i表示i节臂目标角度，θ_i-1表示i节臂当前角度，θ_i-2表示i节臂前一步的角度。
7. 根据权利要求6所述的臂架控制方法，其特征在于，如果i节臂被操作手锁定，w_i为0。
8. 根据权利要求5所述的臂架控制方法，其特征在于，在所述步骤2中：将空间轨迹规划分成臂架旋转和机械臂平面收展分别计算，并分别获得所述各节臂收展角度和臂架目标旋转角度。
9. 根据权利要求5所述的臂架控制方法，其特征在于，所述数学模型如下：

   

   

   

   其中，θ_min≤θ_i≤θ_max，θ_i为i节臂与i-1节臂的夹角，θ_min表示i节臂最小收展角度，θ_max表示i节臂最大收展角度，θ₀为旋转角度，l_i为各节臂长度，x_end,y_end,z_end为末端点坐标，N为臂架的节臂数量。
10. 一种混凝土泵车，包括臂架，其特征在于，采用如权利要求1至9所述任意一项所述的臂架控制方法。

Images