CN104679037B - 基于dsp嵌入式***的燃气流量调节控制装置及控制方法 - Google Patents

Abstract

本发明公开了一种基于DSP嵌入式***的燃气流量调节控制装置，包括阀门调节器碟片、减速器、下壳体、上壳体、齿轮组、控制器、力矩电机、旋转轴，下壳体设有第一轴向通气孔，上壳体内设有与第一轴向通气孔连通的第二轴向通气孔，阀门调节器碟片与上述第二轴向通气孔匹配，下壳体的中部设有减速器安装孔，上壳体的中部设有与减速器安装孔连通的旋转轴安装孔，减速器设置在减速器安装孔中，旋转轴设置在旋转轴安装孔中，控制器的电机控制信号输出端连接力矩电机的控制信号输入端，力矩电机的输出轴依次通过齿轮组和减速器连接旋转轴。本发明控制精度高，传动效率高，实时响应，可解决现有燃气流量调节装置中动态响应特性差的问题。

Description

基于DSP嵌入式***的燃气流量调节控制装置及控制方法

技术领域

本发明属于自动控制领域，具体涉及一种基于DSP(Digital Signal Processing，数字信号处理器)嵌入式***的燃气流量调节控制装置及控制方法。

技术背景

为保证燃气流量调节装置的通用性和可移植性，燃气流量调节控制装置安装在燃气流量调节执行装置的内部，以闭环形式实现燃气流量调节执行装置的高精度控制，一般能达到秒级。燃气流量调节装置的动态特性依赖于产生的燃气、自由容积和阀门动作，并且在燃气流量调节过程中存在负调特性：当期望燃气流量减少时，快速增大燃气发生器喷喉面积，燃气流量先减小再增大至预期的调节值；反之亦然。为保证燃气流量调节装置的动态特性，一般要求燃气流量调节控制装置能实时动态响应各参数变化，并给出控制规律，控制执行机构做出响应。

目前，国内外的燃气流量调节装置为基于螺旋传动和拨叉组合的燃气流量调节装置，该装置中电机带动丝杠旋转驱动螺母直线运动，螺母的移动推动拨叉及输出轴旋转，实现电机输出与流量调节装置输出轴间的传动。此装置最大的不足在于传动比小、整体传动效率低、响应特性差，主要体现在螺母的直线运动推动拨叉作旋转运动的过程中出现了大量的能量损失，另外，受空间尺寸限制，电机输出力矩有限，导致可调节负载有限，且工作过程中存在卡滞(螺母拨叉要求工艺制造水平高，工作时易磨损卡滞)现象，可靠性低。

发明内容

本发明的目的是针对上述技术问题，提供一种基于DSP嵌入式***的燃气流量调节控制装置及控制方法，该装置和方法的控制精度高，传动效率高，实时响应，可解决现有燃气流量调节装置中动态响应特性差、空间占用大、安装调节不方便以及不能有效解决负调特性的问题。

为实现此目的，本发明所设计的基于DSP嵌入式***的燃气流量调节控制装置，它包括阀门调节器碟片、减速器、下壳体、设置在下壳体上的上壳体、设置在下壳体内的齿轮组、控制器和力矩电机、安装在阀门调节器碟片中部的旋转轴，其中，所述下壳体设有第一轴向通气孔，上壳体内设有与第一轴向通气孔连通的第二轴向通气孔，所述阀门调节器碟片与上述第二轴向通气孔匹配，所述下壳体的中部设有减速器安装孔，上壳体的中部设有与减速器安装孔连通的旋转轴安装孔，所述减速器设置在减速器安装孔中，所述旋转轴设置在旋转轴安装孔中，所述控制器的电机控制信号输出端连接力矩电机的控制信号输入端，所述力矩电机的输出轴依次通过齿轮组和减速器连接旋转轴，所述力矩电机通过齿轮组、减速器和旋转轴控制阀门调节器碟片转动从而实现第二轴向通气孔与外界通断的控制。

所述减速器安装孔的底部设有旋转变压器，所述旋转变压器的定子与下壳体固定连接，旋转变压器的转子与减速器固定连接，上述旋转变压器的反馈信号输出端连接控制器的角度反馈信号输入端。

所述控制器包括数字信号处理器、CAN(Controller Area Network，控制器局域网络)驱动器、晶振和电机驱动器，其中，所述数字信号处理器的CAN控制信号通信端通过CAN驱动器连接固冲发动机控制***，数字信号处理器的时钟信号输入端连接晶振，数字信号处理器的多通道缓冲串口连接电机驱动器的转速控制信号输入端，数字信号处理器的I/O(input/output，输入输出端口)接口连接电机驱动器的使能信号输入端，所述数字信号处理器的角度反馈信号输入端连接旋转变压器的反馈信号输出端。

一种基于DSP嵌入式***的燃气流量调节控制方法，它包括如下步骤：

步骤1：所述数字信号处理器的主程序模块通过CAN总线中断接收固冲发动机控制***给出的控制燃气流量调节装置阀门调节器碟片所需调节的角度值；

步骤2：路径规划模块读取旋转变压器当前的角度反馈值，根据固冲发动机控制***的控制要求计算出阀门调节器碟片转动运行位移轨迹，并将路径规划标志置位，位置PID(比例(proportion)、积分(integration)、微分(differentiation)控制)模块以路径规划模块计算出的阀门调节器碟片转动运行位移轨迹得到的角度为当前目标角度值，读取当前旋转变压器的角度反馈值，计算得到输出量速度值；

步骤3：速度PID模块以位置PID模块计算得出的速度值作为输入量，计算得出电机驱动器的转速控制信号，电机驱动器根据上述转速控制信号控制力矩电机输出轴的转动调节，力矩电机输出轴的旋转通过齿轮组、减速器和旋转轴带动阀门调节器碟片动作，阀门调节器碟片改变第二轴向通气孔的喉部面积，从而实现燃气流量调节控制。

本发明的有益效果是：

1、本发明采用模块化设计，各模块功能独立，更利于布置，提高了流量调节装置内部空间使用率；

2、本发明的集成度高、体积小、驱动能力强，可驱动较大的负载(减速器减速比1:300，可驱动负载大)；

3、本发明的控制精度高、响应速度快、算法适应性强，可方便地适应流量调节装置动态特性，解决其负调特性；

4、本发明的装配调节及拆卸方便、可靠性高、环境适应性强(路径规划算法和双PID算法一起应用，在变负载情况下也能很好的调节)。

本发明设计的上述基于DSP嵌入式***的闭环控制方式，相比传统的基于螺旋传动和拨叉组合的燃气流量纯机械式调节方式，具有更高的控制精度和传动效率，具有更好的响应实时性(现有的拨叉方式间隙误差大，变负载影响大，响应慢)，可解决现有此类控制装置中动态响应特性差、空间占用大(本发明中减速器结构紧密，占用空间小，减速比大)、安装调节不方便以及不能有效解决负调特性的问题(本发明的路径规划和PID算法可进行计算调节路径，算法提前可知调节运行轨迹，有效解决负调特性)。本发明适用于高精度固冲发动机燃气流量调节装置的控制，具有较好的应用价值和推广前景。

附图说明

图1为本发明的原理框图；

图2为本发明的数字信号处理器4.1的原理框图；

图3为本发明的控制原理逻辑图。

图4为本发明的俯视结构示意图。

图5为本发明一个方向的立体剖视结构示意图。

图6为本发明另一个方向的剖视结构示意图。

其中、1—阀门调节器碟片、2—下壳体、2.1—第一轴向通气孔、2.2—减速器安装孔、3—上壳体、3.1—第二轴向通气孔、3.2—旋转轴安装孔、4—控制器、4.1—数字信号处理器、4.11—主程序模块、4.12—中断控制模块、4.13—路径规划模块、4.14—位置PID模块、4.15—速度PID模块、4.2—CAN驱动器、4.3—晶振、4.4—电机驱动器、5—力矩电机、6—旋转轴、7—减速器、8—齿轮组、9—旋转变压器、10—固冲发动机控制***

具体实施方式

以下结合附图和实施例对本发明作进一步的详细说明：

本发明的基于DSP嵌入式***的燃气流量调节控制装置及控制方法通过控制固冲发动机燃气流量调节装置阀门调节器碟片的转动，以改变发动机喷管喉部面积达到调节空燃比(空气质量流率/燃气质量流率之比)，充分利用推进剂的能量，使固冲发动机获得最佳工作性能。本发明采用模块化结构，闭环设计，能保证较高的控制精度。

它的具体结构如图1～6所示，包括阀门调节器碟片1、减速器7、下壳体2、设置在下壳体2上的上壳体3、设置在下壳体2内的齿轮组8、控制器4和力矩电机5、安装在阀门调节器碟片1中部的旋转轴6，其中，所述下壳体2设有第一轴向通气孔2.1，上壳体3内设有与第一轴向通气孔2.1连通的第二轴向通气孔3.1，所述阀门调节器碟片1与上述第二轴向通气孔3.1匹配，所述下壳体2的中部设有减速器安装孔2.2，上壳体3的中部设有与减速器安装孔2.2连通的旋转轴安装孔3.2，所述减速器7设置在减速器安装孔2.2中，所述旋转轴6设置在旋转轴安装孔3.2中，所述控制器4的电机控制信号输出端连接力矩电机5的控制信号输入端，所述力矩电机5的输出轴依次通过齿轮组8和减速器7连接旋转轴6，所述力矩电机5通过齿轮组8、减速器7和旋转轴6控制阀门调节器碟片1转动从而实现第二轴向通气孔3.1与外界通断的控制(力矩电机5的输出轴固定连接齿轮组8的初始端的一个齿轮，齿轮组8末端的一个齿轮与减速器7的齿轮啮合，减速器7与旋转轴6固定连接)。

上述技术方案中，所述减速器安装孔2.2的底部设有旋转变压器9，所述旋转变压器9的定子与下壳体2固定连接，旋转变压器9的转子与减速器7固定连接，上述旋转变压器9的反馈信号输出端通过旋转数字转换器、电平转换芯片与控制器4的角度反馈信号输入端连接。

力矩电机5输出的转矩通过齿轮组8传递给减速器7，减速器7输出的转矩通过旋转轴6驱动阀门调节器碟片1转动，来控制通气孔有效作用面积的大小。

上述技术方案中，所述控制器4包括数字信号处理器4.1、CAN驱动器4.2、晶振4.3和电机驱动器4.4，其中，所述数字信号处理器4.1的CAN控制信号通信端通过CAN驱动器4.2连接固冲发动机控制***10，数字信号处理器4.1的时钟信号输入端连接晶振4.3，数字信号处理器4.1的多通道缓冲串口连接电机驱动器4.4的转速控制信号输入端，数字信号处理器4.1的I/O接口连接电机驱动器4.4的使能信号输入端，所述数字信号处理器4.1的角度反馈信号输入端连接旋转变压器9的反馈信号输出端。

上述技术方案中，电机驱动器4.4通过采样电阻实现过流保护，通过二极管实现过压保护。

上述技术方案中，所述旋转轴6、减速器7和旋转变压器9为同轴设置。

上述技术方案中，所述数字信号处理器4.1包括主程序模块4.11、中断控制模块4.12、路径规划模块4.13、位置PID模块4.14和速度PID模块4.15；所述数字信号处理器4.1为现有处理器，其型号优选为TMS320F28335。

其中，所述主程序模块4.11用于根据固冲发动机控制***10给出的阀门调节器碟片1的目标角度值与旋转变压器9的角度反馈值的比较结果，向电机驱动器4.4发出指令，控制阀门调节器碟片1旋转至目标角度位置，从而控制燃气流量；主程序模块4.11将旋转变压器9反馈的角度信息与阀门调节器碟片1的目标角度值进行比较，如有偏差则补发指令对阀门调节器碟片1旋转角度进行修正，形成闭环控制；主程序模块4.11实时监测旋转变压器9的输出值，若出现正弦或余旋丢相、正弦或余旋输出值超过设置的阀值时，数字信号处理器4.1保持上一次输出的角度值不变，直到旋转变压器9输出正常。

所述中断控制模块4.12用于对主程序模块4.11与电机驱动器4.4和旋转变压器9之间的信号传输进行时序控制，所述主程序模块4.11向电机驱动器4.4发出电机控制指令，主程序模块4.11接收旋转变压器9的角度反馈值；中断控制模块4.12设有两个计时器，一个是控制周期计时器，另一个是角度检测值反馈计时器，初始化时两个计时器都置0，主程序模块4.11接收到固冲发动机控制***10的调节指令后，将控制周期计时器置1，控制周期计时器开始计时，并计算得出当前电机驱动器4.4的转速控制信号，计时到5毫秒时，给出主程序输出控制信号标识；计时器2计时到20毫秒时置1，给出主程序将力矩电机角度检测值反馈给控制***的标识；

所述路径规划模块4.13用于计算出力矩电机5的规划运行位移轨迹，该力矩电机5的规划运行位移轨迹的计算方式为：路径规划模块4.13以固冲发动机控制***10给出的阀门调节器碟片1的目标角度值为终点目标位置，以旋转变压器9当前的角度反馈值为起点，根据固冲发动机控制***10的控制要求进行计算，得出力矩电机5的规划运行位移轨迹；

所述位置PID模块4.14用于计算力矩电机5的速度输出量dq_out，该计算力矩电机5的速度输出量dq_out的计算方式为：位置PID模块4.14将固冲发动机控制***10给出的阀门调节器碟片1的目标角度值与旋转变压器9的角度反馈值之差作为输入量，根据位置PID模块4.14的位置PID比例系数Kp、积分系数Ki和微分系数Kd，利用常规的PID算法计算力矩电机5的速度输出量dq_out；

所述速度PID模块4.15用于计算电机驱动器4.4的转速控制信号输出量dqv_out，该电机驱动器4.4的转速控制信号输出量dqv_out的计算方式为：速度PID模块4.15以上述力矩电机5的速度输出量dq_out作为输入量，再根据速度PID模块4.15的速度PID比例系数Kp、积分系数Ki和微分系数Kd，利用常规的PID算法计算出电机驱动器4.4的转速控制信号输出量dqv_out。

一种基于DSP嵌入式***的燃气流量调节控制方法，它包括如下步骤：

步骤1：所述数字信号处理器4.1的主程序模块4.11通过CAN总线中断接收固冲发动机控制***10给出的控制燃气流量调节装置阀门调节器碟片1所需调节的角度值；

步骤2：路径规划模块4.13读取旋转变压器9当前的角度反馈值，根据固冲发动机控制***10的控制要求计算出阀门调节器碟片1转动运行位移轨迹，并将路径规划标志置位，位置PID模块4.14以路径规划模块4.13计算出的阀门调节器碟片1转动运行位移轨迹得到的角度为当前目标角度值，读取当前旋转变压器9的角度反馈值，计算得到输出量速度值；

步骤3：速度PID模块4.15以位置PID模块4.14计算得出的速度值作为输入量，计算得出电机驱动器4.4的转速控制信号，电机驱动器4.4根据上述转速控制信号控制力矩电机5输出轴的转动调节，力矩电机5输出轴的旋转通过齿轮组8、减速器7和旋转轴6带动阀门调节器碟片1动作，阀门调节器碟片1改变第二轴向通气孔3.1的喉部面积，从而实现燃气流量调节控制。

上述技术方案中，所述每个数字信号处理器4.1的控制周期都重新进行位置PID模块4.14和速度PID模块4.15的计算；

当中断控制模块4.12的控制周期计时器的控制周期标志位为1时，速度PID模块4.15输出计算得出的当前转速控制信号通过电机驱动器4.4控制力矩电机5转动，当中断控制模块4.12的控制周期计时器的控制周期标志位为0时，以路径规划模块4.13计算得出的力矩电机5的规划运行位移轨迹得到的当前目标角度值作为目标角度，再读取旋转变压器9的角度反馈值，重新进行位置PID模块4.14和速度PID模块4.15的计算，速度PID模块4.15再输出计算得到的当前转速控制信号通过电机驱动器4.4控制力矩电机5转动调节，直至调节到路径规划最终目标角度，即***目标位置角度；中断控制模块4.12给出主程序模块4.11输出控制信号、当前旋转变压器9的反馈信号和控制周期的标识位，并根据主程序模块4.11给出的标志进行路径规划计算、位置PID计算和速度PID计算。

本发明中的数字信号处理器4.1采用PID控制算法和路径规划算法相结合的方式，路径规划模块4.13的路径规划算法以固冲发动机控制***10要求的阀门调节器碟片1目标角度为终点目标位置，以旋转变压器9当前的角度反馈值为起点，根据固冲发动机控制***10的控制要求进行计算得出路径规划的运行位移轨迹，周期与控制周期计时器的控制周期一致；位置PID模块4.14将固冲发动机控制***10给出的阀门调节器碟片1的目标角度值与旋转变压器9的角度反馈值之差作为输入量，根据位置PID模块4.14的位置PID比例系数Kp、积分系数Ki和微分系数Kd，利用常规的PID算法计算力矩电机5的速度输出量dq_out；速度PID模块4.15以上述力矩电机5的速度输出量dq_out作为输入量，再根据速度PID模块4.15的速度PID比例系数Kp、积分系数Ki和微分系数Kd，利用常规的PID算法计算出电机驱动器4.4的转速控制信号输出量dqv_out。输出转速控制信号控制力矩电机5转动，从而控制阀门调节器碟片1转动到当前路径规划目标角度位置；如此循环执行，直至调节到路径规划最终目标角度，即阀门调节器碟片1的目标位置角度。

本发明的工作过程为：固冲发动机控制***10根据固冲发动机吸入压缩空气量的实时参数，计算出阀门调节器碟片1的目标角度值，数字信号处理器4.1计算得出力矩电机5的规划运行位移轨迹并给出当前电机驱动器4.4的转速控制信号，转速控制信号操作电机驱动器4.4发出指令控制电机转动。力矩电机5控制阀门调节器碟片1转动的过程中(力矩电机5输出的动力依次经过齿轮组8、减速器7和旋转轴6传递给阀门调节器碟片1)，旋转变压器9记录下力矩电机5旋转的角度并实时反馈给数字信号处理器4.1，数字信号处理器4.1将旋转变压器9反馈的角度信息与阀门调节器碟片1的目标角度值进行比较，如有偏差，则数字信号处理器4.1继续发指令给电机驱动器4.4对阀门调节器碟片1的旋转角度进行修正，直至调节到位，形成闭环控制。力矩电机5通过控制阀门调节器碟片1旋转，改变喉部面积，从而控制燃气流量。

与此同时，数字信号处理器4.1实时监测旋转变压器9的输出值，若出现正弦或余旋丢相、正弦或余旋输出值超过设置的阀值时，数字信号处理器4.1保持上一次输出的角度值不变，直到旋转变压器9输出正常。

本说明书未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (4)

1.一种基于DSP嵌入式***的燃气流量调节控制装置，其特征在于：它包括阀门调节器碟片(1)、减速器(7)、下壳体(2)、设置在下壳体(2)上的上壳体(3)、设置在下壳体(2)内的齿轮组(8)、控制器(4)和力矩电机(5)、安装在阀门调节器碟片(1)中部的旋转轴(6)，其中，所述下壳体(2)设有第一轴向通气孔(2.1)，上壳体(3)内设有与第一轴向通气孔(2.1)连通的第二轴向通气孔(3.1)，所述阀门调节器碟片(1)与上述第二轴向通气孔(3.1)匹配，所述下壳体(2)的中部设有减速器安装孔(2.2)，上壳体(3)的中部设有与减速器安装孔(2.2)连通的旋转轴安装孔(3.2)，所述减速器(7)设置在减速器安装孔(2.2)中，所述旋转轴(6)设置在旋转轴安装孔(3.2)中，所述控制器(4)的电机控制信号输出端连接力矩电机(5)的控制信号输入端，所述力矩电机(5)的输出轴依次通过齿轮组(8)和减速器(7)连接旋转轴(6)，所述力矩电机(5)通过齿轮组(8)、减速器(7)和旋转轴(6)控制阀门调节器碟片(1)转动从而实现第二轴向通气孔(3.1)与外界通断的控制；

所述减速器安装孔(2.2)的底部设有旋转变压器(9)，所述旋转变压器(9)的定子与下壳体(2)固定连接，旋转变压器(9)的转子与减速器(7)固定连接，上述旋转变压器(9)的反馈信号输出端连接控制器(4)的角度反馈信号输入端；

所述控制器(4)包括数字信号处理器(4.1)、CAN驱动器(4.2)、晶振(4.3)和电机驱动器(4.4)，其中，所述数字信号处理器(4.1)的CAN控制信号通信端通过CAN驱动器(4.2)连接固冲发动机控制***(10)，数字信号处理器(4.1)的时钟信号输入端连接晶振(4.3)，数字信号处理器(4.1)的多通道缓冲串口连接电机驱动器(4.4)的转速控制信号输入端，数字信号处理器(4.1)的I/O接口连接电机驱动器(4.4)的使能信号输入端，所述数字信号处理器(4.1)的角度反馈信号输入端连接旋转变压器(9)的反馈信号输出端；

所述数字信号处理器(4.1)包括主程序模块(4.11)、中断控制模块(4.12)、路径规划模块(4.13)、位置PID模块(4.14)和速度PID模块(4.15)；

其中，所述主程序模块(4.11)用于根据固冲发动机控制***(10)给出的阀门调节器碟片(1)的目标角度值与旋转变压器(9)的角度反馈值的比较结果，向电机驱动器(4.4)发出指令，控制阀门调节器碟片(1)旋转至目标角度位置，从而控制燃气流量；

所述中断控制模块(4.12)用于对主程序模块(4.11)与电机驱动器(4.4)和旋转变压器(9)之间的信号传输进行时序控制，所述主程序模块(4.11)向电机驱动器(4.4)发出电机控制指令，主程序模块(4.11)接收旋转变压器(9)的角度反馈值；

所述路径规划模块(4.13)用于计算出力矩电机(5)的规划运行位移轨迹，该力矩电机(5)的规划运行位移轨迹的计算方式为：路径规划模块(4.13)以固冲发动机控制***(10)给出的阀门调节器碟片(1)的目标角度值为终点目标位置，以旋转变压器(9)当前的角度反馈值为起点，根据固冲发动机控制***(10)的控制要求进行计算，得出力矩电机(5)的规划运行位移轨迹；

所述位置PID模块(4.14)用于计算力矩电机(5)的速度输出量dq_out，该计算力矩电机(5)的速度输出量dq_out的计算方式为：位置PID模块(4.14)将固冲发动机控制***(10)给出的阀门调节器碟片(1)的目标角度值与旋转变压器(9)的角度反馈值之差作为输入量，根据位置PID模块(4.14)的位置PID比例系数Kp、积分系数Ki和微分系数Kd，利用常规的PID算法计算力矩电机(5)的速度输出量dq_out；

所述速度PID模块(4.15)用于计算电机驱动器(4.4)的转速控制信号输出量dqv_out，该电机驱动器(4.4)的转速控制信号输出量dqv_out的计算方式为：速度PID模块(4.15)以上述力矩电机(5)的速度输出量dq_out作为输入量，再根据速度PID模块(4.15)的速度PID比例系数Kp、积分系数Ki和微分系数Kd，利用常规的PID算法计算出电机驱动器(4.4)的转速控制信号输出量dqv_out。

2.根据权利要求1所述的基于DSP嵌入式***的燃气流量调节控制装置，其特征在于：所述旋转轴(6)、减速器(7)和旋转变压器(9)为同轴设置。

3.一种根据权利要求1所述基于DSP嵌入式***的燃气流量调节控制装置的控制方法，其特征在于，它包括如下步骤：

步骤1：所述数字信号处理器(4.1)的主程序模块(4.11)通过CAN总线中断接收固冲发动机控制***(10)给出的控制燃气流量调节装置阀门调节器碟片(1)所需调节的角度值；

步骤2：路径规划模块(4.13)读取旋转变压器(9)当前的角度反馈值，根据固冲发动机控制***(10)的控制要求计算出阀门调节器碟片(1)转动运行位移轨迹，并将路径规划标志置位，位置PID模块(4.14)以路径规划模块(4.13)计算出的阀门调节器碟片(1)转动运行位移轨迹得到的角度为当前目标角度值，读取当前旋转变压器(9)的角度反馈值，计算得到输出量速度值；

步骤3：速度PID模块(4.15)以位置PID模块(4.14)计算得出的速度值作为输入量，计算得出电机驱动器(4.4)的转速控制信号，电机驱动器(4.4)根据上述转速控制信号控制力矩电机(5)输出轴的转动调节，力矩电机(5)输出轴的旋转通过齿轮组(8)、减速器(7)和旋转轴(6)带动阀门调节器碟片(1)动作，阀门调节器碟片(1)改变第二轴向通气孔(3.1)的喉部面积，从而实现燃气流量调节控制。

4.根据权利要求3所述的控制方法，其特征在于：每个数字信号处理器(4.1)的控制周期都重新进行位置PID模块(4.14)和速度PID模块(4.15)的计算；

当中断控制模块(4.12)的控制周期计时器的控制周期标志位为1时，速度PID模块(4.15)输出计算得出的当前转速控制信号通过电机驱动器(4.4)控制力矩电机(5)转动，当中断控制模块(4.12)的控制周期计时器的控制周期标志位为0时，以路径规划模块(4.13)计算得出的力矩电机(5)的规划运行位移轨迹得到的当前目标角度值作为目标角度，再读取旋转变压器(9)的角度反馈值，重新进行位置PID模块(4.14)和速度PID模块(4.15)的计算，速度PID模块(4.15)再输出计算得到的当前转速控制信号通过电机驱动器(4.4)控制力矩电机(5)转动调节，直至调节到路径规划最终目标角度，即***目标位置角度；中断控制模块(4.12)给出主程序模块(4.11)输出控制信号、当前旋转变压器(9)的反馈信号和控制周期的标识位，并根据主程序模块(4.11)给出的标志进行路径规划计算、位置PID计算和速度PID计算。