CN115840444A - 基于驾驶员行为的混凝土泵车臂架轨迹规划方法 - Google Patents

Abstract

本发明公开了一种基于驾驶员行为的混凝土泵车臂架轨迹规划方法，步骤包括：步骤1：以混凝土泵车的中心O为原点建立相对坐标系x‑y‑z，另外，以现场地平面建立坐标系x‑o‑y；步骤2：对混凝土泵车以及现场环境进行建模；步骤3：对混凝土泵车的臂架转动进行规划；步骤4：对混凝土泵车的臂架关节进行运动规划，通过各臂架关节展角的变化，实现臂架末端位置从A'到达目标点B。本发明的方法，考虑末端臂架惯性小，借鉴混凝土泵车驾驶员操作经验，臂架的轨迹规划采取从末端臂架逐级向前端臂架遍历的方案，同时，臂架的泵送姿态采用“拱型”，实现了臂架运动的避障、运动节能以及减振。

Description

基于驾驶员行为的混凝土泵车臂架轨迹规划方法

技术领域

本发明属于混凝土泵车轨迹规划技术领域，涉及一种基于驾驶员行为的混凝土泵车臂架轨迹规划方法。

背景技术

混凝土泵车全称臂架式混凝土泵车，它是在汽车底盘上将泵送机构和臂架系统集为一体的设备，其中泵送机构的作用是用来泵送混凝土，臂架的作用是用来布料，在工作过程中，混凝土泵车通过控制臂架运动变换浇筑位置。目前，混凝土泵车臂架运动控制仍采用手动操作的模式，主要依靠操作员的经验；然而在施工现场，由于混凝土泵车是室外作业，工作环境恶劣，随着以人为本的智能化发展理念，混凝土泵车臂架实现自动控制将是发展方向。

现有的混凝土泵车的臂架分别设置有4节臂架、5节臂架、6节臂架等多种形式，属于多自由度冗余机械臂，各个臂架的运动控制一方面要避免与施工现场的建筑物发生碰撞；另一方面，考虑到臂架惯性大，在运动规划中，要尽量减小臂架的加减速过猛导致对钢结构产生大的冲击、振动；还有一方面，臂架规划要考虑混凝土泵车的泵送姿态(即在进行混凝土泵送时臂架的姿态)，要减少混凝土泵送时的阻力，从而达到节能环保的目的。

目前，臂架运动控制多数采用的是梯度投影优化算法，该方法的缺点在于求逆运算计算量大，而且还可能存在无解、奇异性问题；还有一种方法是利用传统优化方式的人工势场法进行规划，但是该方法容易陷入局部最小值，导致臂架未到达目标点就停止运动，以及在规划的过程中需要不断对每节臂架进行调整，会使臂架不断启停造成较大冲击，增大臂架的振动。因此，针对上述问题，亟需综合考虑减震、节能以及避障，并且借鉴泵车驾驶员的操作经验对臂架的运动进行整体规划，研制一种更有效的针对性控制方法。

发明内容

本发明的目的是提供一种基于驾驶员行为的混凝土泵车臂架轨迹规划方法，解决了现有技术中存在的计算量大，而且还可能存在无解、奇异性的问题；或者在规划的过程中需要不断对每节臂架进行调整，会使臂架不断启停造成较大冲击，增大臂架振动的问题。

本发明所采用的技术方案是，一种基于驾驶员行为的混凝土泵车臂架轨迹规划方法，按照以下步骤具体实施：

步骤1：以混凝土泵车的中心O为原点建立相对坐标系x-y-z，另外，以现场地平面建立坐标系x-o-y；

步骤2：对混凝土泵车以及现场环境进行建模；

步骤3：对混凝土泵车的臂架转动进行规划，

根据现场的混凝土泵车、墙面障碍物在坐标系x-o-y平面位置的投影，实线OA为初始位置时臂架在坐标系x-o-y平面投影，此时臂架的转角为

虚线AA′为混凝土泵车转动其臂架末端轨迹在坐标系x-o-y平面投影；虚线OA′为转台运动完成后的臂架转角为/>

此时/>

步骤4：对混凝土泵车的臂架关节进行运动规划，

通过各臂架关节展角的变化，实现臂架末端位置从A′到达目标点B，

以混凝土泵车的中心O为原点，在臂架所在平面内建立相对坐标系x′-o-z，在该坐标系下混凝土泵车的各个关节点的坐标分别为：O1(0，0)，O2(x′_O2，z_O2)，…，On-1(x′_n-1，z_n-1)，On(x′_On，z_On)，位置A′以及目标位置B的坐标为：A′(x′_A′，z_A′)，B(x′_B，z_B)。

本发明的有益效果是，考虑末端臂架惯性小，借鉴混凝土泵车驾驶员操作经验，臂架的轨迹规划采取从末端臂架逐级向前端臂架遍历的方案，同时，臂架的泵送姿态采用“拱型”，实现了臂架运动的避障、运动节能以及减振。具体包括以下几个方面：1)实现了混凝土泵车的臂架运动的避障；2)基于操作司机的经验，优先调整末端臂架关节运动，减小臂架振动；3)通过控制混凝土泵车的姿态，降低混凝土泵送过程的阻尼，实现混凝土泵车的节能。

附图说明

图1是本发明方法的整体流程框图；

图2是本发明方法在x-y-z坐标系下的臂架；

图3是图2现场混凝土泵车、墙面障碍物在坐标系x-o-y平面位置的投影；

图4是本发明方法中的臂架转动规划的具体流程框图；

图5是本发明方法以混凝土泵车中心为原点，在臂架所在平面内建立相对坐标系x′-o-z；

图6是本发明方法中的臂架关节运动规划流程框图；

图7是本发明实施例中臂架开始工作时在初始位置的姿态示意图；

图8是本发明实施例中臂架到达最终目标位置的规划姿态示意图。

具体实施方式

下面结合附图和具体实施方式对本发明进行详细说明。

参照图1，本发明的混凝土泵车臂架轨迹规划方法，基于驾驶员行为，按照以下步骤具体实施：

步骤1：以混凝土泵车(车体外廓尺寸)的中心O为原点建立相对坐标系x-y-z，另外，以现场地平面建立坐标系x-o-y；在相对坐标系x-y-z中，臂架第一关节点O1即与坐标系x-o-y的原点O重合，所以在图2中仅标示了第一关节点O1，省略了原点O的标注；

步骤2：对混凝土泵车以及现场环境进行建模，

见图2，混凝土泵车共有n节臂架，各臂架关节点依次记为O1、O2、…、On-1、On，各臂架的展角依次记为α₁、α₂、…、α_n-1、α_n，各臂架的长度依次记为l1、l2、…、ln-1、ln；

利用D-H参数法(此为现有技术)，得到各臂架关节点的笛卡尔坐标是：O1(0，0，0)，O2(x_O2，y_O2，z_O2)…On-1(x_On-1，y_On-1，z_On-1)，On(x_On，y_On，z_On)，根据混凝土泵车的结构特点，将上述笛卡尔坐标系坐标转化为极坐标系坐标，混凝土泵车各关节点极坐标的转角相同，得到混凝土泵车的关节点极坐标是：

/>

见图2，在施工现场设置有等高的墙面障碍物，墙面障碍物P1P2的两端点坐标分别为P1(x_P1，y_P1，h)、P2(x_p2，y_P2，h)，其中，h为墙面障碍物的高度，得到墙面障碍物的两端点极坐标为

和/>

见图2，臂架末端的初始位置为A(x_A，y_A，z_A)，臂架末端的目标位置为B(x_B，y_B，z_B)，则极坐标为

建模完毕；

步骤3：对混凝土泵车的臂架转动进行规划，

参照图3，根据图2所示现场的混凝土泵车、墙面障碍物在坐标系x-o-y平面位置的投影，臂架转动规划是为了实现臂架末端从初始位置A到达位置A′的转台运动；

见图3，实线OA为初始位置时臂架在坐标系x-o-y平面投影，此时臂架的转角为

虚线AA′为混凝土泵车转动其臂架末端轨迹在坐标系x-o-y平面投影；虚线OA′为转台运动完成后的臂架转角为/>

此时/>

参照图4，混凝土泵车臂架的转动规划的具体过程是：

3.1)确认转动范围内有无墙面障碍物，

在图2的投影平面内，根据墙面障碍物的投影线段P1P2与臂架转动扇形区(图2中阴影扇形区)的相对位置关系，判断臂架转动范围内是否有墙面障碍物，如果转动范围内无墙面障碍物，则执行步骤3.5)，实施转台运动；如果转动范围内有墙面障碍物，则执行步骤3.2)，

转动范围内是否有墙面障碍物的判断条件是：

A1)若

r_P1＜min(r_A，r_B)且z_On＜h，则确定臂架转动范围内有墙面障碍物；

A2)若

r_P2＜min(r_A，r_B)且z_On＜h，则确定臂架转动范围内有墙面障碍物；

A3)若

且min(r_P1，r_P2)＜min(r_A，r_B)且z_On＜h，则确定臂架转动范围内有墙面障碍物；

其中，h为墙面障碍物的高度；

除去上述三种情况，其它情况下，均认为臂架转动范围内无墙面障碍物；

3.2)确定臂架转动过程中是否会与墙面障碍物碰撞，

3.2.1)计算臂架上可能的碰撞点k，

前述步骤2中设定了混凝土泵车总共有n节臂架，如果z_Oi＜h＜z_Oi+1，则第i节臂架能够确定与墙面障碍物发生碰撞，根据第i节臂架的两个端点坐标Oi(x_Oi，y_Oi，z_Oi)和Oi+1(x_Oi+1，y_Oi+1，z_Oi+1)，得到第i节臂架的空间直线方程，得到第i节臂架上与墙面障碍物可能的碰撞点k(x_k，y_k，z_k)，表达式是：

则得到碰撞点k的极距为：

3.2.2)臂架转动过程中，确认碰撞点k是否碰撞，

在坐标系x-o-y的平面内，墙面障碍物的投影线段为P1P2，以相对坐标系的原点O为圆心，以可能的碰撞点k的极距r_k为半径作圆；然后，判断坐标系x-o-y平面内线段P1P2与圆是否有交点，有以下可能性：

如果无交点，则在碰撞点k不会碰撞，即臂架转动与墙面障碍物无碰撞，则转入步骤3.5)，进行转台运动；

如果有交点，则在碰撞点k会碰撞，即臂架转动与墙面障碍物有碰撞，碰撞臂架为第i节臂架，则执行步骤3.3)；

3.3)将第i节臂架上扬，

将第i节臂架的展角增大，即α_i＝α_i+Δα，Δα为臂架调节增量角；

3.4)确定第i节臂架的展角α_i是否超过极限角度，判断条件是：

根据混凝土泵车的工作原理，各个臂架的展角都有上下极限范围，分别记为max(α_i)以及min(α_i)，则有以下两种情况：

B1)若max(α_i)＜α_i，说明第i节臂架超过极限位置角度，则α_i＝α_i-Δα且i＝i-1，转回步骤3.3)；

B2)若max(α_i)＞α_i，说明第i节臂架未超过极限位置角度，则转回步骤3.2)；

3.5)进入转台运动，

臂架末端从初始位置A运动到位置A′，此时，转台的角度从

运动到/>

结束臂架的转动规划；

步骤4：对混凝土泵车的臂架关节进行运动规划，

臂架关节的运动规划是通过各臂架关节展角的变化，实现臂架末端位置从A′到达目标点B，在图3中，虚线A′B为混凝土泵车关节运动实现的路径投影；

参照图5，是以混凝土泵车的中心O为原点，在臂架所在平面内建立相对坐标系x′-o-z，在该坐标系下混凝土泵车的各个关节点的坐标分别为：O1(0，0)，O2(x′_O2，z_O2)，…，On-1(x′_n-1，z_n-1)，On(x′_On，z_On)，位置A′以及目标位置B的坐标为：A’(x′_A′，z_A′)，B(x′_B，z_B)，

参照图6，混凝土泵车的臂架关节的运动规划的具体过程是：

4.1)系统初始化，

记录臂架末端在位置A′时各臂架的展角：α′_i＝α_i，i＝1、2、…n；

4.2)判断臂架末端第n和第n-1节臂架运动能否到达目标点，

固定好臂架O1，O2…On-1的展角，判断第n和第n-1节臂架展角α_n-1以及α_n，臂架末端能否到达目标位置，判断条件是：

C1)若L＜ln-1+ln，L是关节点On-2到B点的距离，则臂架末端第n和第n-1节臂架运动能够到达目标点B，执行步骤4.3)，计算臂架末端到达目标位置B的关节角；

C2)若L＞ln-1+ln，则臂架末端第n和第n-1节臂架运动不能到达目标点B，则跳转执行步骤4.6)，进行臂架展角调整；

4.3)计算臂架末端第n和第n-1节臂架的展角，

已知，目标点为B(x′_B，z_B)，前n-2节臂架的展角分别是α₁、α₂、…、α_n-2，末端最后两节臂架长度分别为ln-1、ln，第n-1关节点为On-1(x′_On-1，z_on-1)；

然后，求解臂架末端到达目标位置B，臂架末端关节n以及n-1的关节展角α_n-1和α_n，表达式见式(2)：

其中，α₁、α₂、…、α_n-2为臂架的展角；ln-1、ln为后两节臂架长度；x′_B、z_B分别为B(x′_B，z_B)的横纵坐标；x′_On-2、z_On-2分别为关节点On-2的横纵坐标；

根据式(2)求解得到关节展角α_n-1和α_n；

4.4)进行臂架关节运动与墙面障碍物的碰撞检测，具体过程是：

4.4.1)在图2所示的相对坐标系x-y-z中，已知墙面障碍物两端点的坐标是P1(x_p1，y_P1，h)，P2(x_P2，y_P2，h)，则墙面障碍物的空间方程组的表达式是：

已知，其中三个关节点的坐标分别是01(0，0，0)、On-1(x_On-1，y_On-1，z_On-1)、On(x_On，y_On，z_On)，则臂架平面方程组的表达式是：

其中a、b、c是平面方程的常量，联立上述方程组(3)和(4)，求解得到的x、y、z的值即为交点H(x_H，y_H，z_H)的坐标，交点H即为障碍物上可能会与臂架发生碰撞的碰撞点H；

4.4.2)如图6所示，计算交点H在坐标系x′-o-z中的坐标H(x′_H，z_H)，表达式是：

4.4.3)计算可能的碰撞点在混凝土泵车中的臂架编号，

已知关节点Oi(x′_i，z_i)、Oi+1(x′_i+1，z_i+1)和碰撞点H(x′_H，z_H)，

其中，x′_i、x′_i+1、x′_H是分别是关节点Oi、Oi-1和碰撞点H的横坐标；

若x′_i＜x′_H＜x′_i+1，则混凝土泵车可能的碰撞臂架为第i节臂架；

4.4.4)如图6所示，在坐标系x′-o-z中，根据关节点Oi(x′_i，z_i)、Oi+1(x′_i+1，z_i+1)，得到第i节臂架在坐标系x′-o-z中的线段方程，根据该线段方程，当x′＝x′_H1时，得到点H1(x′_H1，z_H1)，点H1即为臂架上可能会与障碍物发生碰撞的碰撞点，表达式是：

则存在以下两种情况：

D1)若z_H1-z_H＞0，则臂架未发生碰撞，执行步骤4.5)；

D2)若z_H1-z_H≤0，则臂架发生碰撞，执行步骤4.6)；

4.5)进行臂架姿态的“拱型”判断，

在混凝土泵车施工时，当臂架为直线时，此时泵送混凝土的阻尼最小，然而臂架泵送姿态很难保证臂架为直线，为了减小泵送阻尼，尽量保证臂架泵送姿态为“拱型”，即相邻臂架的夹角比较小，则判断臂架泵送姿态为拱型的条件为：相邻臂架夹角小于30度，表达式是：

|α₁+α₂+…α_i-1+α_i-π*(i-1)|≤30° (7)

其中α₁、α₂、α₃…α_i分别为混凝土泵车的各臂架展角，

若式(7)成立，则确定当前规划的臂架姿态为“拱型”，臂架关节运动规划完成，输出规划结果；

若式(7)不成立，则当前规划的臂架姿态不为“拱型”，则执行步骤4.6)；

4.6)遍历第n-2节臂架展角，

在图6中，A′点的仰角为θ_A′，目标点B的仰角为θ_B，α_n-2＞max(α_n-2)若θ_A′＞θ_B，则α_n-2＝α_n-2-Δα；否则α_n-2＝α_n-2+Δα，Δα为调整角；

若α_n-2＞max(α_n-2)或α_n-2＜min(α_n-2)，则展角α_n-2超出关节的极限角范围，即第n-2节臂架展角遍历完，α_n-2＝α′_n-2，执行步骤4.7)，其中max(α_n-2)以及min(α_n-2)为第n-2节臂架的最大以及最小展角；否则展角α_n-2未超出关节的极限角范围，返回步骤4.2)；

4.7)遍历第n-3节臂架展角，

若θ_A′＞θ_B，则α_n-3＝α_n-3-Δα；否则α_n-3＝α_n-3+Δα，Δα为调整角；

若α_n-3＞max(α_n-3)或α_n-3＜min(α_n-3)，展角α_n-3超出关节的极限角范围，即第n-3节臂架展角遍历完，α_n-3＝α′_n-3，执行步骤4.8)；其中max(α_n-3)以及min(α_n-3)为第n-3节臂架的最大以及最小展角；否则展角α_n-3未超出关节的极限角范围，返回步骤4.2)；

4.8)循环遍历第n-4，n-5…直到第2节臂架的展角，

以此类推，循环遍历第n-4，n-5…直到第2节臂架，如果所有臂架展角遍历过程都没有达到需要的规划要求，则执行步骤4.9)；

4.9)遍历第1节臂架展角，

若θ_A′＞θ_B，则α₁＝α₁-Δα；否则α₁＝α₁+Δα，Δα为调整角；

若α₁＞max(α₁)或α₁＜min(α₁)，展角α₁超出关节的极限角范围，即第1节臂架展角遍历完，α₁＝α′₁，执行步骤4.10)；其中max(α₁)以及min(α₁)为第1节臂架的最大以及最小展角；否则展角α₁未超出关节的极限角范围，返回步骤4.2)；

4.10)将最后一次未发生碰撞的臂架姿态作为最终的规划结果，

所有臂架展角都遍历完成，没有找到最优的臂架姿态，则以最后一次不会发生碰撞的臂架姿态作为规划结果输出，即成。

实施例

以三一重工制造的SY5418THB-52E型号的四节臂架的混凝土泵车为例，其中，以下长度单位均为米，四节臂架的长度分别为10.380、9.23、8.75、9.9，四个关节展角范围分别为90°、180°、180°、270°。

见图7，是实施例的施工场景，以混凝土泵车所在位置为原点，建立x-y-z坐标系，该施工环境下，现场有等高的墙面障碍物，墙面障碍物两个端点坐标为P1(20，8，15)、P2(-20，8，15)；初始状态时臂架初始转角为0，初始姿态时各臂架展角依次为75°、145°、145°、145°；则混凝土泵车的各关节点坐标为：O1(0，0，0)、O2(2.65，0，10.04)、O3(9.66，0，16.04)、O4(18.36，0，16.95)、O5(27.05，0，12.2)。设混凝土泵车的臂架末端的初始位置为A(27，0，12)，目标位置为B(10，15，19)。

在上述施工场景下，采用本发明的规划方法，利用matlab进行编程，实现对四节臂架进行轨迹规划。

在上述工况下，首先，进行臂架转动的规划：臂架转动范围有墙面障碍物，但转动不会发生碰撞，所以，混凝土泵车直接进行转台运动，从初始位置A运动到A′，表1给出了臂架到达点A′时各节臂架的展角。然后，根据进行臂架关节的运动规划，表2给出了臂架末端到达目标点B时各节臂架的展角，图8为臂架最终到达目标位置规划的姿态。

表1、在点A′时各臂架展角

表2、臂架末端到达目标点B时各臂架展角

上述的实施例说明，本发明的规划方法能够完美的实现混凝土泵车的臂架轨迹规划方法，显著提高了工作效率，确保了施工安全。

Claims (5)

1.一种基于驾驶员行为的混凝土泵车臂架轨迹规划方法，其特征在于，按照以下步骤具体实施：

步骤1：以混凝土泵车的中心O为原点建立相对坐标系x-y-z，另外，以现场地平面建立坐标系x-o-y；

步骤2：对混凝土泵车以及现场环境进行建模；

步骤3：对混凝土泵车的臂架转动进行规划，

根据现场的混凝土泵车、墙面障碍物在坐标系x-o-y平面位置的投影，实线OA为初始位置时臂架在坐标系x-o-y平面投影，此时臂架的转角为

虚线AA′为混凝土泵车转动其臂架末端轨迹在坐标系x-o-y平面投影；虚线OA′为转台运动完成后的臂架转角为

此时

步骤4：对混凝土泵车的臂架关节进行运动规划，

通过各臂架关节展角的变化，实现臂架末端位置从A′到达目标点B，

以混凝土泵车的中心O为原点，在臂架所在平面内建立相对坐标系x′-o-z，在该坐标系下混凝土泵车的各个关节点的坐标分别为：O1(0，0)，O2(x′_O2，z_O2)，…，On-1(x′_n-1，z_n-1)，On(x′_On，z_On)，位置A′以及目标位置B的坐标为：A’(x′_A′，z_A′)，B(x′_B，z_B)。

2.根据权利要求1所述的基于驾驶员行为的混凝土泵车臂架轨迹规划方法，其特征在于：所述的步骤1中，在相对坐标系x-y-z中，臂架第一关节点O1即与坐标系x-o-y的原点O重合。

3.根据权利要求1所述的基于驾驶员行为的混凝土泵车臂架轨迹规划方法，其特征在于：所述的步骤2中，建模具体过程是，

混凝土泵车共有n节臂架，各臂架关节点依次记为O1、O2、...、On-1、On，各臂架的展角依次记为α₁、α₂、...、α_n-1、α_n，各臂架的长度依次记为l1、l2、...、ln-1、ln；

利用D-H参数法，得到各臂架关节点的笛卡尔坐标是：O1(0，0，0)，O2(x_O2，y_O2，z_O2)...On-1(x_On-1，y_On-1，z_On-1)，On(x_On，y_On，z_On)，根据混凝土泵车的结构特点，将上述笛卡尔坐标系坐标转化为极坐标系坐标，混凝土泵车各关节点极坐标的转角相同，得到混凝土泵车的关节点极坐标是：

在施工现场设置有等高的墙面障碍物，墙面障碍物P1P2的两端点坐标分别为P1(x_p1，y_P1，h)、P2(x_P2，y_P2，h)，其中，h为墙面障碍物的高度，得到墙面障碍物的两端点极坐标为

和

臂架末端的初始位置为A(x_A，y_A，z_A)，臂架末端的目标位置为B(x_B，y_B，z_B)，则极坐标为

建模完毕。

4.根据权利要求1所述的基于驾驶员行为的混凝土泵车臂架轨迹规划方法，其特征在于：所述的步骤3中，混凝土泵车臂架的转动规划的具体过程是：

3.1)确认转动范围内有无墙面障碍物，

根据墙面障碍物的投影线段P1P2与臂架转动扇形区的相对位置关系，判断臂架转动范围内是否有墙面障碍物，

如果转动范围内无墙面障碍物，则执行步骤3.5)，实施转台运动；如果转动范围内有墙面障碍物，则执行步骤3.2)，

转动范围内是否有墙面障碍物的判断条件是：

A1)若

且z_On＜h，则确定臂架转动范围内有墙面障碍物；

A2)若

且z_On＜h，则确定臂架转动范围内有墙面障碍物；

A3)若

且min(r_P1，r_P2)＜min(r_A，r_B)且z_On＜h，则确定臂架转动范围内有墙面障碍物；

除去上述三种情况，其它情况下，均认为臂架转动范围内无墙面障碍物；

3.2)确定臂架转动过程中是否会与墙面障碍物碰撞，

3.2.1)计算臂架上可能的碰撞点k，

混凝土泵车总共有n节臂架，如果z_Oi＜h＜z_Oi+1，则第i节臂架能够确定与墙面障碍物发生碰撞，根据第i节臂架的两个端点坐标Oi(x_Oi，y_Oi，z_Oi)和Oi+1(x_Oi+1，y_Oi+1，z_Oi+1)，得到第i节臂架的空间直线方程，得到第i节臂架上与墙面障碍物可能的碰撞点k(x_k，y_k，z_k)，表达式是：

则得到碰撞点k的极距为：

3.2.2)臂架转动过程中，确认碰撞点k是否碰撞，

在坐标系x-o-y的平面内，墙面障碍物的投影线段为P1P2，以相对坐标系的原点O为圆心，以可能的碰撞点k的极距r_k为半径作圆；然后，判断坐标系x-o-y平面内线段P1P2与圆是否有交点，有以下可能性：

如果无交点，则在碰撞点k不会碰撞，即臂架转动与墙面障碍物无碰撞，则转入步骤3.5)，进行转台运动；

如果有交点，则在碰撞点k会碰撞，即臂架转动与墙面障碍物有碰撞，碰撞臂架为第i节臂架，则执行步骤3.3)；

3.3)将第i节臂架上扬，

将第i节臂架的展角增大，即α_i＝α_i+Δα，Δα为臂架调节增量角；

3.4)确定第i节臂架的展角α_i是否超过极限角度，判断条件是：

各个臂架的展角都有上下极限范围，分别记为max(α_i)以及min(α_i)，则有以下两种情况：

B1)若max(α_i)＜α_i，说明第i节臂架超过极限位置角度，则α_i＝α_i-Δα且i＝i-1，转回步骤3.3)；

B2)若max(α_i)＞α_i，说明第i节臂架未超过极限位置角度，则转回步骤3.2)；

3.5)进入转台运动，

臂架末端从初始位置A运动到位置A′，此时，转台的角度从

运动到

结束臂架的转动规划。

5.根据权利要求1所述的基于驾驶员行为的混凝土泵车臂架轨迹规划方法，其特征在于：所述的步骤4中，混凝土泵车的臂架关节的运动规划的具体过程是：

4.1)系统初始化，

记录臂架末端在位置A′时各臂架的展角：α′_i＝α_i，i＝1、2、...n；

4.2)判断臂架末端第n和第n-1节臂架运动能否到达目标点，

固定好臂架O1，O2…On-1的展角，判断第n和第n-1节臂架展角α_n-1以及α_n，臂架末端能否到达目标位置，判断条件是：

C1)若L＜ln-1+ln，L是关节点On-2到B点的距离，则臂架末端第n和第n一1节臂架运动能够到达目标点B，执行步骤4.3)，计算臂架末端到达目标位置B的关节角；

C2)若L＞ln-1+ln，则臂架末端第n和第n-1节臂架运动不能到达目标点B，则跳转执行步骤4.6)，进行臂架展角调整；

4.3)计算臂架末端第n和第n-1节臂架的展角，

已知，目标点为B(x′_B，z_B)，前n-2节臂架的展角分别是α₁、α₂、...、α_n-2，末端最后两节臂架长度分别为ln-1、ln，第n-1关节点为On-1(x′_On-1，z_On-1)；

然后，求解臂架末端到达目标位置B，臂架末端关节n以及n-1的关节展角α_n-1和α_n，表达式见式(2)：

其中，α₁、α₂、...、α_n-2为臂架的展角；ln-1、ln为后两节臂架长度；x′_B、z_B分别为B(x′_B，z_B)的横纵坐标；x′_On-2、z_On-2分别为关节点On-2的横纵坐标；

根据式(2)求解得到关节展角α_n-1和α_n；

4.4)进行臂架关节运动与墙面障碍物的碰撞检测，具体过程是：

4.4.1)在相对坐标系x-y-z中，已知墙面障碍物两端点的坐标是P1(x_P1，y_P1，h)，P2(x_P2，y_P2，h)，则墙面障碍物的空间方程组的表达式是：

已知其中三个关节点的坐标分别是O1(0，0，0)、On-1(x_On-1，y_On-1，z_On-1)、On(x_On，y_On，z_On)，则臂架平面方程组的表达式是：

其中a、b、c是平面方程的常量，联立上述方程组(3)和(4)，求解得到的x、y、z的值即为交点H(x_H，y_H，z_H)的坐标，交点H即为障碍物上可能会与臂架发生碰撞的碰撞点H；

4.4.2)计算交点H在坐标系x′-o-z中的坐标H(x′_H，z_H)，表达式是：

4.4.3)计算可能的碰撞点在混凝土泵车中的臂架编号，

已知关节点Oi(x′_i，z_i)、Oi+1(x′_i+1，z_i+1)和碰撞点H(x′_H，z_H)，

其中，x′_i、x′_i+1、x′_H是分别是关节点Oi、Oi-1和碰撞点H的横坐标；

若x′_i＜x′_H＜x′_i+1，则混凝土泵车可能的碰撞臂架为第i节臂架；

4.4.4)在坐标系x′-o-z中，根据关节点Oi(x′_i，z_i)、Oi+1(x′_i+1，z_i+1)，得到第i节臂架在坐标系x′-o-z中的线段方程，根据该线段方程，当x′＝x′_H1时，得到点H1(x′_H1，z_H1)，点H1即为臂架上可能会与障碍物发生碰撞的碰撞点，表达式是：

则存在以下两种情况：

D1)若z_H1-z_H＞0，则臂架未发生碰撞，执行步骤4.5)；

D2)若z_H1-z_H≤0，则臂架发生碰撞，执行步骤4.6)；

4.5)进行臂架姿态的“拱型”判断，

在混凝土泵车施工时，当臂架为直线时，此时泵送混凝土的阻尼最小，然而臂架泵送姿态很难保证臂架为直线，为了减小泵送阻尼，尽量保证臂架泵送姿态为“拱型”，即相邻臂架的夹角比较小，则判断臂架泵送姿态为拱型的条件为：相邻臂架夹角小于30度，表达式是：

|α₁+α₂+...α_i-1+α_i-π*(i-1)|≤30° (7)

其中α₁、α₂、α₃...α_i分别为混凝土泵车的各臂架展角，

若式(7)成立，则确定当前规划的臂架姿态为“拱型”，臂架关节运动规划完成，输出规划结果；

若式(7)不成立，则当前规划的臂架姿态不为“拱型”，则执行步骤4.6)；

4.6)遍历第n-2节臂架展角，

A′点的仰角为θ_A′，目标点B的仰角为θ_B，α_n-2＞max(α_n-2)

若θ_A′＞θ_B，则α_n-2＝α_n-2-Δα；否则α_n-2＝α_n-2+Δα，Δα为调整角；

若α_n-2＞max(α_n-2)或α_n-2＜min(α_n-2)，则展角α_n-2超出关节的极限角范围，即第n-2节臂架展角遍历完，α_n-2＝α′_n-2，执行步骤4.7)，其中max(α_n-2)以及min(α_n-2)为第n-2节臂架的最大以及最小展角；否则展角α_n-2未超出关节的极限角范围，返回步骤4.2)；

4.7)遍历第n-3节臂架展角，

若θ_A′＞θ_B，则α_n-3＝α_n-3-Δα；否则α_n-3＝α_n-3+Δα，Δα为调整角；

若α_n-3＞max(α_n-3)或α_n-3＜min(α_n-3)，展角α_n-3超出关节的极限角范围，即第n-3节臂架展角遍历完，α_n-3＝α′_n-3，执行步骤4.8)；其中max(α_n-3)以及min(α_n-3)为第n-3节臂架的最大以及最小展角；否则展角α_n-3未超出关节的极限角范围，返回步骤4.2)；

4.8)循环遍历第n-4，n-5...直到第2节臂架的展角，

以此类推，循环遍历第n-4，n-5...直到第2节臂架，如果所有臂架展角遍历过程都没有达到需要的规划要求，则执行步骤4.9)；

4.9)遍历第1节臂架展角，

若θ_A′＞θ_B，则α₁＝α₁-Δα；否则α₁＝α₁+Δα，Δα为调整角；

若α₁＞max(α₁)或α₁＜min(α₁)，展角α₁超出关节的极限角范围，即第1节臂架展角遍历完，α₁＝α′₁，执行步骤4.10)；其中max(α₁)以及min(α₁)为第1节臂架的最大以及最小展角；否则展角α₁未超出关节的极限角范围，返回步骤4.2)；

4.10)将最后一次未发生碰撞的臂架姿态作为最终的规划结果，

所有臂架展角都遍历完成，没有找到最优的臂架姿态，则以最后一次不会发生碰撞的臂架姿态作为规划结果输出，即成。

Images