CN105007012B - 一种车载转台控制系统及方法 - Google Patents

Abstract

本发明公开了一种车载转台控制系统及控制方法，控制系统包括驱动装置、锁紧装置以及用于控制所述驱动装置和锁紧装置的控制装置，所述驱动装置包括角度传感器、D/A转换芯片、电机驱动器以及驱动电机，所述锁紧装置包括位置传感器、A/D采集芯片、步进电机驱动器以及步进电机，所述控制装置包括数字信号处理器。控制方法包括向数字信号处理器输入指令、转台自动解锁、计算转台的加减速控制曲线、计算所述驱动电机电压并按照该电压驱动转台、以及转台自动锁紧。本发明系统极大简化转台体积，可与其他设备集成，并能实现转台在任意位置的自动锁紧、解锁，还能实现转台的稳定运行和精密定位。

Description

一种车载转台控制系统及方法

技术领域

本发明属于机电控制领域，具体涉及一种基于DSP的车载转台控制系统及方法。

背景技术

转台可以真实地复现被测设备在方位、俯仰、偏航方向的运动特征，在科学实验和武器装备中起着重要作用，是模拟、仿真、测试的关键载体设备。

目前,使用的转台主要有实验室大型驻地转台、车载转台、舰船转台，常用的转台控制系统主要由计算机控制系统、驱动系统和测量系统组成，其计算机控制系统主要由工控计算机和运动控制器组成，工控计算机主要是对转台运行状态进行监控，而运动控制器是伺服控制系统的核心，它采用积分分离的PI算法进行位置与速率的控制，从而实现基于工控计算机指令信号和位置传感器反馈信号的转台控制。

按以上方法进行转台姿态控制时，主要优点是：结构简单、使用简便、可靠性高、造价低，控制方法简单，可满足精度不高的转台定位需求。缺点是：采用运动控制器作为控制系统的核心部件，虽简化了硬件设计、降低了控制算法的难度，但增加了软硬件成本，且转台不易实现与其他设备的无缝集成。

随着微处理器的性能不断提高，转台的控制系统采用具有高运算速度的DSP(简称数字信号处理器)芯片作为控制的核心，以差量式数字PID为控制算法来实现转台的基本功能。

此外，随着神经网络算法的广泛研究，也出现了转台伺服系统的神经网络控制方法，即先建立转台伺服系统的机械动态模型，初始化系统状态等一系列先验知识的基础上选择神经网络逼近未知动态，根据系统跟踪误差、快速终端滑模面及其一阶导数来设计自适应鲁棒有限时间神经网络控制器，采样时刻更新神经网络权值矩阵。但由于神经网络算法本身的复杂性，适用于高性能的工业计算机，硬件成本高、算法复杂，无法在基于DSP的嵌入式系统中采用，有一定的使用局限性，且目前还处于算法研究阶段。

综合上述的转台的控制方法以及各个方法的优缺点，针对基于DSP的车载转台，目前主要存在如下技术问题：(1)硬件成本高，体积大，不易实现与其他设备的无缝集成；(2)由于受到路面颠簸、震动等因素的影响，普通的PI控制算法很难消除静差，以达到较高的控制精度，还可能导致积分过饱和超调；(3)由于受到结构件、锁紧力、环境温度的影响，转台较难实现任意角度下的自动锁紧解锁，并且车载转台在启动和停止时的过冲和急停极易引起的来回震动；(4)车载转台在运动过程中容易出现电流的陡增和速度的突变而出现飞车现象。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种车载转台控制系统及方法，其目的在于，该方法能够满足DSP嵌入式数字信号处理器运行要求，极大简化转台体积，实现与其他设备的集成，其控制算法也能避免车载转台的干扰因素，实现转台的稳定运行和精密定位，并能实现在转台任意位置的自动锁紧、解锁，解决了车载转台的自动化控制等技术难题。

为实现上述目的，按照本发明的一个方面，提供了一种车载转台控制系统，其特征在于，其包括驱动装置、锁紧装置以及用于控制所述驱动装置和锁紧装置的控制装置，其中，

所述驱动装置包括角度传感器、D/A转换芯片、电机驱动器以及驱动电机，所述角度传感器与所述控制装置相电连接，所述控制控制装置同时还与D/A转换芯片相电连接，所述D/A转换芯片连接电机驱动器，所述电机驱动器连接所述驱动电机，所述驱动电机用于驱动转台旋转；

所述锁紧装置包括位置传感器、A/D采集芯片、步进电机驱动器以及步进电机，所述位置传感器与所述A/D采集芯片相电连接，所述A/D采集芯片与所述控制装置相电连接，所述控制控制装置同时还与所述步进电机驱动器相电连接，所述步进电机驱动器与所述步进电机相电连接，所述步进电机用于锁紧转台；

所述控制装置包括数字信号处理器，所述数字信号处理器同时连接所述角度传感器、所述D/A转换芯片、所述A/D采集芯片以及所述步进电机驱动器。

所述角度传感器用于感知转台旋转的角度，所述角度传感器连接所述控制装置。

通过以上发明构思，一方面设置角度传感器能实时采集转台的位置值，另一方面设置位置传感器实时采集步进电机状态，所述位置值和所述步进电机的状态经过数字信号处理器处理后，分别输出控制驱动电机的电压值和步进电机的PWM脉冲，使驱动电机和步进电机按照控制要求进行转台的驱动和锁紧。

进一步的，所述驱动电机为力矩电机。

按照本发明的另一方面，还提供了一种车载转台控制方法，其特征在于，包括如下步骤：

S1：向数字信号处理器输入指令，所述指令包括转台的定位角度和转台的运行速度；

S2：以A/D采集芯片采集位置传感器的位置值，并将该位置值输入至数字信号处理器以对所述位置值进行判断，若该位置值在解锁阈值范围内，则判断转台解锁成功，接着执行步骤S3；否则启动锁紧电机往解锁方向运行，直至解锁成功；

S3：将角度传感器采集的解锁成功时的转台角度反馈至数字信号处理器，根据解锁成功时的转台角度、步骤S1中所述定位角度以及所述运行速度，数字信号处理器计算转台的加减速控制曲线，所述转台的加减速控制曲线包括加速段、恒速段和减速段；

S4：数字信号处理器分别计算所述加速段、恒速段和减速段的驱动电机电压值，并按照所述电压值驱动转台先进行加速转动直到速度达到所述转台的运行速度，再依照所述转台的运行速度进行恒速转动，最后进行减速转动以使转台到达所述转台的定位角度时静止；

S5：在减速转动结束后，以A/D采集芯片采集位置传感器的位置值，当多次采集的位置值均无变化时，判断锁紧电机堵转，否则锁紧电机往锁紧方向继续运行直至堵转；堵转后，接着判断该多次采集的位置值是否在锁紧阈值范围内，若是，则判断锁紧电机锁紧；若否，则启动锁紧电机往锁紧方向继续运行，直至锁紧。

通过以上发明构思，采用加减速控制曲线控制转台先加速、再恒速、最后加速，而不是立即加速至运行速度，并在达到定位角度处又立即停止，因此，能避免了转台的过冲和抖动，实现了平稳启动和停止。

进一步的，所述步骤S4中，实时采集传感器的实测值，计算与给定目标值之间的偏差量，将偏差量代入PID控制算法计算每一时刻输出给驱动电机的电压值，以此实现转台的稳定运行和精密定位。

通过以上发明构思，加减速控制曲线可规划转台的加速段、恒速段和减速段，PID控制算法计算获得的所述驱动电机电压控制量是控制转台速度或者说转台位置的关键量，作为参考或者作为闭环控制的策略，同时还实时采集角度传感器的值来获取转台实际的转过的角度(位置值)，转台的速度以及驱动转台旋转的驱动电机的电流，以电流环、速度环以及位置环三回路的控制策略，最终用于精确控制驱动电机的电压控制量，可进一步实现转台速度和位置的稳定、精确控制。

进一步的，所述步骤S3中，所述加速段的加速度和所述减速段的加速度的绝对值相同。

进一步的，步骤S4中，采用PID控制算法计算所述驱动电机电压值，所述驱动电机电压值u(k)为：

上述公式中，给定值目标角度rin(t)与电机的实际角度yout(t)构成的控制偏差error(t)为error(t)＝rin(t)-yout(t)，t为时刻，T为采样周期，k为采样序号，k＝1、2、……、N，所述N为采样总数，error(k-1)和error(k)分别为第(k-1)和第k个采样周期所得的偏差信号，K_P为比例系数，K_i为积分系数，K_d为偏分系数。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：

1、采用数字信号处理器集成在控制系统中，极大简化了转台体积，实现与其他设备的集成，大大降低了转台的外围体积，使得整个控制系统结构精巧，运输方便。

2、基于DSP的转台系统结合PID控制方法，采用加减速控制曲线进行转台控制，使转台启动和停止时候不会发生过冲和抖动，实现了平稳启动和停止。进一步的，结合电流环、速度环以及位置环三回路的控制策略，提高了控制转台速度和转台位置精确性和稳定性。

3、位置传感器和步进电机在控制装置作用下，对转台进行自动化的锁紧或者解锁，在转台整个测试过程中，不用人为的进行锁紧或者解锁，很大程度上提高了测试效率，降低测试成本。

附图说明

图1是本发明实施例中车载转台结构示意图；

图2是本发明实施例中车载转台中集成的硬件结构之间的连接关系图；

图3是本发明实施例中加减速控制函数曲线图；

图4是本发明实施例中三回路PID控制系统简化框图，三回路包括电流环、速度环、位置环，电流环是指驱动转台运转的力矩电机的电流环，速度换是指转台转动速度的速度环，位置环是指转台角度的位置环。

在所有附图中，相同的附图标记用来表示相同的元件或结构，其中：

1-外框 2-内框 3-中框

4-底座 5-锁紧机构 6-位置传感器

7-控制机构 8-角度传感器

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

图1是本发明实施例中车载转台结构示意图，由图可知，该车载转台包括外框1、中框3以及内框2，外框1、中框3以及内框2通过加强筋板以及圆柱状的轴固定在底座4上。在中框处设置有控制机构7、角度传感器8以及位置传感器6，在外框的侧壁上同样设置有角度传感器8、锁紧机构5，在圆柱状的轴处也设置了锁紧机构5和位置传感器6。该转台上总共设置了三个位置传感器6和三个角度传感器8(图中没有将位置传感器和角度传感器全部画出)。图中的锁紧机构采用的是齿轮结构，采用步进电机将齿轮卡住使其不能旋转，而实现锁紧，或者使步进电机收回，对齿轮进行解锁，使其能继续旋转。

图2是本发明实施例中车载转台中集成的硬件结构之间的连接关系图，其中,上位机用于为操作者提供人机界面，实现对整个系统的在线检测、性能检测、监控和系统的运动管理以及数据处理；下位机(即控制机构)是直接的控制机构，构成转台的数字伺服控制系统，保证转台的稳定运行和位置精度。由图可知，一方面，由角度传感器感知框架(框架是指外框、中框以及内框)的位置，角度传感器将自身的角度信息经过数字隔离器、FPGA(逻辑控制单元)，传输至数字信号处理器DSP。经DSP控制算法处理后，计算出驱动框架运动的电压值，经数字隔离器、D/A转换芯片，将数字量转换为模拟量输出至电机驱动器，以使电机驱动器驱动力矩电机带动框架以一定速度运动。另一方面，由位置传感器感知锁紧电机的位置值，然后该信息经过A/D采集芯片、数字隔离器、FPGA(逻辑控制单元)，传输至数字信号处理器DSP，经DSP处理之后，得到反馈信息，该反馈信息再经数字隔离器传输至步进电机，以控制步进电机执行相应的命令。

本发明中，基于DSP的车载转台控制方法包括对转台运行至指定位置的控制方法和对转台停止时的锁紧以及解锁的控制方法。车载转台的运行方式根据安装位置的不同，有俯仰角、方位角和偏航角。其中，外框的运动实现方位角的变化，中框的运动实现俯仰角的变化，内框的运动实现偏航角的变化。

车载转台一个完整的运行周期包括：由静止状态解锁，然后按照指令运行到指定角度，接着锁紧。根据时间先后顺序，控制转台进行一个完整的运行周期的方法主要包括如下步骤：

S1：向数字信号处理器输入指令，所述指令包括转台的定位角度和转台的运行速度；

S2：以A/D采集芯片采集位置传感器的位置值，并将该位置值输入至数字信号处理器以对所述位置值进行判断，若该位置值在解锁阈值范围内，则判断转台解锁成功，接着执行步骤S3；否则启动锁紧电机往解锁方向运行，直至解锁成功；

S3：将角度传感器采集的解锁成功时的转台角度反馈至数字信号处理器，根据解锁成功时的转台角度、步骤S1中所述定位角度以及所述运行速度，数字信号处理器计算转台的加减速控制曲线，所述转台的加减速控制曲线包括加速段、恒速段和减速段；

S4：数字信号处理器分别计算所述加速段、恒速段和减速段的驱动电机电压值，并按照所述电压值驱动转台先进行加速转动直到速度达到所述转台的运行速度，再依照所述转台的运行速度进行恒速转动，最后进行减速转动以使转台到达所述转台的定位角度时静止；

S5：在减速转动结束后，以A/D采集芯片采集位置传感器的位置值，当多次采集的位置值均无变化时，判断锁紧电机堵转，否则锁紧电机往锁紧方向继续运行直至堵转；堵转后，接着判断该多次采集的位置值是否在锁紧阈值范围内，若是，则判断锁紧电机锁紧；若否，则启动锁紧电机往锁紧方向继续运行，直至锁紧。

作为本发明一个优选实施例，其具体的控制方法为：

S1向数字信号处理器输入指令，所述指令包括转台的定位角度和转台的运行速度。在开始时，转台静止，在接受指令后，转台将以定位指令所确定的运行速度运转至定位角度。

S2：转台自动解锁步骤。以A/D采集芯片采集位置传感器感知的位置值，并将该位置值传送至DSP,DSP对位置传感器的位置值进行判断，若该位置值正好在解锁阈值范围内，则判断转台已解锁，继续步骤三；否则启动锁紧电机往解锁方向运行，锁紧电机为步进电机。锁紧电机往解锁方向运行的同时，实时采集位置传感器的位置值，直至当前位置值在解锁阈值范围内，判断转台处于解锁状态。本发明实施例中，有三个锁紧电机，三个锁紧电机均按照以上方式进行解锁步骤。

S3：将角度传感器采集的解锁成功时的转台角度反馈至数字信号处理器，根据解锁成功时的转台角度、步骤S1中所述定位角度以及所述运行速度，数字信号处理器计算转台的加减速控制曲线，所述转台的加减速控制曲线包括加速段、恒速段和减速段。计算获得转台加减速控制曲线的参数包括：加速段所需时间t₁,运行至减速段所需时间t₂，加速段行进的角度S₁，运行至减速段行进的角度S₂。

利用DSP上的QEP模块采集编码器上的转台当前的角度，根据转台当前角度值、步骤S1中定位角度以及运行速度，计算转台的加减速控制曲线，所述转台的加减速控制曲线包括加速段、恒速段和减速段。其中加速段的加速度等于减速段的加速度。具体的计算过程如下：

根据实际需求给定一个常数作为加速段的加速度a(也即减速段的加速度的绝对值)，已知运行速度v，运行的角度值S_all为定位角度减去转台当前的角度，则能计算出转台从当前角度运行到定位角度所需要的定位时间t_all：

进一步按照下式计算获得加速段所需时间t₁,运行减速段所需时间t₂，加速段行进的角度S₁，运行至减速段行进的角度S₂，其中：

由于加速段和减速段的加速度相同，则

t₂＝t_all-t₁

S₂＝v*(t₂-t₁)+S₁

若当前时刻t≤t₁时，当前时刻行进的位移S₁(t)为：

若当前时刻t满足t₁≤t≤t₂时，当前时刻行进的位移S₂(t)为：

S₂(t)＝S₁+v*(t-t₁)

若当前时刻t满足t₂≤t≤t_all时，当前时刻行进的位移S₃(t)为：

在本步骤中，通过加减速曲线控制方法，使静止的转台先逐渐加速，待转速加至运行速度后，匀速行驶，匀速行驶至将要接近定位角度处，进行减速行驶，直到以较慢的速度运行至定位角度处。采用以上的加减速曲线控制方法，可避免转台在启动时直接从静止状态到达运行速度，停止时直接从运行速度开始急速停止，这样可能产生过冲，从而实现转台的实时快速、准确平稳运动。

S4：数字信号处理器分别计算所述加速段、恒速段和减速段的驱动电机电压值，并按照所述电压值驱动转台先进行加速转动直到速度达到所述转台的运行速度，再依照所述转台的运行速度进行恒速转动，最后进行减速转动以使转台到达所述转台的定位角度时静止。具体为，在每一采样时刻，该采样时刻为毫秒级，具体采用DSP的CPU时钟毫秒级定时中断来实现，采集转台的当前角度值，并根据加减速控制曲线的参数，采用PID控制算法计算驱动电机电压控制量。这里PID控制算法采用电流环、速度环和位置环三回路控制策略。图4是本发明实施例中三回路PID控制系统简化框图，三回路包括电流环、速度环、位置环，电流环是指驱动转台运转的驱动电机(也即力矩电机)的电流环，速度换是指转台转动速度的速度环，位置环是指转台角度的位置环。

为了解决闭环调速中启动和堵转时驱动电机电流过大问题，系统中具有自动限制电枢电流过大的环节，正因为此，才引入电流回路。速度回路的主要作用是对速度进行稳定的控制，使其在定位时不产生振荡，同时，还能使系统更好地进行位置控制，在稳态时具有良好的硬度特性，并对各种扰动有良好的抑制作用。而位置环则进一步提高定位精度，提升系统的总体性能。

PID控制算法表达式如下：

上述公式中，给定值目标角度rin(t)与电机的实际角度yout(t)构成的控制偏差error(t)为error(t)＝rin(t)-yout(t)，t为时刻，T为采样周期，k为采样序号，k＝1，2，……，N，所述N为采样总数，error(k-1)和error(k)分别为第(k-1)和第k个采样周期所得的偏差信号，K_P为比例系数，K_i为积分系数，K_d为偏分系数；u(k)为驱动电机的电压。

步骤S4中，作为闭环控制，采用PID控制算法计算的驱动电机的电流、转台的速度以及转台的角度(即指令值)，与实时采集传感器的值来获取转台实际的转过的角度(位置值)、转台的速度以及驱动转台旋转的驱动电机的电流(即实际值)，求解PID控制算法表达式的u(k)即为电流、速度以及角度各自的校正值，根据校正值三环复合控制计算电压控制量。

在驱动电机控制过程中，为避免硬件出现故障或操作出现错误导致转台的飞转失控，当PID误差值过大时，DSP可立即停止框架运行，实现对转台的保护。

S5：在减速转动结束后，以A/D采集芯片采集位置传感器的位置值，当多次采集的位置值均无变化时，判断锁紧电机堵转，否则锁紧电机往锁紧方向继续运行直至堵转；堵转后，接着判断该多次采集的位置值是否在锁紧阈值范围内，若是，则判断锁紧电机锁紧；若否，则启动锁紧电机往锁紧方向继续运行，直至锁紧。具体的，待转台运行至定位角度，启动锁紧电机往锁紧方向运行，并实时以A/D采集芯片采集的位置传感器的位置值，并连续判断10次采集的位置值是否都无限接近(即步进电机已堵转)，堵转后，则判断该位置值是否在锁紧阈值范围内，若是，则电机已锁紧；若未满足要求，则启动锁紧电机往锁紧方向继续运行，直至锁紧。

为保证锁紧机构的锁紧，这里采用了先堵转再判断位置的方法，同时为避免解锁时由于锁紧力过大而无法解锁，锁紧时采用电流换挡、提高电机运转频率的方法来实现。

本发明实施例中，锁紧机构采用的是齿轮结构，齿轮结构决定其易发生不易解锁的现象。另外，转台上也有一定重量的负载，锁紧时，锁紧力比较大，而在解锁时，为克服锁紧力，使锁紧机构顺利解锁，解锁时驱动电机的电流值要大于锁紧时的电流值，并且锁紧电机运转频率也更低，这里就要采用电流换挡操作来实现。

以上所述为本发明的较佳实施例而已，但本发明不应该局限于该实施例和附图所公开的内容。所以凡是不脱离本发明所公开的精神下完成的等效或修改，都落入本发明保护的范围。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种车载转台控制方法，其特征在于，其包括驱动装置、锁紧装置以及用于控制所述驱动装置和锁紧装置的控制装置，其中，

所述驱动装置包括角度传感器、D/A转换芯片、电机驱动器以及驱动电机，所述角度传感器与所述控制装置相电连接，所述控制装置同时还与D/A转换芯片相电连接，所述D/A转换芯片连接电机驱动器，所述电机驱动器连接所述驱动电机，所述驱动电机用于驱动转台旋转；

所述锁紧装置包括位置传感器、A/D采集芯片、步进电机驱动器以及步进电机，所述位置传感器与所述A/D采集芯片相电连接，所述A/D采集芯片与所述控制装置相电连接，所述控制装置同时还与所述步进电机驱动器相电连接，所述步进电机驱动器与所述步进电机相电连接，所述步进电机用于锁紧转台；

所述控制装置包括数字信号处理器，所述数字信号处理器同时连接所述角度传感器、所述D/A转换芯片、所述A/D采集芯片以及所述步进电机驱动器，

所述驱动电机为力矩电机，

如上所述的车载转台控制方法，包括如下步骤：

S1：向数字信号处理器输入指令，所述指令包括转台的定位角度和转台的运行速度；

S2：以A/D采集芯片采集位置传感器的位置值，并将该位置值输入至数字信号处理器以对所述位置值进行判断，若该位置值在解锁阈值范围内，则判断转台解锁成功，接着执行步骤S3；否则启动锁紧电机往解锁方向运行，直至解锁成功；

S3：将角度传感器采集的解锁成功时的转台角度反馈至数字信号处理器，根据解锁成功时的转台角度、步骤S1中所述定位角度以及所述运行速度，数字信号处理器计算转台的加减速控制曲线，所述转台的加减速控制曲线包括加速段、恒速段和减速段；

S4：数字信号处理器分别计算所述加速段、恒速段和减速段的驱动电机电压值，并按照所述电压值驱动转台先进行加速转动直到速度达到所述转台的运行速度，再依照所述转台的运行速度进行恒速转动，最后进行减速转动以使转台到达所述转台的定位角度时静止；

S5：在减速转动结束后，以A/D采集芯片采集位置传感器的位置值，当多次采集的位置值均无变化时，判断锁紧电机堵转，否则锁紧电机往锁紧方向继续运行直至堵转；堵转后，接着判断该多次采集的位置值是否在锁紧阈值范围内，若是，则判断锁紧电机锁紧；若否，则启动锁紧电机往锁紧方向继续运行，直至锁紧。

2.如权利要求1所述的一种车载转台控制方法，其特征在于，所述步骤S3中，所述加速段的加速度和所述减速段的加速度的绝对值相同。

3.如权利要求2所述的一种车载转台控制方法，其特征在于，步骤S4中，采用PID控制算法计算所述驱动电机电压值，所述驱动电机电压值u(k)为：

<mrow> <mi>u</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>K</mi> <mi>P</mi> </msub> <mi>e</mi> <mi>r</mi> <mi>r</mi> <mi>o</mi> <mi>r</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>K</mi> <mi>i</mi> </msub> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>j</mi> <mo>=</mo> <mn>0</mn> </mrow> <mi>k</mi> </munderover> <mi>e</mi> <mi>r</mi> <mi>r</mi> <mi>o</mi> <mi>r</mi> <mrow> <mo>(</mo> <mi>j</mi> <mo>)</mo> </mrow> <mi>T</mi> <mo>+</mo> <msub> <mi>K</mi> <mi>d</mi> </msub> <mfrac> <mrow> <mi>e</mi> <mi>r</mi> <mi>r</mi> <mi>o</mi> <mi>r</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>-</mo> <mi>e</mi> <mi>r</mi> <mi>r</mi> <mi>o</mi> <mi>r</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow> <mi>T</mi> </mfrac> </mrow>

上述公式中，给定值目标角度rin(t)与电机的实际角度yout(t)构成的控制偏差error(t)为error(t)＝rin(t)-yout(t)，t为时刻，T为采样周期，k为采样序号，k＝1、2、……、N，所述N为采样总数，error(k-1)和error(k)分别为第(k-1)和第k个采样周期所得的偏差信号，K_P为比例系数，K_i为积分系数，K_d为偏分系数。