一种臂架轨迹规划的控制方法及泵车与流程

1.本发明属于泵车臂架技术领域，具体涉及到一种臂架轨迹规划的控制方法及泵车。


背景技术：

2.泵车作为混凝土的输送设备，在如今的建筑施工中经常被用到。泵车输送混凝土的过程主要由泵送系统、臂架系统完成。泵送系统将混凝土注入臂架管道中，臂架系统负责支撑管道，将混凝土输送至指定的施工目标位置。
3.泵车工作环境较为复杂，由于泵车臂架关节多长度较长，布料施工时多节臂架协调操作难度较大，操作工如果不熟练，会影响布料的平稳性、连续性及效率，给出料口施工人员带来不便。现有的臂架通常有6节，还有一个回转台，控制器上需要设置7个摇杆来分别进行控制，在操作过程中容易出现臂架反复移动、幅度大、振动大，而导致浇筑点振动大的问题。
4.泵车：一种能把混凝土输送到具体施工点的机械设备。臂架：泵车上可架设输送管，输送混凝土的一种可折叠的机械结构。倾角传感器：一种检测臂架角度、加速度和角速度的传感器。
5.中国发明(申请号cn202110110018.7)公开了一种混凝土泵车臂架控制系统、方法及混凝土泵车，其中，臂架包括多节臂，相邻臂在同一竖直面内可转动地连接，臂架控制系统包括：多个变幅驱动部件，变幅驱动部件设在相邻两节臂之间；多个倾角检测部件，一一对应地安装于多节臂的始端，被配置为检测所在臂振动时始端在三个正交方向上的加速度和角速度；回转角度检测部件，被配置为检测臂架在水平面内的回转角度；和控制器，被配置为根据多个倾角检测部件和回转角度检测部件的检测值得出多节臂的姿态，但是没有公开如何通过控制器控制各臂架运动以及控制臂架端点轨迹的方法。
6.中国发明(申请号cn202010183877.4)公开了泵车臂架随动控制系统，其特征在于，包括控制器、操作装置、数字多路阀、倾角传感器和回转编码器；操作装置设置在布料管出料口；倾角传感器与回转编码器用于实时获取臂架的位姿，并将获取到的位姿信号传输至控制器；控制器与倾角传感器、回转编码器之间采用can总线实现信号传输；数字多路阀与控制器之间采用can总线实现信号传输与控制；操作装置通过can总线与控制器连接；操作装置为带有万向摇杆的随动装置，万向摇杆可在水平面内的任意方向做往复运动。本发明还公开了一种泵车臂架随动控制方法。虽然通过控制系统简化了对多节臂架的控制，但是仍然需要人为控制臂架的整体方向和运动速度，对操作工要求较高，且多级臂架需要同时工作，对控制器的要求较高、影响系统响应速度和精度。
7.现有泵车产品，利用传感器建立臂架末端坐标。预设固定轨迹路线，工作时沿着简单的设定路线移动。针对现有技术缺点如下：(1)轨迹路线粗放单一；(2)不能随意预设，稍复杂的轨迹不能设定，适应性不强；(3)轨迹运动实施时偏差较大，不能做到精准控制。
8.如何提高泵车臂架浇筑点的移动精度、简化臂架的操控过程、减小臂架振动是需
要解决的问题。


技术实现要素：

9.本发明的目的是针对现有技术存在的问题，提供一种臂架轨迹规划的控制方法及泵车，该方案移动精度高、振动小、控制方便的优点，可有效的使臂架在有限空间内平稳工作、精准布料。
10.为实现上述目的，本发明采用的技术方案是：一种臂架轨迹规划的控制方法，包括如下步骤：
11.s1：控制器获取臂架出料口的运动轨迹，控制器连接的数据处理模块将运动轨迹分散成若干个离散点；
12.s2：通过倾角传感器测试出臂架的各级支臂之间的夹角，所述数据处理模块根据各个夹角计算出出料口的位置坐标，将出料口的位置坐标与离散点坐标进行对比，选定下一离散点坐标；
13.s3：所述数据处理模块根据预设的运动学反解算法，计算出各级支臂的转动角度，由控制器向动力机构输出；
14.s4：所述控制器发出指令，各级支臂分别按照控制器输出的结果转动，出料口依次通过离散点，进行浇筑。
15.上述方法中，步骤s1中，根据需要向控制器输入轨迹点坐标，数据处理模块将轨迹点坐标分散成若干个离散点，离散点越密集则出料口的轨迹越准确；步骤s2，通过倾角传感器得到各级支臂之间的夹角，数据处理模块计算出出料口的位置坐标，根据现有坐标位置选定下一离散点，确认运动方向；步骤s3中，由于各级支臂的长度一定，起始点坐标位置一定，离散点和现有位置坐标已知，通过运动学反解算法可以求出各级支臂需要转动的角度，由控制器向动力机构输出；步骤s4中，通过操作指令摇杆控制臂架进行前进，各级支臂分别按照控制器输出的结果转动，使出料口移动到下一离散点，过程中出料口沿轨迹进行浇筑，也可同时操作回转摇杆使臂架进行左右动作。轨迹点需包含路径中的转折点。
16.进一步的，步骤s1中，所述控制器先设为普通工作档位，将一级支臂移动到位后，所述控制器对一级支臂进行锁定，所述控制器由普通工作档位切换为等高轨迹或垂直起降运动档位。
17.在某些不需要全部支臂进行运动的情况下，首先可将一级支臂移动后进行锁定，减少活动支臂的数量，同理也可将1-3级支臂一起锁定，减少数据处理模块的计算量，减少移动的支臂数量提高臂架运行稳定性，选定档位后，通过指令摇杆、回转摇杆来对控制器进行输入，等高轨迹档位下指令摇杆为一级支臂的摇杆，垂直起降运动档位下指令摇杆为二级支臂的摇杆。
18.进一步的，步骤s1中，按等差值的方式对轨迹点进行分散，设定离散点的分布密度。
19.按等差值的方式对轨迹点进行分散，使离散点分布均匀，提高出料口移动轨迹的稳定性，根据轨迹精度要求设定离散点的分布密度。
20.进一步的，步骤s3中，所述数据处理模块根据最优控制算法选择优先解进行输出，选择支臂活动数量少的方案。
21.最优控制算法逻辑中，包含逻辑控制模块，以尽量少动臂，不超出运动范围、根据臂架姿态可以连续性动臂为原则，优先选择少动臂的方案，减小振动。通过臂架泵排量限制，利用最优算法，确定控制最少的臂架阀。确定臂架阀的开度大小。
22.进一步的，步骤s3中，将处于下一离散点时的各级支臂的转角θi’与当前各级支臂的转角θi相减求出关节变化量
△
θi＝θi
’‑
θi，根据
△
θi求出动力机构的伸缩变化量
△
l，控制器以
△
l作为信号输出到动力机构上。
23.通过运动学反解算法计算出的θi’与θi相减得出各级关节变化量
△
θi，由于动力机构的连接关系固定，可求出
△
l，控制器以
△
l作为信号可直接驱动动力机构的运动。
24.计算时，将求出的当前浇筑点e和下一离散点e’所对应的关节变量的值相减得到的便是臂架浇注口从e到e’关节变量的变化量
△
θi＝θi
’‑
θi,由角度与液压缸伸缩量的变化关系将
△
θi变换成液压缸伸缩的变化量
△
l，以
△
l作为控制系统的输入信号就可以实现泵车臂架浇筑口沿浇筑轨迹从e点运动到e’。那么将浇筑轨迹离散出的每一个点所对应的关节变量求出来，并预存起来再求出相邻两点的关节变量的变化量，就可以实现泵车臂架浇注口沿浇筑轨迹从起始点到终止点的自动连续浇筑。
25.当浇筑轨迹已知的情况下利用差值的方法将预设轨迹离散成一系列浇筑点，利用臂架姿态反解算法可以求出每一个浇筑点所对应的一组反解。只要离散出来的浇筑点的间距合理，泵车臂架浇注口就能沿着浇筑轨迹平稳的运动到下一浇筑点。离散的浇筑点的坐标值是已知的。根据运动学反解的方法可以解出任一浇筑点所对应的一组反解，设为(θ11，θ，12，θ13，θ14，θ15)以及下一浇筑点所对应的一组反解，设为(θ21，θ22，θ 23，θ24，θ25)，以此类推计算出所有浇筑点角度(θi1，θi2，θi3，θi4，θi5) (i从1到11)。
26.进一步的，步骤s3中，轨迹点为等高状态，臂架包括6级支臂时，利用运动学反解算法求出任一离散点时各级支臂之间的夹角，运动学反解算法计算公式如下：
[0027][0028][0029][0030][0031]
其中，为出料口的速度，为关节的速度，为各个关节的速度，x1为出料口横坐标，y1为出料口纵坐标，a
1-a6为各关节的角度，j为雅可比矩阵。
[0032]
在臂架平面内定义一个x-o-y平面坐标系，臂架出料口在平面内运动，为简化模型，用第一和第二节臂做分析。设当前第一、二节臂的角度分别为a1和a2，臂长分别为l1和
l2，可计算出此时的出料口位置为：
[0033]
x1＝l1cosa1 l2cos(a1 a
2-180)
[0034]
＝l1cosa1 l2cosa1cosa
2-l2sina1sina2[0035]
y1＝l1sina1 l2sin(a1 a
2-180)
[0036]
＝l1sina1 l2sina1cosa2 l2cosa1sina2[0037]
可写出雅可比矩阵为：
[0038][0039]
由雅可比矩阵j可建立出料口与关节角度之间的关系如下：
[0040][0041]
其中，为出料口的速度，为关节的速度，当已知出料口位置时，可按下式求得关节位置的变化量，进而可以控制关节的运动角度和方向。
[0042][0043]
将两个关节扩展可得到上述的六个关节的逆运动学公式以及臂架包含任意个关节时的逆运动学公式，模型等高功能仿真验证时，期望的出料口轨迹为一条水平线，即x1不断变化同时y1保持不变，且这两个值都是已知，因此为求出关节的运动量，只需用矩阵j-1
乘上给定的出料口位置x1和y1就可以，图5为基于所建模型的等高功能的验证，可以看出出料口基本是沿着水平线运动。
[0044]
泵车臂架等高轨迹规划控制具体过程中：先遥控结构拨至等高档位；操作遥控器一臂或回转，一臂摇杆此时作为指令摇杆控制臂架前进或后退，臂架通过各种组合动作，末端始终在等高面上移动。锁臂档位拨至1.2.3挡，可分别锁住1,1 2,1 2 3臂。只操作其他臂架实现等高功能。
[0045]
一种泵车，使用如权利要求1-6任一项所述的臂架轨迹规划的控制方法，包括控制器、臂架，所述臂架包括多级支臂，相邻的两支臂间在同一竖直平面内可转动的连接，相邻的两支臂之间设置有动力机构，每节所述支臂的起始端设置有倾角传感器，所述臂架的一级支臂转动连接在回转机构上，所述回转机构转动连接在所述泵车上，所述臂架的末级支臂的端部设有出料口，所述控制器电性连接所述动力机构、倾角传感器、回转机构，所述控制器包含有锁臂开关、档位开关、摇杆，所述控制器连接有数据处理模块。
[0046]
上述方案中，臂架起着对混凝土输送管道进行支撑和定位的作用，通常包含多级，通过转动连接的方式可在xz平面上折叠和展开，减小运输时候的体积，动力机构对臂架的展开和折叠提供动力，倾角传感器对相邻两级支臂的夹角进行测定，回转机构对一级支臂进行转动，使臂架能够在y轴向移动，泵车可通过转动电机连接回转机构，末级支臂的端部设有出料口，即输送管道的出口，控制器连接数据处理模块，进行运算、收集信号和输出信号控制部件，倾角传感器将臂架角度、臂架加速度和角速度信息输入到控制器，控制器将计算结果输出到动力机构和回转机构，锁臂开关能够设置对部分支臂进行锁定，通常对低级数的支臂，如1-3级进行锁定，确定工作范围，减小数据处理模块的运算量，档位开关能够设置智能工作状态或者普通工作状态，普通工作状态可对各级支臂和回转机构分别进行操
作，智能工作状态包含等高运动、垂直起降运动状态，通过摇杆对支臂进行操作，通过各级支臂的夹角以及各级支臂的长度，数据处理模块能够计算出出料口的位置坐标，数据处理模块可以包含硬件或软件。
[0047]
本方案的目的是减少泵车在工作时的复杂操作，使泵车的臂架操作更简单。
[0048]
基本方案是通过倾角传感器采集臂架姿态角度数据，计算臂架末端坐标，将末端轨迹按照等高要求离散成一系列距离可控的浇筑点；利用臂架姿态反解算法可以求出每一个浇筑点所对应的臂架角度；将浇筑轨迹离散出的每一个点所对应的关节变量求出来，并计算油缸变化量，用以连续控制油缸的伸缩量。实现臂架等高的控制方法。
[0049]
臂架轨迹规划控制的作用：不再需要人工分别操纵多个臂架的动作，只需预先给定一定运动轨迹及运动方向，控制器计算出每节臂架的运动量，臂架将会自动按照预定轨迹进行混凝土浇筑，比如自动实现布料杆末端水平、等高直线运动，垂直方向等幅垂直起降运动。操作更加方便、快捷。
[0050]
采用智能控制技术能够有效改善臂架操作的平稳性，能对臂架的运动过程进行控制，以给出臂架最优运动过程，可以有效减少臂架的运动时间，提高臂架的运动稳定性避免臂架运动过程的失控。为此，选定臂架的常用工作范围，并将工作范围内的浇筑点利用离散算法，离散成多个数组集，利用软件中预定的最优控制算法计算出各节臂的转动角度，控制器输出后，控制机构的运动参数，从而完成智能浇筑过程。对于一节或二节臂不需很大的转动角度的工况，可采取锁定一节臂和二节臂的方法减小控制器不必要的计算过程，避免在高空作业时一节臂和二节臂不容易监控带来的安全隐患。
[0051]
进一步的，所述控制器设有遥控结构，所述锁臂开关、档位开关、摇杆设于所述遥控结构上，每级所述支臂对应一个摇杆，所述回转机构对应设有回转摇杆。
[0052]
遥控结构方便人工操作，每级所述支臂对应一个摇杆来进行操作，回转机构对应设有回转摇杆来操作。
[0053]
进一步的，所述动力机构包含有液压缸，所述档位开关包含有等高运动、垂直起降运动及普通工作状态档位。
[0054]
液压缸工作稳定，压力大，普通工作状态下通过对应的摇杆分别对各级支臂、回转机构进行控制，等高运动、垂直起降运动下只有指令摇杆和回转摇杆进行工作，在等高运动或垂直起降运动状态下，指令摇杆控制前进后退或者上下移动，指令摇杆可以是支臂摇杆临时切换而来，回转摇杆控制回转机构左右转动。
[0055]
进一步的，所述末级支臂端部设置有位置传感器，所述位置传感器电性连接所述控制器。
[0056]
通过位置传感器进一步为出料口位置进行检测，提高位置精度，位置传感器将位置信息输入到控制器，方便进行校对。
[0057]
与现有技术相比，本发明的有益效果是：
[0058]
1.通过在臂架安装倾角传感器配合动力机构，可随时掌握和控制臂架姿态，通过将关节角度变化量转换为动力机构伸缩变化量，提高动力机构的运动精确度，减小误差，方便对出料口相对车身的坐标进行控制；
[0059]
2.通过将臂架末端轨迹离散成若干浇筑点和离散点，通过离散点的密度控制可以提高轨迹运行的精度，通过浇筑点设置，可以实现多种轨迹的浇筑工作，并分别计算对应臂
架角度，对臂架姿态进行控制；
[0060]
3.通过运动学反解算法、最优控制算法，对臂架姿态和运行流畅度进行控制，使运行过程中最少的臂架运动，减小臂架振动，臂架减振效果好，且臂架控制精准，为轨迹实现提供保障；
[0061]
4.移动浇筑过程中只需要提供运行指令，出料口就会自动沿轨迹浇筑，避免现有技术中对各级支臂分别操作的问题，操作方便。
附图说明
[0062]
图1为本发明中泵车的遥控结构的正视图；
[0063]
图2为本发明中泵车的臂架轨迹算法图；
[0064]
图3为本发明中泵车的臂架等高轨浇筑轨迹图；
[0065]
图4为本发明中泵车的臂架变化示意图；
[0066]
图5为本发明中臂架等高浇筑验证效果图；
[0067]
图中：101、遥控结构；102、锁臂开关；103、摇杆；104、档位开关。
具体实施方式
[0068]
下面将结合本发明中的附图，对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动条件下所获得的所有其它实施例，都属于本发明保护的范围。在本发明的描述中，需要说明的是，术语前、后、左、右等指示方位或位置关系为基于附图所示的方位或位置关系，或者是该发明产品使用时惯常摆放的方位或位置关系，仅是为了便于描述本发明或简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造或操作，因此不能理解为对本发明的限制。
[0069]
实施例1
[0070]
如图1-5所示，一种泵车，使用如权利要求1-6任一项所述的臂架轨迹规划的控制方法，包括控制器、臂架，所述臂架包括多级支臂，相邻的两支臂间在同一竖直平面内可转动的连接，相邻的两支臂之间设置有动力机构，每节所述支臂的起始端设置有倾角传感器，所述臂架的一级支臂转动连接在回转机构上，所述回转机构转动连接在所述泵车上，所述臂架的末级支臂的端部设有出料口，所述控制器电性连接所述动力机构、倾角传感器、回转机构，所述控制器包含有锁臂开关102、档位开关104、摇杆103，所述控制器连接有数据处理模块。
[0071]
上述方案中，臂架起着对混凝土输送管道进行支撑和定位的作用，通常包含多级，通过转动连接的方式可在xz平面上折叠和展开，减小运输时候的体积，动力机构对臂架的展开和折叠提供动力，倾角传感器对相邻两级支臂的夹角进行测定，回转机构对一级支臂进行转动，使臂架能够在y轴向移动，泵车可通过转动电机连接回转机构，末级支臂的端部设有出料口，即输送管道的出口，控制器连接数据处理模块，进行运算、收集信号和输出信号控制部件，倾角传感器将臂架角度、臂架加速度和角速度信息输入到控制器，控制器将计算结果输出到动力机构和回转机构，锁臂开关102能够设置对部分支臂进行锁定，通常对低级数的支臂，如1-3级进行锁定，确定工作范围，减小数据处理模块的运算量，档位开关104
能够设置智能工作状态或者普通工作状态，普通工作状态可对各级支臂和回转机构分别进行操作，智能工作状态包含等高运动、垂直起降运动状态，通过摇杆103对支臂进行操作，通过各级支臂的夹角以及各级支臂的长度，数据处理模块能够计算出出料口的位置坐标，数据处理模块可以包含硬件或软件。采用6级支臂的泵车结构。
[0072]
本方案的目的是减少泵车在工作时的复杂操作，使泵车的臂架操作更简单。
[0073]
基本方案是通过倾角传感器采集臂架姿态角度数据，计算臂架末端坐标，将末端轨迹按照等高要求离散成一系列距离可控的浇筑点；利用臂架姿态反解算法可以求出每一个浇筑点所对应的臂架角度；将浇筑轨迹离散出的每一个点所对应的关节变量求出来，并计算油缸变化量，用以连续控制油缸的伸缩量。实现臂架等高的控制方法。
[0074]
臂架轨迹规划控制的作用：不再需要人工分别操纵多个臂架的动作，只需预先给定一定运动轨迹及运动方向，控制器计算出每节臂架的运动量，臂架将会自动按照预定轨迹进行混凝土浇筑，比如自动实现布料杆末端水平、等高直线运动，垂直方向等幅垂直起降运动。操作更加方便、快捷。
[0075]
采用智能控制技术能够有效改善臂架操作的平稳性，能对臂架的运动过程进行控制，以给出臂架最优运动过程，可以有效减少臂架的运动时间，提高臂架的运动稳定性避免臂架运动过程的失控。为此，选定臂架的常用工作范围，并将工作范围内的浇筑点利用离散算法，离散成多个数组集，利用软件中预定的最优控制算法计算出各节臂的转动角度，控制器输出后，控制机构的运动参数，从而完成智能浇筑过程。对于一节或二节臂不需很大的转动角度的工况，可采取锁定一节臂和二节臂的方法减小控制器不必要的计算过程，避免在高空作业时一节臂和二节臂不容易监控带来的安全隐患。
[0076]
进一步的，所述控制器设有遥控结构101，所述锁臂开关102、档位开关104、摇杆103 设于所述遥控结构101上，每级所述支臂对应一个摇杆103，所述回转机构对应设有回转摇杆103。
[0077]
遥控结构101方便人工操作，每级所述支臂对应一个摇杆103来进行操作，回转机构对应设有回转摇杆103来操作。遥控结构101结构型号为fst 727/n spectrum b，控制器和数据处理模块均设于遥控结构101上。
[0078]
进一步的，所述动力机构包含有液压缸，所述档位开关104包含有等高运动、垂直起降运动及普通工作状态档位。
[0079]
液压缸工作稳定，压力大，普通工作状态下通过对应的摇杆103分别对各级支臂、回转机构进行控制，等高运动、垂直起降运动下只有指令摇杆103和回转摇杆103进行工作，在等高运动或垂直起降运动状态下，指令摇杆103控制前进后退或者上下移动，指令摇杆 103可以是支臂摇杆103临时切换而来，回转摇杆103控制回转机构左右转动。等高运动时指令摇杆103是一级支臂的摇杆103，垂直起降时指令摇杆103是二级支臂的摇杆103。
[0080]
一种臂架轨迹规划的控制方法，包括如下步骤：
[0081]
s1：控制器获取臂架出料口的运动轨迹，控制器连接的数据处理模块将运动轨迹分散成若干个离散点；
[0082]
s2：通过倾角传感器测试出臂架的各级支臂之间的夹角，所述数据处理模块根据各个夹角计算出出料口的位置坐标，将出料口的位置坐标与离散点坐标进行对比，选定下一离散点坐标；
[0083]
s3：所述数据处理模块根据预设的运动学反解算法，计算出各级支臂的转动角度，由控制器向动力机构输出；
[0084]
s4：所述控制器发出指令3，各级支臂分别按照控制器输出的结果转动，出料口依次通过离散点，进行浇筑。
[0085]
上述方法中，步骤s1中，根据需要向控制器输入轨迹点坐标，数据处理模块将轨迹点坐标分散成若干个离散点，离散点越密集则出料口的轨迹越准确；步骤s2，通过倾角传感器得到各级支臂之间的夹角，数据处理模块计算出出料口的位置坐标，根据现有坐标位置选定下一离散点，确认运动方向；步骤s3中，由于各级支臂的长度一定，起始点坐标位置一定，离散点和现有位置坐标已知，通过运动学反解算法可以求出各级支臂需要转动的角度，由控制器向动力机构输出；步骤s4中，通过操作指令摇杆103控制臂架进行前进，各级支臂分别按照控制器输出的结果转动，使出料口移动到下一离散点，过程中出料口沿轨迹进行浇筑，也可同时操作回转摇杆103使臂架进行左右动作。
[0086]
进一步的，步骤s1中，调整档位开关104为普通工作档位，将一级支臂移动到位后，锁臂开关102选择锁定一级支臂，将控制器的档位开关104选定为等高轨迹或垂直起降运动档位。
[0087]
在某些不需要全部支臂进行运动的情况下，首先可将一级支臂移动后进行锁定，减少活动支臂的数量，同理也可将1-3级支臂一起锁定，减少数据处理模块的计算量，减少移动的支臂数量提高臂架运行稳定性，选定档位后，通过指令摇杆103、回转摇杆103来对控制器进行输入，等高轨迹档位下指令摇杆103为一级支臂的摇杆103，垂直起降运动档位下指令摇杆103为二级支臂的摇杆103。
[0088]
进一步的，步骤s1中，按等差值的方式对轨迹点进行分散，设定离散点的分布密度。
[0089]
按等差值的方式对轨迹点进行分散，使离散点分布均匀，提高出料口移动轨迹的稳定性，根据轨迹精度要求设定离散点的分布密度。
[0090]
进一步的，步骤s3中，所述数据处理模块根据最优控制算法选择优先解进行输出，选择支臂活动数量少的方案。
[0091]
最优控制算法逻辑中，包含逻辑控制模块，以尽量少动臂，不超出运动范围、根据臂架姿态可以连续性动臂为原则，优先选择少动臂的方案。通过臂架泵排量限制，利用最优算法，确定控制最少的臂架阀。确定臂架阀的开度大小。
[0092]
进一步的，步骤s3中，将处于下一离散点时的各级支臂的转角θi’与当前各级支臂的转角θi相减求出关节变化量
△
θi＝θi
’‑
θi，根据
△
θi求出动力机构的伸缩变化量
△
l，控制器以
△
l作为信号输出到动力机构上。
[0093]
通过运动学反解算法计算出的θi’与θi相减得出各级关节变化量
△
θi，由于动力机构的连接关系固定，可求出
△
l，控制器以
△
l作为信号可直接驱动动力机构的运动。
[0094]
如图2，计算时，将求出的当前浇筑点e和下一离散点e’所对应的关节变量的值相减得到的便是臂架浇注口从e到e’关节变量的变化量
△
θi＝θi
’‑
θi,由角度与液压缸伸缩量的变化关系将
△
θi变换成液压缸伸缩的变化量
△
l，以
△
l作为控制系统的输入信号就可以实现泵车臂架浇筑口沿浇筑轨迹从e点运动到e’。那么将浇筑轨迹离散出的每一个点所对应的关节变量求出来，并预存起来再求出相邻两点的关节变量的变化量，就可以实现泵
车臂架浇注口沿浇筑轨迹从起始点到终止点的自动连续浇筑。
[0095]
如图3，当浇筑轨迹已知的情况下利用差值的方法将预设轨迹离散成一系列浇筑点，利用臂架姿态反解算法可以求出每一个浇筑点所对应的一组反解。只要离散出来的浇筑点的间距合理，泵车臂架浇注口就能沿着浇筑轨迹平稳的运动到下一浇筑点。离散的浇筑点的坐标值是已知的。根据运动学反解的方法可以解出任一浇筑点所对应的一组反解，设为 (θ11，θ，12，θ13，θ14，θ15)以及下一浇筑点所对应的一组反解，设为(θ21，θ22，θ23，θ24，θ25)，以此类推计算出所有浇筑点角度(θi1，θi2，θi3，θi4，θi5)(i从1到11)。
[0096]
进一步的，步骤s3中，轨迹点为等高状态，臂架包括6级支臂时，利用运动学反解算法求出任一离散点时各级支臂之间的夹角，运动学反解算法计算公式如下：
[0097][0098][0099][0100][0101]
其中，为出料口的速度，为关节的速度，为各个关节的速度，x1为出料口横坐标，y1为出料口纵坐标，a
1-a6为各关节的角度，j为雅可比矩阵。出料口的速度和关节的速度可以通过遥控器进行设定，最终即通过各个离散点的坐标，算出各个关节的角度。
[0102]
在臂架平面内定义一个x-o-y平面坐标系，臂架出料口在平面内运动，为简化模型，用第一和第二节臂做分析。设当前第一、二节臂的角度分别为a1和a2，臂长分别为l1和l2，可计算出此时的出料口位置为：
[0103]
x1＝l1cosa1 l2cos(a1 a
2-180)
[0104]
＝l1cosa1 l2cosa1cosa
2-l2sina1sina2[0105]
y1＝l1sina1 l2sin(a1 a
2-180)
[0106]
＝l1sina1 l2sina1cosa2 l2cosa1sina2[0107]
可写出雅可比矩阵为：
[0108][0109]
由雅可比矩阵j可建立出料口与关节角度之间的关系如下：
[0110]
[0111]
其中，为出料口的速度，为关节的速度，当已知出料口位置时，可按下式求得关节位置的变化量，进而可以控制关节的运动角度和方向。
[0112][0113]
将两个关节扩展可得到上述的六个关节的逆运动学公式以及臂架包含任意个关节时的逆运动学公式，模型等高功能仿真验证时，期望的出料口轨迹为一条水平线，即x1不断变化同时y1保持不变，且这两个值都是已知，因此为求出关节的运动量，只需用矩阵乘上给定的出料口位置x1和y1就可以。
[0114]
泵车臂架等高轨迹规划控制具体过程中：先遥控结构101拨至等高档位；操作遥控器一臂或回转，一臂摇杆103此时作为指令摇杆103控制臂架前进或后退，臂架通过各种组合动作，末端始终在等高面上移动。锁臂档位拨至1.2.3挡，可分别锁住1,1 2,1 2 3臂。只操作其他臂架实现等高功能。
[0115]
实施例2
[0116]
本实施例的一种泵车，在实施例1的基础上进行进一步的优化：
[0117]
进一步的，所述末级支臂端部设置有位置传感器，所述位置传感器电性连接所述控制器。
[0118]
通过位置传感器进一步为出料口位置进行检测，提高位置精度，位置传感器将位置信息输入到控制器，方便进行校对。
[0119]
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。