论文部分内容阅读
摘 要：本文探讨了一种基于四元数的扩展卡尔曼滤波器（EKF），该滤波器用于使用三轴陀螺仪、加速度计和磁强计估计非完整球形机器人的姿态。低成本惯性测量单元（IMU）和磁强计安装在球形机器人上，并将测量数据与EKF进行融合，以确定机器人的姿态。球形机器人的姿态时间参数曲线，因此是验证其全局态度的理想曲线。在圆形和三叶形轨迹上进行了室内实验。建立了利用估计姿态对机器人运动学进行积分，并与参考轨迹进行比较。
关键词：球形;机器人;全向运动
一、介绍
姿态和方位估计是导航和控制飞行器、仿人机器人、室内定位和跟踪应用的关键步骤。利用惯性、GPS、里程表、激光、雷达和声纳传感器来估计移动机器人的姿态。由于测量不准确，低成本传感器很少提供良好的性能在不同的环境中。 随着微机电系统技术的出现，使用紧凑、低成本的惯性测量单元的情况呈指数增长。 惯性传感器的输出结合磁强计和基于视觉的系统进行融合，确定移动机器人的航向角。一般来说，态度可以分为三种形式，即欧拉角、方向余弦矩阵和四元数。由于九种元素，方向余弦矩阵需要很大的存储空间，欧拉角受到一个称为“万向节锁”的奇点的影响，即一在特定的方向上失去一个自由度。四元数只需要四个元素来存储，而不会受到欧拉角所面临的奇点，因此它是首选。
姿态估计算法的验证需要昂贵的三自由度含气模拟器。球形机器人相对于其他轮式机构的最大优势之一是全向移动。得到的各种数学模型在Euler-Poincare方程中得到了不同执行器配置的球形机器人摆动驱动在和滑动质量驱动。公式提供了使用三个内部转子的球形动力学的几何可控性证明。
二、项目的研究思路和方法
（一）运动模块设计
本项目中使用TB6612FNG微型双路直流电机驱动模块。电机自带编码器，实现力矩、速度以及点位控制，完成球形机器人前进、后退以及速度控制。TB6612FNG内部集成了2个H桥电机驱动电路、6个控制信号输入端（其中2个为PWM控制信号输入端、4个为方向信号输入端），可同时控制球形机器人2个电机的正转、反转和制动。球形机器人转向动作由舵机控制，舵机具有位置（角度）伺服驱动功能，满足球形机器人的转向要求。通过设置单片机的PWM占空比值来控制舵机偏转角，从而实现球型机器人的转向控制。
（二）姿态检测模块设计
姿态检测模块选用MPU-6050六轴传感器，该模块集成了3轴微机电系统（micro-electro-mechanical system，MEMS）陀螺仪和3轴MEMS加速度计，以及一个可扩展的数字运动处理器（digital motion processor，DMP）。当产生外部中断时，单片机进入中断，并读取MPU-6050六轴传感器中的各姿态参数值，此参数值通过卡尔曼滤波、增量式PID控制算法修正后发送给球体的控制单元，实时调整球体姿态，控制球体在运动过程中的自身平衡。
（三）控制系统软件设计
因球形机器人控制系统具有非线性、不稳定的特点，相应的软件设计就成为了关键。根据系统要求，软件主要任务是完成系统的初始化设置、球体姿态信息采集、卡尔曼滤波、角速度检测、电机双闭环PID控制以及蓝牙通信等。
（1）双闭环PID
控制当球型机器人运动时，球体姿态随之改变，为了保证球体自身平衡，需设计相应的控制方法，这里采用工程实践中应用最为广泛的增量式PID控制算法，分别设计球体的姿态PID控制和球体速度PID控制。
三、模型建立
图1所示的球形机器人示意图由一个在水平面上滚动的质量为m、半径为r的球壳组成。假设机器人的重心与几何中心。点O1的位置坐标关于O用（x，y）表示。车身骨架的定位（Xb，Yb，Zb）相对于惯性系（Xi，Yi，Zi）的机器人由旋转矩阵R∈SO给出。该机器人有三个独立控制的转子，转子上有惯性轮轴线。因此，三个独立的力矩作用在生成车身坐标系。表示转子角度乘以Ψi，i=1，2，3。球形机器人的配置空间，忽略转子角度，Q=R2×SO由曲线q（t）=（x（t），y（t），R（t））∈q。球形机器人受到无滑移约束。
式中，ω∈R3表示物体角速度，Ω∈R3表示物体角速度是机器人的空间角速度。的运动学球形机器人由下式给出
其中旋转矩阵R的行用R1，R2，R3表示。
IMU由加速度计、陀螺仪和磁力计组成，安装在球形机器人的中心位置，提供原始数据测量。陀螺仪测量物体的角速度ω=（ωx，ωy，ωz）。加速度计和磁强计分别测量线加速度（ax，ay，az）和地球惯性系中的磁场矢量（mx，my，mz）。傳感器假定读数具有恒定的偏差b=（ba，bg，bm）和被独立的高斯白噪声污染η=（ηa，ηΨ，ηm）加速度计的平均值和方差为零σ2 = （σ2a， σ2g， σ2m），陀螺仪和磁强计。因此，9自由度传感器模型有
从加速度计和磁强计的数据组成部分，可以得到如下欧拉角
测量矢量包括每个时代的加速度计和磁强计读数。 惯性系中的参考加速度a0和磁场矢量m0被选择为a0=（0，0，1）a分别为dM0=（MX，0，MZ）。 物体框架内的加速度和磁场矢量可以在任意时刻k下计算为
关于xk的雅可比是状态方程中的观测矩阵Hk。 通过泰勒展开，F和H的雅可比矩阵是f和h的偏导数
扩展卡尔曼滤波是一种两步迭代随机算法，它给出了每个时代的状态和误差协方差。它首先根据系统模型预测状态先验，然后是根据实际观测结果进行估计。 假设状态的初始状态和误差协方差矩阵分别为 和 ，而Kk是卡尔曼增益。 Qk和Rk是过程噪声协方差矩阵和测量噪声协方差矩阵。
四、研究成果展示
参考文献：
[1]刘家念.球形机器人未来发展方向研究[J].科技创新与应用，2015（18）：61.
[2]杨伟.一种全向运动的球形机器人研究[D].天津大学，2018.
[3]吴晗，徐开芸，朱昊，李家钊.基于单片机的球型机器人控制系统设计[J].机械设计与制造工程，2018，47（11）：53-57.
作者简介：
王淼（1998-），男，汉族，安徽芜湖人，江苏大学17级本科生，专业自动化，研究方向为自动控制
冯纪超（1999-），男，汉族，江苏徐州人，江苏大学17级本科生，专业自动化，研究方向为自动控制
储金城（1999-），男，汉族，江苏常州人，江苏大学17级本科生，专业自动化，研究方向为自动控制
基金项目：本文是江苏大学2019年度大学生科研课题立项资助项目，项目编号：18B007
关键词：球形;机器人;全向运动
一、介绍
姿态和方位估计是导航和控制飞行器、仿人机器人、室内定位和跟踪应用的关键步骤。利用惯性、GPS、里程表、激光、雷达和声纳传感器来估计移动机器人的姿态。由于测量不准确，低成本传感器很少提供良好的性能在不同的环境中。 随着微机电系统技术的出现，使用紧凑、低成本的惯性测量单元的情况呈指数增长。 惯性传感器的输出结合磁强计和基于视觉的系统进行融合，确定移动机器人的航向角。一般来说，态度可以分为三种形式，即欧拉角、方向余弦矩阵和四元数。由于九种元素，方向余弦矩阵需要很大的存储空间，欧拉角受到一个称为“万向节锁”的奇点的影响，即一在特定的方向上失去一个自由度。四元数只需要四个元素来存储，而不会受到欧拉角所面临的奇点，因此它是首选。
姿态估计算法的验证需要昂贵的三自由度含气模拟器。球形机器人相对于其他轮式机构的最大优势之一是全向移动。得到的各种数学模型在Euler-Poincare方程中得到了不同执行器配置的球形机器人摆动驱动在和滑动质量驱动。公式提供了使用三个内部转子的球形动力学的几何可控性证明。
二、项目的研究思路和方法
（一）运动模块设计
本项目中使用TB6612FNG微型双路直流电机驱动模块。电机自带编码器，实现力矩、速度以及点位控制，完成球形机器人前进、后退以及速度控制。TB6612FNG内部集成了2个H桥电机驱动电路、6个控制信号输入端（其中2个为PWM控制信号输入端、4个为方向信号输入端），可同时控制球形机器人2个电机的正转、反转和制动。球形机器人转向动作由舵机控制，舵机具有位置（角度）伺服驱动功能，满足球形机器人的转向要求。通过设置单片机的PWM占空比值来控制舵机偏转角，从而实现球型机器人的转向控制。
（二）姿态检测模块设计
姿态检测模块选用MPU-6050六轴传感器，该模块集成了3轴微机电系统（micro-electro-mechanical system，MEMS）陀螺仪和3轴MEMS加速度计，以及一个可扩展的数字运动处理器（digital motion processor，DMP）。当产生外部中断时，单片机进入中断，并读取MPU-6050六轴传感器中的各姿态参数值，此参数值通过卡尔曼滤波、增量式PID控制算法修正后发送给球体的控制单元，实时调整球体姿态，控制球体在运动过程中的自身平衡。
（三）控制系统软件设计
因球形机器人控制系统具有非线性、不稳定的特点，相应的软件设计就成为了关键。根据系统要求，软件主要任务是完成系统的初始化设置、球体姿态信息采集、卡尔曼滤波、角速度检测、电机双闭环PID控制以及蓝牙通信等。
（1）双闭环PID
控制当球型机器人运动时，球体姿态随之改变，为了保证球体自身平衡，需设计相应的控制方法，这里采用工程实践中应用最为广泛的增量式PID控制算法，分别设计球体的姿态PID控制和球体速度PID控制。
三、模型建立
图1所示的球形机器人示意图由一个在水平面上滚动的质量为m、半径为r的球壳组成。假设机器人的重心与几何中心。点O1的位置坐标关于O用（x，y）表示。车身骨架的定位（Xb，Yb，Zb）相对于惯性系（Xi，Yi，Zi）的机器人由旋转矩阵R∈SO给出。该机器人有三个独立控制的转子，转子上有惯性轮轴线。因此，三个独立的力矩作用在生成车身坐标系。表示转子角度乘以Ψi，i=1，2，3。球形机器人的配置空间，忽略转子角度，Q=R2×SO由曲线q（t）=（x（t），y（t），R（t））∈q。球形机器人受到无滑移约束。
式中，ω∈R3表示物体角速度，Ω∈R3表示物体角速度是机器人的空间角速度。的运动学球形机器人由下式给出
其中旋转矩阵R的行用R1，R2，R3表示。
IMU由加速度计、陀螺仪和磁力计组成，安装在球形机器人的中心位置，提供原始数据测量。陀螺仪测量物体的角速度ω=（ωx，ωy，ωz）。加速度计和磁强计分别测量线加速度（ax，ay，az）和地球惯性系中的磁场矢量（mx，my，mz）。傳感器假定读数具有恒定的偏差b=（ba，bg，bm）和被独立的高斯白噪声污染η=（ηa，ηΨ，ηm）加速度计的平均值和方差为零σ2 = （σ2a， σ2g， σ2m），陀螺仪和磁强计。因此，9自由度传感器模型有
从加速度计和磁强计的数据组成部分，可以得到如下欧拉角
测量矢量包括每个时代的加速度计和磁强计读数。 惯性系中的参考加速度a0和磁场矢量m0被选择为a0=（0，0，1）a分别为dM0=（MX，0，MZ）。 物体框架内的加速度和磁场矢量可以在任意时刻k下计算为
关于xk的雅可比是状态方程中的观测矩阵Hk。 通过泰勒展开，F和H的雅可比矩阵是f和h的偏导数
扩展卡尔曼滤波是一种两步迭代随机算法，它给出了每个时代的状态和误差协方差。它首先根据系统模型预测状态先验，然后是根据实际观测结果进行估计。 假设状态的初始状态和误差协方差矩阵分别为 和 ，而Kk是卡尔曼增益。 Qk和Rk是过程噪声协方差矩阵和测量噪声协方差矩阵。
四、研究成果展示
参考文献：
[1]刘家念.球形机器人未来发展方向研究[J].科技创新与应用，2015（18）：61.
[2]杨伟.一种全向运动的球形机器人研究[D].天津大学，2018.
[3]吴晗，徐开芸，朱昊，李家钊.基于单片机的球型机器人控制系统设计[J].机械设计与制造工程，2018，47（11）：53-57.
作者简介：
王淼（1998-），男，汉族，安徽芜湖人，江苏大学17级本科生，专业自动化，研究方向为自动控制
冯纪超（1999-），男，汉族，江苏徐州人，江苏大学17级本科生，专业自动化，研究方向为自动控制
储金城（1999-），男，汉族，江苏常州人，江苏大学17级本科生，专业自动化，研究方向为自动控制
基金项目：本文是江苏大学2019年度大学生科研课题立项资助项目，项目编号：18B007