CN105806326B - 基于陀螺罗经的数字伺服控制系统及处理方法 - Google Patents

Abstract

本发明提供的基于陀螺罗经的数字伺服控制系统，主控处理模块用于分采集8字线圈检测模块，电磁摆检测模块和加速度计检测模块的信号，对信号进行处理，启动陀螺仪，并生成俯仰电机运行控制指令和方位电机运行控制指令，实时跟踪并控制俯仰电机和方位电机运行。该系统可以直接利用主控处理模块进行软件调试，调试方法简单，能够实现柔性化设计，也避免了现有的伺服控制系统中，由于元器件的特性受环境温度影响，导致系统稳定性差的问题。基于陀螺罗经的数字伺服控制系统的处理方法，能够对采集到的8字线圈输出的信号，电磁摆的输出信号和加速度计的输出信号进行处理，控制罗经始终在子午面内，稳定指北。

Description

基于陀螺罗经的数字伺服控制系统及处理方法

技术领域

本发明涉及船舶导航领域，尤其涉及基于陀螺罗经的数字伺服控制系统及处理方法。

背景技术

陀螺罗经也称电罗经，是利用陀螺仪的定轴性和进动性这两大基本特性，结合地球自转矢量和重力矢量，借助控制设备和阻尼设备而制成的提供真北基准的一种指向仪器，可以帮助船舶确定艏向或目标方位的精密助航设备。与传统的磁罗经相比，有精度高，航向稳定，不受磁场影响的特点。陀螺罗经内设有陀螺球，俯仰电机，方位电机，电磁摆，加速度计，磁传感器，温度传感器等。其中，陀螺球是电罗经运行的关键部件,只有陀螺球内的陀螺马达高速旋转才具有定轴性和进动性；电磁摆，方位8字线圈，俯仰8字线圈才有信号输入和输出。方位电机和俯仰电机分别指受方位随动信号和俯仰随动信号控制，用以驱动随动部分运动的微型电机。电磁摆是陀螺罗经的阻尼部件，检测罗经的阻尼周期。加速度计用于检测陀螺球的水平位置和俯仰位置。磁传感器用于粗略检测真北航向，以粗略真北航向启动电罗经，使得电罗经上电后自动工作，快速指真北。温度传感器用于检测电路工作温度和环境温度，使电路工作在最佳工作状态。

伺服控制指对物体运动的位置、速度及加速度等变化量的有效控制。现有的运用在陀螺罗经上的伺服控制系统多为模拟伺服控制系统，包括随动放大器电路。随动放大器电路把陀螺罗经中检测失配角的8字线圈输出的信号进行放大，解调，校正，再功率放大后，控制伺服电机实现闭环控制，使陀螺罗经中的随动部分始终跟踪陀螺仪中陀螺主轴所指的位置。其中，随动放大器电路有两部分：一部分供方位随动系统用，方位随动部分使贮液缸在方位上跟踪陀螺球的运动。另一部分供倾斜随动系统用，倾斜随动部分使贮液缸在倾斜方向上跟踪陀螺球的运动。两部分随动放大器电路均由传感器、放大器、伺服电机、减速齿轮等组成，只是个别元件参数设置不同。传感器由装在陀螺球上的电磁铁和贮液缸上的“8”字线圈构成，“8”字线圈输出的随动信号经放大器放大后驱动伺服电机，经齿轮减速后，带动贮液缸跟随陀螺球的运动并保持一定的相对位置。

为了保证随动放大器电路既有良好的稳态精度又有良好的动态特性，在放大器中加入了“解调—校正网络—调制”的校正电路，包括：交流放大电路，相敏解调/调制电路，直流放大电路，功率放大电路。交流放大电路和直流放大电路都是用运算放大器组成的一般放大电路，校正电路中的阻容网络可以用来改善随动放大器电路的动态特性。相敏解调/调制电路由一块集成模拟开关LM7512实现。解调器的作用是把放大了的交流信号变换成直流信号，其大小和极性与交流信号的幅度和相位对应。模拟开关LM7512相当于一个双刀双掷开关，调制器在参考信号控制下选择模拟开关LM7512中两路相位相反的信号中的一路信号，再经过滤波就得到了大小和极性与交流信号的幅度和相位对应的直流信号。伺服电机是交流电机，经过校正的直流信号需要再调制成400Hz交流信号，才能驱动电机转动。调制器的作用与解调器的作用正好相反，它把经过校正的直流信号再调制成400Hz方波信号。功率放大电路采用了集成运算放大器，对调制后的方波进行功率放大，再输入伺服电机。

现有的伺服控制系统在进行系统调试时，大部分都是通过调整硬件电路上的元器件来实现，例如调整阻容网络中的电阻值和电容值，系统调试麻烦，同时由于大部分元器件的特性都会受环境温度影响，这样就导致了伺服控制系统的稳定性容易受到环境温度变化的影响，而产生漂移，该伺服控制系统难以实现柔性化设计。

发明内容

针对现有技术中存在的上述不足，本发明在于怎么提供一种调试方法简单，系统稳定性更好的基于陀螺罗经的数字伺服控制系统。

本发明还在于怎么提供一种基于陀螺罗经的数字伺服控制系统的处理方法。

为解决上述技术问题，实现发明目的，本发明采用的技术方案如下：

基于陀螺罗经的数字伺服控制系统，包括主控处理模块，8字线圈检测模块，电磁摆检测模块和加速度计检测模块；8字线圈检测模块用于检测陀螺罗经中失配角的8字线圈输出的信号；电磁摆检测模块用于检测电磁摆的输出信号；加速度计检测模块用于检测加速度计的输出信号；主控处理模块用于获得8字线圈检测模块，电磁摆检测模块和加速度计检测模块采集的信号后，对信号进行处理，启动陀螺仪，并根据信号处理的结果生成俯仰电机运行控制指令和方位电机运行控制指令，实时跟踪并控制俯仰电机和方位电机运行。

作为上述方案的进一步优化，所述8字线圈输出的信号包括俯仰方向上的俯仰8字线圈信号和方位方向上的方位8字线圈信号；加速度计的输出信号包括俯仰方向上的俯仰加速度计信号和方位方向上的方位加速度计信号。

作为上述方案的进一步优化，所述主控处理模块的处理芯片型号为TMS320C2812。

上述的基于陀螺罗经的数字伺服控制系统的处理方法，所述主控处理模块的处理步骤如下：

1）当采样时间达到时，主控处理模块采集以下数据：俯仰8字线圈信号，方位8字线圈信号，俯仰加速度计信号，方位加速度计信号和电磁摆的输出信号；

2）将步骤1中采集到的俯仰8字线圈信号，方位8字线圈信号和电磁摆的输出信号进行软件相敏检波运算，并获得以下数据：电磁摆的输出信号和俯仰8字线圈信号在俯仰方向上的俯仰倾斜误差，电磁摆的输出信号和方位8字线圈信号在方位方向上的方位倾斜误差，以及陀螺仪和随动球之间的角度误差；

3）采集磁传感器检测到的磁北方位角度；

4）首先利用步骤1中采集到的俯仰加速度计信号和方位加速度计信号，计算陀螺仪在俯仰方向上的俯仰倾斜角度和在方位方向上的方位倾斜角度；根据步骤2中获得的俯仰倾斜误差，方位倾斜误差和角度误差，校正俯仰倾斜角度和方位倾斜角度，直至俯仰倾斜角度和方位倾斜角度分别位于预设的俯仰倾斜角度范围和方位倾斜角度范围内时，启动陀螺仪，并判断陀螺仪是否处于正常的工作状态，如果陀螺仪工作正常，执行步骤5；否则停止陀螺仪工作，上传陀螺仪的异常信息，操作结束；

5）根据步骤3中获得的磁北方位角度控制陀螺仪的转动速度，如果磁北方位角度与真实磁北角度的差值位于±30°内时，保持陀螺仪当前的转动速度，否则加快陀螺仪当前的转动速度；

6）生成俯仰电机运行控制指令和方位电机运行控制指令，实时跟踪并控制俯仰电机和方位电机运行。

作为上述方案的进一步优化，所述步骤6中主控处理模块生成俯仰电机运行控制指令和方位电机运行控制指令，实时跟踪并控制俯仰电机和方位电机运行的处理步骤如下：

6a）主控处理模块中设有四种运动指令：寻找磁北，陀螺启动，正常运行或关闭伺服电机；

6b）当运动指令为寻找磁北时，使能方位电机和俯仰电机，首先利用检测的电磁摆的输出信号，俯仰8字线圈信号和俯仰加速度计信号进行PID运算，得到俯仰电机运行控制指令，发送给俯仰电机；然后判断方位环是否旋转完一周，直至方位环旋转一周完毕后，执行步骤6c；

6c）循环判断陀螺仪是否找到磁北，直至找到磁北后，设置寻找磁北完毕标志，执行步骤6f；

6d）当获得的运动指令为陀螺启动时，判断陀螺仪是否工作正常，如陀螺仪工作正常，执行步骤6e，否则执行步骤6i；

6e）使能方位电机和俯仰电机，利用检测的电磁摆的输出信号，俯仰8字线圈信号和俯仰加速度计信号进行PID运算，得到俯仰电机运行控制指令，发送给俯仰电机；利用检测的电磁摆的输出信号，方位8字线圈信号和方位加速度计信号进行PID运算，得到方位电机运行控制指令，发送给方位电机，执行步骤6f；

6f）在俯仰电机和方位电机转动的期间，判断俯仰电机转速和方位电机转速是否分别小于俯仰电机最大转速和方位电机最大转速，如果是，执行步骤6g，否则执行步骤6h；

6g）根据俯仰倾斜误差和方位倾斜误差，生成提高俯仰电机转速和方位电机转速的提速指令，或降低俯仰电机转速和方位电机转速的降速指令，发送给俯仰电机和方位电机，处理步骤结束；

6h）生成降低俯仰电机转速和方位电机转速的降速指令，发送给俯仰电机和方位电机，直至检测到俯仰倾斜误差和方位倾斜误差位于预设的俯仰倾斜误差范围和方位倾斜误差范围内时，处理步骤结束；

6i）禁止方位电机和俯仰电机启动，设置陀螺仪工作异常标志，处理步骤结束；

6j）当获得的运动指令为正常运行时，判断陀螺仪是否工作正常，如陀螺仪工作正常，执行步骤6e，否则执行步骤6i；

6k）当获得的运动指令为关闭伺服电机时，禁止方位电机和俯仰电机启动，处理步骤结束。

相比于现有技术，本发明具有如下优点：

本发明提供的基于陀螺罗经的数字伺服控制系统，陀螺罗经中失配角的8字线圈输出的信号，电磁摆的输出信号和加速度计的输出信号，直接传送给主控处理模块进行处理后，启动陀螺仪，并生成俯仰电机运行控制指令和方位电机运行控制指令，实时跟踪并控制俯仰电机和方位电机运行。该系统可以直接利用主控处理模块进行软件调试，调试方法简单，能够实现柔性化设计，也避免了现有的伺服控制系统中，由于元器件的特性受环境温度影响，导致系统稳定性差的问题。

本发明提供的基于陀螺罗经的数字伺服控制系统的处理方法，能够对采集到的陀螺罗经中失配角的8字线圈输出的信号，电磁摆的输出信号和加速度计的输出信号进行处理，控制陀螺罗经始终在子午面内，稳定指北。

附图说明

图1为实施例中主控处理模块处理步骤的流程图。

图2为实施例中主控处理模块控制俯仰电机和方位电机运行的流程图。

具体实施方式

下面结合实施例对本发明作进一步详细的描述，但本发明的实施方式不限于此。

实施例：

基于陀螺罗经的数字伺服控制系统，包括主控处理模块，8字线圈检测模块，电磁摆检测模块和加速度计检测模块；8字线圈检测模块用于检测陀螺罗经中失配角的8字线圈输出的信号；电磁摆检测模块用于检测电磁摆的输出信号；加速度计检测模块用于检测加速度计的输出信号；主控处理模块用于获得8字线圈检测模块，电磁摆检测模块和加速度计检测模块采集的信号后，对信号进行处理，启动陀螺仪，并根据信号处理的结果生成俯仰电机运行控制指令和方位电机运行控制指令，实时跟踪并控制俯仰电机和方位电机运行。所述8字线圈输出的信号包括俯仰方向上的俯仰8字线圈信号和方位方向上的方位8字线圈信号；加速度计的输出信号包括俯仰方向上的俯仰加速度计信号和方位方向上的方位加速度计信号。

陀螺罗经中失配角的8字线圈输出的信号，电磁摆的输出信号和加速度计的输出信号，直接传送给主控处理模块进行处理后，启动陀螺仪，并生成俯仰电机运行控制指令和方位电机运行控制指令，实时跟踪并控制俯仰电机和方位电机运行。该系统可以直接利用主控处理模块进行软件调试，调试方法简单，能够实现柔性化设计，也避免了现有的伺服控制系统中，由于元器件的特性受环境温度影响，导致系统稳定性差的问题。

该系统高度集成化、全数字化、智能化、模块化和网络化。数字伺服控制，就是将微处理机引入控制系统，使微处理机成为控制系统的一个环节，同时选用适合于微处理机控制的各种现代伺服元件和接口电路，组成一个数字伺服系统，应用软件程序实现数字比较、数字调节运算（数字滤波），以及模拟系统中控制回路所需要的各种附加功能。数字伺服控制主要是检测陀螺罗经上的电磁摆、加速度计以及陀螺罗经内部的8字线圈的信号，在地球自转和船舶因恶劣海况摇摆时，自动控制俯仰电机和方位电机，使陀螺罗经始终处于地理水平面，稳定指北。

具体实施时，主控处理模块的处理器可采用了德州仪器数字信号处理器TMS320C2812，该处理器具有强大浮点运算能力，支持最多达6路PWM输出的电机控制输出接口，集成有A/D、CAN、SPI、UART等多种通信端口。该芯片强大的计算能力，较大容量的Flash、ROM和RAM空间，对伺服控制算法设计方面允许调整和改进的空间很大，使系统性能可持续提升、完善。

该系统主控处理模块可以对采集到的数据进行数字滤波，为8字线圈和电磁摆提供激磁电压，并对8字线圈和电磁摆的输出信号进行信号调理和数据采集，进而为伺服控制系统提供误差修正的反馈信号。加速度计检测模块可以为系统为加速度计预留的接口，用于修正船舶摇摆时系统的摇摆误差。主控处理模块还可以控制加热器启动，采集各温度传感器检测到的温度值。

方位电机和俯仰电机分别指安装在陀螺罗经上，分别受方位随动信号和俯仰随动信号控制，用以驱动随动部分运动的微型电机。该数字伺服系统，能够实现俯仰和方位两个方向随动联动控制，减少随动控制超调量，提高阻尼，减少振荡，改善指向精度。具体实施时，驱动俯仰电机和方位电机的控制参数通过CAN通讯发送给主控处理模块，由其直接驱动俯仰电机和方位电机。该系统在俯仰轴上的检测方式是结合电磁摆和加速度计进行检测，其中加速度计可采用高精度的MEMS三轴加速度计实现。根据主控处理模块检测的基座加速度情况，改变电磁摆信号对于俯仰和方位控制的影响，以改善系统在摇摆和受冲击时艏向数值剧烈波动的情况，改善系统在综合海况下的动态精度，以达到国际标准要求。

陀螺仪中包含陀螺球，陀螺仪内部有高速旋转的电机，高速旋转的物体具有定轴性，由于有地理磁场的原因，电磁摆就输出与真实磁北的夹角信号，且在俯仰和方位上有都有俯仰和方位的信号的8字线圈，本实施例的数字伺服就是保持陀螺仪动态平衡，稳定指北。

陀螺仪视运动的存在导致其陀螺主轴不能指北，因此必须利用陀螺的进动性在其水平轴上施加合适的外力矩，使陀螺主轴进动的角速度和地球自转角速度在子午面上的分量大小相等，方向相同。

仅在陀螺罗经水平轴上施加外力矩只能使陀螺罗经主轴找北而不能稳定指北，其找北的运动轨迹为一等幅椭圆，要使陀螺罗经主轴稳定指北，必须施加合适的阻尼力矩。本设计中采用方位电机产生该阻尼力矩，伺服控制系统的输入信号为方位8字线圈和电磁摆偏角信号。

上述的基于陀螺罗经的数字伺服控制系统的处理方法，如图1所示，所述主控处理模块的处理步骤如下：

1）当采样时间达到时，主控处理模块采集（AD数据采集）以下数据：俯仰8字线圈信号，方位8字线圈信号，俯仰加速度计信号，方位加速度计信号和电磁摆的输出信号；该方法运行前，可以进行系统初始化，更新系统数据。由于采用AD数据采集，所以需要对采集的数据进行解算，解算完后，停止AD采样

2）将步骤1中采集到的俯仰8字线圈信号，方位8字线圈信号和电磁摆的输出信号进行软件相敏检波运算，并获得以下数据：电磁摆的输出信号和俯仰8字线圈信号在俯仰方向上的俯仰倾斜误差，电磁摆的输出信号和方位8字线圈信号在方位方向上的方位倾斜误差，以及陀螺仪和随动球之间的角度误差；其中，软件相敏检波运算采用的计算方法仍是现有的相敏检波运算，只是该系统不采用模拟电路进行相敏检波运算，而是通过软件编程实现。相敏检波是为提高已知频率微弱信号的检测精度和实时性,采用先进的dsp芯片设计了多路数据采集系统的硬件电路,采用平方和开方运算的双线性逼近方法实现数字相敏检波。其中，俯仰倾斜误差，方位倾斜误差和角度误差是从陀螺仪，俯仰加速度计，方位加速度计中读取出来的。

3）采集磁传感器检测到的磁北方位角度；这个数据主要用于加速陀螺仪启动，缩短稳定时间；

4）首先利用步骤1中采集到的俯仰加速度计信号和方位加速度计信号，计算陀螺仪在俯仰方向上的俯仰倾斜角度和在方位方向上的方位倾斜角度；根据步骤2中获得的俯仰倾斜误差，方位倾斜误差和角度误差，校正俯仰倾斜角度和方位倾斜角度，直至俯仰倾斜角度和方位倾斜角度分别位于预设的俯仰倾斜角度范围和方位倾斜角度范围内时，此时，认为校正成功，启动陀螺仪，并判断陀螺仪是否处于正常的工作状态，如果陀螺仪工作正常，执行步骤5；否则停止陀螺仪工作，上传陀螺仪的异常信息，操作结束；校正是为了提高陀螺仪指北的精确度。其中，俯仰加速度计能输出俯仰倾斜角度，方位加速度计能输出方位倾斜角度。加速度计包括俯仰方向上的俯仰加速度计，和方位方向上的方位加速度计。校正是通过操作操作单元或校正设备上的按键来增减俯仰方向或者方位方向上的增量。

5）根据步骤3中获得的磁北方位角度控制陀螺仪的转动速度，如果磁北方位角度与真实磁北角度的差值位于±30°内时，保持陀螺仪当前的转动速度，否则加快陀螺仪当前的转动速度；通常，磁北方位角度与真实磁北角度的差值位于±30°内，且陀螺仪在正常转动速度下工作时，磁北方位角度与真实磁北角度的差值会在1小时内达到±1°。所以磁北方位角度与真实磁北角度的差值大于±30°时，则需要加快陀螺仪的转动速度，使得陀螺仪在短时间内，磁北方位角度与真实磁北角度的差值在±1°内。

6）生成俯仰电机运行控制指令和方位电机运行控制指令，实时跟踪并控制俯仰电机和方位电机运行。最后，还可以进行系统变量清零和看门狗计数器清零。

罗经初始上电工作正常，使能方位电机并开始工作，罗经方位运转2圈，消除磁罗经误差，启动陀螺仪。启动后不对磁罗经信号进行处理，磁罗经的主要作用是找到真实磁北（简称真北）附近的角度后启动陀螺仪，使罗经快速稳定指北。一旦启动陀螺仪，磁罗经信号就仅参考，不做其它处理应用。

陀螺仪工作状态的判定：首先设备供电正常，其次俯仰加速度计和方位加速度计输出信号正常；再次磁罗经工作是否正常；（是否正常在主控芯片中都有信号检测口对信号进行检测，并把检测值与设定值进行比较。）若都正常，启动陀螺仪，若任何一个地方不正常均需要排除故障后才能启动陀螺仪。

该处理方法能够对采集到的陀螺罗经中失配角的8字线圈输出的信号，电磁摆的输出信号和加速度计的输出信号进行处理，控制陀螺罗经始终在子午面内，稳定指北。该方法可以直接利用主控处理模块进行软件调试，调试方法简单，能够实现柔性化设计，也避免了现有的伺服控制系统中，由于元器件的特性受环境温度影响，导致系统稳定性差的问题。

如图2所示，主控处理模块生成俯仰电机运行控制指令和方位电机运行控制指令，实时跟踪并控制俯仰电机和方位电机运行的处理步骤如下：

6a）主控处理模块中设有四种运动指令：寻找磁北，陀螺启动，正常运行或关闭伺服电机；

6b）当运动指令为寻找磁北时，说明陀螺仪动态平衡,使能方位电机和俯仰电机，首先利用检测的电磁摆的输出信号，俯仰8字线圈信号和俯仰加速度计信号进行PID运算，得到俯仰电机运行控制指令，发送给俯仰电机；这里根据俯仰电机增量来保持倾斜平衡,利用比例、积分、微分（PID）回路计算得到的输出信号控制方位电机的运行速率。然后判断方位环是否旋转完一周，这样做的主要原因是消除磁北的自差，直至方位环旋转一周完毕后，执行步骤6c；方位环用于固定陀螺仪，方位环只要是电罗经汇电环以上的固定部件均可，主要来标示需要旋转一周。

6c）循环判断陀螺仪是否找到磁北，直至找到磁北后，设置寻找磁北完毕标志，执行步骤6f；

6d）当获得的运动指令为陀螺启动时，判断陀螺仪是否工作正常，如陀螺仪工作正常，执行步骤6e，否则执行步骤6i；

6e）使能方位电机和俯仰电机，利用检测的电磁摆的输出信号，俯仰8字线圈信号和俯仰加速度计信号进行PID运算，得到俯仰电机运行控制指令，发送给俯仰电机；利用检测的电磁摆的输出信号，方位8字线圈信号和方位加速度计信号进行PID运算，得到方位电机运行控制指令，发送给方位电机，执行步骤6f；

PID算法，本质上是要一个系统的输出达到理想的目标状态。P是比例调节，根据理想态和当前态的差距，乘以一个系数，进行调节；I是积分环节，是消除静态误差的；D是微分环节，用来预测差值的变化对调节进行修正的。

在过程控制中，按偏差的比例（P）、积分（I）和微分（D）进行控制的PID控制器（亦称PID调节器）是应用最为广泛的一种自动控制器。 具有原理简单，易于实现，适用面广，控制参数相互独立，参数的选定比较简单等优点；而且在理论上可以证明，对于过程控制的典型对象——“一阶滞后＋纯滞后”与“二阶滞后＋纯滞后”的控制对象，PID控制器是一种最优控制。PID调节规律是连续系统动态品质校正的一种有效方法，它的参数整定方式简便，结构改变灵活（PI、PD等）。

6f）在俯仰电机和方位电机转动的期间，判断俯仰电机转速和方位电机转速是否分别小于俯仰电机最大转速和方位电机最大转速，如果是，执行步骤6g，否则执行步骤6h；对俯仰电机转速和方位电机转速进行限制，是为了在不断提高俯仰电机转速和方位电机转速的同时，防止俯仰电机转速和方位电机转速超出最大转速，使得俯仰电机转速和方位电机损坏，使得罗经中的陀螺仪保持动态平衡，稳定指北，

6g）根据俯仰倾斜误差和方位倾斜误差，生成提高俯仰电机转速和方位电机转速的提速指令，或降低俯仰电机转速和方位电机转速的降速指令，发送给俯仰电机和方位电机，处理步骤结束；该步骤中，如果俯仰倾斜误差和方位倾斜误差较大，提高俯仰电机转速和方位电机转速的提速指令，使得俯仰倾斜误差和方位倾斜误差在短时间内快速达标。如果俯仰倾斜误差和方位倾斜误差已达标，则可以选择保持或降低俯仰倾斜误差和方位倾斜误差。具体转速增大减小的幅度根据误差与标准误差的差值大小决定。

6h）生成降低俯仰电机转速和方位电机转速的降速指令，发送给俯仰电机和方位电机，直至检测到俯仰倾斜误差和方位倾斜误差位于预设的俯仰倾斜误差范围和方位倾斜误差范围内时，处理步骤结束；此时，俯仰电机转速和方位电机转速已超出最大转速，为了防止俯仰电机转速和方位电机损坏，必须要降低俯仰电机转速和方位电机转速。

6i）禁止方位电机和俯仰电机启动，设置陀螺仪工作异常标志，处理步骤结束；

6j）当获得的运动指令为正常运行时，判断陀螺仪是否工作正常，如陀螺仪工作正常，执行步骤6e，否则执行步骤6i；

6k）当获得的运动指令为关闭伺服电机时，禁止方位电机和俯仰电机启动，处理步骤结束。

该方法直接利用主控处理模块进行软件调试，能准确驱动俯仰电机和方位电机正常运转，使得陀螺仪指北，调试方法简单，能够实现柔性化设计。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (2)

1.基于陀螺罗经的数字伺服控制系统的处理方法，其特征在于，所述基于陀螺罗经的数字伺服控制系统包括主控处理模块，8字线圈检测模块，电磁摆检测模块和加速度计检测模块；8字线圈检测模块用于检测陀螺罗经中失配角的8字线圈输出的信号；电磁摆检测模块用于检测电磁摆的输出信号；加速度计检测模块用于检测加速度计的输出信号；主控处理模块用于获得8字线圈检测模块，电磁摆检测模块和加速度计检测模块采集的信号后，对信号进行处理，启动陀螺仪，并根据信号处理的结果生成俯仰电机运行控制指令和方位电机运行控制指令，实时跟踪并控制俯仰电机和方位电机运行；

所述主控处理模块的处理步骤如下：

1）当采样时间达到时，主控处理模块采集以下数据：俯仰8字线圈信号，方位8字线圈信号，俯仰加速度计信号，方位加速度计信号和电磁摆的输出信号；

2）将步骤1中采集到的俯仰8字线圈信号，方位8字线圈信号和电磁摆的输出信号进行软件相敏检波运算，并获得以下数据：电磁摆的输出信号和俯仰8字线圈信号在俯仰方向上的俯仰倾斜误差，电磁摆的输出信号和方位8字线圈信号在方位方向上的方位倾斜误差，以及陀螺仪和随动球之间的角度误差；

3）采集磁传感器检测到的磁北方位角度；

4）首先利用步骤1中采集到的俯仰加速度计信号和方位加速度计信号，计算陀螺仪在俯仰方向上的俯仰倾斜角度和在方位方向上的方位倾斜角度；根据步骤2中获得的俯仰倾斜误差，方位倾斜误差和角度误差，校正俯仰倾斜角度和方位倾斜角度，直至俯仰倾斜角度和方位倾斜角度分别位于预设的俯仰倾斜角度范围和方位倾斜角度范围内时，启动陀螺仪，并判断陀螺仪是否处于正常的工作状态，如果陀螺仪工作正常，执行步骤5；否则停止陀螺仪工作，上传陀螺仪的异常信息，操作结束；

5）根据步骤3中获得的磁北方位角度控制陀螺仪的转动速度，如果磁北方位角度与真实磁北角度的差值位于±30°内时，保持陀螺仪当前的转动速度，否则加快陀螺仪当前的转动速度；

6）生成俯仰电机运行控制指令和方位电机运行控制指令，实时跟踪并控制俯仰电机和方位电机运行。

2.如权利要求1所述的基于陀螺罗经的数字伺服控制系统的处理方法，其特征在于，所述步骤6中主控处理模块生成俯仰电机运行控制指令和方位电机运行控制指令，实时跟踪并控制俯仰电机和方位电机运行的处理步骤如下：

6a）主控处理模块中设有四种运动指令：寻找磁北，陀螺启动，正常运行或关闭伺服电机；

6b）当运动指令为寻找磁北时，使能方位电机和俯仰电机，首先利用检测的电磁摆的输出信号，俯仰8字线圈信号和俯仰加速度计信号进行PID运算，得到俯仰电机运行控制指令，发送给俯仰电机；然后判断方位环是否旋转完一周，直至方位环旋转一周完毕后，执行步骤6c；

6c）循环判断陀螺仪是否找到磁北，直至找到磁北后，设置寻找磁北完毕标志，执行步骤6f；

6d）当获得的运动指令为陀螺启动时，判断陀螺仪是否工作正常，如陀螺仪工作正常，执行步骤6e，否则执行步骤6i；

6e）使能方位电机和俯仰电机，利用检测的电磁摆的输出信号，俯仰8字线圈信号和俯仰加速度计信号进行PID运算，得到俯仰电机运行控制指令，发送给俯仰电机；利用检测的电磁摆的输出信号，方位8字线圈信号和方位加速度计信号进行PID运算，得到方位电机运行控制指令，发送给方位电机，执行步骤6f；

6f）在俯仰电机和方位电机转动的期间，判断俯仰电机转速和方位电机转速是否分别小于俯仰电机最大转速和方位电机最大转速，如果是，执行步骤6g，否则执行步骤6h；

6g）根据俯仰倾斜误差和方位倾斜误差，生成提高俯仰电机转速和方位电机转速的提速指令，或降低俯仰电机转速和方位电机转速的降速指令，发送给俯仰电机和方位电机，处理步骤结束；

6h）生成降低俯仰电机转速和方位电机转速的降速指令，发送给俯仰电机和方位电机，直至检测到俯仰倾斜误差和方位倾斜误差位于预设的俯仰倾斜误差范围和方位倾斜误差范围内时，处理步骤结束；

6i）禁止方位电机和俯仰电机启动，设置陀螺仪工作异常标志，处理步骤结束；

6j）当获得的运动指令为正常运行时，判断陀螺仪是否工作正常，如陀螺仪工作正常，执行步骤6e，否则执行步骤6i；

6k）当获得的运动指令为关闭伺服电机时，禁止方位电机和俯仰电机启动，处理步骤结束。