CN103853172A - 智能臂空间运动规划方法及装置 - Google Patents

Abstract

本发明公开了一种智能臂空间运动规划方法及装置，该方法包括：确定所述臂架末端在臂架平面坐标系中的第一坐标及在臂架投影坐标系中的第二坐标；确定所述智能臂处于所述目标位置时臂架末端在所述臂架投影坐标系中的第三坐标；并计算得到所述智能臂的转台旋转角度；计算所述智能臂在臂架平面坐标系中的坐标改变量；并根据优化算法确定所述智能臂的相邻两节臂架之间夹角的角度差；分别控制所述智能臂转台及臂架的运动。本发明提供的智能臂空间运动规划方法便于自动控制智能臂在空间中的移动，提高智能臂架的使用性能。

Description

智能臂空间运动规划方法及装置

技术领域

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

背景技术

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

由于在如公式（1）所示的三维坐标计算方法基础上进行的臂架空间运动规划非常复杂，很多智能臂架相关专利中都对臂架的空间运动规划避而不谈，直接将臂架空间的运动简化为直线动作，即不考虑转台运动的自动控制。若手动操作泵车转台配合臂架的运动，臂架末端又难以走出比较直的轨迹，由此导致 智能臂架的实际使用性能大大降低。 

发明内容

有鉴于此，本发明提出一种智能臂空间运动规划方法及装置，以实现自动控制智能臂在空间中的移动，提高智能臂架的使用性能。 

一方面，提供一种智能臂空间运动规划方法，应用于具有n节臂架的智能臂，其中所述n大于等于2，所述智能臂空间运动规划方法包括： 

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

根据接收的用于指示所述智能臂运动到目标位置的运动指令及所述第二坐标，确定所述智能臂处于所述目标位置时臂架末端在所述臂架投影坐标系中的第三坐标；并根据所述第二坐标及所述第三坐标计算得到所述智能臂的转台旋转角度； 

根据所述第一坐标、测量得到的所述智能臂处于当前位置时各臂架与水平面之间的倾角值、所述智能臂各节臂架的长度计算所述智能臂从当前位置运动到目标位置时在臂架平面坐标系中的坐标改变量；并根据所述坐标改变量确定约束条件，再根据预设的优化算法，确定所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差； 

根据所述转台旋转角度及所述相邻两节臂架之间夹角的角度差，分别控制所述智能臂转台及臂架的运动。 

进一步地，所述“根据接收的用于指示所述智能臂运动到目标位置的运动指令及所述第二坐标，确定所述智能臂处于所述目标位置时臂架末端在所述臂架投影坐标系中的第三坐标”的步骤包括： 

根据接收的遥控器发送的所述运动指令及所述第二坐标(x_n,z_n)，确定所述智能臂处于所述目标位置时臂架末端在所述臂架投影坐标系中的第三坐标 (x_n+1,z_n+1)；具体如下： 

$\left\{\begin{array}{c}{x}_{n+1}=x_n+R{C}_x\\ z_{n+1}=z_n+R{C}_z\end{array}\right.$

其中RC_x、RC_z分别代表所述遥控器万向手柄在x、z方向的解析输入量。 

进一步地，所述“根据所述第二坐标及所述第三坐标计算得到所述智能臂的转台旋转角度”的步骤通过下述方式计算得到转台旋转角度

其中，

分别为所述臂架末端在当前位置及目标位置与所述臂架投影坐标系的坐标原点形成的向量。 

进一步地，所述约束条件包括： 

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

其中，

分别为所述臂架末端在当前位置及目标位置与所述臂架投影坐标系的坐标原点形成的向量，在j>1时θ_j为第j-1节臂架与第j节臂架之间的夹角；θ₁为第1节臂架与水平面的夹角，L_j为第j节臂架的长度；Δx、Δy分别为所述臂架末端从当前位置运动到目标位置时在所述臂架平面的水平坐标改变量及竖直坐标改变量；臂架倾角α_j为第j节臂架与水平面的夹角，由安装在第j节臂架根部的传感器测量得到；且夹角θ_j与臂架倾角α_j的对应关系为 

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

进一步地，所述优化算法以使所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差的平方和最小为优化目标，所述优化目标可以表示为： 

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

其中P_i是表征第i节臂架运动的开关量，取值0或1；在i>1时Δθ_i为第i-1节臂架与第i节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差；Δθ₁为第1节臂架与水平面之间夹角在当前位置与目标位置时的变化量。 

进一步地，所述“根据所述转台旋转角度及所述相邻两节臂架之间夹角的角度差，分别控制所述智能臂转台及臂架的运动”的步骤包括：根据转台旋转角度控制用于驱动所述转台运动的马达的实时流量，并根据所述角度差控制用于驱动所述智能臂运动的油缸的实时流量。 

进一步地，所述根据所述转台旋转角度及所述相邻两节臂架之间夹角的角度差，分别控制所述智能臂转台及臂架的运动的步骤还包括：根据预设的电磁阀电流与马达流量的对应关系，及所述马达的实时流量确定所述马达对应电磁阀的电流值；根据预设的电磁阀电流与油缸流量的对应关系，及所述油缸的实时流量确定所述油缸对应电磁阀的电流值。 

另一方面，提供一种智能臂空间运动规划装置，应用于具有n节臂架的智能臂，其中所述n大于等于2，所述智能臂空间运动规划装置包括：坐标转换单元，用于根据所述智能臂处于当前位置时臂架末端在臂架空间坐标系中的坐标，确定所述臂架末端在臂架平面坐标系中的第一坐标及在臂架投影坐标系中的第二坐标；旋转角度确定单元，用于根据接收的用于指示所述智能臂运动到目标位置的运动指令及所述第二坐标，确定所述智能臂处于所述目标位置时臂架末端在所述臂架投影坐标系中的第三坐标；并根据所述第二坐标及所述第三坐标计算得到所述智能臂的转台旋转角度；夹角差确定单元，用于根据所述第一坐标、测量得到的所述智能臂处于当前位置时各臂架与水平面之间的倾角值、所述智能臂各节臂架的长度计算所述智能臂从当前位置运动到目标位置时在臂架平面坐标系中的坐标改变量；并根据所述坐标改变量确定约束条件，再根据预设的优化算法，确定所述智能臂的相邻两节臂架之间的夹角在所述智能臂处 于当前位置与目标位置时的角度差； 

运动控制单元，用于根据所述转台旋转角度及所述相邻两节臂架之间夹角的角度差，分别控制所述智能臂转台及臂架的运动。 

进一步地，所述旋转角度确定单元包括：目标坐标确定子单元，用于根据接收的遥控器发送的所述运动指令及所述第二坐标(x_n,z_n)，确定所述智能臂处于所述目标位置时臂架末端在所述臂架投影坐标系中的第三坐标(x_n+1,z_n+1)；具体如下： 

$\left\{\begin{array}{c}{x}_{n+1}=x_n+R{C}_x\\ z_{n+1}=z_n+R{C}_z\end{array}\right.$

其中RC_x、RC_z分别代表所述遥控器万向手柄在x、z方向的解析输入量。 

进一步地，所述运动控制单元包括：流量控制子单元，用于根据转台旋转角度控制用于驱动所述转台运动的马达的实时流量，并根据所述角度差控制用于驱动所述智能臂运动的油缸的实时流量；电流控制子单元，用于根据预设的电磁阀电流与马达流量的对应关系，及所述马达的实时流量确定所述马达对应电磁阀的电流值；根据预设的电磁阀电流与油缸流量的对应关系，及所述油缸的实时流量确定所述油缸对应电磁阀的电流值。 

本发明智能臂空间运动规划方法及装置通过将立体三维坐标拆分成两个平面二维坐标，求解智能臂的相邻两节臂架之间的夹角在智能臂处于当前位置与目标位置时的角度差以及转台旋转角度，并转化为臂架及转台电磁阀（多路阀）的驱动电流，统筹规划智能臂架空间运动时臂架平面内的运动与转台的运动，实现了臂架的空间直线运动，便于自动控制智能臂在空间中的移动，提高智能臂架的使用性能。 

附图说明

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

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

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

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

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

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

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

具体实施方式

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

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

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

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

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

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

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

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

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

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

其中，

分别为臂架末端在A点及B点与臂架投影坐标系的坐标原点形成的向量，×为向量叉积运算符号，

为对应向量的长度，·为乘法运算；可以理解的是，还可以根据

用三角函数的反余弦（acos）、反正切（atan）、反余切（acot）函数计算转台旋转角度。 

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

对于臂架末端在臂架平面内的运动，如图5所示，其中角度θ_j及θ_j'（j可以在1至n之间取值自然数）在j>1时分别表示臂架末端位于A′及B′点时第j-1节臂架与第j节臂架之间的夹角；在j=1时分别表示臂架末端位于A′及B′点时第1节臂架与水平面之间的夹角；假设在一个规划周期中，希望臂架末端点从 A′点（臂架末端当前位置在臂架平面内的对应位置）移至B′点（臂架末端目标位置在臂架平面内的对应位置），这里的A′点与图4中的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{\Δ}{\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{\Δ}{\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{\Δ}{\mathrm{\θ}}_j)-(i-1)\·\mathrm{\π})-\sin (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π}))\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.$

步骤15：以坐标改变量确定约束条件；具体地，在理想状态下在臂架末端的运动过程中，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{\Δ}{\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{\Δ}{\mathrm{\θ}}_j)-(i-1)\·\mathrm{\π})-\sin (\underset{j=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{\θ}}_j-(i-1)\·\mathrm{\π}))=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)$

步骤16：根据预设的优化算法，以使智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差最小为优化目标，确定智能臂 的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差； 

优化目标可以表示为： 

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

其中P_i是表征第i节臂架运动的开关量，取值0或1(当臂架出现往极限角度方向运动或者不合适的角度（如臂架干涉）时，该开关量可以取值为0，表示此时需要停止相应臂架的运动，并重新求解其余臂架的角度变化量)；在i>1时Δθ_i为第i-1节臂架与第i节臂架之间的夹角在智能臂处于当前位置与目标位置时的角度差；Δθ₁为第1节臂架与水平面之间夹角在当前位置与目标位置时的变化量。 

需要说明的是，除了以角度差最小为优化目标以外，还可以以臂架油缸流量变化最小或者臂架变化节数最少等作为优化目标，均根据控制要求确定。 

步骤17：根据转台旋转角度及角度差，控制智能臂转台及臂架的运动，具体可以包括： 

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

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

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

需要说明的是，步骤12及13用于计算转台旋转角度，其与用于计算臂架 在不同位置时的角度差的步骤14至步骤16之间没有先后次序关系。 

本实施例中通过将立体三维坐标拆分成两个平面二维坐标，求解智能臂的相邻两节臂架之间的夹角在智能臂处于当前位置与目标位置时的角度差以及转台旋转角度，并转化为臂架及转台电磁阀（多路阀）的驱动电流，统筹规划智能臂架空间运动时臂架平面内的运动与转台的运动，实现了臂架的空间直线运动，便于程序处理；空间位置规划基于当前位置进行下一步规划，避免长远规划点可能出现的臂架（转台）往复运动。 

图6为本发明实施例提供的智能臂空间运动规划装置的结构示意图，该矫正装置应用于具有n节臂的智能臂，其中n大于等于2，图1-图4的解释说明可以适应用本实施例，如图6所示，智能臂空间运动规划装置包括： 

坐标转换单元61，用于根据智能臂处于当前位置时臂架末端在臂架空间坐标系中的坐标，确定臂架末端在臂架平面坐标系中的第一坐标及在臂架投影坐标系中的第二坐标； 

旋转角度确定单元62，用于根据接收的用于指示智能臂运动到目标位置的运动指令（可以由遥控器直接发送给该旋转角度确定单元）及第二坐标，确定智能臂处于目标位置时臂架末端在臂架投影坐标系中的第三坐标；并根据第二坐标及第三坐标计算得到智能臂的转台旋转角度； 

夹角差确定单元63，用于根据第一坐标、测量得到的智能臂处于当前位置时各臂架与水平面之间的倾角值（可以由各臂架根部的传感器直接发送给夹角差确定单元）、智能臂各节臂架的长度计算智能臂从当前位置运动到目标位置时在臂架平面坐标系中的坐标改变量；并根据纵坐标改变量确定约束条件，再根据预设的优化算法，以使智能臂的相邻两节臂架之间的夹角在智能臂处于当前位置与目标位置时角度差的平方和最小为优化目标，确定智能臂的相邻两节臂架之间的夹角在智能臂处于当前位置与目标位置时的角度差； 

运动控制单元64，用于根据转台旋转角度及角度差，分别控制智能臂转台及臂架的运动。 

具体地，旋转角度确定单元62包括： 

目标坐标确定子单元（该图未示出），用于根据接收的遥控器发送的运动指令及第二坐标(x_n,z_n)，确定智能臂处于目标位置时臂架末端在臂架投影坐标系中的第三坐标(x_n+1,z_n+1)；具体如下： 

$\left\{\begin{array}{c}{x}_{n+1}=x_n+R{C}_x\\ z_{n+1}=z_n+R{C}_z\end{array}\right.$

其中RC_x、RC_z分别代表遥控器万向手柄在x、z方向的解析输入量。 

运动控制单元64包括： 

流量控制子单元（该图未示出），用于根据转台旋转角度控制用于驱动转台运动的马达的实时流量，并根据角度差控制用于驱动智能臂运动的油缸的实时流量； 

电流控制子单元（该图未示出），用于根据预设的电磁阀电流与马达流量的对应关系，根据马达的实时流量确定马达对应电磁阀的电流值；根据预设的电磁阀电流与油缸流量的对应关系，根据油缸的实时流量确定油缸对应电磁阀的电流值。 

本实施例中通过将立体三维坐标拆分成两个平面二维坐标，对两个平面内的运动进行协调规划，实现了臂架的空间直线行走，便于程序处理；空间位置规划基于当前位置进行下一步规划，避免长远规划点可能出现的臂架（转台）往复运动。 

需要说明的是，本发明各实施例以智能臂的行走规划为例进行解释说明，可以理解，本发明智能臂的行走规划方法及装置可以适用于泵车等工程机械的智能臂的行走规划。 

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

Claims (10)

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

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

根据接收的用于指示所述智能臂运动到目标位置的运动指令及所述第二坐标，确定所述智能臂处于所述目标位置时臂架末端在所述臂架投影坐标系中的第三坐标；并根据所述第二坐标及所述第三坐标计算得到所述智能臂的转台旋转角度；

根据所述第一坐标、测量得到的所述智能臂处于当前位置时各臂架与水平面之间的倾角值、所述智能臂各节臂架的长度计算所述智能臂从当前位置运动到目标位置时在臂架平面坐标系中的坐标改变量；并根据所述坐标改变量确定约束条件，再根据预设的优化算法，确定所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差；

根据所述转台旋转角度及所述相邻两节臂架之间夹角的角度差，分别控制所述智能臂转台及臂架的运动。

2.根据权利要求1所述智能臂空间运动规划方法，其特征在于，所述“根据接收的用于指示所述智能臂运动到目标位置的运动指令及所述第二坐标，确定所述智能臂处于所述目标位置时臂架末端在所述臂架投影坐标系中的第三坐标”的步骤包括：

根据接收的遥控器发送的所述运动指令及所述第二坐标(x_n,z_n)，确定所述智能臂处于所述目标位置时臂架末端在所述臂架投影坐标系中的第三坐标(x_n+1,z_n+1)；具体如下：

$\left\{\begin{array}{c}{x}_{n+1}=x_n+R{C}_x\\ z_{n+1}=z_n+R{C}_z\end{array}\right.$

其中RC_x、RC_z分别代表所述遥控器万向手柄在x、z方向的解析输入量。

3.根据权利要求2所述智能臂空间运动规划方法，其特征在于，所述“根据所述第二坐标及所述第三坐标计算得到所述智能臂的转台旋转角度”的步骤通过下述方式计算得到转台旋转角度

其中，

分别为所述臂架末端在当前位置及目标位置与所述臂架投影坐标系的坐标原点形成的向量。

4.根据权利要求1-3中任一项所述智能臂空间运动规划方法，其特征在于，所述约束条件包括：

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

其中，

分别为所述臂架末端在当前位置及目标位置与所述臂架投影坐标系的坐标原点形成的向量，在j>1时，θ_j为第j-1节臂架与第j节臂架之间的夹角；θ₁为第1节臂架与水平面的夹角，L_j为第j节臂架的长度；Δx、Δy分别为所述臂架末端从当前位置运动到目标位置时在所述臂架平面的水平坐标改变量及竖直坐标改变量；臂架倾角α_j为第j节臂架与水平面的夹角，由安装在第j节臂架根部的传感器测量得到；且夹角θ_j与臂架倾角α_j的对应关系为

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

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

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

其中P_i是表征第i节臂架运动的开关量，取值0或1；在i>1时Δθ_i为第i-1节臂架与第i节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差；Δθ₁为第1节臂架与水平面之间夹角在当前位置与目标位置时的变化量。

6.根据权利要求1-3中任一项所述智能臂空间运动规划方法，其特征在于，所述“根据所述转台旋转角度及所述相邻两节臂架之间夹角的角度差，分别控制所述智能臂转台及臂架的运动”的步骤包括：

根据转台旋转角度控制用于驱动所述转台运动的马达的实时流量，并根据所述角度差控制用于驱动所述智能臂运动的油缸的实时流量。

7.根据权利要求6所述智能臂空间运动规划方法，其特征在于，所述“根据所述转台旋转角度及所述相邻两节臂架之间夹角的角度差，分别控制所述智能臂转台及臂架的运动”的步骤还包括：

根据预设的电磁阀电流与马达流量的对应关系，及所述马达的实时流量确定所述马达对应电磁阀的电流值；

根据预设的电磁阀电流与油缸流量的对应关系，及所述油缸的实时流量确定所述油缸对应电磁阀的电流值。

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

坐标转换单元（61），用于根据所述智能臂处于当前位置时臂架末端在臂架空间坐标系中的坐标，确定所述臂架末端在臂架平面坐标系中的第一坐标及在臂架投影坐标系中的第二坐标；

旋转角度确定单元（62），用于根据接收的用于指示所述智能臂运动到目标位置的运动指令及所述第二坐标，确定所述智能臂处于所述目标位置时臂架末端在所述臂架投影坐标系中的第三坐标；并根据所述第二坐标及所述第三坐标计算得到所述智能臂的转台旋转角度；

夹角差确定单元（63），用于根据所述第一坐标、测量得到的所述智能臂处于当前位置时各臂架与水平面之间的倾角值、所述智能臂各节臂架的长度计算所述智能臂从当前位置运动到目标位置时在臂架平面坐标系中的坐标改变量；并根据所述坐标改变量确定约束条件，再根据预设的优化算法，确定所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差；

运动控制单元（64），用于根据所述转台旋转角度及所述相邻两节臂架之间夹角的角度差，分别控制所述智能臂转台及臂架的运动。

9.根据权利要求8所述智能臂空间运动规划装置，其特征在于，所述旋转角度确定单元（62）包括：

目标坐标确定子单元，用于根据接收的遥控器发送的所述运动指令及所述第二坐标(x_n,z_n)，确定所述智能臂处于所述目标位置时臂架末端在所述臂架投影坐标系中的第三坐标(x_n+1,z_n+1)；具体如下：

$\left\{\begin{array}{c}{x}_{n+1}=x_n+R{C}_x\\ z_{n+1}=z_n+R{C}_z\end{array}\right.$

其中RC_x、RC_z分别代表所述遥控器万向手柄在x、z方向的解析输入量。

10.根据权利要求8或10所述智能臂空间运动规划装置，其特征在于，所述运动控制单元（64）包括：

流量控制子单元，用于根据转台旋转角度控制用于驱动所述转台运动的马达的实时流量，并根据所述角度差控制用于驱动所述智能臂运动的油缸的实时流量；

电流控制子单元，用于根据预设的电磁阀电流与马达流量的对应关系，及所述马达的实时流量确定所述马达对应电磁阀的电流值；

根据预设的电磁阀电流与油缸流量的对应关系，及所述油缸的实时流量确定所述油缸对应电磁阀的电流值。

Images