CN103806667B - 一种混凝土泵车及臂架控制方法与控制装置 - Google Patents

Abstract

本发明公开了一种臂架控制方法，包括以下几个步骤：步骤1：根据臂架当前位置至下一个目标位置建立数学模型；根据各节臂移动量最小和运动平稳性原则，建立目标约束函数；步骤2：获取遥控器动作信号、臂架的各节臂当前状态角度信号、臂架当前旋转角度信号；步骤3：通过计算获得各节臂移动角度和臂架目标旋转角度；步骤4：判断是否超出臂架移动范围；步骤5：如果超出臂架移动范围，停止控制臂架动作；否则，控制臂架动作。本发明还提供了混凝土泵车和臂架控制装置。本发明臂架运动量最小、平稳性和连续性好、精确度高，作业范围大。

Description

一种混凝土泵车及臂架控制方法与控制装置

技术领域

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

背景技术

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

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

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

发明内容

有鉴于此，本发明提出一种混凝土泵车及臂架控制方法与控制装置。以实现臂架运动量最小和平稳性好，及增大臂架作用范围。

一方面，本发明提供了一种臂架控制方法，包括以下几个步骤：

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

步骤2：获取遥控器动作信号、臂架的各节臂当前状态角度信号、臂架当前旋转角度信号；

步骤3：通过计算获得各节臂移动角度和臂架目标旋转角度；

步骤4：判断各节臂移动角度是否超出臂架移动范围；

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

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

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

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

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

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

进一步地，根据各节臂角度与驱动各节臂动作的液压油缸伸缩量函数关系，计算液压油缸伸缩量，进行控制臂架动作。

进一步地，在步骤3中：将空间轨迹规划分成臂架旋转和机械臂平面动作两个规划分别进行计算，分别获得各节臂移动角度和臂架目标旋转角度。

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

$\underset{i=1}{\overset{N}{\Σ}}cos\left({\mathrm{\θ}}_0\right)l_{i}cos(\underset{j=1}{\overset{i}{\Σ}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π})=x_{end}$

$\underset{i=1}{\overset{N}{\Σ}}{l}_{i}sin(\underset{j=1}{\overset{i}{\Σ}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π})=y_{end}$

$\underset{i=1}{\overset{N}{\Σ}}-sin\left({\mathrm{\θ}}_0\right)l_{i}cos(\underset{j=1}{\overset{i}{\Σ}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π})=z_{end}$

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

进一步地，还包括步骤6，如果遥控器停止动作，停止臂架动作；如果遥控器继续动作，返回步骤1。

另一方面还提供了一种臂架控制装置，包括：遥控装置、信号采集装置、信号预处理装置、计算模块装置、输出装置，信号采集装置包括各节臂角度传感器、转台旋转角度传感器、无线信号接收装置，无线信号接收装置接收遥控装置动作信号；各节臂角度传感器信号和转台旋转角度传感器信号及遥控装置动作信号经过信号预处理装置处理后，通过计算模块装置获得各节臂移动角度，再通过输出装置控制臂架动作。

进一步地，遥控装置包括节臂锁定装置。

另一方面还提供了一种混凝土泵车，包括上述的臂架控制装置

本发明提供的一种混凝土泵车及臂架控制方法与控制装置，根据各节臂移动量最小和运动平稳性原则，建立目标约束函数；通过目标约束函数，确定一个最优解，实现臂架运动的连续性；同时，提高了臂架运动平稳性和精确控制，防止臂架运动突变不稳定的情况发生。减少各节臂移动量，节能减排的作用。另外，为了简化计算，将空间轨迹规划分成臂架旋转和机械臂平面动作两个规划分别进行计算，分别获得各节臂移动角度和臂架目标旋转角度，然后判断是否超出臂架移动范围，实现了臂架智能控制的目的。为了进一步提高臂架运动的连续性和平稳性能，还综合考虑臂架铰接连杆机构关系，融入运动学、动力学关系，调整轨迹算法；控制电流为斜坡函数。为了增大臂架作业范围，防止臂架与障碍物相碰，增加了节臂锁定功能，如果i节臂被操作手锁定，i节臂的权重w_i为0。这样操作者可根据当前臂架姿态进行任意节臂的锁定实现目标点的轨迹规划，使得操作更加灵活，扩大施工作业范围和臂架规划策略通用性。

附图说明

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

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

图2为本发明臂架平面运动规划计算流程示意图；

图3为本发明臂架装置结构示意图。

具体实施方式

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

如图1和图2所示，本发明优选的一种臂架控制方法，包括以下几个步骤：

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

步骤2：获取遥控器动作信号、臂架的各节臂当前状态角度信号、臂架当前旋转角度信号；

步骤3：通过计算获得各节臂移动角度和臂架目标旋转角度；

步骤4：判断各节臂移动角度是否超出臂架移动范围；

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

具体实施如下：

1)建立坐标系

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

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

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

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

$\underset{i=1}{\overset{N}{\Σ}}cos\left({\mathrm{\θ}}_0\right)l_{i}cos(\underset{j=1}{\overset{i}{\Σ}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π})=x_{end}$

$\underset{i=1}{\overset{N}{\Σ}}{l}_{i}sin(\underset{j=1}{\overset{i}{\Σ}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π})=y_{end}$

$\underset{i=1}{\overset{N}{\Σ}}-sin\left({\mathrm{\θ}}_0\right)l_{i}cos(\underset{j=1}{\overset{i}{\Σ}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π})=z_{end}$

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

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

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

$\underset{i=1}{\overset{N}{\Σ}}{l}_{i}cos(\underset{j=1}{\overset{i}{\Σ}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π})=x_{end}\⇔{\mathrm{ceq}}_1\left(x\right)=\underset{i=1}{\overset{N}{\Σ}}{l}_{i}cos(\underset{j=1}{\overset{i}{\Σ}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π})-x_{end}=0$

$\underset{i=1}{\overset{N}{\Σ}}{l}_i\sin (\underset{j=1}{\overset{i}{\Σ}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π})=y_{end}\⇔{\mathrm{ceq}}_2\left(x\right)=\underset{i=1}{\overset{N}{\Σ}}{l}_i\sin (\underset{j=1}{\overset{i}{\Σ}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π})-x_{end}=0$

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

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

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

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

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

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

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

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

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

$g\left(\mathrm{\θ}\right)=\left(\begin{array}{cc}\left(\begin{array}{c}\sqrt{w}\·\left({\mathrm{\θ}}_i-{\mathrm{\θ}}_{i-1}\right)\\ \sqrt{w}\·\left(\left({\mathrm{\θ}}_i-x_{i-1}\right)-\left({\mathrm{\θ}}_{i-1}-{\mathrm{\θ}}_{i-2}\right)\right)\end{array}\right)& i=\mathrm{1...}N\\ \left(\sqrt{\mathrm{\σ}}{\mathrm{ceq}}_i\left(\mathrm{\θ}\right)\right)& i=1,2\\ \left(\begin{array}{c}\sqrt{\mathrm{\σ}}\min \left(0,{\mathrm{\θ}}_i-{\mathrm{\θ}}_{i\min }\right)\\ \sqrt{\mathrm{\σ}}\min \left(0,{\mathrm{\θ}}_{\max }-\mathrm{\θ}\right)\end{array}\right)& i=\mathrm{1...}N\end{array}\right)$

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

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

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

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

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

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

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

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

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

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

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

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

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

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

本发明提供的一种臂架控制方法，根据各节臂移动量最小和运动平稳性原则，建立目标约束函数；通过目标约束函数，确定一个最优解，实现臂架运动的连续性；同时，提高了臂架运动平稳性和精确控制，防止臂架运动突变不稳定的情况发生。减少各节臂移动量，节能减排的作用。另外，为了简化计算，将空间轨迹规划分成臂架旋转和机械臂平面动作两个规划分别进行计算，分别获得各节臂移动角度和臂架目标旋转角度，然后判断是否超出臂架移动范围，防止与障碍物相碰，实现了臂架智能控制的目的。为了进一步提高臂架运动的连续性和平稳性能，还综合考虑臂架铰接连杆机构关系，融入运动学、动力学关系，调整轨迹算法；控制电流为一个斜坡函数。为了增大臂架作业范围，防止臂架与障碍物相碰，增加了节臂锁定功能，如果i节臂被操作手锁定，i节臂的权重w_i为0，这样操作者可根据当前臂架姿态进行任意节臂的锁定实现目标点的轨迹规划，使得操作更加灵活，扩大施工作业范围和臂架规划策略通用性。

如图3所示，本发明还提供了一种臂架控制装置，包括：遥控装置、信号采集装置、信号预处理装置、计算模块装置、输出装置，信号采集装置包括各节臂角度传感器、转台旋转角度传感器、无线信号接收装置，无线信号接收装置接收遥控装置动作方向和速度信号；各节臂角度传感器信号和转台旋转角度传感器信号及遥控装置动作信号经过信号预处理装置处理后，计算模块装置根据上述的臂架控制方法程序获得各节臂移动角度，再通过输出装置控制臂架动作。在遥控装置上设置有设定节臂锁定装置。用于操作手设定节臂锁定。本发明还提供了一种混凝土泵车，包括上述的臂架控制装置。

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

Claims (9)

1.一种臂架控制方法，其特征在于，包括以下几个步骤：

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

步骤2：获取遥控器动作信号、臂架的各节臂当前状态角度信号、臂架当前旋转角度信号；

步骤3：通过计算获得各节臂移动角度和臂架目标旋转角度；

步骤4：判断各节臂移动角度是否超出臂架移动范围；

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

2.根据权利要求1所述的臂架控制方法，其特征在于，目标约束函数包括如下：

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

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

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

3.根据权利要求2所述的臂架控制方法，其特征在于，如果第i节臂被操作手锁定，i节臂的权重w_i为0。

4.根据权利要求1所述的臂架控制方法，其特征在于，在步骤5中：根据各节臂角度与驱动各节臂动作的液压油缸伸缩量函数关系，计算液压油缸伸缩量，进行控制臂架动作。

5.根据权利要求1所述的臂架控制方法，其特征在于，数学模型如下：

$\underset{i=1}{\overset{N}{\&Sigma;}}cos\left({\mathrm{\&theta;}}_0\right)l_{i}cos(\underset{j=1}{\overset{i}{\&Sigma;}}{\mathrm{\&theta;}}_j-(i-1)\&CenterDot;\mathrm{\&pi;})=x_{end}$

$\underset{i=1}{\overset{N}{\&Sigma;}}{l}_{i}sin(\underset{j=1}{\overset{i}{\&Sigma;}}{\mathrm{\&theta;}}_j-(i-1)\&CenterDot;\mathrm{\&pi;})=y_{end}$

$\underset{i=1}{\overset{N}{\&Sigma;}}-sin\left({\mathrm{\&theta;}}_0\right)l_{i}cos(\underset{j=1}{\overset{i}{\&Sigma;}}{\mathrm{\&theta;}}_j-(i-1)\&CenterDot;\mathrm{\&pi;})=z_{end}$

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

6.根据权利要求1至5任意一项所述的臂架控制方法，其特征在于，还包括步骤6，如果遥控器停止动作，停止臂架动作；如果遥控器继续动作，返回步骤1。

7.一种臂架控制装置，其特征在于，包括：遥控装置、信号采集装置、信号预处理装置、计算模块装置、输出装置，信号采集装置包括各节臂角度传感器、转台旋转角度传感器、无线信号接收装置，无线信号接收装置接收遥控装置动作信号；各节臂角度传感器信号和转台旋转角度传感器信号及遥控装置动作信号经过信号预处理装置处理后，通过计算模块装置获得各节臂移动角度，再通过输出装置控制臂架动作。

8.根据权利要求7所述的臂架控制装置，其特征在于，遥控装置包括节臂锁定装置。

9.一种混凝土泵车，其特征在于，包括如权利要求7或8所述的臂架控制装置。