CN103914082A - 智能臂空间运动控制方法及装置 - Google Patents

Abstract

本发明公开了一种智能臂空间运动控制方法及装置，该方法包括：接收各角度传感器感测及发送的智能臂处于当前位置时各节臂架与水平面之间的夹角，并接收遥控器发送的运动指令；计算智能臂从当前位置运动到目标位置时的坐标改变量；并确定约束条件，再根据预设的优化算法，结合各臂架动作能力和/或遥控器的手柄开度变化确定智能臂的相邻两节臂架之间的夹角在智能臂处于当前位置与目标位置时的角度差；根据转台旋转角度控制智能臂转台的运动，以及根据相邻两节臂架之间夹角的角度差控制智能臂臂架的运动。本发明提供的控制方法能避免智能臂架空间运动时臂架油缸出现速度突变和/或避免智能臂架空间运动时大油缸急速换向造成的臂架末端抖动的现象。

Description

智能臂空间运动控制方法及装置

技术领域

本发明涉及智能臂控制技术领域，特别涉及智能臂空间运动控制方法及装置。

背景技术

目前，对于混凝土泵车臂架的控制，基本上采取单个遥控手柄控制单个臂架或者转台。一般来说，操作者将混凝土从泵车料斗中输送到楼盘的随意位置，然后由工人将混凝土均匀摊铺开，虽然相比人工运输混凝土节省了大量人力物力，但还是存在着繁重的劳动，一方面泵车操作者需注意泵车臂架姿态，考虑操作的正确性，误操作易导致臂架撞上障碍物；若动作臂架幅度过大，对臂架末端软管下的搀扶者也会带来伤害。另一方面工人摊铺混凝土也很麻烦。在此背景下，泵车操作者希望有一种简单的操作方式来实现混凝土的典型工况均匀布料，泵车制造者也希望制造出更高性能的混凝土泵车，为其扩大市场占有率，由此，智能臂架技术应运而生。

混凝土泵车臂架（如智能臂）在三维空间的坐标（X Y Z）可以如图1所示，该三维空间的确定方式可以如下：在泵车臂架可达的空间内，以泵车转台与第1节臂架始端铰接处作为坐标原点，以车身的料斗方向为x正半轴，以垂直于车身的方向为y轴正半轴，根据笛卡尔坐标系右手定则，可确定z轴；h表示垂足。当然，根据实际需要可以以其他的点为坐标原点，确定其他的方向为x正半轴，并相应确定y轴及z轴。

对于混凝土泵车臂架末端空间位置在水平面的投影h的获取，采用三维坐标计算方式，即：

$\left\{\begin{array}{c}X=\cos \left({\mathrm{\α}}_0\right)\·\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\·\cos {\mathrm{\α}}_i\\ Y=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\·\sin {\mathrm{\α}}_i\\ Z=\sin \left({\mathrm{\α}}_0\right)\·\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\·\sin {\mathrm{\α}}_i\end{array}\right.---\left(1\right)$

其中，X、Y、Z为臂架末端点的三维坐标值，n为臂架的节数，L_i为第i节臂架的长度，α₀为转台旋转角度，α_i（i>0）表示第i节臂架与水平面的夹角。

智能臂架控制技术通过采集臂架及转台的实时角度，通过一系列计算及控制输出，实现臂架末端的水平方向、垂直方向（或者复合）平稳的运动，其控制示意图如图2所示，利用一个万向手柄以及转台手柄，将遥控器切换至自动模式下，实现臂架末端的水平方向、垂直方向（或者复合）平稳的运动。

智能臂架控制技术对臂架末端的位置精度有着很高的要求，各臂架的运动速度如果与驱动规划量不一致，则会导致臂架末端出现位置偏差甚至大幅震荡。

1）臂架在其极限角度停止或者启动时容易造成臂架末端位置出现大抖动，如图3所示，在A点，2#臂架油缸到达其极限位置（1#、2#臂架夹角到180°），1#、3#臂架油缸速度出现阶跃突变，由于在实际过程中，臂架油缸的速度不可能出现阶跃突变，此时臂架末端位置一定会出现偏差，在臂架油缸速度调节的过程中，受发动机转速和臂架泵供油的影响，臂架油缸速度也会出现波动，导致臂架末端位置出现抖动。若强制臂架降速，在运动规划之外的运动会造成臂架末端走位偏差；

2）臂架大油缸有杆腔、无杆腔快速换向时，受臂架泵供油影响，容易造成臂架末端位置出现大抖动，如图4所示，以右上角的臂架俯视图为例，在臂架末端从B点到C点的转弯过程中，几乎所有臂架油缸速度都出现正负的变化，这对臂架末端的精确控制是非常不利的，一是因为臂架油缸有杆腔、无杆腔快速换向时，存在一定的压力建立过程，油缸越大，建立过程越缓慢，而且此时油缸是不会动作的；二是受臂架泵供油影响，若各臂架油缸流量分配不平衡，也会导致臂架油缸不按给定速度来响应，从而导致臂架末端的位置偏差及抖动。

发明内容

有鉴于此，本发明提出一种智能臂空间运动控制方法及装置，以避免智能臂架空间运动时臂架油缸出现速度突变和/或避免智能臂架空间运动时臂架油缸有杆腔、无杆腔快速换向。

一方面，提供一种智能臂空间运动控制方法，应用于具有n节臂架的智能臂，其中所述n大于等于2，所述智能臂空间运动控制方法包括：接收各角度传感器感测及发送的所述智能臂处于当前位置时各节臂架与水平面之间的夹角，并接收用于控制所述智能臂运动的遥控器发送的运动指令；根据所述各节臂架与水平面之间的夹角、所述智能臂各节臂架的长度及所述运动指令计算所述智能臂从当前位置运动到目标位置时的坐标改变量；并根据所述坐标改变量确定约束条件，再根据预设的优化算法，同时结合各臂架动作能力和/或所述遥控器的手柄开度变化确定所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差；根据所述运动指令确定转台旋转角度，并根据所述转台旋转角度控制所述智能臂转台的运动，以及根据所述相邻两节臂架之间夹角的角度差控制所述智能臂臂架的运动。

进一步地，所述优化算法以使所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差的平方和最小为优化目标，所述优化目标表示为：其中，系数K_i根据所述智能臂第i节臂架动作能力及所述遥控器的手柄开度变化确定；在i>1时Δθ_i为第i-1节臂架与第i节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差；Δθ₁为第1节臂架与水平面之间夹角在当前位置与目标位置时的变化量。

进一步地，所述“结合各臂架动作能力确定所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差”的步骤包括：根据预设的所述智能臂各节臂架之间夹角的最大值及最小值，判断所述智能臂在目标位置时各节臂架之间夹角是否在对应的所述最小值与所述最大值之间的范围内；将臂架之间夹角处于对应的所述最小值与所述最大值之间的范围外的臂架对应的系数K_i确定为零，并重新确定所述智能臂中其他节臂与其相邻臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差。

进一步地，所述“结合各臂架动作能力确定所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差”的步骤包括还包括：根据预设的转台角度的最大值及最小值，判断所述转台旋转角度是否在所述转台角度的最小值与所述转台角度的最大值之间的范围内；在所述转台旋转角度位于所述转台角度的最小值与所述转台角度的最大值之外的范围时，停止所述智能臂各节臂架的动作。

进一步地，所述“结合各臂架动作能力确定所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差”的步骤包括还包括：将臂架之间夹角处于对应的所述智能臂各节臂架之间夹角的最小值与最大值之间的范围内且位于预设的增速范围内的臂架对应的系数K_i减小，将臂架之间夹角处于对应的所述最小值与所述最大值之间的范围内且位于预设的减速范围内的臂架对应的系数K_i增大。

进一步地，所述“结合所述遥控器的手柄开度变化确定所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差”的步骤包括：比较预存的手柄开度值与所述遥控器的手柄的当前开度值；在所述预存的手柄开度值与所述遥控器的手柄的当前开度值的差值大于预设阈值时，将大臂对应的系数K_i清零，以锁定所述大臂，以及在间隔预设时间后，控制所述大臂对应的系数K_i由大变小；将所述手柄开度值更新为所述遥控器的手柄的当前开度值。

另一方面，提供一种智能臂空间运动控制装置，应用于具有n节臂架的智能臂，其中所述n大于等于2，所述智能臂空间运动控制装置包括：接收单元，用于接收各角度传感器感测及发送的所述智能臂处于当前位置时各节臂架与水平面之间的夹角，并接收用于控制所述智能臂运动的遥控器发送的运动指令；夹角差确定单元，根据所述各节臂架与水平面之间的夹角、所述智能臂各节臂架的长度及所述运动指令计算所述智能臂从当前位置运动到目标位置时的坐标改变量；并根据所述坐标改变量确定约束条件，再根据预设的优化算法，同时结合各臂架动作能力和/或所述遥控器的手柄开度变化确定所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差；运动控制单元，用于根据所述运动指令确定转台旋转角度，并根据所述转台旋转角度控制所述智能臂转台的运动，以及根据所述相邻两节臂架之间夹角的角度差控制所述智能臂臂架的运动。

进一步地，所述夹角差确定单元（63）包括：优化子单元，用于以使所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差的平方和最小为优化目标，所述优化目标表示为：其中，系数K_i根据所述智能臂第i节臂架动作能力及所述遥控器的手柄开度变化确定；在i>1时Δθ_i为第i-1节臂架与第i节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差；Δθ₁为第1节臂架与水平面之间夹角在当前位置与目标位置时的变化量。

进一步地，所述优化子单元包括：第一处理模块，用于根据预设的所述智能臂各节臂架之间夹角的最大值及最小值，判断所述智能臂在目标位置时各节臂架之间夹角是否在对应的所述最小值与所述最大值之间的范围内，并将臂架之间夹角处于对应的所述最小值与所述最大值之间的范围外的臂架对应的系数K_i确定为零，且重新确定所述智能臂中其他节臂与其相邻臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差，以及将臂架之间夹角处于对应的所述智能臂各节臂架之间夹角的最小值与最大值之间的范围内且位于预设的增速范围内的臂架对应的系数K_i减小，将臂架之间夹角处于对应的所述最小值与所述最大值之间的范围内且位于预设的减速范围内的臂架对应的系数K_i增大；还用于根据预设的转台角度的最大值及最小值，判断所述转台旋转角度是否在所述转台角度的最小值与所述转台角度的最大值之间的范围内；在所述转台旋转角度位于所述转台角度的最小值与所述转台角度的最大值之外的范围时，将所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差确定为零，以停止所述智能臂各节臂架的动作。

进一步地，所述优化子单元包括：第二处理模块，用于比较预存的手柄开度值与所述遥控器的手柄的当前开度值，在所述预存的手柄开度值与所述遥控器的手柄的当前开度值的差值大于预设阈值时，将大臂对应的系数K_i清零，以锁定所述大臂，以及在间隔预设时间后，控制所述大臂对应的系数K_i由大变小，以及将所述手柄开度值更新为所述遥控器的手柄的当前开度值。

本发明智能臂空间运动控制方法及装置通过各臂架动作能力对每节臂架的动作角度进行分配和限制，能实现臂架到位的启停对臂架系统冲击小，臂架末端运行平稳；和/或，通过遥控器的手柄开度变化对每节臂架的动作角度进行分配和限制，能缓解臂架转弯，大油缸急速换向造成的臂架末端抖动的现象。

附图说明

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

图1为现有智能臂在三维空间的坐标示意图；

图2为现有智能臂架控制示意图；

图3为现有智能臂架多路阀响应电流示意图；

图4为现有智能臂架末端转弯臂架多路阀电流；

图5为本发明实施例一提供的智能臂空间运动控制方法流程图；

图6为本发明实施例提供的智能臂空间运动控制方法中大臂权值处理的流程图；

图7为本发明实施例提供的智能臂空间运动控制方法中运动规划的流程图；

图8为本发明实施例提供的智能臂空间运动规划方法中坐标系分解示意图；

图9为本发明实施例提供的智能臂空间运动规划方法中转台旋转角度求解示意图；

图10为本发明实施例提供的智能臂空间运动规划方法中相邻臂架之间夹角在不同位置的差值求解示意图；

图11为本发明实施例提供的智能臂空间运动控制方法中角度差处理的流程图；

图12为本发明实施例提供的智能臂空间运动控制装置的结构框图。

具体实施方式

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

图5为本发明实施例提供的智能臂空间运动控制方法的流程图，该智能臂空间运动控制方法应用于具有n节臂的智能臂，其中n大于等于2；如图5所示，该智能臂空间运动控制方法包括：

步骤11：接收并解析用于控制所述智能臂运动的遥控器发送的运动指令；

步骤12：根据该运动指令判断遥控器手柄的当前开度值（即遥控信号）相对于上次开度值变化量是否大于预设阈值，并所述遥控器的手柄的当前开度值与上次开度值的差值大于预设阈值时，执行步骤13，否则，执行步骤14；

步骤13：执行大臂权值处理子程序，对大臂权值进行处理的步骤具体参见图6的解释说明；

步骤14：根据接收各角度传感器感测及发送的所述智能臂处于当前位置时各节臂架与水平面之间的夹角，对智能臂的运动进行协调规划，具体参见图7的解释说明；

步骤15：判断各臂架目标角度值是否在预设的理想范围之内，如否则执行步骤16，若是，则执行步骤17；

步骤16：执行角度处理子程序，即调整所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差值，具体步骤详见图11的解释说明；

步骤17：输出控制量，该控制量可以包括根据所述运动指令确定的转台旋转角度、以及所述相邻两节臂架之间夹角的角度差值，以便于根据所述转台旋转角度控制所述智能臂转台的运动，以及根据所述相邻两节臂架之间夹角的角度差值控制所述智能臂臂架的运动。

可以理解的是根据转台旋转角度及臂架夹角角度差Δθ_i，控制转台及臂架的运动，即通过求解出以及Δθ_i，臂架的运动轨迹就已确定；具体可以包括：

首先对转台旋转角度及臂架夹角角度差进行平滑滤波并计算用于驱动转台运动的马达的速度及用于驱动智能臂运动的油缸的速度；当然根据实际需要，可以预先设置最大速度及最小速度以对马达和油缸的速度进行限定；

然后根据转台旋转角度确定的马达速度控制马达的实时流量，并根据角度差确定的油缸速度控制油缸的实时流量。

再根据预设的电磁阀电流与马达流量之间的对应关系（可以根据试验预先测量获得），根据马达的实时流量确定马达对应电磁阀的电流值；以及根据预设的电磁阀电流与油缸流量的对应关系（可以根据试验预先测量获得），根据油缸的实时流量确定油缸对应电磁阀的电流值，进而实现对智能臂及转台运动的统筹控制。

本实施例通过对大臂角度调整的权值及行走规划后的角度差进行相应调整，并通过在对智能臂架控制量进行规划求解时，在角度变化量前面加上与臂架动作能力相关的时变系数，对每节臂架的动作能力进行分配和限制，能实现臂架到位的启停对臂架系统冲击小，臂架末端运行平稳；而且臂架转弯，大油缸急速换向造成的臂架末端抖动现象能大为缓解。

图6为本发明实施例提供的智能臂空间运动控制方法中大臂权值处理的流程图，如图6所示，具体包括：

步骤131：比较预存的手柄开度值（即上周期的开度值）与所述遥控器的手柄的当前开度值；

步骤132：将所述手柄开度值更新为所述遥控器的手柄的当前开度值；需要说明的是，步骤132与步骤133可以并行执行，也可以在步骤133之后执行；即满足在“比较”操作之后再更新手柄开度值即可；

步骤133：判断上周期的开度值与所述遥控器的手柄的当前开度值的差值是否大于预设阈值，并在大于该预设阈值时，执行步骤134，否则执行步骤138；可以理解的是，具体操作时，步骤131及步骤133可以与步骤12合并，即此处步骤131及步骤133为冗余操作；

步骤134：开始计时；

步骤135：将大臂权值清零，以锁定所述大臂；

步骤136：判断计时时间是否到预设时间阈值ET，若是，则执行步骤138；若否，则执行步骤137；其中该预设时间阈值ET是一个经过调试的值，以实际调试过程中效果最好的值为准；

步骤137：判断计时时间是否到预设时间阈值的一半，若是，执行步骤139，否则返回至步骤135；需要说明的是，此步骤中的判断条件不限定为预设时间阈值的一半，在阈值之内的值都可以，均能产生一个随计时时间变化的K_i值（其具体含义可以参见步骤76的解释说明）；

步骤138：各臂架的权值随角度值变化，以便将各权值输出；其中，权值随角度值变化就相当于正常情况下的变化，即遥控器手柄开度不变或者变化较小，K_i值完全由角度动作能力决定；

步骤139：各臂架的权值随计时值变化，以便将各权值输出。

本实施例为减小油缸有杆腔、无杆腔快速换向造成臂架末端出现大抖动的影响，实时监控万向手柄的输入量，并与上一程序周期存储的万向手柄开度值进行比较，若发现输入量变化较大，则说明臂架末端正在进行转弯的动作，经过分析发现大臂有杆腔、无杆腔快速换向对智能臂架的运行精度影响比较大，此时锁定1#、2#等大臂，并在一定时间之后给相应臂架一个随计时时间变化的K值，使K值由大往小变化，最终成为该臂架油缸在当前臂架夹角下对应的值，从而限制各节臂角度的变化，使臂架大油缸逐步缓慢换向，这样臂架大油缸急起急停造成的臂架末端点的位置波动及抖动现象大为缓解。

图7本发明实施例提供的智能臂空间运动控制方法中运动规划的流程图，如图7示，具体包括：

步骤71：根据智能臂处于当前位置时臂架末端在臂架空间坐标系中的坐标，确定所述臂架末端在臂架平面坐标系中的第一坐标及在臂架投影坐标系中的第二坐标；

具体如图8所示，将立体三维坐标拆分成两个平面二维坐标，分别为臂架平面坐标和臂架投影坐标，其中，该臂架平面坐标为臂架平面坐标系[O;x,y]中的坐标，智能臂在该臂架平面内只有直线移动没有转动，该臂架投影坐标为臂架投影坐标系[O;x,z]中的坐标，智能臂在该臂架投影平面只有转动没有直线移动；在建立上述第一坐标及第二坐标后，在臂架平面及臂架投影平面内对智能臂的运动进行协调规划，详见以下描述；

步骤72：根据接收的用于指示智能臂运动到目标位置的运动指令（即遥控器发送的运动指令）及第二坐标，确定智能臂处于目标位置时臂架末端在臂架投影坐标系中的第三坐标；

如图9所示，假设在一个规划周期中，希望臂架末端点从A点（臂架末端点当前位置在臂架投影坐标系中的对应位置）移至B点（臂架末端点目标位置在臂架投影坐标系中的对应位置），B点坐标值(x_n+1,z_n+1)是在A点坐标值(x_n,z_n)的基础上，加上遥控器的运动指令得到，即：

$\left\{\begin{array}{c}{x}_{n+1}=x_n+{\mathrm{RC}}_x\\ z_{n+1}=z_n+{\mathrm{RC}}_z\end{array}\right.---\left(2\right)$

其中RC_x、RC_z分别代表遥控器万向手柄（其移动方向详见图9中遥控器中的指示箭头，其与向量的方向基本一致）在x、z方向的解析输入量。

步骤73：根据第二坐标及第三坐标计算得到智能臂的转台旋转角度；

继续参见图9，具体可以通过下述方式计算得到转台旋转角度：

其中，分别为臂架末端在A点及B点与臂架投影坐标系的坐标原点形成的向量，×为向量叉积运算符号，为对应向量的长度，·为乘法运算；可以理解的是，还可以根据用三角函数的反余弦（acos）、反正切（atan）、反余切（acot）函数计算转台旋转角度。

步骤74：根据第一坐标、测量得到的智能臂处于当前位置时各臂架与水平面之间的倾角值、智能臂各节臂架的长度计算智能臂从当前位置运动到目标位置时在臂架平面坐标系中的坐标改变量；可以理解的是，测量得到的智能臂处于当前位置时各臂架与水平面之间的倾角值与测量得到的智能臂处于当前位置时各臂架与竖直面之间的倾角值是等同的，因为两个夹角是互余的，即两个夹角的和为90度；

对于臂架末端在臂架平面内的运动，如图10所示，其中角度θ_j及θ_j'（j可以在1至n之间取值自然数）在j>1时分别表示臂架末端位于A′及B′点时第j-1节臂架与第j节臂架之间的夹角；在j=1时分别表示臂架末端位于A′及B′点时第1节臂架与水平面之间的夹角；假设在一个规划周期中，希望臂架末端点从A′点（臂架末端当前位置在臂架平面内的对应位置）移至B′点（臂架末端目标位置在臂架平面内的对应位置），这里的A′点与图9中的A点均对应臂架末端在三维空间的当前位置，B′点与图4中的B点均对应臂架末端在三维空间的目标位置，对于n节臂架系统，A′点的坐标表示可以如下：

$\left\{\begin{array}{c}{x}_n=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\·\cos (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π})\\ y_n=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\·\sin (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π})\end{array}\right.---\left(4\right)$

B′点的坐标表示可以如下：

$\left\{\begin{array}{c}{x}_{n+1}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\·\cos (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}({\mathrm{\θ}}_j+{\mathrm{\Δ\θ}}_j)-(i-1)\·\mathrm{\π})\\ y_{n+1}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\·\sin (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}({\mathrm{\θ}}_j+{\mathrm{\Δ\θ}}_j)-(i-1)\·\mathrm{\π})\end{array}\right.---\left(5\right)$

其中，Δθ_j（j>1时）为第j-1节臂架与第j节臂架之间夹角在B′点与A′点时的变化量；Δθ₁为第1节臂架与水平面之间夹角在B′点与A′点时的变化量；L_i为第i节臂架的长度，n是臂架的节数；臂架夹角θ_j与臂架倾角α_j的对应关系为：

${\mathrm{\α}}_j=\underset{i=1}{\overset{j}{\mathrm{\Σ}}}{\mathrm{\θ}}_i-(j-1)\·\mathrm{\π}---\left(6\right)$

其中，臂架倾角α_j为第j节臂架与水平面之间的夹角，可以由安装在第j节臂架根部的角度传感器测量得到；

由式（5）减去式（4），可得：

$\left\{\begin{array}{c}\mathrm{\Δx}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\·(\cos (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}({\mathrm{\θ}}_j+{\mathrm{\Δ\θ}}_j)-(i-1)\·\mathrm{\π})-\cos (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π}))\\ \mathrm{\Δy}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\·(\sin (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}({\mathrm{\θ}}_j+{\mathrm{\Δ\θ}}_j)-(i-1)\·\mathrm{\π})-\sin (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π}\left)\right)\end{array}\right.---\left(7\right)$

其中，Δx、Δy为臂架末端从A′点运动到B′点时臂架平面坐标系的坐标改变量。

由于本实施例空间位置规划是基于当前位置进行下一步规划，当前位置与目标位置之间的距离非常小，我们可以对式（7）进行非线性方程线性化处理，即：

f(x+Δx)＝f(x)+f(x)′Δx  （8）

根据式（8），式（7）可简化为：

$\left\{\begin{array}{c}\mathrm{\Δx}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{a}_i\·\mathrm{\Δ}{\mathrm{\θ}}_i\\ \mathrm{\Δy}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{b}_i\·\mathrm{\Δ}{\mathrm{\θ}}_i\end{array}\right.---\left(9\right)$

其中， $\left\{\begin{array}{c}{a}_k=-\underset{i=k}{\overset{n}{\mathrm{\Σ}}}{L}_i\·\sin (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π})\\ b_k=\underset{i=k}{\overset{n}{\mathrm{\Σ}}}{L}_i\·\cos (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π})\end{array}\right.$

步骤75：以坐标改变量确定约束条件；具体地，在理想状态下在臂架末端的运动过程中，A′点与B′点的纵坐标保持不变；同时A′点与B′点的横坐标之差即为图4中的故，根据公式（7）约束条件可以表示为：

$\left\{\begin{array}{c}\mathrm{\Δx}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\·(\cos (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}({\mathrm{\θ}}_j+{\mathrm{\Δ\θ}}_j)-(i-1)\·\mathrm{\π})-\cos (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π}))=\left|\stackrel{\→}{\mathrm{OB}}\right|-\left|\stackrel{\→}{\mathrm{OA}}\right|\\ \mathrm{\Δy}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\·(\sin (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}({\mathrm{\θ}}_j+{\mathrm{\Δ\θ}}_j)-(i-1)\·\mathrm{\π})-\sin (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π}\left)\right)=0\end{array}\right.---\left(10\right)$

其中，分别为臂架末端在当前位置及目标位置与臂架投影坐标系的坐标原点形成的向量，θ_j（j>1时）为第j-1节臂架与第j节臂架之间的夹角；θ₁为第1节臂架与水平面的夹角，L_j为第j节臂架的长度；

相应地，根据公式（9）约束条件可以表示为：

$\left\{\begin{array}{c}\mathrm{\Δx}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{a}_i\·\mathrm{\Δ}{\mathrm{\θ}}_i=\left|\stackrel{\→}{\mathrm{OB}}\right|-\left|\stackrel{\→}{\mathrm{OA}}\right|\\ \mathrm{\Δy}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{b}_i\·\mathrm{\Δ}{\mathrm{\θ}}_i=0\end{array}\right.---\left(11\right)$

步骤76：根据预设的优化算法，以使智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差最小为优化目标，确定智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差，进而便于根据转台旋转角度及角度差，控制泵车智能臂及转台的运动；上述优化目标可以表示为：

$\min \left(\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{(K_i\·\mathrm{\Δ}{\mathrm{\θ}}_i)}^2\right)---\left(12\right)$

其中，在i>1时Δθ_i为第i-1节臂架与第i节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差；Δθ₁为第1节臂架与水平面之间夹角在当前位置与目标位置时的变化量；系数K_i根据所述智能臂第i节臂架动作能力及所述遥控器的手柄开度变化确定；即K_i为与臂架动作能力以及遥控器万向手柄开度变化相关的时变系数，其中臂架动作能力的含义如下：若第i节臂架与第i-1节臂架之间的夹角需要满足在10°至170°之间，若当前第i节臂架与第i-1节臂架之间夹角在20°至160°之间时，可以认为第i节臂架的动作能力较大，此时可以将K_i设置为较小值且基本不变化，在10°至20°之间或160°至170°之间时，可以认为第i节臂架的动作能力较小，此时可以将K_i设置为较大值（即对应的Δθ_i较小，实现臂架极限角度的智能缓冲）；此外，遥控器万向手柄开度变化较大通常体现在臂架的运动方向改变、臂架的启动及转弯等动作时，即油缸状态急剧变化时，此时可以将K_i设置为较大值；上述系数K_i可以同时根据智能臂第i节臂架动作能力及遥控器的手柄开度变化确定，具体操作时，可以根据实际操作经验，设置不同臂架动作能力及遥控器的手柄开度变化时，对应系数K_i的具体值；总之，通过增大或减小臂架的K_i值（其为时变的权值因子，对每节臂架的动作能力进行分配和限制），则式（12）中对应的Δθ_i将会对应减小或者增大，从而从规划中改变了臂架的速度，从而实现臂架油缸的平缓停止及启动。

需要说明的是，除了以角度差最小为优化目标以外，还可以以臂架油缸流量变化最小或者臂架变化节数最少等作为优化目标，均根据控制要求确定。此外，用于计算转台旋转角度的各步骤与用于计算臂架在不同位置时的角度差的步骤之间没有先后次序关系。

本实施例中通过将立体三维坐标拆分成两个平面二维坐标，求解智能臂的相邻两节臂架之间的夹角在智能臂处于当前位置与目标位置时的角度差以及转台旋转角度，并转化为臂架及转台电磁阀（多路阀）的驱动电流，统筹规划智能臂架空间运动时臂架平面内的运动与转台的运动，实现了臂架的空间直线行走，便于程序处理；空间位置规划基于当前位置进行下一步规划，避免长远规划点可能出现的臂架（转台）往复运动；同时以油缸动作变化率最小为目标优化函数，油缸的规划运行速度连续、平稳，通过自动控制技术，臂架末端运行时抖动较小，提高了布料精度，极大的降低了臂架操作难度。同时通过在对智能臂架控制量进行规划求解时，在角度变化量前面加上与臂架动作能力相关的时变系数K，对每节臂架的动作能力进行分配和限制，能实现臂架到位的启停对臂架系统冲击小，臂架末端运行平稳；而且臂架转弯，大油缸急速换向造成的臂架末端抖动现象能大为缓解。

为保证臂架末端的平稳运行，对各臂架需设定加减速区间，通过改变权值K_i来改变各臂架在不同运动区间内的运动速度，以免突然启动或者停止各臂架时臂架末端出现大的抖动，如图11所示的本发明实施例提供的智能臂空间运动控制方法中角度差处理的流程图，具体可以包括：

步骤161：根据所述智能臂中其他节臂与其相邻臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差与当前角度值的和得到各节臂在所述智能臂处于目标位置时的角度值（即在调用本子程序之前已经求出各臂架的角度变化值）；

步骤162：判断根据上述运动指令确定的转台旋转角度是否在预设的转台角度的最小值与最大值之间的范围内；该最大值及最小值即为极限值；若是，则执行步骤164，否则执行步骤163；

步骤163：在所述转台旋转角度位于所述转台角度的最小值与所述转台角度的最大值之外的范围时，将所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差确定为零，以停止所述智能臂各节臂架的动作（根据需要还可以设置报警动作）；

步骤162与163的目的在于要保证转台的角度值合理，若臂架旋转角度将要超过极限值（转台角度超过±360°或单侧支撑时的相应极限值），需立即停止臂架的动作，否则在外围的安全保护程序停止转台马达动作之后，臂架油缸仍然运动显然是不合理的；

步骤164：根据预设的所述智能臂各节臂架之间夹角的最大值及最小值，判断所述智能臂在目标位置时各节臂架之间夹角是否在对应的所述最小值与所述最大值之间的范围内，若是，则执行步骤166；若否则，执行步骤165；

步骤165：将臂架之间夹角处于对应的所述最小值与所述最大值之间的范围外（包括处于最大值或最小值的情况）的臂架对应的系数K_i确定为零（即锁定到位的臂架使其不参与运动规划），并重新确定所述智能臂中其他节臂与其相邻臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差，返回至步骤161；

步骤164与165的目的在于要保证各个臂架的展开或者收拢角度值在留有一定余量的前提下是合理的，例如2#臂架往180°方向继续展开或者往20°方向继续收拢以及6#臂架与4#臂架或者5#臂架干涉时，需停止相应臂架油缸的动作，此时将此臂架的K值设置为0，即将此臂架剔除出运动规划，保证其余臂架合理的动作。

步骤166：在臂架之间夹角处于对应的所述智能臂各节臂架之间夹角的最小值与最大值之间的范围内时，进一步判断臂架之间夹角是否且位于预设的增速范围内或位于预设的减速范围内；若是，则执行步骤167；否则，执行步骤168；

步骤167：将位于预设的增速范围内的臂架对应的系数K_i减小，将臂架之间夹角处于对应的所述最小值与所述最大值之间的范围内且位于预设的减速范围内的臂架对应的系数K_i增大；

步骤166与167的目的在于若臂架新的角度值进入了其油缸速度限制区间，需实时改变权值K，假设2#臂架的减速区间是160°～180°，若对臂架油缸动作速度不加以限制，规划出来的油缸速度会是一个比较大的值，当臂架夹角到达180°时，油缸会突然停止动作，如图2所示，这种情况显然也是我们所不愿意看到的，此时，若增大2#臂架的K值，则式（4）中对应的Δθ₂将会减小，从而从规划中减小了2#臂架的速度，随着θ₂的逐渐增大，Δθ₂会逐步减小，最终慢慢归于0，从而实现臂架油缸的平缓停止。

步骤168:控制量输出。

可以理解的是，图5中步骤15判断臂架目标角度值是否合理的操作可以包括步骤162、164及166的操作，分别在图5及图11中均设置了判断臂架目标角度值是否合理的操作，是为了方便解释及理解。

图12为本发明实施例提供的智能臂空间运动控制装置的结构示意图，其应用于具有n节臂的智能臂，其中n大于等于2，图5-图11所示实施例的解释说明可以适应用本实施例，如图12所示，智能臂空间运动控制装置包括：

接收单元61，用于接收各角度传感器感测及发送的所述智能臂处于当前位置时各节臂架与水平面之间的夹角，并接收用于控制所述智能臂运动的遥控器发送的运动指令；

夹角差确定单元63，根据所述各节臂架与水平面之间的夹角、所述智能臂各节臂架的长度及所述运动指令计算所述智能臂从当前位置运动到目标位置时的坐标改变量；并根据所述坐标改变量确定约束条件，再根据预设的优化算法，同时结合各臂架动作能力和/或所述遥控器的手柄开度变化确定所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差；

运动控制单元65，用于根据所述运动指令确定转台旋转角度，并根据所述转台旋转角度控制所述智能臂转台的运动，以及根据所述相邻两节臂架之间夹角的角度差控制所述智能臂臂架的运动。

在一种未示出的实施例中，该夹角差确定单元63可以包括：

优化子单元，用于以使所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差的平方和最小为优化目标，所述优化目标表示为：

$\min \left(\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{(K_i\·\mathrm{\Δ}{\mathrm{\θ}}_i)}^2\right)$

其中，系数K_i根据所述智能臂第i节臂架动作能力及所述遥控器的手柄开度变化确定；在i>1时Δθ_i为第i-1节臂架与第i节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差；Δθ₁为第1节臂架与水平面之间夹角在当前位置与目标位置时的变化量。

具体地，所述优化子单元可以包括：

第一处理模块，用于根据预设的所述智能臂各节臂架之间夹角的最大值及最小值，判断所述智能臂在目标位置时各节臂架之间夹角是否在对应的所述最小值与所述最大值之间的范围内，并将臂架之间夹角处于对应的所述最小值与所述最大值之间的范围外的臂架对应的系数K_i确定为零，且重新确定所述智能臂中其他节臂与其相邻臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差，以及将臂架之间夹角处于对应的所述智能臂各节臂架之间夹角的最小值与最大值之间的范围内且位于预设的增速范围内的臂架对应的系数K_i减小，将臂架之间夹角处于对应的所述最小值与所述最大值之间的范围内且位于预设的减速范围内的臂架对应的系数K_i增大；还用于根据预设的转台角度的最大值及最小值，判断所述转台旋转角度是否在所述转台角度的最小值与所述转台角度的最大值之间的范围内；在所述转台旋转角度位于所述转台角度的最小值与所述转台角度的最大值之外的范围时，将所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差确定为零，以停止所述智能臂各节臂架的动作；

第二处理模块，用于比较预存的手柄开度值与所述遥控器的手柄的当前开度值，在所述预存的手柄开度值与所述遥控器的手柄的当前开度值的差值大于预设阈值时，将大臂对应的系数K_i清零，以锁定所述大臂，以及在间隔预设时间后，控制所述大臂对应的系数K_i由大变小；以及将所述将手柄开度值更新为所述遥控器的手柄的当前开度值。

本实施例中通过利用遥控器的一个万向手柄，将遥控器模式切换至自动模式下，基于臂架及转台的实时角度，根据遥控器发出的运动指令，由当前的臂架姿态，通过一系列运算以及特殊位置处理，得出臂架下一时刻的姿态并判断该姿态是否合理，经过姿态变化—油缸长度（转台角度）变化—油缸（马达）流量变换—电磁阀电流变化的逆运算，驱动整个臂架，使其按照遥控器指令动作，实现臂架末端的水平方向、垂直方向（或者复合）平稳的运动，同时还考虑故障保护、安全保护、极限工况等限制，以免造成不必要的损失，可使得现场施工时，臂架操作难度降低，泵送精度提高，从而实现简单、方便的操作臂架，使操作者能准确的将混凝土泵送到目标位置。

需要说明的是，本发明各实施例以泵车智能臂的控制为例进行解释说明，可以理解，本发明智能臂的控制方法及装置可以适用于其他工程机械的智能臂的控制，不限于泵车。

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

Claims (10)

1.一种智能臂空间运动控制方法，应用于具有n节臂架的智能臂，其中所述n大于等于2，其特征在于，所述智能臂空间运动控制方法包括：

接收各角度传感器感测及发送的所述智能臂处于当前位置时各节臂架与水平面之间的夹角，并接收用于控制所述智能臂运动的遥控器发送的运动指令；

根据所述各节臂架与水平面之间的夹角、所述智能臂各节臂架的长度及所述运动指令计算所述智能臂从当前位置运动到目标位置时的坐标改变量；并根据所述坐标改变量确定约束条件，再根据预设的优化算法，同时结合各臂架动作能力和/或所述遥控器的手柄开度变化确定所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差；

根据所述运动指令确定转台旋转角度，并根据所述转台旋转角度控制所述智能臂转台的运动，以及根据所述相邻两节臂架之间夹角的角度差控制所述智能臂臂架的运动。

2.根据权利要求1所述智能臂空间运动控制方法，其特征在于，所述优化算法以使所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差的平方和最小为优化目标，所述优化目标表示为：

$\min \left(\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{(K_i\·\mathrm{\Δ}{\mathrm{\θ}}_i)}^2\right)$

其中，系数K_i根据所述智能臂第i节臂架动作能力及所述遥控器的手柄开度变化确定；在i>1时Δθ_i为第i-1节臂架与第i节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差；Δθ₁为第1节臂架与水平面之间夹角在当前位置与目标位置时的变化量。

3.根据权利要求2所述智能臂空间运动控制方法，其特征在于，所述“结合各臂架动作能力确定所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差”的步骤包括：

根据预设的所述智能臂各节臂架之间夹角的最大值及最小值，判断所述智能臂在目标位置时各节臂架之间夹角是否在对应的所述最小值与所述最大值之间的范围内；

将臂架之间夹角处于对应的所述最小值与所述最大值之间的范围外的臂架对应的系数K_i确定为零，并重新确定所述智能臂中其他节臂与其相邻臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差。

4.根据权利要求2述智能臂空间运动控制方法，其特征在于，所述“结合各臂架动作能力确定所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差”的步骤包括还包括：

根据预设的转台角度的最大值及最小值，判断所述转台旋转角度是否在所述转台角度的最小值与所述转台角度的最大值之间的范围内；

在所述转台旋转角度位于所述转台角度的最小值与所述转台角度的最大值之外的范围时，停止所述智能臂各节臂架的动作。

5.根据权利要求3所述智能臂空间运动控制方法，其特征在于，所述“结合各臂架动作能力确定所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差”的步骤包括还包括：

将臂架之间夹角处于对应的所述智能臂各节臂架之间夹角的最小值与最大值之间的范围内且位于预设的增速范围内的臂架对应的系数K_i减小，将臂架之间夹角处于对应的所述最小值与所述最大值之间的范围内且位于预设的减速范围内的臂架对应的系数K_i增大。

6.根据权利要求2-5中任一项所述智能臂空间运动控制方法，其特征在于，所述“结合所述遥控器的手柄开度变化确定所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差”的步骤包括：

比较预存的手柄开度值与所述遥控器的手柄的当前开度值；

在所述预存的手柄开度值与所述遥控器的手柄的当前开度值的差值大于预设阈值时，将大臂对应的系数K_i清零，以锁定所述大臂，以及在间隔预设时间后，控制所述大臂对应的系数K_i由大变小；

将所述手柄开度值更新为所述遥控器的手柄的当前开度值。

7.一种智能臂空间运动控制装置，应用于具有n节臂架的智能臂，其中所述n大于等于2，其特征在于，所述智能臂空间运动控制装置包括：

接收单元（61），用于接收各角度传感器感测及发送的所述智能臂处于当前位置时各节臂架与水平面之间的夹角，并接收用于控制所述智能臂运动的遥控器发送的运动指令；

夹角差确定单元（63），根据所述各节臂架与水平面之间的夹角、所述智能臂各节臂架的长度及所述运动指令计算所述智能臂从当前位置运动到目标位置时的坐标改变量；并根据所述坐标改变量确定约束条件，再根据预设的优化算法，同时结合各臂架动作能力和/或所述遥控器的手柄开度变化确定所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差；

运动控制单元（65），用于根据所述运动指令确定转台旋转角度，并根据所述转台旋转角度控制所述智能臂转台的运动，以及根据所述相邻两节臂架之间夹角的角度差控制所述智能臂臂架的运动。

8.根据权利要求7所述智能臂空间运动控制装置，其特征在于，所述夹角差确定单元（63）包括：

优化子单元，用于以使所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差的平方和最小为优化目标，所述优化目标表示为：

$\min \left(\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{(K_i\·\mathrm{\Δ}{\mathrm{\θ}}_i)}^2\right)$

其中，系数K_i根据所述智能臂第i节臂架动作能力及所述遥控器的手柄开度变化确定；在i>1时Δθ_i为第i-1节臂架与第i节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差；Δθ₁为第1节臂架与水平面之间夹角在当前位置与目标位置时的变化量。

9.根据权利要求8所述智能臂空间运动控制装置，其特征在于，所述优化子单元包括：

第一处理模块，用于根据预设的所述智能臂各节臂架之间夹角的最大值及最小值，判断所述智能臂在目标位置时各节臂架之间夹角是否在对应的所述最小值与所述最大值之间的范围内，并将臂架之间夹角处于对应的所述最小值与所述最大值之间的范围外的臂架对应的系数K_i确定为零，且重新确定所述智能臂中其他节臂与其相邻臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差，以及将臂架之间夹角处于对应的所述智能臂各节臂架之间夹角的最小值与最大值之间的范围内且位于预设的增速范围内的臂架对应的系数K_i减小，将臂架之间夹角处于对应的所述最小值与所述最大值之间的范围内且位于预设的减速范围内的臂架对应的系数K_i增大；还用于根据预设的转台角度的最大值及最小值，判断所述转台旋转角度是否在所述转台角度的最小值与所述转台角度的最大值之间的范围内；在所述转台旋转角度位于所述转台角度的最小值与所述转台角度的最大值之外的范围时，将所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差确定为零，以停止所述智能臂各节臂架的动作。

10.根据权利要求9所述智能臂空间运动控制装置，其特征在于，所述优化子单元包括：

第二处理模块，用于比较预存的手柄开度值与所述遥控器的手柄的当前开度值，在所述预存的手柄开度值与所述遥控器的手柄的当前开度值的差值大于预设阈值时，将大臂对应的系数K_i清零，以锁定所述大臂，以及在间隔预设时间后，控制所述大臂对应的系数K_i由大变小，以及将所述手柄开度值更新为所述遥控器的手柄的当前开度值。