CN115122302A - 一种基于imu的六自由度并联平台主从控制系统 - Google Patents

Abstract

本发明公开了一种基于IMU的六自由度并联平台主从控制系统，包括IMU、PC机、运动控制器和六自由度并联平台；IMU与PC机通过串口连接，PC机与运动控制器通过普通以太网相连，运动控制器与六自由度并联平台通过工业以太网相连。本发明采用主从控制方式，以IMU作为控制主手，六自由度并联平台作为从手，通过远端机器人上搭载的IMU采集远端机器人的运动状态，并以Ethercat作为通讯协议进行运动控制器和伺服驱动模块之间的通讯，将远端机器人的运动姿态传给六自由度并联平台，完成六自由度并联平台对远端机器人的跟随运动，完成机器人在未知环境中运动姿态的复现，实现对六自由度并联平台的实时远程遥操作。

Description

一种基于IMU的六自由度并联平台主从控制系统

技术领域

本发明涉及机器人控制技术领域，具体涉及一种基于IMU(Inertial MeasurementUnit，惯性测量单元)的六自由度并联平台主从控制系统。

背景技术

六自由度并联平台是一种几何外形为八面体结构的六自由度并联机构,平台具有结构紧凑、刚度大、无累积误差、精度较高、动态响应好、承载能力大等优势,可应用于医疗、航空航天、娱乐、物流、机床设计等领域。在六自由度并联平台位置求解分析中,主要采用的方法有解析法、数值分析法、冗余传感器法和智能算法等。解析法注重得到全部正解,计算量大、运算时间长,求解过程比较复杂,不适合实际应用；冗余传感器法通过在铰接处增加传感器,通过传感器测量旋转角方式求出动平台的位置。智能算法的求解过程较复杂、运算时间较长。

六自由度并联平台具有空间内6个运动自由度，负载能力强、响应速度快、灵活，可以完成复杂的任务指令，同时其兼具高刚度、高精度、大负载的特点，被广泛应用于飞机、轮船、汽车等可靠性测试实验中以及运动模拟、装配、检测等领域。主从式机器人控制，即在机器人控制中引入主手设备，机器人作为从手，通过提取主手的运动姿态控制机器人运动的方式，这种控制方式充分利用了人的智慧，是未知环境中机器人的主要控制方式。

目前，六自由度并联平台的控制有两种方式，一种是基于离线路径规划，即机器人根据预先给定的位姿点数据提前规划出机器人位姿和运动轨迹进行运动的方式；另一种为基于在线路径规划，即机器人根据当前环境实时调整的位姿和运动轨迹进行运动的方式，这种方式当前主要通过人机交互界面或者特种运动输入设备实现。

现有技术主要有两个缺点，其一，采用离线路径规划一般只适用于已知作业环境，而对于未知环境，由于其存在很大的不确定性，该方法存在很大的局限性。其二，基于人机交互界面的在线六自由度并联平台控制存在操作不自然，响应不及时的问题，基于特种运动输入设备的六自由度并联平台在线控制存在开发难度大，成本高的问题。

发明内容

有鉴于此，为了解决现有技术中的上述问题，本发明提出一种基于IMU的六自由度并联平台主从控制系统，采用安装于远端机器人上的IMU实时采集远端机器人的位姿信息，并以Ethercat作为通讯协议传输到本地端的六自由度并联平台，从而实现六自由度并联平台的运动控制。

本发明通过以下技术手段解决上述问题：

一种基于IMU的六自由度并联平台主从控制系统，包括：

IMU，用于采集远端机器人在空间内的三个平移加速度和三个旋转的角加速度；

PC机，用于接收IMU采集的三个平移加速度和三个旋转的角加速度，对加速度进行二次积分和滤波得到X、Y、Z三个轴上的位移变化，对角加速度进行二次积分和滤波得到绕X、Y、Z三个轴转动的角度变化，从而将IMU采集的六个自由度的运动信息转换为六自由度并联平台的位姿信息，将得到的六自由度并联平台的位姿信息进行高通滤波并发送至运动控制器；

运动控制器，用于接收PC机发送的六自由度并联平台的位姿信息，通过运动学反解将六自由度并联平台的位姿信息转换成每个电动缸的缸长变化量，并发送至六自由度并联平台；

六自由度并联平台，用于接收运动控制器发送的每个电动缸的的缸长变化量，根据每个电动缸的的缸长变化量实现前后、左右、上下的平移以及俯仰、横滚和偏航的六个自由度运动，从而实现实时跟随远端机器人的运动。

进一步地，所述PC机包括：

IMU运动数据接收模块，用于接收IMU采集的三个平移加速度和三个旋转的角加速度；

数据处理模块，用于对加速度进行二次积分和滤波得到X、Y、Z三个轴上的位移变化，对角加速度进行二次积分和滤波得到绕X、Y、Z三个轴转动的角度变化，从而将IMU采集的六个自由度的运动信息转换为六自由度并联平台的位姿信息；

第一通讯模块，用于将得到的六自由度并联平台的位姿信息进行高通滤波并发送至运动控制器。

进一步地，所述运动控制器包括：

第二通讯模块，用于接收PC机发送的六自由度并联平台的位姿信息；

运动学反解模块，用于通过运动学反解将六自由度并联平台的位姿信息转换成每个电动缸的缸长变化量；

第三通讯模块，用于将每个电动缸的缸长变化量发送至六自由度并联平台。

进一步地，通过运动学反解将六自由度并联平台的位姿信息转换成每个电动缸的缸长变化量具体如下：

如果给定坐标系B的原点P在坐标系A中的坐标向量为p＝[x_P y_P z_P]^T,六自由度并联平台的姿态用横滚-俯仰-偏转欧拉角表示为(α，β，γ)，则坐标系B相对于坐标系A的旋转矩阵为：

其中：

对于每一个电动缸，建立一下闭环向量表达式：

其中A_i和B_i表示第i个电动缸连接球副，于是有：

为A_i在坐标系A中的坐标，

为B_i在坐标系B中的坐标；

第i个电动缸的缸长变化量q_i等于向量

的模,因此，获得给定六自由度并联平台位姿相应的输入为

对于给定的六自由度并联平台位姿，显然

已知，而

只与六自由度并联平台的设计参数有关，因此，将相关已知参数代入公式可得到相应电动缸的缸长变化量。

进一步地，所述六自由度并联平台包括动平台、静平台和六个伺服驱动模块，每个伺服驱动模块包含伺服驱动器、伺服电机和电动缸；

伺服驱动器一端连接运动控制器，另一端连接伺服电机的一端，伺服电机的另一端连接电动缸的一端，电动缸一端还与固定的静平台相连，另一端与活动的动平台相连。

进一步地，所述高通滤波的滤波器的传递函数为：

序列x(n)经过滤波器后的输出数列y(n)＝H(z)x(n)，则

y(n)＝a₀x(n)-a₁x(n-1)+a₂x(n-2)+b₁y(n-1)-b₂y(n-2)

其中a₀、a₁、a₂、b₁和b₂为由设定的采样频率、系统扰动的上界作为优化目标得到的最优解的系数。

进一步地，所述IMU为含三个平移加速度和三个旋转的角加速度共六个运动自由度的惯性传感器。

进一步地，所述IMU固定在远端机器人上。

进一步地，所述IMU与PC机通过串口连接。

进一步地，所述PC机与运动控制器通过普通以太网相连；所述运动控制器与六自由度并联平台通过工业以太网相连。

与现有技术相比，本发明的有益效果至少包括：

1、本发明采用远端机器人上搭载的IMU实时采集远端机器人的运动状态，并以Ethercat作为通讯协议进行运动控制器和伺服驱动模块之间的通讯，将远端机器人的位姿信息实时传递给从机器人，完成六自由度并联平台对远端机器人的跟随运动，有利于完成未知环境运动姿态的复现，实现远程的实时遥操作。

2、本发明采用主从控制方式控制六自由度并联平台，充分利用了IMU对远端作为主手的机器人运动姿态的提取和六自由度并联平台作为从手对IMU的跟随运动，能够更好的实现机器人在未知环境中运动姿态的复现。

3、本发明采用IMU作为主从控制主手，具有跟随性好、开发难度小、成本低等优势。

4、本发明采用Ethercat作为通讯协议进行运动控制器和伺服驱动模块之间的通讯，可以实现远程、实时的遥操作。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明基于IMU的六自由度并联平台主从控制系统的结构示意图；

图2是本发明六自由度并联平台的结构示意图；

图3是本发明高通数字滤波器的流程图。

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面将结合附图和具体的实施例对本发明的技术方案进行详细说明。需要指出的是，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例，基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例

本发明的目的主要有两个，第一，采用IMU作为六自由度并联平台的运动输入，开发难度低，操作自然，响应及时。第二，本发明采用基于Ethercat的通讯协议进行通讯，将远端机器人的位姿信息实时传递给从机器人，完成六自由度平台对远端机器人的跟随运动，实现远程的实时遥操作。

如图1所示，本发明基于IMU的六自由度并联平台主从控制系统包括IMU、PC机、运动控制器和六自由度并联平台(含伺服驱动模块)，本发明将IMU作为控制主手，六自由度并联平台作为从手，建立IMU的六个自由度与六自由度并联平台的六个自由度的一一对应关系，通过IMU采集远端机器人位姿信息，间接控制六自由度并联平台的运动。

如图2所示，六自由度并联平台包括动平台、静平台和六个伺服驱动模块，每个伺服驱动模块包含伺服驱动器、伺服电机和电动缸；伺服驱动器一端连接运动控制器，另一端连接伺服电机的一端，伺服电机的另一端连接电动缸的一端，电动缸一端还与固定的静平台相连，另一端与活动的动平台相连。

IMU与PC机通过串口连接，PC机与运动控制器通过普通以太网相连，运动控制器与六自由度并联平台的伺服驱动模块通过工业以太网相连。

实现过程为：IMU获取远端机器人在空间内的三个平移加速度和三个旋转的角加速度，作为输入数据，经PC机处理后将六自由度并联平台的位姿信息发送至运动控制器，运动控制器通过反解程序求解六自由度并联平台六个电动缸的伸长量并分别发送至平台的六个伺服驱动模块，从而实现六自由度并联平台跟随远端机器人的实时运动。

本发明基于IMU的六自由度并联平台主从控制系统各部件的具体功能如下：

所述IMU能够采集远端机器人在空间内的三个平移加速度和三个旋转的角加速度，包含X、Y、Z三个轴的平移和旋转等运动输入，可以通过固定在远端机器人上采集远端机器人的位置信息，作为主手对六自由度并联平台进行操控，本实施例中，IMU为含三个平移加速度和三个旋转的角加速度共六个运动自由度的惯性传感器。

所述PC机用来运行控制系统的上位机程序，上位机程序实现功能主要包括接收IMU采集的三个平移加速度和三个旋转的角加速度，通过对加速度的二次积分和滤波得到X、Y、Z三个轴上的位移变化，通过对角加速度的二次积分和滤波得到绕X、Y、Z三个轴转动的角度变化，从而将IMU采集的六个自由度的运动信息转换为六自由度并联平台的位姿信息，将得到的六自由度并联平台的位姿信息进行高通滤波并发送至运动控制器。具体为：在PC机上安装上位机程序，上位机程序主要功能模块包括接收IMU采集的三个平移加速度和三个旋转的角加速度的IMU运动数据接收模块，将加速度和角加速度分别转化为X、Y、Z三个轴的位移变化和角度变化的数据处理模块，以及与运动控制器之间的第一通讯模块等。第一通讯模块通过与PC机连接的以太网将六自由度并联平台的位姿信息发送至运动控制器。

所述运动控制器用于接收PC机发送的六自由度并联平台的位姿信息，通过运动学反解将六自由度并联平台的位姿信息转换成每个电动缸的缸长变化量，并发送至六自由度并联平台。运动控制器通过改变六个电动缸的伸长量实现动平台的位姿变化。运动控制器可以实现六自由度并联平台的运动学反解，通过接收PC机发送的六自由度并联平台的位姿信息解算出每个电动缸的缸长变化量并发送至六自由度并联平台的伺服驱动模块，运动控制器支持Ethercat协议，与伺服驱动模块通过Ethercat协议通讯。运动控制器通过以太网通讯接口接受上位机程序对六自由度并联平台的运动轨迹规划并对收到的控制命令进行命令解析、状态获取、机构运动学模型解算算法及控制算法处理等。运动控制器经过反解算法将上位机发来的位姿信息转换成每个电动缸的的缸长变化量，通过Ethercat总线发送运动控制命令给伺服驱动器，实现六自由度并联平台的运动控制。具体为：运动控制器内置下位机程序，下位机程序包含与上位机程序通讯的第二通讯模块，即实现PC机发送的六自由度并联平台位姿信息的接收，运动学反解模块，即根据坐标变换原理由六自由度并联平台位姿信息求解六个电动缸的伸长量，以及与伺服驱动模块通讯的第三通讯模块，即将运动学反解模块求得的电动缸伸长量发送至六自由度并联平台的伺服驱动模块。

六自由度并联平台运动学反解问题是已知末端执行器的位姿，求获得该位姿对应的驱动器输入长度q。如果给定坐标系B的原点P在坐标系A中的坐标向量为p＝[x_P y_P z_P]^T,六自由度并联平台的姿态用横滚-俯仰-偏转(Roll-Pitch-Yaw)欧拉角表示为(α，β，γ)，则坐标系B相对于坐标系A的旋转矩阵为：

其中：

对于每一个电动缸，建立一下闭环向量表达式：

其中A_i和B_i表示第i个电动缸连接球副，于是有：

为A_i在坐标系A中的坐标，

为B_i在坐标系B中的坐标；

第i个电动缸的缸长变化量q_i等于向量

的模,因此，获得给定六自由度并联平台位姿相应的输入为

对于给定的六自由度并联平台位姿，显然

已知，而

只与六自由度并联平台的设计参数有关，因此，将相关已知参数代入公式可得到相应电动缸的缸长变化量。

所述六自由度并联平台用于接收运动控制器发送的每个电动缸的的缸长变化量，根据每个电动缸的的缸长变化量实现前后、左右、上下的平移以及俯仰、横滚和偏航等六个自由度运动，从而实现实时跟随远端机器人的运动。其中，六自由度并联平台的伺服驱动模块使用Ethercat通讯。

其中，为消除IMU的累积误差，PC机在接收IMU采集的数据以及求解远端机器人在X、Y、Z三个轴的位移变化和角度变化的过程中通过上位机程序进行高通滤波处理，为实现IMU操控六自由度并联平台，为保障跟随运动的实时性，运动控制器与驱动模块之间采用Ethercat协议交换数据。

如图3所示，由于积分过程实际是一个低通滤波器，所以积分后的信号中出现了较多的低频信号噪声。对积分后的信号可以用一个高通滤波器进行滤波，以削弱数值积分在信号中产生的低频噪声。为了使得系统的频率响应满足要求，要尽可能保证信号在通带内没有纹波。所设计的滤波器的传递函数为：

序列x(n)经过滤波器后的输出数列y(n)＝H(z)x(n)，则

y(n)＝a₀x(n)-a₁x(n-1)+a₂x(n-2)+b₁y(n-1)-b₂y(n-2)

其中a₀、a₁、a₂、b₁和b₂为由设定的采样频率、系统扰动的上界作为优化目标得到的最优解的系数。这些系数影响了输入响应的增益，同时也影响系统的稳定性。

本发明基于IMU的六自由度并联平台主从控制系统采用主从控制方式，以IMU作为控制主手，以Ethercat作为通讯协议，实现对六自由度并联平台远程的实时遥操作控制。通过远端机器人上搭载的IMU采集远端机器人的运动状态，并以Ethercat作为通讯协议进行运动控制器和伺服驱动模块之间的通讯，将远端机器人的运动姿态传给六自由度并联平台，完成六自由度并联平台对远端机器人的跟随运动，完成机器人在未知环境中运动姿态的复现，实现对六自由度并联平台的实时远程遥操作。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种基于IMU的六自由度并联平台主从控制系统，其特征在于，包括：

IMU，用于采集远端机器人在空间内的三个平移加速度和三个旋转的角加速度；

PC机，用于接收IMU采集的三个平移加速度和三个旋转的角加速度，对加速度进行二次积分和滤波得到X、Y、Z三个轴上的位移变化，对角加速度进行二次积分和滤波得到绕X、Y、Z三个轴转动的角度变化，从而将IMU采集的六个自由度的运动信息转换为六自由度并联平台的位姿信息，将得到的六自由度并联平台的位姿信息进行高通滤波并发送至运动控制器；

运动控制器，用于接收PC机发送的六自由度并联平台的位姿信息，通过运动学反解将六自由度并联平台的位姿信息转换成每个电动缸的缸长变化量，并发送至六自由度并联平台；

六自由度并联平台，用于接收运动控制器发送的每个电动缸的的缸长变化量，根据每个电动缸的的缸长变化量实现前后、左右、上下的平移以及俯仰、横滚和偏航的六个自由度运动，从而实现实时跟随远端机器人的运动。

2.根据权利要求1所述的基于IMU的六自由度并联平台主从控制系统，其特征在于，所述PC机包括：

IMU运动数据接收模块，用于接收IMU采集的三个平移加速度和三个旋转的角加速度；

数据处理模块，用于对加速度进行二次积分和滤波得到X、Y、Z三个轴上的位移变化，对角加速度进行二次积分和滤波得到绕X、Y、Z三个轴转动的角度变化，从而将IMU采集的六个自由度的运动信息转换为六自由度并联平台的位姿信息；

第一通讯模块，用于将得到的六自由度并联平台的位姿信息进行高通滤波并发送至运动控制器。

3.根据权利要求1所述的基于IMU的六自由度并联平台主从控制系统，其特征在于，所述运动控制器包括：

第二通讯模块，用于接收PC机发送的六自由度并联平台的位姿信息；

运动学反解模块，用于通过运动学反解将六自由度并联平台的位姿信息转换成每个电动缸的缸长变化量；

第三通讯模块，用于将每个电动缸的缸长变化量发送至六自由度并联平台。

4.根据权利要求1所述的基于IMU的六自由度并联平台主从控制系统，其特征在于，通过运动学反解将六自由度并联平台的位姿信息转换成每个电动缸的缸长变化量具体如下：

如果给定坐标系B的原点P在坐标系A中的坐标向量为p＝[x_P y_P z_P]^T,六自由度并联平台的姿态用横滚-俯仰-偏转欧拉角表示为(α，β，γ)，则坐标系B相对于坐标系A的旋转矩阵为：

其中：

对于每一个电动缸，建立一下闭环向量表达式：

其中A_i和B_i表示第i个电动缸连接球副，于是有：

为A_i在坐标系A中的坐标，

为B_i在坐标系B中的坐标；

第i个电动缸的缸长变化量q_i等于向量

的模,因此，获得给定六自由度并联平台位姿相应的输入为：

对于给定的六自由度并联平台位姿，显然

已知，而

只与六自由度并联平台的设计参数有关，因此，将相关已知参数代入公式可得到相应电动缸的缸长变化量。

5.根据权利要求1所述的基于IMU的六自由度并联平台主从控制系统，其特征在于，所述六自由度并联平台包括动平台、静平台和六个伺服驱动模块，每个伺服驱动模块包含伺服驱动器、伺服电机和电动缸；

伺服驱动器一端连接运动控制器，另一端连接伺服电机的一端，伺服电机的另一端连接电动缸的一端，电动缸一端还与固定的静平台相连，另一端与活动的动平台相连。

6.根据权利要求1所述的基于IMU的六自由度并联平台主从控制系统，其特征在于，所述高通滤波的滤波器的传递函数为：

序列x(n)经过滤波器后的输出数列y(n)＝H(z)x(n)，则

y(n)＝a₀x(n)-a₁x(n-1)+a₂x(n-2)+b₁y(n-1)-b₂y(n-2)

其中a₀、a₁、a₂、b₁和b₂为由设定的采样频率、系统扰动的上界作为优化目标得到的最优解的系数。

7.根据权利要求1所述的基于IMU的六自由度并联平台主从控制系统，其特征在于，所述IMU为含三个平移加速度和三个旋转的角加速度共六个运动自由度的惯性传感器。

8.根据权利要求1所述的基于IMU的六自由度并联平台主从控制系统，其特征在于，所述IMU固定在远端机器人上。

9.根据权利要求1所述的基于IMU的六自由度并联平台主从控制系统，其特征在于，所述IMU与PC机通过串口连接。

10.根据权利要求1所述的基于IMU的六自由度并联平台主从控制系统，其特征在于，所述PC机与运动控制器通过普通以太网相连；所述运动控制器与六自由度并联平台通过工业以太网相连。

Images