CN110053054B - 基于Android手机IMU的机械臂示教方法 - Google Patents

Abstract

一种基于Android手机IMU的机械臂示教方法，包括如下步骤：标定固连手机的坐标系与机械臂末端执行器坐标系，依据Android操作系统底层函数接口调用IMU加速度数据a_x、a_y、a_z以及陀螺仪数据g_x、g_y、g_z，执行自定姿态解析器方法并通过OpenGLES绘制动态图，根据Android手机端发送的俯仰角

偏航角θ与横滚角γ进行逆运动学求解并控制驱动器运行，Android手机接收驱动器反馈位移数据构造控制误差并进行PI修正。

Description

基于Android手机IMU的机械臂示教方法

技术领域

本发明涉及一种利用Android手机进行机械臂示教的方法。

背景技术

随着现代化工业的迅速发展以及“工业4.0”概念的提出，人们对于工业生产的要求越来越高。作为工业生产的重要一部分，工业机器人凭借其生产效率、工作精度以及成本等方面诸多优势，也越来越受到关注。示教器是工业机器人的重要组成部分，其功能以及操作的便捷性快速成为国内外的研究重点。

传统机器人示教器通过示教器上的按钮对机器人进行编程来控制机器人的运动，这使得示教器有着笨重的外壳，操作的便捷性很难得到保证。同时，市场上诸多厂商生产的机器人大都有自己研发控制器，示教器依赖不同的控制器很难进行平台间的移植。

随着各种移动终端的普及，机器人示教器也向着便携式的方向发展，目前，市场上针对移动智能终端开发的操作系统主要有苹果公司的iOS系统，谷歌公司的Android系统以及微软的Windows Phone系统。其中，Android操作系统以其强大的开放性、丰富的第三方开发、优秀的内置服务、Google强大的长期维护支持等特点，在机器人示教器行业应用较为广泛。

发明内容

本发明针对传统示教器外壳笨重，难以提供便捷操作，不同平台间可移植性差的缺点，提出一种适用于工业机械臂基于Android手机的示教方法。

本发明基于Android操作系统，通过调用手机内嵌式IMU数据，实时计算手机在三维空间中的姿态以及固连手机端坐标系各个轴上的速度、位移，将其给定为机械臂末端姿态与数据，通过控制器逆运动学求解实现路径跟踪，以达到机械臂示教目的。

一种基于Android手机IMU的机械臂示教方法，具体步骤如下：

(1)标定坐标系，设固连在手机上的IMU空间坐标系为x-y-z，机械臂末端执行器的坐标系为x'-y'-z'，调整手机姿态，使得固连在手机上的传感器坐标轴x与机械臂末端执行器坐标轴x'重合，y轴与y'轴重合，z轴与z'轴重合。

(2)Android手机端采用如图1所示的姿态解析器，设定计算周期T，利用Android系统自带的底层接口函数获取嵌入式IMU器件采集到的加速度信号与陀螺仪信号，以此作为输入，进行姿态解析运算。同时，适当选取低通滤波系数β以滤除加速度计中的重力加速度成分来获得线性加速度值。位移x_n与线性加速度a_n满足：

速度v_n与线性加速度的关系为：

v_n＝a_nT (2)

对于轨迹路径的计算采用分段式，由于IMU传感器具有一定的误差，实验测定Android手机静止状态下各轴速度输出均值为v_stop，当|v_n|＜v_stop时，Android手机端判定为静止状态，此时设定v_n＝0m/s。因此，一条完整的示教路径可以被分解为若干条初速度为0m/s的直线运动轨迹。调节姿态解析器中的PI控制器参数K_p，K_i，可得到较为理想的控制效果。手机的空间姿态通过OpenGLES实时绘制在屏幕上，并将四元数q₀，q₁，q₂，q₃转化为俯仰角

偏航角θ与横滚角γ,其中：

将姿态角与速度值包装成数据报通过Socket发送给Staubli机器人控制器。

(3)Staubli机器人控制器接收到Android手机发送的数据进行逆运动学求解并执行控制，将执行后末端执行器运动的位移通过Socket发送给Android手机端。

(4)Android手机接收控制器发送的位移数据x'，结合步骤(2)中的位移x_n构造位移误差e_x，即

e_x＝x_n-x' (6)

然后对e_x做PI运算，结果计入下一周期的速度运算。

(5)重复运行步骤(1)、(2)、(3)、(4)。

优选地，步骤(1)中标定坐标系，通过事先调整手机姿态使得固连坐标系与机械臂末端执行器坐标系重合，使得之后的各项运算不用考虑坐标系映射关系，简化了计算步骤，节省CPU计算开销。

优选地，步骤(2)所述的利用低通滤波器滤除加速度计中的重力加速度成分来获得线性加速度值，具体是选取滤波系数β使得滤波器既能较准确的过滤重力，又能保证稳定的滤波效果。

优选地，步骤(2)所述的轨迹路径分段式计算方法，通过构造死区：

滤除了加速度计数据“漂移”现象所产生的影响，同时使得轨迹路径能够用多段直线运动来近似表示。

优选地，步骤(2)所述的误差的具体处理方法是：通过机械臂控制器Socket发送的数据与Android手机端计算数据构造误差表达式，采用PI反馈控制，将误差规划到下一周期的运算中，能够有效降低本发明运行中的动态误差。

优选地，步骤(2)中所需要计算的速度加速度传感器经过均值滤波方法得到较为平滑的加速度数据后积分得到：如图1所示，已知加速度传感器采样周期T内连续10次的采样值分别为a₀,a₁,…,a₉,经均值滤波，T时间端内的平均速度

满足下式

优选地，步骤(2)中采用Mahony互补滤波算法融合陀螺仪与加速度计数据进行姿态解析计算。将算法实现步骤封装成输入为三轴加速度数据与陀螺仪数据的姿态解析器，其中，通过陀螺仪的积分来获得四轴的旋转角度，再利用加速度计的比例积分运算来修正陀螺仪的积分结果。具体算法步骤如下：

(1)将加速度计的数据进行归一化处理，已知x轴加速度a_x，y轴加速度a_y，z轴加速度a_z，得到归一化后x轴加速度

y轴加速度

z轴加速度

(2)根据当前姿态的四元数值q₀，q₁，q₂，q₃估算出x，y，z三轴重力分量

用于和加速度计实际测量得到的各轴重力分量进行对比。

(3)利用叉积计算估计的重力和实际的重力两个向量之间的误差，x轴误差为e_x,y轴误差为e_y,z轴误差为e_z。

(4)在t₀时刻，将计算得到的x，y，z轴误差进行PI运算，比例增益为K_p，积分系数为K_i，得到的结果用于修正陀螺仪数据，x轴陀螺仪测量值为g_x，y轴陀螺仪测量值为g_y，z轴测量值为g_z。

(5)利用一阶近似毕卡算法更新四元数，得到经过T时间后新的姿态的四元数。

优选地，步骤(2)中线性加速度值由加速度计经过一阶低通滤波算法得到，滤波系数β大小取决于t/(t+dt)，当前采样值为X(n)，当前滤波输出值为Y(n)，上一周期滤波输出值为Y(n-1)滤波算法公式为：

Y(n)＝βX(n)+(1-β)Y(n-1) (25)

一阶低通滤波法采用本次采样值与上次滤波输出值进行加权，使得输出对输入又反馈作用。可见，β对滤波结果有很大影响，β偏大，则结果更偏向于当前采样值，系统灵敏度高，滤波效果不稳定；β偏小，结果偏向于前一周期滤波输出值，系统滤波效果稳定，但对于数据起伏变化敏感度较小。

本发明的优点是：利用Android手机嵌入式IMU数据计算手机姿态，并将计算结果反馈给控制器，实现了对机械臂的便捷示教；对于不同生产商的机械臂都具有适用性。

附图说明

图1为本发明的姿态解析器系统结构图。

图2为本发明实施例控制系统框图，系统采用位移反馈PI控制。

具体实施方式

以下结合附图和实施例对本发明技术方案作进一步描述。

实施例

以本发明配合STAUBLI TX90机械臂使用为例，具体实施本发明的步骤如下：

(1)标定坐标系，设固连在手机上的IMU空间坐标系为x-y-z，机械臂末端执行器的坐标系为x'-y'-z'，调整手机姿态，使得固连在手机上的传感器坐标轴x与机械臂末端执行器坐标轴x'重合，y轴与y'轴重合，z轴与z'轴重合。

(2)Android手机端采用如图1所示的姿态解析器，设定计算周期T，利用An droid系统自带的底层接口函数获取嵌入式IMU器件采集到的加速度信号与陀螺仪信号，以此作为输入，进行姿态解析运算。同时，适当选取低通滤波系数β以滤除加速度计中的重力加速度成分来获得线性加速度值。位移x_n与线性加速度a_n满足公式(1)，速度v_n与线性加速度的关系满足公式(2)。对于轨迹路径的计算采用分段式，由于IMU传感器具有一定的误差，实验测定Android手机静止状态下各轴速度输出均值为v_stop，当|v_n|＜v_stop时，Android手机端判定为静止状态，此时设定v_n＝0m/s。因此，一条完整的示教路径可以被分解为若干条初速度为0m/s的直线运动轨迹。调节姿态解析器中的PI控制器参数K_p，K_i，可得到较为理想的控制效果。手机的空间姿态通过OpenGLES实时绘制在屏幕上，并将四元数q₀，q₁，q₂，q₃转化为俯仰角

偏航角θ与横滚角γ,转换关系如公式(3)、(4)、(5)所示，将姿态角与速度值包装成数据报通过Socket发送给Staubli机器人控制器。

(3)Staubli机器人控制器接收到Android手机发送的数据进行逆运动学求解并执行控制，将执行后末端执行器运动的位移通过Socket发送给Android手机端。

(4)Android手机接收控制器发送的位移数据x'，结合步骤(2)中的位移x_n构造如公式(6)位移误差e_x，然后对e_x做PI运算，结果计入下一周期的速度运算。

(5)重复运行步骤(1)、(2)、(3)、(4)。

通过以上设计的姿态解析器以及PI控制，结合Socket网络通信，实现了对StaubliTX90机械臂的运动轨迹示教。因此，通过实施本发明方法，实现了机械臂的智能示教，同时，由于Android系统基于Linux的架构，使得本发明方法在应对不同厂商生产的机械臂时具有较高的可移植性。

Claims (5)

1.一种基于Android手机IMU的机械臂示教方法，具体步骤如下：

(1)标定坐标系，设固连在手机上的IMU空间坐标系为x-y-z，机械臂末端执行器的坐标系为x'-y'-z'，调整手机姿态，使得固连在手机上的传感器坐标轴x与机械臂末端执行器坐标轴x'重合，y轴与y'轴重合，z轴与z'轴重合；

(2)Android手机端采用姿态解析器，设定计算周期T，利用Android系统自带的底层接口函数获取嵌入式IMU器件采集到的加速度信号与陀螺仪信号，以此作为输入，进行姿态解析运算；同时，利用低通滤波器滤除加速度计中的重力加速度成分来获得线性加速度值；位移x_n与线性加速度a_n满足：

速度v_n与线性加速度的关系为：

v_n＝a_nT (2)

对于轨迹路径的计算采用分段式，由于IMU传感器具有一定的误差，实验测定Android手机静止状态下各轴速度输出均值为v_stop，因此当|v_n|<v_stop时，Android手机端判定为静止状态，此时设定v_n＝0m/s；因此，一条完整的示教路径可以被分解为若干条初速度为0m/s的直线运动轨迹；调节姿态解析器中的PI控制器参数为K_p，K_i可得到较为理想的控制效果；手机的空间姿态通过OpenGLES实时绘制在屏幕上，并将四元数q₀，q₁，q₂，q₃转化为俯仰角

偏航角θ与横滚角γ,其中：

将姿态角与速度值包装成数据报通过Socket发送给Staubli机器人控制器；

(3)Staubli机器人控制器接收到Android手机发送的数据进行逆运动学求解并执行控制，将执行后末端执行器运动的位移通过Socket发送给Android手机端；

(4)Android手机接收控制器发送的位移数据x'，结合步骤(2)中的位移x_n构造位移误差e_x，即

e_x＝x_n-x' (6)

然后对e_x做PI运算，结果计入下一周期的速度运算；

(5)重复运行步骤(1)、(2)、(3)、(4)。

2.根据权利要求1所述的一种基于Android手机IMU的机械臂示教方法，其特征在于：在步骤(1)中标定坐标系，通过事先调整手机姿态使得固连坐标系与机械臂末端执行器坐标系重合，使得之后的各项运算不用考虑坐标系映射关系，简化了计算步骤，节省CPU计算开销。

3.根据权利要求1所述一种基于Android手机IMU的机械臂示教方法，其特征在于：步骤(2)所述的利用低通滤波器滤除加速度计中的重力加速度成分来获得线性加速度值，具体是选取滤波系数β使得滤波器既能较准确的过滤重力，又能保证稳定的滤波效果。

4.根据权利要求1所述一种基于Android手机IMU的机械臂示教方法，其特征在于：步骤(2)所述的轨迹路径分段式计算方法，通过构造死区：

滤除了加速度计数据“漂移”现象所产生的影响，同时使得轨迹路径能够用多段直线运动来近似表示。

5.根据权利要求1所述的一种基于Android手机IMU的机械臂示教方法，步骤(2)所述的误差的具体处理方法是：通过Socket发送的数据与Android手机端计算数据构造误差表达式，采用PI反馈控制，将误差规划到下一周期的运算中，能够有效降低本发明运行中的动态误差。

Images