一种大型机械臂架姿态感知方法及装置

技术领域：

本发明属于工程机械领域，涉及一种大型机械臂架姿态感知方法及装置

背景技术：

大型机械臂是指由一系列铰链和金属臂架构成的用于大型工程浇筑任务的大型待测臂架。现有的待测臂架，一般在实际使用中主要通过铰链连接多段固定长度的臂架，需要经过培训的操作人员操纵遥控器上相应摇杆来控制臂架运动时，需要对每节臂架进行操作，反复调节各个臂架的位置和回转角度使出料口到达浇筑位置上方，运送混凝土的过程中需要工人将安装在机械臂末端的软管拉近目标位置,从而进行混凝土浇筑工作。理想情况下,臂架工作时每段连杆和关节处于同一平面，由安装在泵车上的旋转台提供水平方向旋转，但是，在控制臂架运动时，操作人员往往只能根据操作经验和主观判断操控摇杆进行臂架伸缩动作，当操作员所处的位置没有办法看到整个作业区域或者无法很准确的判断泵车某些向外伸出的部分与建筑物或者其他障碍物的距离时，容易造成警架与现场物体产生干涉。一旦发生碰撞，无论是对泵车自身还是对现场工作人员都将造成无法估量的损失。

在实际工作情况下，传统的臂架姿态感知方法(如视觉图像感知技术、倾角传感器技术、位移传感器技术、基于臂架末端以及历史臂架姿态数据集的感知方法)，容易受外界天气变化影响和累积误差的影响，在此背景下，急需要一种使用可靠的、易实施、高精度的臂架姿态感知技术。

发明内容：

本发明的目的在提供一种大型机械臂架姿态感知方法及装置，用于精确的感知泵车臂架的姿态，提高工作效率和安全性，解决现有技术中臂架姿态感知准确性较低问题。

为实现上述目的，本发明提供一种大型机械臂架姿态感知方法及装置，所述的姿态感知方法包括：所述姿态感知方法包括：获取基站与臂架节点处的标签的距离信息，获取所述大型机械臂架的每节臂架长度信息，利用臂架结构特性和运动特性，通过逐节定位算法解算臂架节点的三维坐标；其中，距离信息包含各个基站之间的距离、标签与基站之间的距离和标签与虚拟基站之间的距离。

其中，虚拟基站是指率先解算出三维坐标信息的臂架节点，将其角色转换成基站，用于下一个臂架节点的测量。

进一步地，所述的大型机械臂架姿态感知方法，其特征在于，包括如下步骤：

S1：机械臂架在初始位置时，在臂架节点处布设标签设备，在机械臂架起始端布设四个及以上基站；

S2：通过基站与标签设备进行通信，测量标签与基站之间的距离和标签与虚拟基站之间的距离；

S3：结合机械臂架的结构特性和运动特性，设计逐节定位算法，基于步骤S2得到距离信息建立数学模型并优化求解，解算得到的臂架节点的三维坐标信息；

S4：机械臂架在连续作业的工况下，重复步骤S2至步骤S3，利用步骤S3得到的t时刻三维坐标信息，以及t+1时刻的三维坐标信息的观测值，滤波后得到t+1时刻的三维坐标信息的最优估计值。

进一步地，所述的大型机械臂架姿态感知方法，所述的逐节定位算法，其特征在于：

按照臂架顺序，依次解算臂架节点的距离信息，距离信息包含各个基站之间的距离、标签与基站之间的距离和标签与虚拟基站之间的距离得到臂架节点的三维坐标信息。

首先获取臂架首个节点的距离信息，通过定位算法解算此节点的坐标，将解算完的节点转换成虚拟基站，用于下一个臂架节点坐标的测量；

然后判断所有臂架节点是否完成解算，否则继续对下一个臂架节点进行测量，通过定位算法解算得到三维坐标信息后，将其转换成虚拟基站，用于对臂架的下一个节点的测量，直到所有臂架节点坐标全部完成测量；

进一步地，所述的大型机械臂架姿态感知方法，其特征在于，步骤S1具体包括：

在机械臂架起始端布设基站，基站布设位置不在同一平面上，在臂架节点处安装标签，标签布设位置位于基站所构成的凸包外；

基站凸包是指将所有基站作为一个点进行连接后组成的几何体，其中凸包指几何体的内部，凸包外指几何体的外部。

进一步地，所述的大型机械臂架姿态感知方法，其特征在于，步骤S2具体包括：

获取基站和标签的距离信息后，对比t+1时刻与t时刻的距离信息，通过对比两个时刻之间的距离信息变化的百分比来判断t+1时刻的测量信息是否可靠；

获得基站与标签的距离信息后，以距离为元素构建欧式距离矩阵用于标签节点坐标解算，对角线元素为零，并且为对称阵。

进一步地，所述的大型机械臂架姿态感知方法，所述的逐节定位算法，其特征在于，步骤S3具体包括：

S31获取首个臂架节点k的距离信息，通过定位算法解算出节点k的三维坐标信息，将其转换成虚拟基站，用于对臂架节点k+1的测量；

S32获取臂架节点k+1的距离信息，其距离信息包含臂架节点k+1与基站和与虚拟基站臂架节点k的距离，通过定位算法解算出节点k+1的三维坐标信息，将其转换成虚拟基站，用于对臂架节点k+2的测量；

S33获取臂架节点k+2的距离信息，其距离信息包含臂架节点k+2与基站和与虚拟基站臂架节点k+1的距离，通过定位算法解算出节点k+2的三维坐标信息，将其转换成虚拟基站，用于对臂架节点k+3的测量；

S34重复步骤S32至步骤S33，直到臂架节点k+n的三维坐标信息解算完成，其中n为臂架总节点数量；

其中，在执行S3的每一个步骤前，需要执行步骤S2获取臂架节点的距离信息，并进行所有臂架节点是否完成解算的判断，否则继续对下一个臂架节点进行测量，直至臂架所有节点的三维坐标信息的完成解算。

进一步地，一种大型机械臂架姿态感知装置，其特征在于所述姿态感知装置包括：测距模块、通信模块、数据处理模块、显示模块，其中：

所述测距模块分为基站设备和标签设备，基站设备用于测量标签设备与基站设备之间的距离；

所述通信模块用于将测距模块的距离信息传输至数据处理模块，以及传输数据处理模块向测距模块发送的指令；

所述数据处理模块用于处理距离数据得到臂架实时姿态信息，以及发送启动、停止等指令；

所述显示模块用于臂架实时姿态信息以及其他状态信息的可视化显示。

与现有的技术相比，本发明具有如下优点：本发明提供的基于逐节定位算法大型机械臂姿态感知方法及装置可以适用于复杂的工作环境下，能提供一种可靠的、易实施、高精度的臂架姿态感知方法，能实时在显示模块直观的展示泵车臂架的姿态，提高臂架作业时的安全性和可靠性；本发明使用的定位算法，可以解决传统定位算法在凸包外定位精度差的问题。

本发明的其它特征和优点将在随后的具体实施方式部分予以详细说明。

附图说明

附图是用来提供对本发明的进一步理解，并且构成说明书的一部分，与下面的具体实施方式一起用于解释本发明，但并不构成对本发明的限制。在附图中：

图1是大型机械臂姿态感知方法的流程图；

图2是大型机械臂姿态感知装置的流程图；

图3是大型机械臂姿态感知装置中的显示模块；

图4是大型机械臂姿态感知装置中的测距模块；

图5是按照泵车真实比例缩放的泵车模拟臂架；

图6是泵车模拟臂架上基站和标签安装位置的三维示意图；

图7是泵车模拟臂架姿态感知结果图。

具体实施方式

以下结合附图对本发明的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本发明，并不用于限制本发明。

实施例一

本发明提出了一种大型机械臂姿态感知方法，以泵车模拟臂架为例，如图1流程图所示，具体的臂架姿态感知方法可按照如下步骤实现：

在实施例一中根据现场环境情况进行模拟，按照泵车真实比例缩放泵车臂架，如图5所示。

步骤S101设备部署。

基站和标签设备部署，图4为测距模块示意图，在模拟臂架节点上安装标签，在臂架起始端布设基站，如图6所示，其中基站坐标分别为(0，0，0)、(100，173，0)、(200，0，0)、(100，58，70)，臂架节点坐标分别为(37，90，121)、(78，210，167)、(110，336，180)、(154，460，145)、(188，553，65)，单位为cm，在显示模块输入基站坐标及数量、标签数量以及解算维度等信息；

步骤S102：获取距离信息。

在实施例一中，通过数据处理模块向测距模块发送测距指令，测距模块通过通信模块将包含各个基站之间的距离、标签与基站之间的距离和标签与虚拟基站之间的距离信息发送至数据处理模块。

步骤S103：利用逐节定位算法解算臂架节点的坐标。

按照臂架顺序，依次解算臂架节点的距离信息，距离信息包含各个基站之间的距离、标签与基站之间的距离和标签与虚拟基站之间的距离得到臂架节点的三维坐标信息。

首先获取臂架首个节点的距离信息，通过定位算法解算此节点的坐标，将解算完的节点转换成虚拟基站，用于下一个臂架节点坐标的测量；

然后判断所有臂架节点是否完成解算，否则继续对下一个臂架节点进行测量，通过定位算法解算得到三维坐标信息后，将其转换成虚拟基站，用于对臂架的下一个节点的测量，直到所有臂架节点坐标全部完成测量；

在实施例一中，获取首个臂架节点1的距离信息，通过定位算法解算出节点1的三维坐标信息，将其转换成虚拟基站，用于对臂架节点2的测量；

获取臂架节点2的距离信息，其距离信息包含臂架节点2与基站和与虚拟基站臂架节点1的距离，通过定位算法解算出节点2的三维坐标信息，将其转换成虚拟基站，用于对臂架节点3的测量；

获取臂架节点3的距离信息，其距离信息包含臂架节点3与基站和与虚拟基站臂架节点2的距离，通过定位算法解算出节点3的三维坐标信息，将其转换成虚拟基站，用于对臂架节点4的测量；

获取臂架节点4的距离信息，其距离信息包含臂架节点4与基站和与虚拟基站臂架节点3的距离，通过定位算法解算出节点4的三维坐标信息，将其转换成虚拟基站，用于对臂架节点5的测量；

获取臂架节点5的距离信息，其距离信息包含臂架节点5与基站和与虚拟基站臂架节点4的距离，通过定位算法解算出节点5的三维坐标信息

其中，在执行S103的每一个步骤前，需要执行步骤S102获取臂架节点的距离信息，并进行所有臂架节点是否完成解算的判断，否则继续对下一个臂架节点进行测量，直至臂架所有节点的三维坐标信息的完成解算。

步骤S104：在显示模块显示解算出的臂架的节点坐标和实时姿态信息。

重复步骤S102至步骤S103，利用步骤S103得到的t时刻三维坐标信息，以及t+1时刻的三维坐标信息的观测值，滤波后得到t+1时刻的三维坐标信息的最优估计值，然后将臂架臂架的节点坐标和实时姿态信息展示在显示模块上。

在实施例一中，如图3所示，在上位机中实时显示臂架节点的通信状态，当臂架节点通信异常时，显示该节点的异常信息；

在实施例一中，在模拟泵车臂架同一姿态下重复300次测量，最终的测量结果如图7所示，臂架节点定位误差的RMSE为14.80cm，x，y，z轴的平均绝对误差的RMSE均在10cm以下，基本满足泵车臂架高精度感知的要求。

本发明实例实现了泵车臂架姿态的精确感知，较于传统的臂架姿态感知方法，更能适应复杂的工程环境，提高了识别的准确性。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。