一种机械臂示教方法、装置及系统与流程

本发明涉及机器人控制、惯性导航、图像定位
技术领域：
，具体涉及一种机械臂示教方法、装置及系统。
背景技术：
：为了使机械臂产生特定轨迹的运动，通常采用一种操作设备或类似装置，由示教人员对该操作设备或类似设备进行手动操作，使机械臂按照希望的动作运动，与此同时，将机械臂的位置行程、机械手停止位置角度及高度等信息记录于控制器中，然后基于所记录的数据生成机械臂的动作程序。在利用操作设备或类似装置对机械臂进行示教时，传统的示教方法需要将动作分解为多个静态动作，然后根据各静态动作的位置、高度和角度信息来确定机械臂的动作信息，不但工作量大，而且由于分解动作精度的限制，并不能充分反映示教人员手臂末端自由运动的信息，示教相对稍复杂动作，示教完成较困难，使机械臂示教受到一些示教动作或者其它外部条件的限制，例如，在示教机械臂使用打磨机打磨工件的动作时，由于在打磨过程中需要不断调整打磨片的角度，如果利用传统的示教方法，很难将这些动作精确分解并确定动作的各种信息，无法精确完成示教。技术实现要素：针对现有技术中的缺陷，本发明提供了一种机械臂示教方法、装置及系统，具有提高示教精确度和效率的优点。本发明提出了一种机械臂示教方法，包括：获取工作端的示教动作数据，所述示教动作数据为惯性测量单元IMU采集的所述工作端在示教动作过程中的数据；根据所述示教动作数据获取工作端的运动轨迹数据和空间姿态数据；根据所述运动轨迹数据和所述空间姿态数据生成运动指令；将所述运动指令发送至机械臂，以使机械臂完成与示教动作相对应的动作。优选地，所述根据所述示教动作数据获取工作端的运动轨迹数据和空间姿态数据，具体包括：根据所述示教动作数据获取所述IMU的第一运动信息；根据所述第一运动信息判断所述IMU的动作状态，若所述IMU处于静止状态，则对所述第一运动信息进行零速修正处理；若所述IMU处于运动状态，则对所述第一运动信息进行惯性导航解算，获取所述IMU的第二运动信息；根据所述第二运动信息获取所述IMU的位置信息序列，并根据所述位置信息序列生成所述IMU的运动轨迹数据和空间姿态数据。优选地，所述根据所述第一运动信息判断所述IMU的动作状态的步骤具体包括：采用阈值自适应零速识别方法，判断所述IMU的动作状态；通过公式一和公式二获取所述阈值自适应零速识别方法中不同时刻的阈值；varN＝var(fN-M,…,fN)公式一GN＝max(varN-5,varN-4…varN)+α公式二其中，varN为第N时刻的方差值，fN-M,…,fN为第N-M时刻至第N时刻的加速度值，α为阈值偏移量，GN为第N次静止时的阈值。优选地，所述对所述第一运动位置信息进行零速修正处理的步骤具体包括：通过公式三，对所述第一运动位置信息进行零速修正处理：其中，为俯仰角，为横滚角，和为预设时间内加速度的均值，b为三维坐标系，g为重力加速度。优选地，所述第一运动信息包括：姿态信息、速度信息和位置信息；相应地，所述对所述第一运动信息进行惯性导航解算的步骤具体包括：通过公式四和公式五，对所述第一位置信息进行惯性导航解算；其中，Δθ1、Δθ2、Δθ3为姿态更新周期三等分时间间隔的陀螺角增量输出；Δθm、ΔVm为速度更新周期速度增量和角度增量，m为时刻，A1和A2为姿态更新周期的系数，A1和A2为常数。优选地，在获取工作端的示教动作数据之前，该方法还包括：获取工作端运动范围内的预标定的基准点，并根据所述基准点建立三维坐标系；相应地，所述根据所述运动轨迹数据和所述空间姿态数据生成运动指令的步骤具体包括：对所述运动轨迹数据和所述空间姿态数据进行三维坐标系转换，获取三维坐标系参数以及对应的时间信息；根据所述三维坐标系参数和所述时间信息获取轨迹姿态信息；对所述轨迹姿态信息进行指令转换生成运动指令。优选地，在根据所述三维坐标系参数和所述时间信息获取轨迹姿态信息的步骤之前，该方法还包括：获取图像采集装置发送的图像数据，所述图像数据为所述图像采集装置采集的所述IMU在示教动作过程中的数据；根据所述图像数据对三维坐标系参数进行修正。本发明还提出了一种机械臂示教装置，包括：第一获取模块，用于获取工作端的示教动作数据，所述示教动作数据为惯性测量单元IMU采集的所述工作端在示教动作过程中的数据；第二获取模块，用于根据所述示教动作数据获取工作端的运动轨迹数据和空间姿态数据；指令生成模块，用于根据所述运动轨迹数据和所述空间姿态数据生成运动指令，并将所述运动指令发送至机械臂，以使机械臂完成与示教动作相对应的动作。优选地，所述第二获取模块，具体用于根据所述示教动作数据获取所述IMU的第一运动信息；根据所述第一运动信息判断所述IMU的动作状态，若所述IMU处于静止状态，则对所述第一运动信息进行零速修正处理；若所述IMU处于运动状态，则对所述第一运动信息进行惯性导航解算，获取所述IMU的第二运动信息；根据所述第二运动信息获取所述IMU的位置信息序列，并根据所述位置信息序列生成所述IMU的运动轨迹数据和空间姿态数据。本发明还提出了一种机械臂示教系统，包括：图像采集装置、IMU和上述机械臂示教装置；所述机械臂示教装置分别与所述图像采集装置和所述IMU连接；所述机械臂示教装置，用于接收所述IMU发送的示教动作数据，所述示教动作数据为IMU采集的所述工作端在示教动作过程中的数据所述工作端在示教动作过程中的数据；根据示教动作数据获取工作端的运动轨迹数据和空间姿态数据；根据所述运动轨迹数据和所述空间姿态数据生成运动指令，并将所述运动指令发送至机械臂，以使机械臂完成与示教动作相对应的动作。由上述技术方案可知，本发明提出的机械臂示教方法基于IMU获取工作端的示教动作数据，并根据示教动作数据生成运动指令，与现有技术相比，提高了示教的精确度和效率。附图说明通过参考附图会更加清楚的理解本发明的特征和优点，附图是示意性的而不应理解为对本发明进行任何限制，在附图中：图1示出了本发明一实施例提供的机械臂示教方法的流程示意图；图2示出了本发明一实施例提供的机械臂示教方法中获取工作端运动轨迹数据和空间姿态数据步骤的流程示意图；图3示出了本发明一实施例提供的机械臂示教方法中未经过零速修正的速度漂移曲线图；图4示出了本发明一实施例提供的机械臂示教方法在经过零速修正的速度漂移曲线图；图5示出了本发明一实施例提供的机械臂示教装置的结构示意图；图6示出了本发明一实施例提供的机械臂示教系统的结构示意图。具体实施方式为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。图1为本发明一实施例提供的机械臂示教方法的流程示意图，参照图1，该机械臂示教方法，包括：110、获取工作端的示教动作数据，所述示教动作数据为惯性测量单元IMU采集的所述工作端在示教动作过程中的数据；需要说明的是，IMU设置在工作端上，示教过程中，作业者手持工作端，执行预定动作，例如对一工件实施特定加工行为；在执行动作的过程中，IMU实时采集数据，并将数据传输至处理器；120、根据所述示教动作数据获取工作端的运动轨迹数据和空间姿态数据；需要说明的是，处理器对示教动作数据进行解算，获得作业者手持工作端的实时运动轨迹数据和空间姿态数据；130、根据所述运动轨迹数据和所述空间姿态数据生成运动指令；140、将所述运动指令发送至机械臂，以使机械臂完成与示教动作相对应的动作。需要说明的是，在通过轨迹编辑模块，实现轨迹记录的后期编辑制作，得到用于驱动机械臂运动的运动指令，传输至机械臂即让机械臂完成作业者手持工作端相同的动作，完成机械臂示教。本发明基于IMU获取工作端的示教动作数据，并根据示教动作数据生成运动指令，与现有技术相比，提高了示教的精确度和效率。图2为本发明一实施例提供的机械臂示教方法中获取工作端运动轨迹数据和空间姿态数据步骤的流程示意图，下面参照图2对步骤120进行详细说明：S1、根据所述示教动作数据获取所述IMU的第一运动信息；S2、根据所述第一运动信息判断所述IMU的动作状态，若所述IMU处于静止状态，则对所述第一运动信息进行零速修正处理；S3、若所述IMU处于运动状态，则对所述第一运动信息进行惯性导航解算，获取所述IMU的第二运动信息；S4、根据所述第二运动信息获取所述IMU的位置信息序列，并根据所述位置信息序列生成所述IMU的运动轨迹数据和空间姿态数据。下面对步骤120的原理进行详细说明：A1，惯性测量单元在静止状态下的初始对准，得到姿态矩阵T0、速度矢量V0、位置矢量P0；A2，利用滑动方差方法进行阈值自适应零速识别判断惯性测量单元的静止状态F1或运动状态F2；A3，F1状态时进行姿态、速度、位置信息的零速修正，得到新的姿态矩阵T1、速度矢量V1、位置矢量P1；A4，F2状态时进行惯性导航解算，包括更新姿态矩阵T2、速度矢量V2、位置矢量P2，获取第二运动信息；A5，修正摄像机图像得到位置信息，得到修正后的姿态矩阵T3、速度矢量V3、位置矢量P3；A6，随着工作端示教过程中的位置更新，记录惯性导航解算得到的位置信息序列，示教完成后，将所述位置信息序列合并为惯性测量单元的三维轨迹；需要说明的是，在示教动作结束之前，惯性导航解算将获取一连串的不同时间点的位置信息。优选的，本发明采用阈值自适应零速识别方法判断所述IMU的动作状态；下面对步骤S2进行详细说明：通过公式一和公式二获取不同时刻的阈值；varN＝var(fN-M,…,fN)公式一GN＝max(varN-5,varN-4…varN)+α公式二其中，varN为第N时刻的方差值，fN-M,…,fN为第N-M时刻至第N时刻的加速度值，α为阈值偏移量，α为0.05，GN为第N次静止时的阈值。需要说明的是，设第N时刻至第N+5时刻的方差值分别为varN,varN+1…,varN+5，则阈值G应满足：G>max(varn,varn+1…,varn+5)+α。图3为本发明一实施例提供的机械臂示教方法中未经过零速修正的速度漂移曲线图，图4为本发明一实施例提供的机械臂示教方法在经过零速修正的速度漂移曲线图，参照图3和图4；通过公式三，对所述第一运动位置信息进行零速修正处理：其中，为俯仰角，为横滚角，和为预设时间内加速度的均值，b为三维坐标系，g为重力加速度。需要说明的是，姿态矩阵T1的各参数修正为：T31=f~xbg---(3-1)]]>T32=f~ybg---(3-2)]]>T33=f~zbg---(3-3)]]>T21=cosψT31T32-sinψT331-T322---(3-4)]]>T22=cosψ1-T322---(3-5)]]>T23=-sinψT31+cosψT32T331-T322---(3-6)]]>T11＝T22T33-T23T32(3‐7)T12＝T23T31-T21T33(3‐8)T13＝T21T32-T22T31(3-9)其中，为一段时间内加速度计的输出均值，Ψ为上一时刻的航向角。本实施例中，第一运动信息包括：姿态信息、速度信息和位置信息；相应地，步骤S3具体包括：通过公式四和公式五，对所述第一位置信息进行惯性导航解算；其中，Δθ1、Δθ2、Δθ3为姿态更新周期三等分时间间隔的陀螺角增量输出；Δθm、ΔVm为速度更新周期速度增量和角度增量，m为时刻，A1和A2为姿态更新周期的系数。下面对步骤S3进行详细说明：姿态矩阵T2在tk+1时刻更新计算可以写成式所示：Q(tk+1)＝Q(tk)·q(h)(4‐1)其中：q(h)为等效旋转矢量四元数，Q(t)为t时刻的姿态四元数；q(h)=cosΦ2+ΦΦsinΦ2---(4-2)]]>其中，Φ和Φ(h)为tk时刻至tk+1时刻的载体坐标系姿态变化的等效旋转矢量，其三子样优化算法如式所示：Φ(h)=Δθ1+Δθ2+Δθ3+920Δθ1×Δθ3+2740Δθ2×(Δθ3-Δθ1)---(4-3)]]>其中，Δθ1、Δθ2、Δθ3为姿态更新周期三等分时间间隔的陀螺角增量输出。则速度V2在k+1时刻更新方程如下式所示：Vk+1＝Vk+Ck(ΔVk+1+ΔVrotm)+ΔVg(4‐4)其中：ΔVg=∫tktk+1gdt---(4-5)]]>ΔVk+1=∫tktk+1fb(t)dt---(4-6)]]>ΔVrotm=12Δθk×ΔVk---(4-7)]]>其中，Δθk为tk至tk+1时间段内角度增量，fb为加速度计的输出值，g为当前位置的重力加速度矢量，f为加速度计三轴输出。为了提高获取的工作端位置信息的精确度，本发明采用图像数据对工作端的位置信息进行修正；获取图像采集装置发送的图像数据，所述图像数据为所述图像采集装置采集的所述IMU在示教动作过程中的数据；根据所述图像数据对三维坐标系参数进行修正修正的过程如下：图像采集装置，例如：摄像机，捕捉到的目标位置信息经过一次积分可以得到IMU运动速度Vc的观测信息，如式(0-1)所示：Vc=VcxVcyVcz---(0-1)]]>根据式(0-2)所示卡尔曼滤波器状态方程：X·=FX+W---(0-2)]]>其中F为状态转移矩阵，X为状态变量，为X的一阶导数；取状态变量如式(0-3)所示：X＝[δVxδVyδVzδpxδpyδpzφxφyφzax0ay0az0ωx0ωy0ωz0]T(0‐3)其中：δVx，δVy，δVz为导航系统速度误差；δpx，δpy，δpz为导航系统位置误差；φx，φy，φz为姿态失准角；ax0，ay0，az0为加速度计零偏；ωx0，ωy0，ωz0为陀螺仪零偏；状态转移矩阵F如式(0-4)所示：F=03×303×3F13Cbn03×3I3×303×303×303×303×303×303×303×303×3-Cbn03×303×303×303×303×303×303×303×303×303×3---(0-4)]]>其中：F13=00g000-g00---(0-5)]]>状态噪声向量W如式所示：W=ωaT01×3ωgT01×301×3T---(0-6)]]>其中：ωa＝[ωaxωayωaz]T为加速度计随机游走；ωg＝[ωgxωgyωgz]T为陀螺仪计随机游走。卡尔曼滤波器的观测方程为：Z＝HX+V(0‐7)其中：V为测量白噪声序列其协方差阵为R。观测矩阵：H＝[I3×303×12](0‐8)将卡尔曼滤波器离散化得出滤波方程：X^k=ΦX^k-1+Kk(Zk-HΦX^k-1)---(0-9)]]>为对状态变量X的当前估计值，H为观测矩阵；Kk＝Pk,k-1HT(HPk,k-1HT+R)-1(0‐10)Pk,k-1＝ΦPk-1ΦT+Qk(0‐11)Pk=(I-KkH)Pk,k-1(I-KkH)-T+KkRkKkT---(0-12)]]>系统噪声方差阵Qk按如下计算：Qk=TM1+T22!M2+T33!M3+...Tnn!Mn---(0-13)]]>其中：Mi+1＝FMi+(FMi)T(0‐14)M1为方差强度矩阵，其非零元为：M1(4,4)=T112ωgx2+T122ωgy2+T132ωgz2---(0-15)]]>M1(4,5)=T11T21ωgx2+T12T22ωgy2+T13T23ωgz2---(0-16)]]>M1(4,6)=T11T31ωgx2+T12T32ωgy2+T13T33ωgz2---(0-17)]]>M1(5,4)=T11T21ωgx2+T12T22ωgy2+T13T23ωgz2---(0-18)]]>M1(5,5)=T212ωgx2+T222ωgy2+T232ωgz2---(0-19)]]>M1(5,6)=T21T31ωgx2+T22T32ωgy2+T23T33ωgz2---(0-20)]]>M1(6,4)=T11T31ωgx2+T12T32ωgy2+T13T33ωgz2---(0-21)]]>M1(6,5)=T21T31ωgx2+T22T32ωgy2+T23T33ωgz2---(0-22)]]>M1(6,6)=T312ωgx2+T322ωgy2+T332ωgz2---(0-23)]]>其中：ωgx，ωgy，ωgz为陀螺仪的随机游走系数。本实施例中，在步骤110之前，还包括：获取工作端运动范围内预标定的基准点，并根据所述基准点建立三维坐标系；相应地，步骤120具体包括：对所述运动轨迹数据和所述空间姿态数据进行三维坐标系转换，获取三维坐标系参数以及对应的时间信息；根据所述三维坐标系参数和所述时间信息获取轨迹姿态信息；对所述轨迹姿态信息进行指令转换生成运动指令。综上所述，该系统基于手持IMU测量空间轨迹，并融合图像定位信息，得到操作者操作工作端的精确空间运动轨迹和姿态，通过数据处理驱动机械臂运动，实现了高效率机械臂示教功能，减少了因机械臂示教过程复杂导致的特殊工件难以示教加工的限制，提高了机械臂示教的精准度和效率；而且，本发明还对工作端的位置信息进行修正，以进一步提高机械臂示教的精确度。对于方法实施方式，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明实施方式并不受所描述的动作顺序的限制，因为依据本发明实施方式，某些步骤可以采用其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施方式均属于优选实施方式，所涉及的动作并不一定是本发明实施方式所必须的。图5为本发明一实施例提供的机械臂示教装置的流程示意图，参照图5，该机械臂示教装置，包括：第一获取模块510、第二获取模块520、指令生成模块530、以及发送模块540，其中；第一获取模块510，用于获取工作端的示教动作数据，所述示教动作数据为惯性测量单元IMU采集的所述工作端在示教动作过程中的数据；第二获取模块520，用于根据所述示教动作数据获取工作端的运动轨迹数据和空间姿态数据；指令生成模块530，用于根据所述运动轨迹数据和所述空间姿态数据生成运动指令；发送模块540，用于将所述运动指令发送至机械臂，以使机械臂完成与示教动作相对应的动作。在本发明的一种可选实施方式中，所述第二获取模块520具体用于根据所述示教动作数据获取所述IMU的第一运动信息；根据所述第一运动信息判断所述IMU的动作状态，若所述IMU处于静止状态，则对所述第一运动信息进行零速修正处理；若所述IMU处于运动状态，则对所述第一运动信息进行惯性导航解算，获取所述IMU的第二运动信息；根据所述第二运动信息获取所述IMU的位置信息序列，并根据所述位置信息序列生成所述IMU的运动轨迹数据和空间姿态数据。在本发明的一种可选实施方式中，所述装置还包括：坐标系建立模块550；所述坐标系建立模块550，用于在获取工作端的运动数据和图像数据之前，获取运动范围内的基准点，并根据所述基准点建立三维坐标系；相应地，所述第二获取模块520，具体获取工作端运动范围内的预标定的基准点，并根据所述基准点建立三维坐标系；相应地，所述根据所述运动轨迹数据和所述空间姿态数据生成运动指令的步骤具体包括：对所述运动轨迹数据和所述空间姿态数据进行三维坐标系转换，获取三维坐标系参数以及对应的时间信息；根据所述三维坐标系参数和所述时间信息获取轨迹姿态信息；对所述轨迹姿态信息进行指令转换生成运动指令。本发明基于手持IMU测量空间轨迹，并融合图像定位信息，得到操作者操作工作端的精确空间运动轨迹和姿态，通过数据处理驱动机械臂运动，实现了高效率机械臂示教功能，减少了因机械臂示教过程复杂导致的特殊工件难以示教加工的限制，提高了机械臂示教的精准度和效率。对于装置实施方式而言，由于其与方法实施方式基本相似，所以描述的比较简单，相关之处参见方法实施方式的部分说明即可。本发明还提出了一种机械臂示教系统，包括：图像采集装置、IMU和上述的机械臂示教装置；所述机械臂示教装置分别与所述图像采集装置和所述IMU连接；所述机械臂示教装置，用于接收所述IMU发送的示教动作数据，所述示教动作数据为IMU采集的所述工作端在示教动作过程中的数据所述工作端在示教动作过程中的数据；根据示教动作数据获取工作端的运动轨迹数据和空间姿态数据；根据所述运动轨迹数据和所述空间姿态数据生成运动指令，并将所述运动指令发送至机械臂，以使机械臂完成与示教动作相对应的动作。图6为本发明一实施例提供的机械臂示教系统的结构示意图，参照图6，该机械臂示教系统，包括：图像采集装置、IMU和上述机械臂示教装置；图中的机械臂即为上文中的工作端，IMU设置在机械臂上；图像采集装置此处举例为摄像头，摄像头的数量及安装位置此处不进行限定，能满足构建三维坐标、记录机械臂的示教动作即可。所述机械臂示教装置分别与所述图像采集装置和所述IMU连接；所述机械臂示教装置，用于通过网络接收所述IMU发送的示教动作数据，所述示教动作数据为IMU采集的所述工作端在示教动作过程中的数据所述工作端在示教动作过程中的数据；根据示教动作数据获取工作端的运动轨迹数据和空间姿态数据；根据所述运动轨迹数据和所述空间姿态数据生成运动指令，并将所述运动指令发送至机械臂，以使机械臂完成与示教动作相对应的动作。应当注意的是，在本发明的装置的各个部件中，根据其要实现的功能而对其中的部件进行了逻辑划分，但是，本发明不受限于此，可以根据需要对各个部件进行重新划分或者组合。本发明的各个部件实施方式可以以硬件实现，或者以在一个或者多个处理器上运行的软件模块实现，或者以它们的组合实现。本装置中，PC通过实现因特网对设备或者装置远程控制，精准的控制设备或者装置每个操作的步骤。本发明还可以实现为用于执行这里所描述的方法的一部分或者全部的设备或者装置程序(例如，计算机程序和计算机程序产品)。这样实现本发明的程序可以存储在计算机可读介质上，并且程序产生的文件或文档具有可统计性，产生数据报告和cpk报告等，能对功放进行批量测试并统计。应该注意的是上述实施方式对本发明进行说明而不是对本发明进行限制，并且本领域技术人员在不脱离所附权利要求的范围的情况下可设计出替换实施方式。在权利要求中，不应将位于括号之间的任何参考符号构造成对权利要求的限制。单词“包含”不排除存在未列在权利要求中的元件或步骤。位于元件之前的单词“一”或“一个”不排除存在多个这样的元件。本发明可以借助于包括有若干不同元件的硬件以及借助于适当编程的计算机来实现。在列举了若干装置的单元权利要求中，这些装置中的若干个可以是通过同一个硬件项来具体体现。单词第一、第二、以及第三等的使用不表示任何顺序。可将这些单词解释为名称。虽然结合附图描述了本发明的实施方式，但是本领域技术人员可以在不脱离本发明的精神和范围的情况下做出各种修改和变型，这样的修改和变型均落入由所附权利要求所限定的范围之内。当前第1页1 2 3