CN103792956A - 智能臂空间运动控制方法及装置 - 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节臂架与水平面的夹角。

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

发明内容

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

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

接收各角度传感器感测及发送的所述智能臂处于当前位置时各节臂架与水平面之间的夹角；

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

在用于控制所述智能臂运动的遥控器的坐标系与所述臂架空间坐标系不对应时，根据所述遥控器的坐标系调整所述臂架投影坐标系及第二坐标，或者根据所述臂架投影坐标系调整遥控器的坐标系，以使所述智能臂的臂架末端在所述遥控器的坐标系中的运动角度与在所述臂架投影坐标系中的运动角度一致，并根据所述遥控器发送的运动指令及所述第二坐标，确定所述智能臂处于目标位置时臂架末端在所述臂架投影坐标系中的第三坐标，且根据所述第二坐标及所述第三坐标计算得到所述智能臂的转台旋转角度；或者，在所述遥控器的坐标系与所述臂架空间坐标系对应时，根据所述运动指令及所述第二坐标，确定所述智能臂处于目标位置时臂架末端在所述臂架投影坐标系中的第三坐标；并根据所述第二坐标及所述第三坐标计算得到所述智能臂的转台旋转角度；

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

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

进一步地，确定所述遥控器的坐标系与所述臂架空间坐标系不对应的方法包括：在收到所述遥控器发送的用于表征所述遥控器的坐标系与所述臂架空间坐标系不对应的相对位置定位信号后，确定所述遥控器的坐标系与所述臂架空间坐标系不对应。

进一步地，所述“根据用于控制所述智能臂运动的遥控器的坐标系调整所述臂架投影坐标系及第二坐标”的步骤包括：根据所述相对位置定位信号获取所述遥控器的坐标系与预设基准方向的夹角，再根据所述遥控器的坐标系与预设基准方向的夹角，调整所述臂架投影坐标系及第二坐标，或者调整遥控器的坐标系。

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

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

进一步地，在所述“确定所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差”的步骤之后还包括：

根据所述角度差及所述智能臂在当前位置时各节臂架与水平面之间夹角确定所述智能臂在目标位置时各节臂架之间夹角；

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

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

进一步地，在所述“根据所述坐标改变量确定约束条件，再根据预设的优化算法，确定所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差”的步骤之前包括：

判断所述智能臂在目标位置时各节臂架之间夹角是否为对应的所述最小值或所述最大值；

并计算所述智能臂在目标位置时各节臂架之间夹角为对应的所述最小值或所述最大值的臂架数量；

在所述臂架数量大于或等于n-1时，直接停止所述智能臂各节臂架的动作；

在所述臂架数量小于n-1时，执行“根据所述坐标改变量确定约束条件，再根据预设的优化算法，确定所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差”的步骤。

进一步地，确定所述智能臂处于当前位置时臂架末端在臂架空间坐标系中的坐标的方法包括：

根据有限元理论，计算所述智能臂的第n节臂在当前载荷下的弹性形变量f_n；并根据所述弹性形变量f_n及所述第n节臂的长度，求取所述智能臂的臂架末端在所述第n节臂的局部坐标系下的坐标；并根据所述各节臂架与水平面之间的夹角，通过坐标旋转和平移实现所述臂架末端在各节臂的局部坐标系下的坐标转换；以及计算所述臂架末端在当前时刻及上一时刻在同一节臂的局部坐标系下的位置差；再根据所述位置差，对所述智能臂处于当前位置时臂架末端在臂架空间坐标系中的坐标进行弹性形变补偿操作。

进一步地，在所述“根据所述位置差，对所述智能臂处于当前位置时臂架末端在臂架空间坐标系中的坐标进行弹性形变补偿操作”的步骤之前包括：

判断所述位置差是否大于预设阈值，在大于所述预设阈值时，对所述智能臂处于当前位置时臂架末端在臂架空间坐标系中的坐标进行弹性形变补偿操作。

另一方面，提供一种智能臂空间运动控制装置，应用于具有n节臂架的智能臂，其中所述n大于等于2，所述智能臂空间运动控制装置包括：接收单元，用于接收各角度传感器感测及发送的所述智能臂处于当前位置时各节臂架与水平面之间的夹角；坐标转换单元，用于根据所述智能臂处于当前位置时臂架末端在臂架空间坐标系中的坐标，确定所述臂架末端在臂架平面坐标系中的第一坐标及在臂架投影坐标系中的第二坐标；旋转角度确定单元，用于在用于控制所述智能臂运动的遥控器的坐标系与所述臂架空间坐标系不对应时，根据用于控制所述智能臂运动的遥控器的坐标系调整所述臂架投影坐标系及第二坐标，或者，根据所述臂架投影坐标系调整遥控器的坐标系，以使所述智能臂的臂架末端在所述遥控器的坐标系中的运动角度与在所述臂架投影坐标系中的运动角度一致，并根据所述遥控器发送的运动指令及所述第二坐标，确定所述智能臂处于所述目标位置时臂架末端在所述臂架投影坐标系中的第三坐标，且根据所述第二坐标及所述第三坐标计算得到所述智能臂的转台旋转角度；或者，在所述遥控器的坐标系与所述臂架空间坐标系对应时，根据所述运动指令及所述第二坐标，确定所述智能臂处于所述目标位置时臂架末端在所述臂架投影坐标系中的第三坐标；并根据所述第二坐标及所述第三坐标计算得到所述智能臂的转台旋转角度；夹角差确定单元，用于根据所述第一坐标、所述各节臂架与水平面之间的夹角、所述智能臂各节臂架的长度计算所述智能臂从当前位置运动到目标位置时在臂架平面坐标系中的坐标改变量；并根据所述坐标改变量确定约束条件，再根据预设的优化算法，确定所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差；运动控制单元，用于根据所述转台旋转角度及臂架夹角角度差，分别控制所述智能臂转台及臂架的运动。

进一步地，所述旋转角度确定单元包括：旋转角度确定子单元，用于在收到所述遥控器发送的用于表征所述遥控器的坐标系与所述臂架空间坐标系不对应的相对位置定位信号后，确定所述遥控器的坐标系与所述臂架空间坐标系不对应，并根据所述相对位置定位信号获取所述遥控器的坐标系与预设基准方向的夹角，再根据所述遥控器的坐标系与预设基准方向的夹角，调整所述臂架投影坐标系及第二坐标，或者调整遥控器的坐标系。

本发明智能臂空间运动控制方法及装置通过将立体三维坐标拆分成两个平面二维坐标，求解智能臂的相邻两节臂架之间的夹角在智能臂处于当前位置与目标位置时的角度差以及转台旋转角度，并转化为臂架及转台电磁阀（多路阀）的驱动电流，统筹规划智能臂架空间运动时臂架平面内的运动与转台的运动，实现了臂架末端的空间直线运动，便于自动控制智能臂在空间中的移动，提高智能臂架的使用性能；同时在遥控器的坐标系与臂架空间坐标系不对应时，根据遥控器的坐标系调整臂架投影坐标系及第二坐标，或者调整遥控器的坐标系，操作者可在任意方位操作臂架，此外，遥控器上的操作方向与臂架的动作方向一致，降低了臂架操作难度。

附图说明

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

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

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

图3为图2提供的智能臂空间运动控制方法中坐标系分解示意图；

图4为图2提供的智能臂空间运动控制方法中坐标系调整示意图；

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

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

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

图8为图7中智能臂弹性形变量计算过程中的坐标转换操作的原理示意图；

图9为图7中根据位置差进行弹性形变补偿操作的原理示意图；

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

具体实施方式

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

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

步骤11：接收各角度传感器感测及发送的智能臂处于当前位置时各节臂架与水平面之间的夹角；其中，各角度传感器（当然还可以是油缸位移传感器等）可以安装在智能臂各臂架的根部。可以理解的是，检测各节臂架与水平面之间的夹角与检测各节臂架与竖直面之间的夹角是等同的，因为两个夹角是互余的，即两个夹角的和为90度。

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

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

步骤13：在用于控制智能臂运动的遥控器的坐标系与臂架空间坐标系不对应时，根据用于控制智能臂运动的遥控器的坐标系调整臂架投影坐标系及第二坐标，或者根据所述臂架投影坐标系调整遥控器的坐标系，以使智能臂的臂架末端在遥控器的坐标系中的运动角度与在臂架投影坐标系中的运动角度一致；具体可以包括：

首先，在收到遥控器发送的用于表征遥控器的坐标系与臂架空间坐标系不对应的相对位置定位信号后，确定遥控器的坐标系与臂架空间坐标系不对应；

其次，根据相对位置定位信号获取遥控器的坐标系与预设基准方向的夹角，再根据遥控器的坐标系与预设基准方向的夹角，调整臂架投影坐标系及第二坐标，或者调整遥控器的坐标系；

如图4所示，在遥控器智能模式下操作臂架过程中，若遥控器（图中未示出）处于a点且其坐标系为以x1轴及y1轴建立的坐标系，此时，遥控器坐标系与臂架空间坐标系对应，即与臂架投影坐标系正好一致（如图4虚线坐标系（以x1′轴及z1′轴建立的坐标系）所示），无需调整臂架投影坐标系，此时沿OM方向扳动万向手柄，臂架末端会沿AB方向（其与OM方向一致）移动；若遥控器处于b点且其坐标系为以x2轴及y2轴建立的坐标系，此时，遥控器坐标系与臂架空间坐标系不对应，则先操作遥控器上的一个开关，再将万向手柄往预设的基准方向移动（如向泵车车头方向扳动，即图4中单向箭头P所示），万向手柄的运动在开关的两个状态下代表不同的含义，如在本实施例中开关被按下，万向手柄的运动信息（即遥控器发送的相对位置定位信号）则代表遥控器坐标系与臂架空间坐标系不对应，且该万向手柄的运动信息包含了其与遥控坐标轴的角度信息，如单向箭头P与坐标轴y2之间的角度，且单向箭头P为预设的基准方向，故可以得知遥控器处于b点时其坐标系的实际情况，计算出遥控器坐标系与臂架投影坐标系的角度之差，从而可以调整臂架投影坐标系（以x1′轴及z1′轴建立的坐标系）为以x2′轴及z2′轴建立的坐标系，使其与以x2轴及y2轴建立的坐标系一致，实现补偿，即在万向手柄之后，遥控器与泵车之间的相对位置定位完成，臂架投影坐标系为x2′轴及z2′轴建立的坐标系，此时沿ON方向扳动万向手柄（此时未配合开关动作，万向手柄的操作直接代表用于控制智能臂动作的运动指令），臂架沿AB方向移动；

需要说明的是，上述万向手柄运动在开关不同状态下的含义不同为一种方式，具体操作时可以用手柄（如1臂手柄）替代开关，即万向手柄运动在手柄不同状态下的含义可以不同。当然，也可以以臂架投影坐标系为基准，采用类似的方法调整遥控器坐标系，使遥控器的坐标系与臂架投影坐标系一致，即将遥控器的运动指令通过坐标转换后再发送，或者接收到遥控器的运动指令后先进行相应转换再控制。

步骤14：在确定遥控器的坐标系与臂架空间坐标系不对应时，根据遥控器发送的运动指令及调整后的第二坐标，确定智能臂处于目标位置时臂架末端在臂架投影坐标系中的第三坐标（可以理解，在确定第三坐标时，若前面步骤中对第二坐标进行了调整，则需要采用的是调整后的第二坐标，若未进行调整，则仍为未调整的第二坐标）；或者，在遥控器的坐标系与臂架空间坐标系对应时根据运动指令及未调整的第二坐标，确定智能臂处于目标位置时臂架末端在臂架投影坐标系中的第三坐标；

如图5所示，假设在一个规划周期中（此图中的坐标系在遥控器的坐标系与臂架空间坐标系不对应时，为调整之后的臂架空间坐标系；在遥控器的坐标系与臂架空间坐标系对应时，为未调整的臂架空间坐标系）希望臂架末端点从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分别代表泵车遥控器万向手柄在x、z方向的解析输入量。

步骤15：在确定遥控器的坐标系与臂架空间坐标系不对应时，根据调整后的第二坐标及第三坐标计算得到智能臂的转台旋转角度（可以理解，在计算转台角度时，若前面步骤中对第二坐标进行了调整，则需要采用的是调整后的第二坐标，若未进行调整，则仍为未调整的第二坐标）；或者，在确定遥控器的坐标系与臂架空间坐标系对应时，根据未调整的第二坐标及第五坐标计算得到智能臂的转台旋转角度；

继续参见图5（该图中的坐标系在遥控器的坐标系与臂架空间坐标系不对应时，为调整之后的臂架空间坐标系；在遥控器的坐标系与臂架空间坐标系对应时，为未调整的臂架空间坐标系），具体可以通过下述方式计算得到转台旋转角度

其中，

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

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

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

可以理解的是，步骤13-15对于所述运动的遥控器的坐标系与所述臂架空间坐标系不对应时，若采用调整遥控器坐标系的方式，具体操作时，可以直接根据所述遥控器发送的运动指令、所述第二坐标及所述运动的遥控器的坐标系与所述臂架空间坐标系之间的差异，确定所述智能臂处于目标位置时臂架末端在所述臂架投影坐标系中的第三坐标，并根据所述第二坐标及所述第三坐标计算得到所述智能臂的转台旋转角度；

步骤16：根据第一坐标、测量得到的智能臂处于当前位置时各臂架与水平面之间的倾角值、智能臂各节臂架的长度计算智能臂从当前位置运动到目标位置时在臂架平面坐标系中的坐标改变量；

对于臂架末端在臂架平面内的运动，如图6所示，其中角度θ_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{\Δ\θ}}_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(\text{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{\π}))\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{\Σ}}}{\mathrm{\α}}_i\·{\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}{\mathrm{\α}}_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.$

步骤17：以坐标改变量确定约束条件；具体地，在理想状态下在臂架末端的运动过程中，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|-|\stackrel{\→}{\mathrm{OA}|}\\ \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{\π}))=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{\Σ}}}{\mathrm{\α}}_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{\Δ\θ}}_i\end{array}\right.---\left(11\right)$

其中，△y等于高度偏差。

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

优化目标可以表示为：

$\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将会对应减小或者增大，从而从规划中改变了臂架的速度，从而实现臂架油缸的平缓停止及启动。

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

步骤19：根据转台旋转角度及臂架夹角角度差，控制转台及臂架的运动，即通过求解出

以及Δθ_i，臂架的运动轨迹就已确定；具体可以包括：

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

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

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

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

本实施例中通过将立体三维坐标拆分成两个平面二维坐标，求解智能臂的相邻两节臂架之间的夹角在智能臂处于当前位置与目标位置时的角度差以及转台旋转角度，并转化为臂架及转台电磁阀（多路阀）的驱动电流，统筹规划智能臂架空间运动时臂架平面内的运动与转台的运动，实现了臂架的空间直线行走，便于程序处理；空间位置规划基于当前位置进行下一步规划，避免长远规划点可能出现的臂架（转台）往复运动；同时以油缸动作变化率最小为目标优化函数，油缸的规划运行速度连续、平稳，通过自动控制技术，臂架末端运行时抖动较小，提高了布料精度，极大的降低了臂架操作难度；优选地，通过泵车与遥控器相对方向定位，操作者可在任意方位操作臂架；通过坐标旋转变换，万向手柄的操作方向与臂架的动作方向一致，降低了臂架操作难度，使得通过操作万向手柄，轻松实现典型工况（矩形、三角形）布料。

图7为本发明实施例二提供的智能臂空间运动控制方法流程图（图2-图6的解释说明可以适用于本实施例）；图8为图7中智能臂弹性形变量计算过程中的坐标转换操作的原理示意图；图9为图7中根据位置差进行弹性形变补偿操作的原理示意图；如图7-图9所示；

步骤701：泵车与遥控器的相对方向定位，可以参见图2中步骤13的解释说明；

步骤702：接收各角度传感器感测及发送的智能臂处于当前位置时各节臂架与水平面之间的夹角，与图2中步骤11的作用基本一致；具体操作时，可以对上述各夹角进行平滑滤波处理；

步骤703：根据有限元理论，计算智能臂的第n节臂在当前载荷下的弹性形变量f_n；并根据弹性形变量f_n及第n节臂的长度，求取智能臂的臂架末端在第n节臂的局部坐标系下的坐标；并根据各节臂架与水平面之间的夹角，通过坐标旋转和平移实现臂架末端在各节臂的局部坐标系下的坐标转换；以及计算臂架末端在当前时刻及上一时刻在同一节臂的局部坐标系下的位置差；

此步骤主要是考虑到臂架是一组细长柔性件的机构组合，在工作时相当于悬臂梁结构，并且在臂架变形的同时又与刚性运动相互作用或耦合，所以臂架实质上是一个柔性体，在泵车实际布料过程中，臂架受自重及混凝土带载影响，会引起臂架弹性变形，在智能臂架的控制过程中，若不对弹性变形加以矫正，则会影响布料精确性；步骤703的具体操作可以解释如下：

经过深入分析智能臂每节臂架变形与载荷的变化关系的得知，臂架平均变形量大概为0.67%，可以认为臂架满足小变形理论。该臂架小变形假定是：

1）臂架变形和载荷成线性关系；

2）只考虑沿作用力方向上的变形（主变形）；

3）可用叠加原理来组合臂架的变形问题。

由于臂架满足上述小变形理论，其弹性形变可以适用以下方式来获取；

首先，由于泵送排量会影响载荷，而载荷会影响弹性形变量的大小，具体操作时可以结合当前泵送排量并基于有限元理论来获得弹性形变量f_n，具体如：

依据有限元理论，根据自重载荷、臂架弯矩、垂直载荷及横向载荷，计算智能臂的第n节臂在当前泵送排量下的弹性形变量f_n；具体操作时，可以利用现有大型应用有限元程序NASTRAN，分析每节臂架在受自重分布载荷、当量弯矩、当量垂直载荷、横向载荷情况下的臂架变形量；对于现有大型应用有限元程序NASTRAN的操作应用均为现有技术，兹不赘述；

其次，根据弹性形变量f_n及第n节臂的长度，求取智能臂的臂架末端在第n节臂的局部坐标系下的坐标；并通过坐标旋转和平移实现臂架末端在各节臂的局部坐标系下的坐标转换；具体操作时，如图8所示，若将臂架抽象为多刚性***，臂架末端的位置为A，然而臂架实际为柔性件的机构组合，柔性臂架存在弹性形变，实际的臂架末端位置为B。显然，随着臂架节数的增加，这种偏差会越来越大，从而对智能臂架末端位置的精确控制造成极大困难；为了矫正柔性臂架的弹性形变，进行如下操作：

aa：可以确定智能臂的臂架末端在第n节臂的局部坐标系[O;x_n,y_n]下的坐标为(x'_n,y'_n)，其中，x'_n=L_n，y'_n=-f_n；该L_n为第n节臂的长度；该局部坐标系[O;x_n,y_n]可以第n节臂的根部为原点，以与第n节臂相切的线为横坐标轴，并相应确定纵坐标轴，该局部坐标系的确定方式可以适用于各节臂；

bb，根据上述坐标(x'_n,y'_n)及第n节臂根部及第n-1节臂根部与水平方向的夹角，通过坐标旋转和平移计算臂架末端在第n-1节臂的局部坐标系下的坐标(x'_n-1,y'_n-1)；具体过程如下：

由安装在第n臂及第n-1节臂根部的两个角度传感器分别对应感测两个倾角值（每节臂根部与水平面的夹角）θ_n和θ_n-1，各局部坐标系下，臂架末端的坐标可通过坐标旋转和平移计算得出：具体如下述公式（13），

$\left[\begin{array}{cc}{x}_{n-1}^{\′}& y_{n-1}^{\′}\end{array}\right]=\left[\begin{array}{cc}{x}_n^{\′}& y_n^{\′}\end{array}\right]\×\left[\begin{array}{cc}\cos ({\mathrm{\θ}}_n-{\mathrm{\θ}}_{n-1})& \sin ({\mathrm{\θ}}_n-{\mathrm{\θ}}_{n-1})\\ -\sin ({\mathrm{\θ}}_n-{\mathrm{\θ}}_{n-1})& \cos ({\mathrm{\θ}}_n-{\mathrm{\θ}}_{n-1})\end{array}\right]+\left[\begin{array}{cc}{L}_{n-1}& -f_{n-1}\end{array}\right]---\left(13\right)$

其中，(x'_n,y'_n)为臂架末端点在[O;x_n,y_n]坐标系下的坐标值，n为臂架的节数，L_n为第n节臂架的长度，θ_n为第n节臂架与水平面的夹角，f_n-1为有限元计算得出的弹性形变量；臂架末端在各节臂架的局部坐标系下的坐标即为其在该臂架的局部坐标系下的位置；具体操作时，可以通过上述坐标转换的操作，一直计算得到臂架末端在智能臂的第1节臂的局部坐标系下的坐标为(x₁',y₁')，即将各个时刻臂架末端的位置均统一到第1节臂的局部坐标系下进行比较。

再次：计算臂架末端在当前时刻及上一时刻在同一节臂的局部坐标系下的位置差，即臂架变形矫正量；具体操作时，可以计算臂架末端在当前时刻及上一时刻在第1节臂的局部坐标系下的位置差；

如图9所示，臂架末端在向右运动的过程中，期望的运动轨迹是A->B->C->D…，但是在实际的运行过程中，受臂架弹性形变的影响，在臂架末端从A（起点）移至B点（坐标为(x,y)）过程中，实际上已经走到了E点（坐标为(x+Dx,y+Dy)）；此时，经过上述各步骤弹性形变的计算，得出在原有行走量的基础上，臂架末端需多行走-Dx的行程，即位置差为(-Dx,Dy)，这样就可以保证臂架末端基本维持在同一高度，便于实现智能臂架弹性形变的矫正；

然后：更新用于表征上述位置差的变量值，如更新为(-Dx,Dy)。

步骤704：判断位置差（可以仅考虑高度方向上的偏差，此时预设阈值对应为高度方向上的预设阈值）是否大于预设阈值，在大于预设阈值时，执行步骤705；反之执行步骤706；

步骤705：根据位置差进行弹性形变补偿操作，具体操作时，可以根据位置差的水平方向的差值Dx及竖直方向的差值Dy补偿智能臂在水平及竖直方向速度值以使臂架末端在不同时刻基本维持在同一高度，即通过在水平方向及竖直方向上速度的控制实现位移的补偿；

通过有限元法计算臂架弹性形变节省大量人力物力财力，且与泵送排量关联，保证形变计算的准确性；并通过臂架弹性形变的矫正，提高了臂架运行精度。

步骤706：接收遥控器发送的行走命令，具体可以参见图2中步骤13及14中的解释说明；

步骤707：确定臂架末端的目标位置，具体可以参见图2中步骤16-18中的解释说明；

步骤708：判断是否需要优化求解；具体包括：

首先判断智能臂在目标位置时各节臂架之间夹角是否为对应的最小值或最大值；

其次，计算智能臂在目标位置时各节臂架之间夹角为对应的最小值或最大值的臂架数量；

步骤709，在臂架数量大于或等于n-1时，直接停止智能臂各节臂架的动作；

步骤710：在臂架数量小于n-1时，求解最优角度变化值，详见图2中步骤18中的解释说明；

步骤711：判断新的角度值是否合理；具体包括：

首先，根据角度差及智能臂在当前位置时各节臂架之间夹角确定智能臂在目标位置时各节臂架之间夹角；

其次，根据预设的智能臂各节臂架之间夹角的最大值及最小值，判断智能臂在目标位置时各节臂架之间夹角是否在对应的最小值与最大值的范围内；若是，则判定为合理，否则，执行步骤712：

步骤712：锁定相应的臂架，并返回步骤710（即重新计算其他臂架与其相邻臂架之间的角度差）；具体如下：

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

步骤713：对Δθ_i以及

平滑滤波，并计算油缸或马达速度；可以参见图2中步骤19的解释说明。

步骤714：转化为油缸或马达的流量；可以参见图2中步骤19的解释说明。

步骤715：根据预设的电流与流量比例特性，转化为相应电磁阀电流；可以参见图2中步骤19的解释说明。

本实施例中通过将立体三维坐标拆分成两个平面二维坐标，求解智能臂的相邻两节臂架之间的夹角在智能臂处于当前位置与目标位置时的角度差以及转台旋转角度，并转化为臂架及转台电磁阀（多路阀）的驱动电流，统筹规划智能臂架空间运动时臂架平面内的运动与转台的运动，实现了臂架的空间直线行走，便于程序处理；优选地，可以实现自动判断并避开臂架干涉工况；泵车与遥控器相对方向定位，操作者可在任意方位操作臂架；通过坐标旋转变换，万向手柄的操作方向与臂架的动作方向一致，降低了臂架操作难度。

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

接收单元60，用于接收各角度传感器感测及发送的智能臂处于当前位置时各节臂架与水平面之间的夹角；

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

旋转角度确定单元62，用于在用于控制智能臂运动的遥控器的坐标系与臂架空间坐标系不对应时，根据用于控制智能臂运动的遥控器的坐标系调整臂架投影坐标系及第二坐标，或者根据所述臂架投影坐标系调整遥控器的坐标系，以使智能臂的臂架末端在遥控器的坐标系中的运动角度与在臂架投影坐标系中的运动角度一致，并根据遥控器发送的运动指令及第二坐标，确定智能臂处于目标位置时臂架末端在臂架投影坐标系中的第三坐标，且根据第二坐标及第三坐标计算得到智能臂的转台旋转角度；或者，在遥控器的坐标系与臂架空间坐标系对应时，根据运动指令（可以由泵车遥控器直接发送给该旋转角度确定单元）及第二坐标，确定智能臂处于目标位置时臂架末端在臂架投影坐标系中的第三坐标；并根据第二坐标及第三坐标计算得到智能臂的转台旋转角度；

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

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

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

旋转角度确定子单元（该图未示出），用于在收到遥控器发送的用于表征遥控器的坐标系与臂架空间坐标系不对应的相对位置定位信号后，确定遥控器的坐标系与臂架空间坐标系不对应，并根据相对位置定位信号获取遥控器的坐标系与预设基准方向的夹角，再根据遥控器的坐标系与预设基准方向的夹角，调整臂架投影坐标系及第二坐标，或者调整遥控器的坐标系。

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

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

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

Claims (10)

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

接收各角度传感器感测及发送的所述智能臂处于当前位置时各节臂架与水平面之间的夹角；

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

在用于控制所述智能臂运动的遥控器的坐标系与所述臂架空间坐标系不对应时，根据所述遥控器的坐标系调整所述臂架投影坐标系及第二坐标，或者根据所述臂架投影坐标系调整遥控器的坐标系，以使所述的臂架末端在所述遥控器的坐标系中的运动角度与在所述臂架投影坐标系中的运动角度一致，并根据所述遥控器发送的运动指令及所述第二坐标，确定所述智能臂处于目标位置时臂架末端在所述臂架投影坐标系中的第三坐标，且根据所述第二坐标及所述第三坐标计算得到所述智能臂的转台旋转角度；或者，在所述遥控器的坐标系与所述臂架空间坐标系对应时，根据所述运动指令及所述第二坐标，确定所述智能臂处于目标位置时臂架末端在所述臂架投影坐标系中的第三坐标；并根据所述第二坐标及所述第三坐标计算得到所述智能臂的转台旋转角度；

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

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

2.根据权利要求1所述智能臂空间运动控制方法，其特征在于，确定所述遥控器的坐标系与所述臂架空间坐标系不对应的方法包括：

在收到所述遥控器发送的用于表征所述遥控器的坐标系与所述臂架空间坐标系不对应的相对位置定位信号后，确定所述遥控器的坐标系与所述臂架空间坐标系不对应。

3.根据权利要求2所述智能臂空间运动控制方法，其特征在于，所述“根据用于控制所述智能臂运动的遥控器的坐标系调整所述臂架投影坐标系及第二坐标”的步骤包括：

根据所述相对位置定位信号获取所述遥控器的坐标系与预设基准方向的夹角，再根据所述遥控器的坐标系与预设基准方向的夹角，调整所述臂架投影坐标系及第二坐标，或者调整遥控器的坐标系。

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

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

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

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

根据所述角度差及所述智能臂在当前位置时各节臂架与水平面之间夹角确定所述智能臂在目标位置时各节臂架之间夹角；

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

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

6.根据权利要求5述智能臂空间运动控制方法，其特征在于，在所述“根据所述坐标改变量确定约束条件，再根据预设的优化算法，确定所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差”的步骤之前包括：

判断所述智能臂在目标位置时各节臂架之间夹角是否为对应的所述最小值或所述最大值；

并计算所述智能臂在目标位置时各节臂架之间夹角为对应的所述最小值或所述最大值的臂架数量；

在所述臂架数量大于或等于n-1时，直接停止所述智能臂各节臂架的动作；

在所述臂架数量小于n-1时，执行“根据所述坐标改变量确定约束条件，再根据预设的优化算法，确定所述智能臂的相邻两节臂架之间的夹角在所述智能臂处于当前位置与目标位置时的角度差”的步骤。

7.根据权利要求1-3中任一项所述智能臂空间运动控制方法，其特征在于，确定所述智能臂处于当前位置时臂架末端在臂架空间坐标系中的坐标的方法包括：

根据有限元理论，计算所述智能臂的第n节臂在当前载荷下的弹性形变量f_n；并根据所述弹性形变量f_n及所述第n节臂的长度，求取所述智能臂的臂架末端在所述第n节臂的局部坐标系下的坐标；并根据所述各节臂架与水平面之间的夹角，通过坐标旋转和平移实现所述臂架末端在各节臂的局部坐标系下的坐标转换；以及计算所述臂架末端在当前时刻及上一时刻在同一节臂的局部坐标系下的位置差；再根据所述位置差，对所述智能臂处于当前位置时臂架末端在臂架空间坐标系中的坐标进行弹性形变补偿操作。

8.根据权利要求7所述智能臂空间运动控制方法，其特征在于，在所述“根据所述位置差，对所述智能臂处于当前位置时臂架末端在臂架空间坐标系中的坐标进行弹性形变补偿操作”的步骤之前包括：

判断所述位置差是否大于预设阈值，在大于所述预设阈值时，对所述智能臂处于当前位置时臂架末端在臂架空间坐标系中的坐标进行弹性形变补偿操作。

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

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

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

旋转角度确定单元（62），用于在用于控制所述智能臂运动的遥控器的坐标系与所述臂架空间坐标系不对应时，根据用于控制所述智能臂运动的遥控器的坐标系调整所述臂架投影坐标系及第二坐标，或者根据所述臂架投影坐标系调整遥控器的坐标系，以使所述智能臂的臂架末端在所述遥控器的坐标系中的运动角度与在所述臂架投影坐标系中的运动角度一致，并根据所述遥控器发送的运动指令及所述第二坐标，确定所述智能臂处于所述目标位置时臂架末端在所述臂架投影坐标系中的第三坐标，且根据所述第二坐标及所述第三坐标计算得到所述智能臂的转台旋转角度；或者，在所述遥控器的坐标系与所述臂架空间坐标系对应时，根据所述运动指令及所述第二坐标，确定所述智能臂处于所述目标位置时臂架末端在所述臂架投影坐标系中的第三坐标；并根据所述第二坐标及所述第三坐标计算得到所述智能臂的转台旋转角度；

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

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

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

旋转角度确定子单元，用于在收到所述遥控器发送的用于表征所述遥控器的坐标系与所述臂架空间坐标系不对应的相对位置定位信号后，确定所述遥控器的坐标系与所述臂架空间坐标系不对应，并根据所述相对位置定位信号获取所述遥控器的坐标系与预设基准方向的夹角，再根据所述遥控器的坐标系与预设基准方向的夹角，调整所述臂架投影坐标系及第二坐标，或者调整遥控器的坐标系。

Images