基于区域规划法的泵车臂架系统反解算法及自动浇筑轨迹

刘凯磊，刘荣升，康绍鹏，强红宾,2，陶扬

(1.江苏理工学院机械工程学院，江苏常州 213001；2.国机重工集团常林有限公司，江苏常州 213136；3.江苏大学国家水泵及系统工程技术研究中心，江苏镇江 212013；4.河南工学院智能工程学院，河南新乡 453000)

0 前言

混凝土泵车是一种利用压力将混凝土沿管道连续输送的工程机械，广泛应用于国家基础设施及军工国防建设。目前，混凝土泵车的使用主要由人工操作来完成，操作者的操作水平直接影响浇筑精度，加之长时间的重复操作容易引起疲劳，会进一步影响到浇筑精度进而使工作效率降低。因此，为了提高浇筑效率、改善工作环境以及减轻操作者劳动强度，智能化自动浇筑作业已成为该领域国内外研究的热点。

国外内学者主要从操控系统设计、运动学分析、柔性动力学分析、臂架振动机制、电液技术开发等方面展开研究，并取得了一定成果。为实现智能化自动浇筑作业，必须对混凝土泵车臂架系统进行运动学分析，然后对浇筑轨迹进行规划，从而为后续浇筑轨迹运动控制提供理论支撑。然而，混凝土泵车臂架系统是一组多自由度的串联机构，一般多自由度串联机器人的运动学反解是机器人领域研究的难点。LEE和LIANG利用14个运动学反解方程和解析方法，得到一般6R机器人的16组反解。RAGHAVAN和ROTH利用分离变量消元的方法将运动学反解问题转化为关节变量半角正切的一元十六次方程求根问题等。对于混凝土泵车臂架而言，运动学反解是实现自动浇筑的最基本前提，现阶段的研究多采用反变换法、几何法、最小关节范数法等。反变换法是机器人研究领域常用的反解算法，即左乘逆矩阵代数法直接求解6个一元方程，求得多组逆解，需要反复左乘逆矩阵，寻求合适的求解方程，其计算过程复杂，耗时长。浇筑过程中，只有臂架末端位置已知，姿态未知，因此更增大了求解的难度，而且根据通用方法反解出的角度随意性大，短时间内变化跨度大，使得臂架跟踪性能差，易产生振动。

因此，为克服目前串联机器人反解算法计算过程复杂、耗时长，不适于工程应用的困难，以五节臂混凝土泵车为研究对象，对混凝土泵车臂架系统进行运动学分析。利用区域轨迹规划法将臂架系统的工作空间进行细分，研究基于区域规划法的泵车臂架系统反解算法；利用插补算法，根据实际浇筑工况，对典型的自动浇筑轨迹进行规划，从而为后续自动浇筑轨迹运动控制提供参考。

1 运动学正解分析

1.1 混凝土泵车臂架系统坐标模型

混凝土泵车臂架系统是一组空间六自由度开环串联机构。根据D-H法建立混凝土泵车臂架机构关节坐标系，如图1所示。

图1 混凝土泵车臂架机构关节坐标系

结合各节臂架的结构尺寸，可以得到D-H矩阵的连杆参数，如表1所示。其中，表示从到+1沿测量的距离；表示从到+1绕旋转的角度；表示从-1到沿测量的距离；表示-1到绕旋转的角度。

表1 混凝土泵车臂架系统的连杆参数

1.2 运动学正解

(1)

(2)

式中:=cos;=sin;=cos;=sin;=cos(+);=sin(+)，以此类推。

由式(2)可以看出臂架末端点的坐标(，，)是关于、、、、、的函数。因此，确定关节变量～，可以确定臂架末端点在基坐标系中的位置，所以针对关节变量的精确控制可实现在机械机构允许范围内的任意浇筑轨迹。

2 基于区域规划法的运动学反解

2.1 运动学反解工况分析

五节臂泵车臂架系统是一组空间六自由度串联机构，根据空间机构学原理，该机构有2个或3个冗余自由度，所以当浇筑轨迹确定时，会有很多冗余反解。一般的求解方法不能求解出唯一的各相对转角，因而无法实现智能化自动浇筑。为解决这一问题，将凝土泵车臂架系统的工作空间进行分区域规划，根据臂架位姿及其运动状态，在区域内设定相关臂架的关节变量，可使运动学反解具有唯一性，从而为泵车臂架系统的实时控制奠定基础。分析混凝土泵车臂架结构，可知混凝土泵车臂架末端的可达区域是一个空间半球形区域，如图2所示。

图2 混凝土泵车臂架末端空间半球形可达区域

回转角可根据公式(3)推导求出：

(3)

由式(3)可知，回转角是关于末端位置点坐标和的函数，当点位置确定时，可以求出唯一回转角。

基于的唯一性，在区域划分时，可将回转角忽略，即将半球刨切成平面，将空间逆解问题限定为平面逆解问题。在平面区域上仅有和坐标，从而简化逆解计算过程。

在唯一确定后，尚有～5个未知数待解，而已知参数只有点的和坐标，仍无法唯一确定各相对转角，实现自动轨迹控制。

根据臂架结构特性，在确定的情况下，若确定～，则仅剩下和两个未知数，进而可根据坐标和，求出唯一解和。

在、、、、和确定时，根据公式(1)和(2)，可求出和：

(4)

式中:、、和为简化公式所定义的参数，其他参数定义与式(2)相同。

2.2 臂架运动学反解的区域轨迹规划原理

根据上述求解思想，本文作者提出基于区域规划的反解算法，其核心是将整个平面操作区域划分为多个规则的求解区域。为快速、准确求得唯一解，每个区域有固定的坐标和关节参数、、供查询，对于浇筑轨迹上的任何一点都可快速求得一组目标关节变量，避免了繁琐、费时的多次迭代。

浇筑过程中，臂架结构若呈现如图3所示的“非正包络”形式，各臂连接处在混凝土泵送时容易受到冲击而造成臂架振动。相反，若呈现如图4所示的“正包络”形式，可以大大减小臂架振动。

图3 泵车臂架“非 图4 泵车臂架“正正包络”形式 包络”形式

因此，将“正包络”引入到区域规划法中。以“正包络”浇筑形式确定每一组关节变量～及、的变化范围，则臂架末端点在平面中的可达区域可由正解公式(2)求得。例如，选取出一组关节变量=0°、=75°、=165°、=160°、=160°～ 240°、=30°～ 170°，经计算，臂架末端点的可达区域如图5所示。

图5 特定角度时臂架末端点P可达区域

按上述方法，将臂架的整个工作空间进行区域规划：

(1)把臂架工作空间等分成若干个正方形区域，依据实际浇筑工况及臂架结构特性确定正方形区域边长为5 m；

(2)通过选取不同关节变量、、的值，按照前面所述方法确定臂架末端的可达区域，但每个区域并不是规则的几何图形，如图5所示，不符合正方形的条件，所以对可达区域所能完全覆盖的正方形进行标记，并记录当前关节变量、、的值，依此类推，规划出每一个新区域；

(3)臂架在跨区域运动时，由于不同区域关节变量、、的值不同，瞬间的角度变化会导致臂架控制不稳定，控制精度变差并出现振动，因此在相邻正方形区域内，各自设置1 m的过渡区，、、以连续性变化实现过渡；

(4) 由于臂架结构的缺陷及“正包络”的限制，臂架自动浇筑的工作空间即上述区域规划的范围，略小于臂架系统可达空间，但满足浇筑工况需求。

通过上述规划方法可将臂架自动浇筑的工作空间划分为42个区域，如图6所示。

图6 泵车臂架末端可达区域平面划分图

依据划分的区域，结合实际浇筑轨迹，可快速计算出浇筑轨迹上各臂架的相对转角，为臂架的智能控制建立基础。

3 自动浇筑轨迹规划

3.1 自动浇筑轨迹实现

依据区域划分，建立臂架浇筑末端坐标与各关节变量之间的一一对应关系。在浇筑轨迹已知的情况下，运用插补算法将轨迹曲线离散成一系列的浇筑点。利用区域规划法求出每个浇筑点所对应的一组关节变量，当求出相邻两个浇筑点所对应关节变量以后，利用控制系统驱动各臂架的执行液压缸，使得各臂同步达到同一组关节变量，使臂架浇筑末端从轨迹的一个离散点运动到下一个离散点，依次循环，臂架浇筑末端从轨迹的起点运动到终点，实现自动浇筑功能。

3.2 直线轨迹实时插补算法

为保证浇筑过程的平稳性，在浇筑起点与终点之间采用插补算法，插入中间点，再用运动学反解算法求出各个关节的角度，通过控制系统对泵车臂架进行实时控制，具体流程如图7 所示。

图7 直线轨迹控制流程

空间直线插补是串联机构理论中不可缺少的基本插补算法，在已知该直线始末两点的位置和姿态下，求各轨迹插补点的位置和姿态。当给定直线始末两点的坐标为(，，)和(，，)，臂架末端从点匀速运动到点，在固定插补步长Δ的条件下，确定插补次数，进而算出插补点的坐标。

综合考虑浇筑过程的稳定性与控制器计算精度，取插补步长Δ为100 mm，则插补直线长度为

(5)

插补总步数为

(6)

则可以实时计算出各插补点的坐标：

(7)

式中：为插补次数，在0～之间变化；、、为第次插补点的坐标值。

3.3 自动浇筑直线轨迹控制流程

混凝土泵车由于其露天工况，控制系统采用的是16位工程机械运动控制器，控制器的CPU、内存等参数远低于工业控制器，但是工作温度、防护等级均高于工业控制器，因此，其计算能力有限。为满足控制系统自动浇筑直线过程中实时性自动控制的要求，设计基于插补算法的控制流程图，如图8所示。

具体步骤如下：

(1)确定直线运动轨迹。在当前状态，通过装在回转机构之间、臂架与臂架之间的6个角度传感器在线实时测量6个关节变量，通过公式(2)计算出臂架末端点当前坐标，根据直线轨迹的终点坐标，判断它是否超出臂架末端点工作范围，若没有超出则确定该点为直接轨迹终点坐标；若超出范围，则以臂架末端点的最远点为终点坐标。

(2)直线运动轨迹离散。通过直线轨迹的起点和终点坐标，计算直线长度，根据插补步长Δ，利用式(5)—(7)计算直线轨迹上的插补点，最终确定每个离散点的坐标。

(3)运动学反解计算与实时控制。确定臂架末端点当前坐标后，根据下一离散点的坐标，判断它是否处于过渡区，若不处于过渡区，根据所在区域标号，查出关节变量、和，并根据式(3)(4)反解计算出、和；如处于过渡区，则根据两个相邻区域标号的关节变量、和进行插值,计算出、和，并根据式(3)(4)反解计算出、和；然后将下一个离散点作为目标点，通过对各个关节实施闭环反馈控制，完成每个离散点跟踪控制，进而完成臂架末端直线浇筑轨迹控制。

图8 直线轨迹控制流程

3.4 典型运动轨迹规划

选取两种典型的直线轨迹进行运动仿真，分析验证直线轨迹控制流程。在实际浇筑过程中，垂直平面内的水平直线和水平面内的斜直线浇筑轨迹最为典型。因此，选取水平直线、斜直线进行轨迹规划，分别如图9、图10所示。

图9 垂直平面内的水平直线轨迹

图10 水平面内的斜直线轨迹

当泵车臂架末端的运动轨迹如图9和图10所示时，各个臂架相对转角均呈现连续性变化，分别如图11和图12所示。

图11 垂直平面内水平直线轨迹时臂架末端相对转角变化曲线 图12 水平平面内斜直线轨迹的相对转角变化曲线

两种典型直线计算结果表明：

(1)泵车臂架末端在垂直平面内运动时，没有回转运动，不变化，为恒定值；在水平面内运动时，有回转运动，、和均发生连续性变化。

(2)泵车臂架末端在非过渡区域内运动时，相对转角、和不发生变化，其值为非过渡区域内设定的相转角值。

(3)泵车臂架末端在过渡区域内运动时，相对转角～均发生连续性变化，变化趋势比较明显。

上述计算结果符合所设计的直线轨迹控制流程，达到了泵车臂架浇筑末端直线行走的目的，并且具有反解唯一、计算量小、实时性高等特点，能够在16位工程机械运动控制器实现。

5 结论

(1)针对混凝土泵车臂架系统臂架末端的反解算法计算量大、耗时长等特点，提出了基于区域规划法的泵车臂架系统反解算法，简化了臂架反解运算过程，使反解具有唯一性，从而提高了在线反解计算速度，适于工程应用。

(2)在泵车臂架末端直线浇筑轨迹的起点和终点之间，采用了插补算法，有效提高了浇筑过程的平稳性，并兼顾了控制器的计算与储存能力。

(3)根据混凝土泵车工况需求，提出了混凝土泵车智能化自动轨迹浇筑实施方法，对两种典型的直线运动轨迹进行了反解计算，验证了该方法的可行性。