CN108061855A - 一种基于mems传感器的球形电机转子位置检测方法 - Google Patents

Abstract

本发明涉及一种基于MEMS传感器的球形电机转子位置检测方法，包括：采集MEMS传感器的原始数据。分别对角速度进行零偏误差补偿，对加速度和磁感应强度进行椭球拟合补偿。将补偿后的角速度、加速度和磁感应强度数据进行基于互补滤波的数据融合，并代入四元数微分方程中得到姿态更新四元数。由姿态更新四元数转换到欧拉角，并结合欧拉角在不同区间的变化特点，做出准确的区间转移判断，实现全角度范围变化的条件下欧拉角的转换。

Description

一种基于MEMS传感器的球形电机转子位置检测方法

技术领域

本发明属于球形电机的控制技术领域，尤其涉及到一种球形电机转子位置检测方法。

背景技术

随着现代工业科技水平的不断发展，机器人、机械手等做复杂运动的精密装置得到了广泛的应用。这类装置往往需要完成空间的多维运动控制，然而从运动学角度来看，转子呈球形结构最有利于多维运动，可以在空间任意方向进行定位、工作，但是其结构分析较为困难。作为电机不可缺少的反馈控制环节，球形电机转子的位置检测是电机控制系统中关键技术之一，并且它的分辨率直接影响到系统的控制精度，因此转子位置检测技术是当前球形电机研究中迫切需要解决的问题。一些学者采用滑轨支架配合旋转编码器或者光电编码器的方法进行球形电机转子位置检测。

但是目前很少有基于MEMS(Micro Electron Mechanical Systems)，即微电子机械系统的球形电机转子位置检测方法。基于滑轨支架配合旋转编码器的测量方法需要特殊的导联结构耦合转子方向与编码器，增加摩擦阻力，给系统运行带来不便。基于光电编码器的测量方法需要在转子表面喷涂颜色，并且为了提高分辨率，需要布置较多的光电编码器，增加了系统的复杂程度。由于MEMS器件成本较低，且可以较精确地测量出球形电机转动的三轴角速度、三轴加速度和三轴磁场强度，通过一定的数据滤波和融合算法即可得到精确的球形电机转子位置。

发明内容

本发明的目的是克服基于滑轨支架配合旋转编码器或光电编码器测量系统中硬件较复杂的弊端，提出了一种基于MEMS传感器的球形电机转子位置检测方法。方案如下：

一种基于MEMS传感器的球形电机转子位置检测方法，包括如下步骤：

(1)定时采集MEMS传感器的原始数据，由三轴陀螺仪得到角速度ω^m＝[ω_x ω_yω_z]^T，三轴加速度计得到加速度a^m＝[a_x a_y a_z]^T，三轴磁强计得到磁感应强度m^m＝[m_x m_ym_z]^T。

(2)分别对角速度进行零偏误差补偿，对加速度和磁感应强度进行椭球拟合补偿：

ω^b＝ω^m-ω₀

a^b＝K₁K₂a^m+a₀

m^b＝K₃K₄m^m+m₀

式中，ω^b、a^b、m^b分别是补偿后的角速度、加速度、磁感应强度值，ω₀、a₀、m₀分别是陀螺仪、加速度计和磁力计的零偏误差，K₁、K₂分别是加速度计灵敏度误差和不正交误差构成的矩阵，K₃、K₄分别是磁力计灵敏度误差和不正交误差构成的矩阵。

(3)将补偿后的角速度、加速度和磁感应强度数据进行基于互补滤波的数据融合，并代入四元数微分方程中得到姿态更新四元数q₊：

ω^g＝ω^b+δ

δ＝K_pe+K_i∫_e

其中为当前时刻四元数的微分，q_-为上一时刻的四元数，T为更新周期，ω^g为经PI补偿后的角速度值，δ为对陀螺仪漂移的补偿量，e为俯仰角、横滚角与偏航角的误差之和，K_p、K_i分别为PI调节的比例参数和积分参数，a^b为加速度计测量得到的重力加速度，v_b为从上一时刻推出的重力加速度的最优估计值，指向量间的叉乘，m^b为三轴磁场强度校正值，p_b为互补滤波器估计的地磁场矢量。

(4)由姿态更新四元数转换到欧拉角即横滚角γ_T、偏航角ψ_T和俯仰角θ_T，并结合欧拉角在不同区间的变化特点，做出准确的区间转移判断，实现全角度范围变化的条件下欧拉角的转换：

其中，γ、ψ、θ分别是经全角度解算后得到的横滚角、偏航角和俯仰角，γ_T、ψ_T、θ_T分别是由姿态更新四元数q₊直接转换出的横滚角、偏航角和俯仰角，A₂₁＝2(q₁q₂+q₀q₃)，A₃₂＝2(q₂q₃+q₀q₁)，

q₊＝[q₀ q₁ q₂ q₃]^T为姿态更新四元数。

本发明的技术效果如下：

1.本发明为基于MEMS器件制成的球形电机转子位置检测系统，其拥有较简单的硬件结构，同时采用数据融合和滤波算法可以较好地消除误差，保证高精度和低计算复杂度。

2.在将陀螺仪、加速度计进行数据处理后，基于四元数更新的姿态解算算法在经过全角度解算后可以较好地反应球机转子的位置变化。

附图说明

图1：数据采集与处理模块硬件结构图

图2：数据采集与处理模块软件结构图

图3：数据接收模块硬件结构图

图4：数据接收模块软件结构图

图5：球形电机转子位置检测系统软件总体设计图

图6：球形电机转子位置解算软件结构图

图7：三轴矢量传感器不正交误差示意图

图8：基于互补滤波的数据融合算法框图

图9：全角度姿态解算算法流程图

图10：绕X轴旋转一周的过程的俯仰角

图11：绕Y轴旋转一周的过程的横滚角

图12：绕Z轴旋转一周的过程的偏航角

具体实施方式

本发明提供了一种基于MEMS器件的球形电机转子位置检测系统的设计方法，并通过测试平台验证其精度，下面结合附图和实例对本发明进行详细的描述。

设计球形电机转子位置检测系统的数据采集与处理模块，如图1、图2所示。其中包含负责控制并进行滤波处理的单片机或FPGA，MEMS器件(以MPU9250为例)和一个无线射频收发模块，而为使数据处理更简单，使MPU9250芯片的中心与球形电机转子的球心重合。编写程序，单片机或FPGA使用I²C总线或SPI总线与MPU9250通信，定时读取一次MPU9250中的三轴角速度、三轴加速度和三轴磁场强度。

球形电机转子位置检测系统与上位机之间通过无线UART模块的串口透传方式进行通信，按照基本的UART串口通信的方法编写数据收发程序。其中单片机UART口的TX与无线射频芯片的RX连接，单片机UART口的RX与无线射频芯片的TX连接。无线射频芯片型号为LC12S，工作频率为2.4GHz，成本低。

数据采集与处理模块的硬件结构图和软件结构图如图3、图4所示。它将解算后的四元数姿态数据通过数据包的形式发送给数据接收模块，每个数据包共有10个字节的数据，包括：数据头、8个字节的四元数姿态数据和数据尾。

球形电机转子位置检测系统软件总体设计图如图5所示。

设计基于四元数法的角度解算方法，陀螺仪通过积分求得姿态角，在短时间内可以提供准确的姿态信息，但是积分计算会使误差累积，并且存在零点漂移问题，因而长期精度较差。而加速度计和磁强计的误差不会随时间积累，但是短期内精度较差。基于对三种惯性传感器的自身特点考虑和减小系统解算误差的系统要求，根据三种惯性传感器在频域上的互补特性，采用基于互补滤波的数据融合算法对三种姿态基础数据进行滤波融合，提高姿态解算的精度和稳定性。

本发明中MEMS器件的中心与球形电机的转子球心重合，可以认为球形电机转子位置检测系统的载体坐标系和地理坐标系重合。球形电机转子在地理坐标系下的空间姿态可看作依次绕航向轴、俯仰轴、横滚轴作基本旋转后的复合结果。虽然采用四元数法进行姿态表达，但还是要用到欧拉角，以便直观地观测转子的角度变化。

基于MEMS惯性传感器的球形电机转子位置系统的姿态解算算法，通过对加速度计、陀螺仪和磁强计采集的加速度、角速度和磁场强度三种数据的解算得到转子的旋转角度。该解算过程是在数据采集与处理模块中完成的。

图6表示转子旋转角度的解算算法的流程，该算法的步骤如下：

(1)系统上电，设定在前1s内系统不运动，采集静止状态下的陀螺仪数据，并对陀螺仪零偏误差进行校正。在首次使用传感器时，对加速度计和磁强计采取椭球拟合的方法对其原始数据进行校准，在之后的程序里将椭球拟合参数代入，实现校正。

(2)三种惯性传感器采集数据进行初始对准。通过对加速度和磁场强度的解算求得姿态角，根据姿态角可以求出当前的姿态矩阵，由姿态矩阵便可得到初始化四元数。

(3)将加速度计、陀螺仪和磁强计采集的数据经补偿后进行基于互补滤波的数据融合，得到修正后的陀螺仪角速度数据。

(4)通过求解四元数微分方程对四元数进行更新，并将四元数进行归一化处理。得到更新后的四元数姿态矩阵。

(5)根据四元数转换欧拉角的特点，结合三轴姿态角在不同区间变化的特征，做出准确的区间转移判断，最后实现全角度下姿态角的解算。

(6)如果系统断电则解算结束，否则返回步骤(3)继续执行姿态解算。

下面对上述步骤进行具体说明。

陀螺仪、加速度计和磁强计的校准

对陀螺仪的数据采用每次上电都采集前50组数据取平均值的方法去除零偏误差。

对加速度计和磁强计采用离线校正的方法，在首次使用该传感器时，对实际测得的三轴矢量传感器(即加速度计或磁强计)在不同姿态下的输出值进行椭球拟合得到传感器的三轴零偏误差、三轴灵敏度误差和三个不正交角九个参数。

已知加速度计和磁强计在不同姿态下测得的矢量的模值是一个常数。令传感器系统误差校准模型为：

式中，Y^m为理想状态下三轴矢量传感器的输出，Y^b为在载体坐标系下测得的三轴分量，Y₀为传感器零偏误差。为三轴灵敏度误差，K₂为三轴间不正交误差。图7为三轴矢量传感器不正交误差示意图。图7中，将z与z₁轴重合，设在xOz平面，x₁与x轴夹角为α；在xOy平面内，y与y₁轴的夹角为β；y₁轴与xOy平面的夹角为η。

测得的矢量模值的平方可以表示为：

||Y^m||²＝(Y^m)^TY^m＝(Y^b-Y₀)^T(K^-1)^T(K^-1)(Y^b-Y₀)

即

式中，

在三维空间内不同姿态角下，三轴矢量传感器测量的实际三轴矢量的输出满足二次型椭球曲面方程。三轴矢量传感器的零偏误差导致该椭球曲面原点不在坐标原点。传感器的灵敏度和不正交误差决定了椭球的长轴和短轴参数。

本发明主要利用最小二乘法进行椭球拟合。设椭球面方程为：

F(ξ,z)＝ξ^Tz＝ax²+by²+cz²+2dxy+2exz+2fyz+2px+2qy+2rz+g＝0

其中，ξ＝[a,b,c,d,e,f,p,q,r,g]^T为待求的二次曲面参数向量；

z＝[x²,y²,z²,2xy,2xz,2yz,2x,2y,2z,1]^T为测量值的组合向量；测量数据(x,y,z)与二次曲面F(ξ,z)＝0的代数距离为F(ξ,z)。

以所测数据到椭球面距离的平方和最小作为判定准则，即：

其中，N为测量数据的个数。

将椭球面方程转化为矩阵相乘的形式：

(X-X₀)^TA(X-X₀)＝1

其中，X₀表示椭球中心点的坐标原点，矩阵A表示椭球曲面最佳的形状参数矩阵；

在被测矢量模值已知的情况下，利用椭球面方程和上式之间的对应关系可以得到：

即：

令可以计算得到参数k_x、k_y、k_z、α、β、η的估计值以及三轴矢量传感器的零偏Y₀如下：

姿态的初始对准

假设三轴加速度计在载体坐标系上的校正值为

则得出转子初始时的俯仰角θ₀和横滚角γ₀分别为

其中，g为测量地点的重力加速度。

当载体的姿态角不为零时，根据地磁场在水平方向上的分量始终指向地磁北极这一理论基础，以及通过加速度计计算出的俯仰角和横滚角，可推出偏航角的公式。

假设磁强计的测量值经校正后沿载体坐标系的三个坐标轴分解得到如下分量：

则载体的初始偏航角ψ₀为：

其中

通过求得的载体初始俯仰角、横滚角和偏航角，求解得出初始四元数，求解公式如下：

式中，q₀、q₁、q₂、q₃为初始四元数的四个元素。

基于互补滤波的数据融合算法

加速度计和磁强计信号具有高频特性，陀螺仪具有低频特性，基于互补滤波器在频域上具有互补的特性，设计互补滤波传递函数满足G₁(s)+G₂(s)＝1，实现对两种特性信号的互补滤波。其中，G₁(s)具有一阶高通滤波的特性，G₂(s)具有一阶低通滤波的特性。互补滤波器采用比例积分补偿的方法，取C(s)＝K_p+K_i/s，其中，K_p是滤波器的比例增益，K_i是滤波器的积分增益。

基于互补滤波的数据融合算法框图如图8所示。

首先对加速度计和磁强计采集的三轴加速度a_b和三轴磁场强度m_b经校正后，做归一化处理。

归一化处理之后，将载体坐标系中测得的磁场强度矢量转换成地理坐标系下的磁场矢量，此矢量值是磁场矢量的测量值，用h表示。

将地理坐标系下磁场矢量测量量h转换到地理坐标系的XOY平面上，则用b来表示h的投影得到的向量，其值为

再把向量b从地理坐标系转换到载体坐标系得到磁场强度矢量的估计值

接下来，求俯仰角、横滚角与偏航角的误差之和e：

e＝e_a+e_m＝a^b×v_b+m^b×p_b

由上式中可以看出，误差e由两部分组成，分别是俯仰角和偏航角的误差e_a和偏航角的误差e_m。e_a是v_b与a^b的叉积，其中a^b为加速度计测量得到的重力加速度，v_b为从上一时刻推出的重力加速度的最优估计值。其中，e_m是m^b与p_b的叉积，其中m^b为三轴磁场强度校正值，p_b为互补滤波器估计的地磁场矢量。

误差e通过一个比例积分环节之后生成对陀螺仪漂移的补偿量δ：

δ＝K_pe+K_i∫e

陀螺仪测得的三轴角速度经补偿后为ω^b，则经过加速度计和磁强计融合校正之后的陀螺仪角速度ω^g为：

ω^g＝ω^b+δ

基于互补滤波的数据融合算法的计算量比较小，在姿态解算中可以提高姿态解算的速度，并且很好地将三种惯性传感器的优缺点互补，实现了三种传感器数据的融合，可以有效地提高球形电机转子位置检测系统的精度。

基于四元数法的姿态更新算法

将初始对准的初始化四元数和经过补偿后的陀螺仪角速度代入四元数微分方程当中，通过龙格－库塔法求解四元数微分方程，求得更新之后的四元数姿态数据，姿态更新后需要对四元数进行归一化处理，并对姿态矩阵进行更新，然后求解欧拉角。

龙格－库塔法求解四元数微分方程的结果如下：

其中，q₀(t+T)、q₁(t+T)、q₂(t+T)、q₃(t+T)分别为当前时刻的四元数更新值，q₀(t)、q₁(t)、q₂(t)、q₃(t)分别为上一时刻的四元数值，T为更新周期。

将姿态更新后的四元数进行归一化处理，作为最终的姿态数据，更新姿态矩阵

全角度解算欧拉角

四元数与欧拉角之间的变换关系如下所示：

四元数转换欧拉角的公式如下

θ_T＝arcsin(2q₀q₂-2q₁q₃)

由上式的表示方式可以看出，俯仰角θ在-90°到90°的范围里，cosθ的值可以确定为正值，则可以实现在常规条件下欧拉角的范围转换(俯仰角的范围为[-90°,90°]，横滚角的范围为[-180°,180°]，偏航角的范围为[0°,360°])

在以下的分析中，令

A₃₁＝2(q₀q₂-q₁q₃)

A₂₁＝2(q₁q₂+q₀q₃)

A₃₂＝2(q₂q₃+q₀q₁)

将cosθ为正值的状态称为状态一，cosθ为负值的状态称为状态二。

则三轴欧拉角的表示方法为：

全角度姿态解算算法流程图如图9所示。

由于不同状态间的切换主要是通过俯仰角位于不同象限，即cosθ的正负性产生的。另外，同样四元数对应在状态一与状态二下的滚转角和偏航角的值彼此处于相互对应的象限(如第一象限对应第三象限，第二象限对应第四象限)，正好相差180°。因此，可以根据两次测量之间的差值是否合理来判断是否应进行状态变换，在本应用中差值设定在90°。

另外，在姿态角解算过程中，每一次计算都要将得到的角度主值转化到相应的完整定义域中，这样在各个定义域的边缘就不可避免会出现大的跳变。但是，基于球机转子姿态角只能在连续范围内变化的事实，如果差值位于360°附近，则应视为连续状态。

因此，可以使用滚转角和偏航角分别与前一拍的绝对差值的和来进行判断，如果在一种状态下的计算过程中绝对差值的和超出180°(两倍允许的最大测量差值)，即可则应视为发生了状态切换。即本应该是依另外一种状态计算的值，错误的在当前状态下计算。此时，应切换至另一种状态对当前拍进行重新计算，并且在下一时刻的计算时根据转换后的状态进行计算。

图10、图11、图12分别为绕X轴、Y轴、Z轴旋转一周后，对应的俯仰角、横滚角和偏航角的角度变化图。

Claims (1)

1.一种基于MEMS传感器的球形电机转子位置检测方法，包括如下步骤：

(1)定时采集MEMS传感器的原始数据，由三轴陀螺仪得到角速度ω^m＝[ω_x ω_y ω_z]^T，三轴加速度计得到加速度a^m＝[a_x a_y a_z]^T，三轴磁强计得到磁感应强度m^m＝[m_x m_y m_z]^T。

(2)分别对角速度进行零偏误差补偿，对加速度和磁感应强度进行椭球拟合补偿：

ω^b＝ω^m-ω₀

a^b＝K₁K₂a^m+a₀

m^b＝K₃K₄m^m+m₀

式中，ω^b、a^b、m^b分别是补偿后的角速度、加速度、磁感应强度值，ω₀、a₀、m₀分别是陀螺仪、加速度计和磁力计的零偏误差，K₁、K₂分别是加速度计灵敏度误差和不正交误差构成的矩阵，K₃、K₄分别是磁力计灵敏度误差和不正交误差构成的矩阵。

(3)将补偿后的角速度、加速度和磁感应强度数据进行基于互补滤波的数据融合，并代入四元数微分方程中得到姿态更新四元数q₊：

<mrow> <msub> <mi>q</mi> <mo>+</mo> </msub> <mo>=</mo> <msub> <mi>q</mi> <mo>-</mo> </msub> <mo>+</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mi>T</mi> </mrow>

ω^g＝ω^b+δ

δ＝K_pe+K_i∫e

<mrow> <mi>e</mi> <mo>=</mo> <msup> <mi>a</mi> <mi>b</mi> </msup> <mo>&amp;CircleTimes;</mo> <msub> <mi>v</mi> <mi>b</mi> </msub> <mo>+</mo> <msup> <mi>m</mi> <mi>b</mi> </msup> <mo>&amp;CircleTimes;</mo> <msub> <mi>p</mi> <mi>b</mi> </msub> </mrow>

其中为当前时刻四元数的微分，q_-为上一时刻的四元数，T为更新周期，ω^g为经PI补偿后的角速度值，δ为对陀螺仪漂移的补偿量，e为俯仰角、横滚角与偏航角的误差之和，K_p、K_i分别为PI调节的比例参数和积分参数，a^b为加速度计测量得到的重力加速度，v_b为从上一时刻推出的重力加速度的最优估计值，指向量间的叉乘，m^b为三轴磁场强度校正值，p_b为互补滤波器估计的地磁场矢量。

(4)由姿态更新四元数转换到欧拉角即横滚角、偏航角和俯仰角，并结合欧拉角在不同区间的变化特点，做出准确的区间转移判断，实现全角度范围变化的条件下欧拉角的转换：

其中，γ、ψ、θ分别是经全角度解算后得到的横滚角、偏航角和俯仰角，γ_T、ψ_T、θ_T分别是由姿态更新四元数q₊直接转换出的横滚角、偏航角和俯仰角，A₂₁＝2(q₁q₂+q₀q₃)，