CN103743921B - 基于惯性传感器的自适应转速测量***及方法 - Google Patents

Abstract

本发明涉及一种基于惯性传感器的自适应转速测量***和基于惯性传感器的自适应转速测量方法，测量***包括转速检测器和显示终端，转速检测器内设有加速度计、陀螺仪、第一微处理器、第一无线传输模块和第一供电电池，显示终端内设有第二无线传输模块、第二微处理器、显示器和第二供电电池；第一无线传输模块与第二无线传输模块无线连接，加速度计的Z轴和陀螺仪的Z轴均与旋转体的转轴平行。本发明结合陀螺仪和加速度计的功能，在旋转体低速转动时，***自适应地根据陀螺仪的输出，计算旋转体的旋转半径，同时输出转速，在旋转体高速转动时，***自适应地根据加速度计输出的向心加速度，并结合低速的计算结果计算处理输出高速转速。

Description

基于惯性传感器的自适应转速测量***及方法

技术领域

本发明涉及旋转体的转速测量技术领域，尤其涉及一种基于惯性传感器的自适应转速测量***及方法。

背景技术

传统的转速测量方法分为两大类，一类是基于霍尔效应的转速测量，另一类是基于光电反射的转速测量。

霍尔效应测量转速需要在旋转体上安装磁铁，并在磁铁的旋转轨迹上就近安装霍尔感应器件，通过微处理器检测霍尔感应器件输出的开关信号来计算转速。通过霍尔效应测量转速的方法存在两点不足，其一是安装要求高，磁铁旋转轨迹上就近安装霍尔器件，距离控制困难，其二是在旋转体低速转速下需要非常多的磁铁点形成更多的开关信号，才能有效测量低转速，但是，通过该方法进行低转速测量时，需要预先有效的控制磁铁数量和磁铁霍尔器件的距离，一旦设计好，转速测量装置将不能变动，容易限制了转速测量的灵活性。

光电反射测量转速，是在旋转体上贴上间断反光条，外部通过光电检测模块检测反光条的反光脉冲来测量转速。通过光电反射测量转速也存在以下两点不足，其一是反光条贴装比较麻烦，需要被测旋转体有平行与转动轴的圆面，其二是光电测试检测端要与反光条平行稳定放置，否则测量不稳或者根本无法测量。

传统的两者转速测量方法均难以灵活和精准的测量出旋转体在低转速运动时的转速，同时在测量设备的安装上复杂度高。

发明内容

本发明所要解决的技术问题是针对现有转速测量方法存在测量设备结构复杂、不易安装和低转速不易测量的问题，提供了一种可以随意安装、结构简单和具有低、高转速自适应测量功能的基于惯性传感器的自适应转速测量***和于惯性传感器的自适应转速测量方法。

为解决上述问题，本发明的一种技术方案是：

一种基于惯性传感器的自适应转速测量***，包括安装在旋转体上的转速检测器和与之进行无线通信的显示终端，所述转速检测器内设有用于输出旋转体三轴加速度的加速度计、用于输出旋转体三轴角速度的陀螺仪、用于控制及进行数据处理的第一微处理器、用于数据传送的第一无线传输模块和用于供电的第一供电电池，第一微处理器分别与加速度计、陀螺仪和第一无线传输模块相连，所述显示终端内设有用于数据传送的第二无线传输模块、第二微处理器、显示器和用于供电的第二供电电池，第二微处理器分别与显示器和第二无线传输模块相连；所述转速检测器中的第一无线传输模块与显示终端中的第二无线传输模块无线连接，所述加速度计的Z轴和陀螺仪的Z轴均与旋转体的转轴平行。

优选地，所述陀螺仪为三轴微机械陀螺仪，加速度计为三轴微机械加速度计。

优选地，所述第一无线传输模块和第二无线传输模块均为2.4G模块。

优选地，所述转速检测器内还设有用于显示转速检测器运行状态的指示模块，指示模块分别与第一微处理器和第一供电电池相连。

相比较于现有技术，本发明的基于惯性传感器的自适应转速测量***结合陀螺仪和加速度计的功能，在旋转体低速转动时，***自适应地根据陀螺仪的输出，计算旋转体的旋转半径，同时输出转速，在旋转体高速转动时，***自适应地根据加速度计输出的向心加速度，并结合低速的计算结果计算处理输出高速转速；本发明中的转速检测器可以随意安装旋转体上，安装方便、无方向和角度要求，有效的解决了低转速不易精确测量和转速检测器不易安装的问题。

本发明的另一种技术方案是：

一种基于惯性传感器的自适应转速测量方法，所述转速测量方法基于本发明所述的基于惯性传感器的自适应转速测量***，所述转速测量方法包括以下两种情形：

第一种情形：旋转体处于低速转动时，由陀螺仪进行转速测量，所述低速转动为旋转体的转速在陀螺仪的测量范围内，陀螺仪输出旋转体的三轴角速度到第一微处理器中处理，得到旋转体的转速值，第一微处理器将转速值通过第一无线传输模块传送到显示终端中显示，同时第一微处理器计算并保存旋转体的旋转半径，所述旋转半径为转速检测器到旋转体转轴的距离；

第二种情形：旋转体处于高速转动时，由加速度计进行转速测量，所述高速转动是指旋转体的转速超出陀螺仪测量范围，包括如下步骤：

a)加速度计输出旋转体的三轴加速度到第一微处理器中保存，

b)第一微处理器根据第一种情形计算得到的旋转半径，计算并保存重力加速度对旋转体的影响值，

c)第一微处理器根据步骤a和步骤b的结果逆向推导出旋转体的转速值，并通过第一无线传输模块传送到显示终端中显示。

优选地，所述第一种情形中计算得到的旋转半径为旋转半径关于加速度计上各轴转动向心加速度的计算表达式，该计算表达式由以下公式计算得到：

$R{\mathrm{\ω}}_X^2=\sqrt{a_Y^2+a_Z^2}............\left(1\right)$

$R{\mathrm{\ω}}_Y^2=\sqrt{a_X^2+a_Z^2}............\left(2\right)$

$R{\mathrm{\ω}}_Z^2=\sqrt{a_Y^2+a_X^2}............\left(3\right)$

其中，ω_X、ω_Y和ω_Z为任意时刻陀螺仪输出的X轴角速度、Y轴角速度和Z轴角速度，R是旋转半径，a_X是旋转体X轴方向上的转动向心加速度，a_Y是旋转体Y轴方向上的转动向心加速度，a_Z是旋转体Z轴方向上的转动向心加速度；

解上述方程(1)(2)(3)得：

a_X＝k₁R…………(4)

a_Y＝k₂R…………(5)

a_Z＝k₃R…………(6)

其中，k₁、k₂和k₃是计算得出的已知系数。

优选地，根据权利要求5所述的基于惯性传感器的自适应转速测量方法，其特征在于，所述第二种情形的

步骤a中加速度计输出旋转体的三轴加速度分别A_X、A_Y和A_Z，

步骤b中重力加速度对旋转体的影响值包括不随旋转体转动变化的重力加速度分量g_XS、g_YS和g_ZS以及随旋转体转动周期性变化的重力加速度分量g_XD、g_YD和g_ZD，在旋转体匀速转动时，以下公式(7)(8)(9)成立：

A_X＝a_X+g_XS+g_XD…………(7)

A_Y＝a_Y+g_YS+g_YD…………(8)

A_Z＝a_Z+g_ZS+g_ZD…………(9)

通过滤波器将g_XD、g_YD和g_ZD滤除，得到相应变化量的幅值g_XDA、g_YDA和g_ZDA，根据公式(7)(8)(9)得到消除g_XD、g_YD和g_ZD影响后的A_XA、A_YA和A_ZA：

A_XA＝a_X+g_XS…………(10)

A_YA＝a_Y+g_YS…………(11)

A_ZA＝a_Z+g_ZS…………(12)

步骤c中旋转体的转速值计算包括如下步骤：

将公式(4)(5)(6)代入公式(10)(11)(12)，得到：

A_XA＝k₁R+g_XS…………(13)

A_YA＝k₂R+g_YS…………(14)

A_ZA＝k₃R+g_ZS…………(15)

因g为地球重力加速度，所以：

$g_{\mathrm{XS}}^2+g_{\mathrm{YS}}^2+g_{\mathrm{ZS}}^2+g_{\mathrm{XDA}}^2+g_{\mathrm{YDA}}^2+g_{\mathrm{ZDA}}^2=g^2............\left(16\right)$

其中g_XDA、g_YDA和g_ZDA为已知量，设所以：

$g_{\mathrm{XS}}^2+g_{\mathrm{YS}}^2+g_{\mathrm{ZS}}^2+k_4=g^2............\left(17\right)$

将公式(13)(14)(15)中的g_XS、g_YS和g_ZS变换为R的方程并代入公式(17)，得到关于R的二元一次方程，解出旋转半径R，后将R代入式(13)(14)(15)，解得g_XS、g_YS和g_ZS；

在任意时刻旋转半径R、g_XS、g_YS和g_ZS为定值，并代入公式(10)(11)(12)，解得任意时刻旋转体三轴方向上的转动向心加速度a_X、a_Y和a_Z，计算向心加速度向量和得到转动向心加速度a，再根据得出最终转速值ω。

相比较于现有技术，本发明的基于惯性传感器的自适应转速测量方法利用陀螺仪检测低速转动的旋转体，并输出相应的转速和旋转半径，利用加速度计检测高速转动的旋转体，并根据加速度计输出的向心加速度同时结合低速的计算结果计算处理输出高速转速，具有低速和高速检测方便、结果精确的优点；本发明的测量方法适用于不同方向、角度安装的旋转体，在测量时，只需将转速检测器随意安装旋转体上即可，安装十分简单方便。

附图说明

图1是本发明基于惯性传感器的自适应转速测量***的原理框图。

图2是本发明基于惯性传感器的自适应转速测量方法的实施例一结构示意图。

图3是本发明基于惯性传感器的自适应转速测量方法的实施例二结构示意图。

图4是本发明基于惯性传感器的自适应转速测量方法的实施例三结构示意图。

具体实施方式

下面结合附图和实施例进一步详细说明本发明，但本发明的保护范围并不限于此。

参照图1，本发明的一种基于惯性传感器的自适应转速测量***，包括安装在旋转体上的转速检测器和与之相连并进行通信的显示终端。转速检测器内设有加速度计、陀螺仪、第一微处理器、第一无线传输模块、第一供电电池和指示模块，第一微处理器分别与加速度计、陀螺仪、第一无线传输模块和指示模块相连，第一供电电池分别与加速度计、陀螺仪、第一微处理器、第一无线传输模块和指示模块相连，用于转速检测器的内部供电。显示终端内设有第二无线传输模块、第二微处理器、显示器和第二供电电池，第二微处理器分别与显示器和第二无线传输模块相连，第二供电电池分别与第二无线传输模块、第二微处理器和显示器相连，用于显示终端内部供电。其中，陀螺仪为三轴微机械陀螺仪，加速度计为三轴微机械加速度计。第一无线传输模块和第二无线传输模块均为2.4G模块。

所述加速度计用于输出旋转体三轴加速度，陀螺仪用于输出旋转体三轴角速度，第一微处理用于控制及进行数据处，第一无线传输模块用于数据传送，第一无线传输模块与第二无线传输模块无线连接，指示模块用于显示转速检测器运行状态的，指示模块为LED灯。在转速检测器安装时，将转速检测器固定到旋转体上，使其不会振动，另外，需要将加速度计的Z轴和陀螺仪的Z轴与旋转体的转轴平行设计。

当旋转体在低转速时，由陀螺仪进行检测输出三轴角速度，并由第一微处理器输出计算的转速，当旋转体在高转速时，由加速度进行检测输出三轴加速度，由第一微处理器结合陀螺仪检测的结果计算并输出的转速。第一微处理器输出的计算结果由第一无线传输模块传送到第二无线传输模块中，在第二微处理器的控制下，输出到显示器显示。

实施例一

通过本发明的***实现的基于惯性传感器的自适应转速测量方法，包括以下两种情形：

第一种情形：旋转体处于低速转动时，由陀螺仪进行转速测量，所述低速转动为旋转体的转速在陀螺仪的测量范围内，目前三轴微机械陀螺仪的测量范围仅2000度/秒，约330转/分，即旋转体的转速小于等于330转/分时，属于低速转动范围。此时，陀螺仪输出的三轴角速度的向量合成即为旋转体的转速值。陀螺仪输出旋转体的三轴角速度到第一微处理器中处理，得到旋转体的转速值，第一微处理器将转速值通过第一无线传输模块传送到显示终端中显示，同时第一微处理器计算并保存旋转体的旋转半径，所述旋转半径为转速检测器到旋转体转轴的距离。其中，第一种情形中计算得到的旋转半径为旋转半径关于加速度计上各轴转动向心加速度的计算表达式。

任意时刻，陀螺仪的转速方向与向心加速度的方向垂直，所以空间内三维向心加速度与三维转速满足以下公式，旋转半径关于加速度计上各轴转动向心加速度的计算表达式由以下公式计算得到：

$R{\mathrm{\ω}}_X^2=\sqrt{a_Y^2+a_Z^2}............\left(1\right)$

$R{\mathrm{\ω}}_Y^2=\sqrt{a_X^2+a_Z^2}............\left(2\right)$

$R{\mathrm{\ω}}_Z^2=\sqrt{a_Y^2+a_X^2}............\left(3\right)$

其中，ω_X、ω_Y和ω_Z为任意时刻陀螺仪输出的X轴角速度、Y轴角速度和Z轴角速度，R是旋转半径，a_X是旋转体X轴方向上的转动向心加速度，a_Y是旋转体Y轴方向上的转动向心加速度，a_Z是旋转体Z轴方向上的转动向心加速度；

解上述方程(1)(2)(3)得：

a_X＝k₁R…………(4)

a_Y＝k₂R…………(5)

a_Z＝k₃R…………(6)

其中，k₁、k₂和k₃是计算得出的已知系数。

第二种情形：旋转体处于高速转动时，由加速度计检测转动向心加速度反算转速，所述高速转动是指旋转体的转速超出陀螺仪测量范围。加速度计检测转动向心加速度反算转速的前提：一是消除重力加速度带来的影响，二是旋转半径已知。在第一种情形下，旋转半径的计算表达式处于已知，可以直接根据第一微处理器输出的旋转半径进行计算。

然而，重力加速度的影响与旋转体的转轴的空间位置有关系，本实施例以旋转体2的转轴2.1是倾斜的为例，即对旋转体2任意安装的情形进行详细说明，具体参照图2，转速检测器1固定安装在旋转体2上，在旋转体2的转轴2.1是倾斜情况下，旋转体2在某个倾斜的方向上做匀速转动时，重力加速度会对旋转体2产生两种影响，一是在重力方向上转动带来的周期性变化的重力加速度分量g_D，这种重力加速度变化周期与转动周期相等，可以通过数字低通滤波消除，另外一种是不随转动变化的重力加速度分量g_S，这种重力加速度分量可以通过方程解出。

旋转体处于高速转动时的转速计算包括如下步骤：

a)加速度计输出旋转体的三轴加速度到第一微处理器中保存，三轴加速度分别A_X、A_Y和A_Z；

b)第一微处理器根据第一种情形计算得到的旋转半径，计算并保存重力加速度对旋转体的影响值，重力加速度对旋转体的影响值包括不随旋转体转动变化的重力加速度分量g_XS、g_YS和g_ZS以及随旋转体转动周期性变化的重力加速度分量g_XD、g_YD和g_ZD，在旋转体匀速转动时，转动向心加速度a_X、a_Y和a_Z为未知的恒定值，g_XS、g_YS和g_ZS也是未知的恒定值，以下公式(7)(8)(9)成立：

A_X＝a_X+g_XS+g_XD…………(7)

A_Y＝a_Y+g_YS+g_YD…………(8)

A_Z＝a_Z+g_ZS+g_ZD…………(9)

其中，通过滤波器将g_XD、g_YD和g_ZD滤除，得到相应变化量的幅值g_XDA、g_YDA和g_ZDA，幅值g_XDA、g_YDA和g_ZDA为已知值，根据公式(7)(8)(9)得到消除g_XD、g_YD和g_ZD影响后的A_XA、A_YA和A_ZA：

A_XA＝a_X+g_XS…………(10)

A_YA＝a_Y+g_YS…………(11)

A_ZA＝a_Z+g_ZS…………(12)

c)第一微处理器根据步骤a和步骤b的结果逆向推导出旋转体的转速值，并通过第一无线传输模块传送到显示终端中显示。具体计算如下：

将公式(4)(5)(6)代入公式(10)(11)(12)，得到：

A_XA＝k₁R+g_XS…………(13)

A_YA＝k₂R+g_YS…………(14)

A_ZA＝k₃R+g_ZS…………(15)

因g为地球重力加速度，又g_S和g_D的向量和等于重力加速度，所以成立，其中g是地球重力加速度，为已知量。且 $g_D^2=g_{\mathrm{XDA}}^2+g_{\mathrm{YDA}}^2+g_{\mathrm{ZDA}}^2,$ 所以：

$g_{\mathrm{XS}}^2+g_{\mathrm{YS}}^2+g_{\mathrm{ZS}}^2+g_{\mathrm{XDA}}^2+g_{\mathrm{YDA}}^2+g_{\mathrm{ZDA}}^2=g^2............\left(16\right)$

其中g_XDA、g_YDA和g_ZDA为已知量，设所以：

$g_{\mathrm{XS}}^2+g_{\mathrm{YS}}^2+g_{\mathrm{ZS}}^2+k_4=g^2............\left(17\right)$

将公式(13)(14)(15)中的g_XS、g_YS和g_ZS变换为R的方程并代入公式(17)，得到关于R的二元一次方程，解出旋转半径R，后将R代入式(13)(14)(15)，解得g_XS、g_YS和g_ZS；

在任意时刻旋转半径R、g_XS、g_YS和g_ZS为定值，并代入公式(10)(11)(12)，解得任意时刻旋转体三轴方向上的转动向心加速度a_X、a_Y和a_Z，计算向心加速度向量和得到转动向心加速度a，再根据得出最终转速值ω。

本发明利用陀螺仪检测低速转动的旋转体，并输出相应的转速和旋转半径，利用加速度计检测高速转动的旋转体，并根据加速度计输出的向心加速度同时结合低速的计算结果计算处理输出高速转速，具有低速和高速检测方便、结果精确的优点；本发明的测量方法适用于不同方向、角度安装的旋转体，在测量时，只需将转速检测器随意安装旋转体上即可，安装十分简单方便。

实施例二

参照图3，转速检测器1固定安装在旋转体2上，本实施例与实施例一的区别在于，旋转体2的转轴2.1是竖直的，加速度计的XY轴方向感应向心加速度。此时，重力加速度在加速度计的Z轴方向上，且无变化，所以陀螺仪测量范围内的转速等于ω_Z，匀速转动时其中A_X和A_Y为加速度计的XY轴输出，ω_Z为陀螺仪Z轴输出，即可计算出旋转半径R。后续任何时刻旋转体的转速即可根据R和A_X和A_Y的值计算，具体计算过程参照实施例一，在此不再赘述。

实施例三

参照图4，转速检测器1固定安装在旋转体2上，本实施例与实施例一的区别在于，旋转体2的转轴2.1是水平的，此时，加速度计的XY轴方向感应向心加速度，同时XY轴有重力加速度的正交周期性影响，Z轴加速度A_Z＝0。所以陀螺仪测量范围内的转速等于ω_Z，匀速转动时，先对A_X和A_Y进行数字滤波，消除周期性影响的重力加速度，得到A_XA和A_YA，且有成立，其中ω_Z为陀螺仪Z轴输出，A_XA和A_YA是XY轴输出数据经过数字滤波消除重力加速度影响的值，解方程可得旋转半径R。后续任何时刻即可根据R、A_XA和A_YA的值计算转速，具体计算过程参照实施例一，在此不再赘述。

上述说明中，凡未加特别说明的，均采用现有技术中的技术手段。

Claims (5)

1.一种基于惯性传感器的自适应转速测量***，包括安装在旋转体上的转速检测器和与之进行无线通信的显示终端，其特征在于，所述转速检测器内设有用于输出旋转体三轴加速度的加速度计、用于输出旋转体三轴角速度的陀螺仪、用于控制及进行数据处理的第一微处理器、用于数据传送的第一无线传输模块和用于供电的第一供电电池，第一微处理器分别与加速度计、陀螺仪和第一无线传输模块相连，所述显示终端内设有用于数据传送的第二无线传输模块、第二微处理器、显示器和用于供电的第二供电电池，第二微处理器分别与显示器和第二无线传输模块相连；所述转速检测器中的第一无线传输模块与显示终端中的第二无线传输模块无线连接，所述加速度计的Z轴和陀螺仪的Z轴均与旋转体的转轴平行,所述陀螺仪为三轴微机械陀螺仪，加速度计为三轴微机械加速度计,所述转速检测器内还设有用于显示转速检测器运行状态的指示模块，指示模块分别与第一微处理器和第一供电电池相连，当旋转体处于低速转动即旋转体的转速在陀螺仪的测量范围内时由陀螺仪进行转速测量，当旋转体处于高速转动即旋转体的转速超出陀螺仪测量范围时由加速度计进行转速测量。

2.根据权利要求1所述的基于惯性传感器的自适应转速测量***，其特征在于，所述第一无线传输模块和第二无线传输模块均为2.4G模块。

3.一种基于惯性传感器的自适应转速测量方法，其特征在于，所述转速测量方法基于权利要求1所述的基于惯性传感器的自适应转速测量***，所述转速测量方法包括以下两种情形：

第一种情形：旋转体处于低速转动时，由陀螺仪进行转速测量，所述低速转动为旋转体的转速在陀螺仪的测量范围内，陀螺仪输出旋转体的三轴角速度到第一微处理器中处理，得到旋转体的转速值，第一微处理器将转速值通过第一无线传输模块传送到显示终端中显示，同时第一微处理器计算并保存旋转体的旋转半径，所述旋转半径为转速检测器到旋转体转轴的距离；

第二种情形：旋转体处于高速转动时，由加速度计进行转速测量，所述高速转动是指旋转体的转速超出陀螺仪测量范围，包括如下步骤：

a)加速度计输出旋转体的三轴加速度到第一微处理器中保存，

b)第一微处理器根据第一种情形计算得到的旋转半径，计算并保存重力加速度对旋转体的影响值，

c)第一微处理器根据步骤a和步骤b的结果逆向推导出旋转体的转速值，并通过第一无线传输模块传送到显示终端中显示。

4.根据权利要求3所述的基于惯性传感器的自适应转速测量方法，其特征在于，所述第一种情形中计算得到的旋转半径为旋转半径关于加速度计上各轴转动向心加速度的计算表达式，该计算表达式由以下公式计算得到：

${\mathrm{R\ω}}_X^2=\sqrt{a_Y^2+a_Z^2}...\left(1\right)$

${\mathrm{R\ω}}_Y^2=\sqrt{a_X^2+a_Z^2}...\left(2\right)$

${\mathrm{R\ω}}_Z^2=\sqrt{a_Y^2+a_X^2}...\left(3\right)$

其中，ω_X、ω_Y和ω_Z为任意时刻陀螺仪输出的X轴角速度、Y轴角速度和Z轴角速度，R是旋转半径，a_X是旋转体X轴方向上的转动向心加速度，a_Y是旋转体Y轴方向上的转动向心加速度，a_Z是旋转体Z轴方向上的转动向心加速度；

解上述方程(1)(2)(3)得：

a_X＝k₁R…………(4)

a_Y＝k₂R…………(5)

a_Z＝k₃R…………(6)

其中，k₁、k₂和k₃是计算得出的已知系数。

5.根据权利要求4所述的基于惯性传感器的自适应转速测量方法，其特征在于，所述第二种情形的

步骤a中加速度计输出旋转体的三轴加速度分别A_X、A_Y和A_Z，

步骤b中重力加速度对旋转体的影响值包括不随旋转体转动变化的重力加速度分量g_XS、g_YS和g_ZS以及随旋转体转动周期性变化的重力加速度分量g_XD、g_YD和g_ZD，在旋转体匀速转动时，以下公式(7)(8)(9)成立：

A_X＝a_X+g_XS+g_XD…………(7)

A_Y＝a_Y+g_YS+g_YD…………(8)

A_Z＝a_Z+g_ZS+g_ZD…………(9)

通过滤波器将g_XD、g_YD和g_ZD滤除，得到相应变化量的幅值g_XDA、g_YDA和g_ZDA，根据公式(7)(8)(9)得到消除g_XD、g_YD和g_ZD影响后的A_XA、A_YA和A_ZA：

A_XA＝a_X+g_XS…………(10)

A_YA＝a_Y+g_YS…………(11)

A_ZA＝a_Z+g_ZS…………(12)

步骤c中旋转体的转速值计算包括如下步骤：

将公式(4)(5)(6)代入公式(10)(11)(12)，得到：

A_XA＝k₁R+g_XS…………(13)

A_YA＝k₂R+g_YS…………(14)

A_ZA＝k₃R+g_ZS…………(15)

因g为地球重力加速度，所以：

$g_{XS}^2+g_{YS}^2+g_{ZS}^2+g_{XDA}^2+g_{YDA}^2+g_{ZDA}^2=g^2...\left(16\right)$

其中g_XDA、g_YDA和g_ZDA为已知量，设所以：

$g_{XS}^2+g_{YS}^2+g_{ZS}^2+k_4=g^2...\left(17\right)$

将公式(13)(14)(15)中的g_XS、g_YS和g_ZS变换为R的方程并代入公式(17)，得到关于R的二元一次方程，解出旋转半径R，后将R代入式(13)(14)(15)，解得g_XS、g_YS和g_ZS；

在任意时刻旋转半径R、g_XS、g_YS和g_ZS为定值，并代入公式(10)(11)(12)，解得任意时刻旋转体三轴方向上的转动向心加速度a_X、a_Y和a_Z，计算向心加速度向量和得到转动向心加速度a，再根据得出最终转速值ω。