CN111618861A - 一种基于四轴结构的双随动智能手臂控制方法 - Google Patents

Abstract

本发明提供一种基于四轴机械手臂的双随动智能手臂控制方法，包括以下步骤：液压缸工作带动机械臂运动并确定目标物；确定目标物在世界坐标系下的位姿；进行机械臂运动补偿将机械臂运动到目标位姿。本发明将随动智能手臂与机器人眼在手相结合，构建双随动式智能控制系统。在加入姿态传感器的基础上加入惯性导航系统，刷新频率大于100HZ，在船舶和汽车颠簸时，高效测量载体的速度、偏航角和位置等姿态信息，使机械手末端相对大地保持不动。同时利用机械结构末端视觉(声呐)装置的反馈(刷新频率大于10Hz)进行目标监测和运动补偿。既能抵消海浪和颠簸，又能通过眼在手对目标进行监测和系统修正，实现系统双随动控制。

Description

一种基于四轴结构的双随动智能手臂控制方法

技术领域

本发明涉及智能控制领域,具体涉及一种基于四轴结构的双随动智能手臂控制方法。

背景技术

现有海上资源勘测开发任务或应用于某些特定环境下的基于汽车或船舶为载体的智能手臂大多仅仅依靠视觉引导。基于位置的视觉引导控制下目标物体由2D转化为3D具有高度非线性，且由于计算量较大造成结果存在误差，当图像出现噪声时会对机械臂控制产生影响；混合视觉引导控制的方法受图像噪声的影响大，且需求解单应矩阵的多次迭代十分繁琐；基于图像的视觉引导控制下机械臂控制系统需进行实时的逆运动学解算，计算位姿变化量与相机获取目标物体信息变化量间的雅可比矩阵，计算量及计算难度与其余方法相较不降反增，且逆运动学解算的雅克比矩阵在图像坐标系下具有大量奇异点将会影响机械臂系统的稳定性；基于学习训练的视觉引导控制方法可以较为准确的估算出机械臂手眼系统间的非线性特性，但需要在控制机械臂运动前进行大量的样本学习，耗费时间且资源有限；基于运动的视觉引导控制方法需要相机更新画面保持高帧率，现有技术水平控制帧率最高达1200HZ。如果要提高视觉引导运动估计算法的效率则需要增加视觉引导控制的循环速度，却会对机械臂控制系统的规划轨迹优良性造成影响。

发明内容

本发明的发明目的是：提供一种既能抵消海浪和颠簸，又能通过眼在手对目标进行监测和系统修正，实现系统双随动控制的基于四轴结构的智能手臂控制方法。

本发明提供一种基于四轴结构的双随动智能手臂控制方法，包括以下步骤：

液压缸工作带动机械臂运动并确定目标物；

确定目标物在世界坐标系下的位姿；

进行机械臂运动补偿将机械臂运动到目标位姿。

进一步的，所述确定目标物在世界坐标系下的位姿，包括以下步骤：

将目标物包含的几何特征图像处理算法库集成于智能相机；

搜寻目标物，提取目标物几何特征，求出目标坐标及深度信息，将目标的三维重构与相机的位置信息计算相结合，利用图像坐标系、相机坐标系、世界坐标系的关系得到边缘特征点P在相机中的坐标系，通过欧拉角得到P点在世界坐标系中的方向向量，其中世界坐标系为任意基准；

计算出P点的世界坐标。

进一步的，所述进行机械臂运动补偿将机械臂运动到目标位姿包括，

通过姿态仪得到载物平台的位姿信息和速度，建立姿态仪到关节坐标系的数学关系，结合三个旋转轴的绝对角度通过机械臂三轴逆解算法得到建议速度和目标角度。

进一步的，所述进行机械臂运动补偿将机械臂运动到目标位姿包括，

机械臂速度最终由速度前馈、加速度前馈与PID控制共同决定。

为了保证上述方法的实施，本发明还提供一种双随动智能手臂，所述双随动智能手臂包括，伸缩机构(1)、航向模块(2)、横滚模块(3)和俯仰模块(4)；

受到颠簸后，双随动智能手臂控制系统快速作出响应，伸缩机构(1)进入工作状态，根据相对目标保持姿态不变，更改航向模块(2)、横滚模块(3)和俯仰模块(4)姿态，抵消海浪或颠簸，保持机械臂随动控制。

本发明的有益效果是：

本发明将随动智能手臂与机器人眼在手相结合，构建双随动式智能控制系统。在加入姿态传感器的基础上加入惯性导航系统，刷新频率大于100HZ，在船舶和汽车颠簸时，高效测量载体的速度、偏航角和位置等姿态信息，使机械手末端相对大地保持不动。同时利用机械结构末端视觉装置的反馈(刷新频率大于10Hz)进行目标监测和运动补偿。既能抵消海浪和颠簸，又能通过眼在手对目标进行监测和系统修正，实现系统双随动控制。

PID控制方法受系统的参数及环境的变化的影响不大，并且能够用于非线性系统和多变量复杂的对象，收敛速度较快、鲁棒性较强，最为突出的是该控制方法可以通过不断在线实时修正控制规则和调节系统参数来改善控制系统的性能，使用模糊PID控制，可以支持系统离线调用。加入速度前馈与加速度前馈后，提高系统控制精度。与视觉测量相结合，对机械臂进行运动补偿，可以实现快速精确随动载物平台与目标自身运动，系统测量精度与响应时间均有所提高，消除环境误差。

附图说明

图1为随动测量系统机械部分向下作业时示意图。

图2随动测量系统机械部分向上作业时示意图。

图3为本发明方法流程图。

图4为两条向量确定点P示意图。

图5相机坐标系与世界坐标系示意图。

具体实施方式

本发明的发明构思是：测量承载平台的姿态(航向、俯仰、横滚)和GPS位姿(经度、维度和深度)，以及机械装置的位姿，通过算法预估平台因洋流、海浪或颠簸引起的位姿、速度和角度变化。确定目标物位姿，通过数据处理与算法，以及速度前馈、加速度前馈、模糊自适应PID算法控制伸缩机构、航向模块、俯仰模块和横滚模块的随动，结合眼在手进行运动补偿能够精确地保持相对目标的姿态不变。

下面结合附图对本发明的具体实施方式以及工作原理作进一步详细说明。

如图1和图2所示，随动测量系统机械部分由伸缩机构1、航向模块2、横滚模块3和俯仰模块4组成。伸缩机构1在初始位置时，机械臂处于收储状态。向下作业时(图1)，受到颠簸后，控制系统快速作出响应，伸缩机构1进入工作状态，航向模块2、横滚模块3和俯仰模块4受到控制开始做出相应响应；向上作业时(图2)同上。

本发明机械手臂采用四轴结构(1轴+3轴或3轴+1轴结构)：1轴—液压缸：使其工作状态或者收储状态。3轴---ZXY旋转轴：三个旋转轴相交于一点即原点，采用四轴结构可以实现力矩小和静态平衡。

在载物平台上安装姿态仪，结合惯性导航系统，得到载物平台的速度、偏航角、位姿和偏转等姿态信息，不断刷新姿态，频率大于100HZ；根据相对目标保持姿态不变，更改航向模块2、横滚模块3和俯仰模块4姿态，抵消海浪或颠簸，保持机械臂随动控制。在智能手臂末端安装摄像头，刷新频率大于10Hz，通过眼在手监测目标，进行运动补偿，实现系统双随动控制，提高系统控制精度。

如图3所示，双随动控制系统将机器人眼在手与随动智能手臂相结合，安装于船舶或汽车平台。本发明目标在载物平台上安装姿态传感器和惯性导航系统，刷新频率大于100Hz，船舶或汽车在行驶过程中，受到洋流、海浪或颠簸时，软件系统接收姿态传感器和惯性导航系统测得的平台位姿信息，快速计算安装平台的变动趋势并控制航向模块2、横滚模块3和俯仰模块4，使得安装于俯仰模块4的监测装置或其他装置相对目标保持姿态不变。结合固定在机械臂上的智能相机通过视觉测量完成机械手运动补偿，在机器人底座保持不动前提下，监测到目标变动后，运动机械臂各关节使关节末端跟随物体，完成目标物的监测和抓取。

在姿态仪和GPS定位系统测得平台位姿信息后，系统会计算出各机械部分的运动状态并发送相应指令，在速度环内系统会给出相应速度。直接计算速度与加速度并带入结果会降低系统响应精度，因此加入速度前馈与加速度前馈。速度前馈就是速度环给出目标速度后，目标速度和需要的速度之间有一个比例系数K_V，理论上的需要速度乘以这个比例系数等于目标速度，直接将其输出，这个部分就是前馈。加速度前馈同理。加入速度前馈和加速度前馈可以提高系统响应精度，此时机械手V＝K_V*v、a＝K_a*a。

式中，V-目标速度，K_V-速度比例系数，v-理论速度，a-目标加速度，K_a-加速度比例系数，a-理论加速度。

但在机械臂的随动控制中如果单纯考虑速度前馈与加速度前馈的比例控制系统会由于阻力和消耗产生稳态误差，并不能完全满足机械臂末端的随动控制，在此基础上加入模糊自适应PID控制会消除稳态误差，在无需知道模型结构或者模型参数的条件下，机械臂由以往状态经验信息的累积和对未来状态趋势的预期作用由当前状态到达期望状态。PID控制直接与速度前馈与加速度前馈并联，超调时PID就输出负值，不足输出正值。此时在电流环内，交流分量开始由PID输出，直流分量由前馈输出，可以有效减少PID输出的变化范围，在输出斜率不变的情况下，提高系统响应速度，消除稳态误差。

面向机械臂的PID控制规律为

式中，K_p为比例系数，e(t)为成比例地反应控制系统的偏差信号；T_I为积分时间常数，K_I＝K_p/T_I，为积分系数；T_D为微分时间常数，K_D＝K_p*T_D，为微分系数。

为进一步提高系统响应和执行速度，采用模糊PID控制器。

模糊控制器以误差e和误差变化率ec作为输入，修正的ΔK_P、ΔK_I、ΔK_D作为输出。通过给定值r(t)与实际输出值y(t)构成偏差e(t)＝r(t)-y(t)，根据被控对象的状态自动调整PID三个控制参数的取值，参照模糊合成推理设计分数阶PID参数的模糊矩阵表，算出参数代入公式(1)计算：

其中，ΔK_P、ΔK_I、ΔK_D为输出值，为K_P、K_I、K_D的增量。K_P0、K_I0、K_D0为保留器保留的上一次K_P、K_I、K_D的值。

PID参数随系统误差变化率可表示如下：

式中，a、b、c、d、g、h均为常数，c<0，其余均大于0。

由此可见，单纯采用PID控制仍会存在系统误差。在高精度机械臂随动控制系统中，目标跟踪误差主要由输入信号造成，但由于水下资源勘探或水上风力检测环境的复杂性无法准确测得干扰信号，对其进行补偿，因此可以对其余形式的参数进行补偿，提高系统的稳定性和快速性。在模糊自适应PID控制基础上，加入速度前馈与加速度前馈，得到机械臂控制V＝K_V*v+K_a*a+PID(公式含义指机械臂速度最终由速度前馈、加速度前馈与PID控制共同决定，PID指模糊自适应PID控制)，可以提高控制系统目标跟踪能力与随动性，提高系统响应速度与精度。

由于在机械方面存在制造和安装误差，因此在系统方面需建立一套误差测试和正交补偿的方法，本发明提供一种水下四轴坐标变化和关节逆解算法与软件，实现准确的定位。

在本发明实施过程中，通过姿态仪得到载物平台的位姿信息和速度后，建立姿态仪到关节坐标系的数学关系，结合三个旋转轴的绝对角度通过机械臂三轴逆解算法可以得到目标角度。

第一，通过载物平台上的姿态仪获得姿态仪位姿curposA、姿态仪速度curspeedA,通过编码器获得三个旋转轴的旋转角度curAngle，根据GPS计算或通过给定值得到目标位姿goalpos，确定刷新频率algXms，不断刷新平台位姿。某时刻姿态仪获得位姿信息curposA为

第二，根据刷新频率不断修正goalpos；

第三，根据curposA和goalpos，求解俯仰模块角度angle3[0]、横滚模块角度angle3[1]和航向模块角度angle3[2]的目标角度。

首先，将载体坐标系转换为地球坐标系。

载体坐标系为：

通过坐标转换，将其转换为地球坐标系为：

得到俯仰、横滚和航向角分别为

将所得单位转换为角度单位，得到三轴目标角度。

结合惯性导航系统与视觉测量系统，在基于姿态仪的载物平台的机械臂随动控制系统的基础上，加入刷新频率大于10Hz的智能相机，对目标物进行视觉测量，进行目标监测与运动补偿，提高系统随动精度。

如图4所示，将目标物包含的所有几何特征图像处理算法库集成于智能相机前端；搜寻目标物得到第一图，提取目标物几何特征，求出目标坐标及深度信息，将目标的三维重构与相机的位置信息计算相结合，利用图像坐标系、相机坐标系、世界坐标系的关系得到边缘特征点P在相机中的坐标系，通过欧拉角得到P点在世界坐标系中的方向向量，其中世界坐标系为任意基准；得到第二图,第二图为与第一图不同角度拍摄点P获取的图像，分别对照两幅图中的点P获取点P世界坐标。

如图5所示，根据两幅图得到的两条向量，计算出P点的世界坐标，更新相机位姿信息及计算新三维点世界坐标。

在新三维点世界坐标计算中，根据相机焦距F，利用式(6)求出第一图中特征最大边缘点某点P的相机坐标系坐标Pc，

式中，F为相机镜头的焦距(mm)，Xc，Yc，Zc为点P在相机坐标系中的坐标(xc，yc，zc)，(u,v)为点的像素坐标，其余为相机内部参数，f_x、f_y分别为x轴和y轴上的归一化焦距，u0,v0为图像中心，单位为像素。

此时的P在相机坐标系中的坐标(xc，yc，zc)，记为Pc。假设此时Pc＝(x0，y0，z0)，为得到Pc点在世界坐标系中坐标(xw，yw，cw)，对Pc进行三次反向旋转:

第一次旋转：原始相机坐标系C绕z轴旋转了θ_Z变C1，此时Pc＝(x0，y0，z0)，单独将Pc点绕z轴旋转-θ_Z，得到P1＝(x1，y1，z1)，为C1中Ow坐标，Ow坐标表示世界坐标系的原点；

第二次旋转：C1绕y轴旋转了θ_y变为C2，此时P1＝(x1，y1，z1)，将P1点绕y轴旋转-θ_y，得到P2＝(x2，y2，z2)，为C2中Ow坐标；

第三次旋转：C2绕x轴旋转了θ_x变为C3系，此时P2＝(x2，y2，z2)，将P2点绕x轴旋转-θ_x，得到P3＝(x3，y3，z3)，为C3中Ow的坐标。

以上θ_x、θ_y、θ_Z为由相机旋转矩阵求解得到的三个轴的欧拉角。

于是世界坐标系中，相机的位置坐标为(-x3，-y3，-z3)。

旋转后得到点P_C在相机坐标系中新的坐标值，记为Pc'，Pc'的值等于世界坐标系中向量OP的值，即Pc'的值与Oc的世界坐标值之和为P点的世界坐标Pw。

将第二幅图中的点P代入式(3)，经三次旋转后得到点P的世界坐标。获取两幅图像中向量OP的交点，即为目标物体在世界坐标系下的坐标。

结合多传感器融合与运动姿态估计，基于GPS和姿态仪等数据融合，获得船体或车辆的位姿、速度与加速度；以智能手臂精度与目标视觉(声呐)测量融合的“眼在手”模式，获得目标物的位姿、速度与加速度，以速度前馈、加速度前馈和模糊PID控制，实现对目标物的高度、高精度、不滞后的随动跟踪，构建双随动控制算法与系统，实时获得数据并计算船体或车辆的位姿与运动，借助上位机目标物的视觉测量，构建运动控制算法和系统，实现随动基体(船体或车辆)的位移和波动，随动目标物的位移和波动(或跟踪偏差)。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种基于四轴结构的双随动智能手臂控制方法，其特征在于，包括以下步骤：

液压缸工作带动机械臂运动并确定目标物；

确定目标物在世界坐标系下的位姿；

进行机械臂运动补偿将机械臂运动到目标位姿。

2.如权利要求1所述的一种基于四轴结构的双随动智能手臂控制方法，其特征在于，

所述确定目标物在世界坐标系下的位置，包括以下步骤：

将目标物包含的几何特征图像处理算法库集成于智能相机；

搜寻目标物，提取目标物几何特征，求出目标坐标及深度信息，将目标的三维重构与相机的位置信息计算相结合，得到边缘特征点P在相机中的坐标系，通过欧拉角得到P点在世界坐标系中的方向向量，其中世界坐标系为任意基准；

计算出P点的世界坐标。

3.如权利要求1所述的一种基于四轴结构的双随动智能手臂控制方法，其特征在于，

所述进行机械臂运动补偿将机械臂运动到目标位姿包括，

通过姿态仪得到载物平台的位姿信息和速度，建立姿态仪到关节坐标系的数学关系，结合三个旋转轴的绝对角度通过机械臂三轴逆解算法得到建议速度和目标角度。

4.如权利要求1所述的一种基于四轴结构的双随动智能手臂控制方法，其特征在于，

所述进行机械臂运动补偿将机械臂运动到目标位姿包括，

机械臂速度最终由速度前馈、加速度前馈与PID控制共同决定。

5.一种采用权利要求1所述方法控制的双随动智能手臂，其特征在于，

所述双随动智能手臂包括，伸缩机构(1)、航向模块(2)、横滚模块(3)和俯仰模块(4)；

受到颠簸后，双随动智能手臂控制系统快速作出响应，伸缩机构1进入工作状态，根据相对目标保持姿态不变，更改航向模块(2)、横滚模块(3)和俯仰模块(4)姿态，抵消海浪或颠簸，保持机械臂随动控制。

Images