大型机械臂姿态感知方法及系统与流程

本发明涉及工程器械技术领域，尤其涉及一种大型机械臂姿态感知方法及系统。

背景技术

大型机械臂是指由一系列铰链和金属臂架构成的用于大型工程浇筑任务的大型待测臂架。现有的待测臂架，一般在实际使用中主要通过铰链连接多段固定长度的臂架，运送混凝土的过程中需要工人将安装在机械臂末端的软管拉近目标位置，并在浇筑过程中引导完成。理想情况下，臂架工作时每段连杆和关节处于同一平面，由安装在泵车上的旋转台提供水平方向旋转，但是，由于臂架长度一般较长，旋转过程中易出现旋转振动和弹性形变，且周围环境可能出现树木、建筑、人体等障碍物的干扰，致使机械臂作业过程出现碰撞，从而导致极为致命的工程事故，造成人力财力方面的重大损失。

传统的臂架姿态感知方法大多数是基于倾角传感器的测量，两个连杆之间装一个倾角传感器，然后从转台到臂尖依次求出臂架的姿态；还有根据其液压驱动系统的几何结构，通过液压缸的行程传感器测量液压杆的伸长量，再计算出相互连杆间的夹角，以确定姿态，这些方法都是利用从转台到末端每一段连杆与倾角的几何关系得到末端位置；也有用粒子群定位方法对臂架关节点进行定位，采用大规模随机取点寻找臂架关节点和末端位置，计算臂架姿态；另外，还有人利用双目视觉系统，对机械臂图像进行实时监控，提取三维图像特征，完成臂架姿态计算。

但是，以上描述的方法存在一些弊端：1、基于角度和连杆几何关系往往存在累积误差与弹性形变，对于姿态感知效率较低，精确度不高；2、传统源定位姿态感知方法，采用随机取点的方法，没有充分利用待测臂架的运动特性，计算复杂度高，实时性差；3、双目视觉系统造价高，过度依赖于摄像头视角，同时忽略其运动特性和室外环境可能出现的雨水、遮挡物(如树木)，影响系统稳定性。

技术实现要素：

本发明的目的是克服了上述问题或者至少部分的解决上述问题，结合工程机械臂运动特性，提供一种利用数学优化方法的高精度低复杂度的机械臂姿态感知方法及系统，通过该感知方法和系统得到的仿真数据与机械臂实时状态非常接近，误差很小。

为了实现上述目的，本发明的一个发明目的是提供一种大型机械臂姿态感知方法，所述方法是采集各基站和待测臂架各采集点间的三维距离信息，根据大型机械臂的运动特性和空间几何关系，将三维距离信息降维计算得二维距离信息，基于源定位方法结合大型机械臂的结构以二维距离信息为基础建立数学模型，求解得二维计算信息，将二维计算信息升维成三维计算信息，获得的所有三维计算信息即为该大型机械臂的实时感知姿态；其中，距离信息包含各基站与基站间的距离，各测距传感器与测距传感器间的距离和各基站与各测距传感器间的距离。

优选的，所述大型机械臂姿态感知方法包括如下步骤：

s1，臂架在初始位置时，布设完全覆盖待测臂架关节点或期望点环境的若干个无线基站，在待测臂架关节点或期望点处安装测距传感器；

s2，通过测距传感器测量各基站与待测臂架各关节点或期望点之间的距离，根据测得的各三维距离信息建立距离数据库；

s3，根据大型机械臂架在作业时，具有工作于一个平面和关节点联动协作的运动特性和空间几何关系，将步骤s2所得距离数据库中的各三维距离信息降低维度计算得二维距离信息，将所得各二维距离信息更新至距离数据库；

s4，根据源定位方法，结合机械臂架设有多个关节点的结构特征，基于步骤s3所得各二维距离信息建立数学模型并优化求解，获得待测臂架各关节点或期望点的二维计算信息，将各二维计算信息使用步骤s3的逆过程还原为其三维计算信息；

s5，臂架进行某一次动态作业时，重复步骤s2至步骤s4，此时将步骤s4所得t时刻三维计算信息作为步骤s2的t+x时刻三维距离信息进行仿真计算，所得t+x时刻的所有三维计算信息即为该大型机械臂t+x时刻的实时感知姿态；其中，x为同一臂架动态作业时t的变化值。

作为优选的，在步骤s1中，所述布设安装的具体设备包括多个坐标已知的基站和未知的关节点测距传感器。

作为优选的，步骤s1具体包括：

s11，布设无线基站，所述无线基站的信号范围完全覆盖待测臂架关节点或期望点环境，同时标定基站坐标；

s12，在待测臂架关节点或期望点处安装测距传感器，测距传感器要求可以测距和通信。

作为优选的，步骤s12具体还应满足：布设基站数目最少为4，且布设位置不应在一个平面上，基站应分布在臂架工作平面的两边。

作为优选的，步骤s2中测量距离的具体方法包括飞行时差测距法、到达时间法、到达时间差法、接收信号强度指示法、到达角度法或方向角法等。

进一步优选的，步骤s2所提到的距离数据库包括步骤s1保存的各基站坐标、各基站之间的距离、各测距传感器之间的距离和各基站与各测距传感器之间的距离，其具体步骤包括：

s21：各基站和各测距传感器之间相互发送信号，测量各基站与基站间的距离、各测距传感器与测距传感器间的距离和各基站与各测距传感器之间的距离(三维距离信息)，将上述所得三维距离信息进行信息处理；

s22：根据步骤s21信息处理后的关节点或期望点的初始三维距离信息或t-x时刻关节点或期望点的历史三维计算信息，通过公式dij＝cτij获得t时刻各锚点三维计算信息，(本发明中不限于此方法测距，还可通过现有技术其他任意测距方法如信号强度、到达角度等方法进行测距)；

s23：根据步骤s21和步骤s22所得三维距离信息和三维计算信息建立距离数据库；

其中，dij为各测距传感器到各基站之间的距离，c为光速，τij为测量参数信号传播时间。

大型机械臂架在作业时，具有工作于一个平面和关节点联动协作的运动特性，步骤s3降维的目的在于结合机械臂运动特性降低计算坐标的维度，从而降低算法的复杂度，作为优选的，步骤s3具体过程包括：

s31：在三维参考坐标下确定待测臂架工作平面，并建立参考坐标系；

s32：根据步骤s2所建立的距离数据库遍历所有测距传感器的三维距离信息，先将步骤s2所得的t时刻距离向工作平面进行投影，然后利用待测臂架的运动特性做降维处理，得二维距离信息；

s33：遍历所有测距传感器，将计算所得二维距离信息更新至距离数据库。

作为优选的，所述的s4步骤中建模和求解过程具体包括：

s41：根据待测臂架的结构特征，结合步骤s3距离数据库的二维距离信息，确定数学模型中所需的一些矩阵和参数，建立数学模型；

s42：利用优化方法求解步骤s41中建立的数学模型，并将优化求解得到的测距传感器二维计算信息，使用步骤s3的逆过程还原成测距传感器的三维计算信息，更新到距离数据库中，用于t+x时刻待测臂架姿态感知任务。

作为优选的，步骤s41所提到的距离阵是由锚节点距离、测距传感器之间的距离与转化后的测距传感器与基站之间的距离。

作为优选的，步骤s42中所提到的优化方法应根据步骤s41所建立的模型选取合适的优化方法，具体方法包括：基于测距平方的最小二乘法(sr-ls)、欧式距离法(edm)、半正定规划(sdp)等。

由于实际所使用的泵车臂架是存在于三维空间的，所以步骤s42解算之后还应该将二维计算信息转化为三维计算信息，转化方法是步骤s32的逆变化。

作为优选的，在步骤s5动态作业中，由于相邻时刻间测距传感器坐标位置不会发生较大变化，可以使用t-x时刻的计算值(即计算信息)构成t时刻的数学模型参数，并再次重复步骤s2到s4的过程；即将步骤s4所得t时刻三维计算信息作为步骤s2的t+x时刻三维距离信息进行仿真计算，所得t+x时刻的所有三维计算信息即为该大型机械臂t+x时刻的实时感知姿态；其中，x为同一臂架动态作业时t的变化值；需要说明的是，在同一臂架动态作业中，步骤s5中x在初始化时可设为定值。

本发明的另一个目的是提供一种基于数学优化方法的工程机械臂姿态感知系统，包括多个基站、若干测距传感器和信息处理中心；其中，

所述基站包含通信模块，用于定时向其余基站发送带有特定距离信息的无线信号，同时用于与各测距传感器之间进行信息传递，多个基站布设需完全覆盖待测臂架工作环境；

所述测距传感器包含通信模块和信息处理模块，其中通信模块用于接收基站发射的信号、向信息处理中心传输数据、呼叫等功能，信息处理模块用于距离信息的初步处理；

所述信息处理中心包含通信模块和数据处理模块，用于将t时刻接收的关节点或期望点的三维距离信息解算为三维计算信息，或根据t时刻的三维计算信息继续解算为t+x时刻的三维计算信息。

与现有技术相比，本发明提供的基于数学优化方法的泵车机械臂精确姿态感知方法及系统，首先采用基本测距方法获得传感器节点与基站间和传感器间距离，然后利用机械臂架运动特性降低维度，减少了运算复杂度，基于源定位方法建立了数学模型，同时优化求解五个关节点的坐标，避免了积累误差，保证了坐标计算精度，适用于各种同类型大型机械臂的姿态感知任务，弥补了现存方法成本、复杂度、精度三个方面的不足。

附图说明

图1为本发明的姿态感知方法流程图；

图2为本发明的姿态感知系统设施布设示例图；

图3为本发明的姿态感知方法的坐标降维示例图；

图4为本发明的姿态感知方法投影降维后的示意图；

图5为本发明的姿态感知方法的仿真结果。

具体实施方式

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

以测泵车机械臂架实时姿态为例，如图1流程图所示，本发明提供的基于数学优化方法的大型机械臂姿态感知方法可按照如下步骤实现：

(1)本实例的基站布设和测距传感器安装如图2所示，其中，本实例布设了如三角形所示的四个基站q以及五个关节点测距传感器p，建立了以旋转台为原点的三维直角坐标系。

(2)测距阶段，由基站发出测距信号，测距传感器接收信号，解析信号并获得基站与测距传感器之间的距离；由于步骤s1所布设的基站距离测距传感器距离不远，非视距误差可以忽略，具体测距方法为：

通过toa技术，得到各测距传感器到四个基站之间的时间，则测量距离可由光速c与测量参数信号传播时间τij相乘得到：

dij＝cτij(1)

将计算得到的各测距传感器到四个基站之间的距离dij、各测距传感器之间的初始距离或t-x时刻各测距传感器之间的计算距离(即t-x时刻所得三维计算信息)，以及各基站之间的距离保存在距离数据库中。

(3)根据机械臂运动特性和空间几何关系，降低姿态感知系统计算维度，更新距离数据库；

假设测距传感器pi坐标为(xi，yi，zi)，降维后的测距传感器坐标为(ai，bi)，图3展示出了待测臂架竖直方向工作平面的投影过程，a’oz平面是待测臂架的工作平面，其在xoy面上的投影是y＝kx(k＝tanθ)，a和a’展示了一个投影实例，具体的投影过程按式(2)和式(3)进行解算：

其中，(xi，yi，zi)是第i个传感器的三维距离信息，θ是待测臂架工作平面与x轴的夹角，对应的(ai，bi)是降维以后的二维距离信息，dij为投影后的dij；

投影后的坐标平面如图4所示，三维距离信息投影降维完成后将所得二维距离信息加入距离数据库。

(4)建立数学模型(本实施例中构建欧式距离阵)以及求解模型，调用优化包求解并获得每个关节点的精确坐标结果，并将求得的结果更新到距离数据库中，具体的构建欧式距离阵以及建模求解的过程如下：

(a)：根据经典欧式距离阵构成方法，欧式距离阵g应由各基站间的距离、各测距传感器间的距离以及各基站与各测距传感器的距离构成，具体构成如下：

其中，a是基站集合，s是测距传感器集合，dij是降维以后的距离；

(b)：根据经典欧式距离阵方法建立如式(4)的求解数学模型：

其中，lkj是第k个测距传感器到第j个测距传感器之间的距离，那么当k和j相差为1时，lkj就是连杆的长度，h是加权矩阵，求解的矩阵x矩阵满足：

(c)：调用凸优化包求解测距传感器的二维计算信息，然后使用步骤s3的逆过程还原测距传感器的三维计算信息，并在距离数据库内更新；

(5)当旋转台旋转运动或待测臂架作业时，调用距离数据库并重复步骤s2至s4。

图5所示的仿真是假设噪声为均值(均值为0)、方差为1的高斯噪声情况下的结果，其中实线部分是臂架真实姿态，虚线部分使用了本发明提供的方法后重现的臂架姿态，具体的仿真数据如下表(单位：m)：

由上表及图5可以看出，使用本发明所提出的方法后，系统所计算的测距传感器计算信息点已经非常接近真实坐标。

上述只是本发明的较佳实施例，并非对本发明作任何形式上的限制。虽然本发明已以较佳实施例揭露如上，然而并非用以限定本发明。因此，凡是未脱离本发明技术方案的内容，依据本发明技术实质对以上实施例所做的任何简单修改、等同变化及修饰，均应落在本发明技术方案保护的范围内。