CN111654212A - 一种位置随动伺服控制装置及控制方法 - Google Patents

Abstract

本发明公开了一种位置随动伺服控制装置，包括DSP、CPLD和电机驱动模块，所述电机驱动模块包括旋转变压器解码电路、旋转变压器励磁电路、换向时序电路、DAC转换电路、检测电路和电机驱动器，DSP通过SPI总线与所述CPLD连接；旋转变压器解码电路包括双通道旋转变压器；旋转变压器励磁电路包括依次连接的波形发生器、仪表放大器和第二缓冲器；换向时序电路包括第一数字隔离器；DAC转换电路包括依次连接的第二数字隔离器、DAC和差分放大器；检测电路包括第三数字隔离器和仪表放大器。本发明的控制系统采用DSP+CPLD的控制架构，减少了DSP数字处理器的运算量，能够提高执行算法的实时性和控制精度。

Description

一种位置随动伺服控制装置及控制方法

技术领域

本发明属于自动控制和电力拖动领域，更具体地，涉及一种位置随动伺服控制装置及控制方法。

背景技术

在一些光电监视、观测系统中，为了增加系统工作的灵活性和机动性，常需要增加位置随动系统，随动系统一般由执行机构、位置传感器、惯性传感器、补偿装置及控制电路组成，用于实现目标自动搜索跟踪，解决由载体扰动引起的成像模糊问题，实现稳像控制。

目前的位置随动装置多以光电码盘作为位置传感器，以挠性陀螺作为惯性传感器，存在体积大、成本高、控制精度低等缺点；并且在一些随动系统中，作为执行机构的电机多采用霍尔换向方式，使电机运行效率降低，由于霍尔换向要面临接线数量多、传感器易损坏、不能适应高温和潮湿环境等问题，使的整个系统的可靠性大大降低。此外，大部分装置的控制系统多采用单片机架构，存在对数据处理及控制算法执行实时性不高的缺点。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种位置随动伺服控制装置及控制方法，用于自动控制系统中，实现高精度目标观测、跟踪及稳像控制。

为实现上述目的，按照本发明的一个方面，提供了一种位置随动伺服控制装置，其特征在于，包括DSP、CPLD和电机驱动模块，所述DSP通过SPI总线和I/O口与所述CPLD连接，所述电机驱动模块包括旋转变压器励磁电路、旋转变压器解码电路、换向时序电路、DAC转换电路、检测电路和电机驱动器，其中：

所述旋转变压器励磁电路包括依次连接的波形发生器、仪表放大器和第一缓冲器，所述旋转变压器解码电路包括依次连接的第二缓冲器、旋变解码器和旋转变压器，所述第一缓冲器连接所述旋转变压器，所述波形发生器连接所述CPLD，以输出所述旋转变压器所需的正弦信号；所述第二缓冲器连接所述CPLD，以让DSP获得旋转变压器测量的电机转轴的角度信息；

所述换向时序电路包括第一数字隔离器，所述CPLD通过所述第一数字隔离器连接电机驱动器的三个霍尔传感器信号端，以提供电机的换向时序信号；

所述DAC转换电路包括依次连接的第二数字隔离器、DAC和差分放大器，所述第二数字隔离器连接所述CPLD，所述差分放大器连接电机驱动器的两个电压控制端口，以输出电机驱动器所需的控制电压；

所述检测电路包括第三数字隔离器和与该第三数字隔离器连接的仪表放大器，所述仪表放大器还连接电机驱动器的电流检测端，所述第三数字隔离器还连接所述DSP的ADC。

优选地，所述DSP的SCI模块通过串口驱动器与MEMS角速率陀螺相连，以获得MEMS角速率陀螺测量的电机转轴的角速率信号。

优选地，所述DSP通过地址/数据总线与SRAM存储器相连，用于程序运行过程中，临时变量的存储。

优选地，所述DSP还通过CAN总线连接CAN隔离收发器，所述CAN隔离收发器连接主控系统，所述主控系统用于发送控制、参数读写、目标角度信息命令给DSP，同时接收DSP返回的电机的俯仰和/或方位信息。

优选地，所述电机驱动模块为两组，两个所述旋转变压器分别用于获得电机的俯仰信息或方位信息。

优选地，所述旋转变压器为双通道旋转变压器，其包括粗机和单机，所述粗机和单机分别连接一旋变解码器，每个所述旋变解码器分别连接一所述第一缓冲器，每个所述第一缓冲器分别与所述CPLD连接。

按照本发明的另一个方面，还提供了所述的一种位置随动伺服控制装置的控制方法，其特征在于，包括以下步骤：

1)主控系统通过CAN总线发出目标失调角命令给所述DSP，然后所述DSP中断接收主控系统给出的控制命令；

2)定位模块以主控系统给出的目标失调角θ为输入量，以旋转变压器测得的角度值为反馈量，通过位置PID模块获得输出量速度值；

速度PID模块以位置PID模块获得的输出量速度值为输入量，并利用旋转变压器测得的角度值获得电机的运行速度，再以电机的运行速度为反馈量获得电机驱动器控制电压信号并输送给DAC电路；同时，CPLD根据旋转变压器测得的当前角度值给出换向信号给电机驱动器，以控制电机旋转并实现电机旋转的闭环控制。

优选地，步骤2)中，换向时序在控制电机运行前通过测试完成，具体包括以下子步骤：

2.1)电机脱开所有负载，旋转变压器与电机同轴安装；

2.2)将电机每两相之间进行通电；

2.3)电机通电顺序依次为AB-AC-BC-CA-CB-BA,该顺序为电机正向旋转，电机每两相绕组通电时，会使电机转子发生旋转然后静止，根据电机静止位置，向前推30°电角度做换向点，依次记录换向点位置P0-P6；其中，A、B、C为三相；

2.4)电机反向旋转时，按与步骤2.3)相反的通电顺序给电机的两相绕组通电，换向点的位置测试方法同步骤2.3)，依次记录换向点位置N0-N6；

2.5)根据步骤2.3)和步骤2.4)的换向点位置编写换向表，并根据换向表编写相应换向程序下载到CPLD中。

优选地，还包括步骤3)：稳像模块根据读取的MEMS角速率陀螺的当前角速率作为反馈值，以零作为输入量，通过速度PID模块获得电机驱动器控制电压信号并驱动电机向载体扰动相反的方向运行，以隔离载体扰动，实现观测装置稳像控制。

优选地，还包括步骤4)：跟踪模块接收主控系统发送的目标失调角θ，以目标失调角θ作为反馈量，以零作为输入量，通过位置PID模块获得输出量速度值；再以位置PID模块获得的输出量速度值为输入量，以MEMS角速率陀螺获得的角速率为反馈量，获得电机驱动器控制电压信号，实现目标跟踪的同时隔离载体扰动，实现稳像控制。

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

1)本发明的控制装置通过采用粗精双通道旋转变压器作为位置传感器，提高了控制跟踪精度，利用旋转变压器实时角度值产生相应的电机换向信号，能够提高电机换向的稳定性，增强系统的可靠性；使用MEMS角速率陀螺作为惯性速率测量器件，体积小、成本低。

2)本发明的控制装置将数字控制电路和电机驱动模块通过数字隔离器隔离，能够提高整个系统的抗干扰能力。

3)本发明的控制装置实时检测电机的驱动电流大小，出现过流时及时向主控系统反馈状态信息，提高了系统的安全性。

4)本发明的控制系统采用DSP+CPLD的控制架构，将角度解算和电机换向功能由可编程ASIC器件实现，减少了DSP的运算量，能够提高执行算法的实时性和控制精度。

5)本发明的控制装置采用模块化、通用化设计，各功能模块之间相互独立，集成度高。

附图说明

图1为本发明硬件逻辑框图。

图2为本发明的定位模块控制原理框图。

图3为本发明的稳像模块控制原理框图。

图4为本发明的跟踪模块控制原理框图。

图5为本发明主程序逻辑框图。

图6为本发明定时中断控制逻辑框图。

具体实施方式

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

如图1～图6所示，一种位置随动伺服控制装置，包括DSP(digital signalprocesso)、CPLD(Complex Programmable Logic Device)和电机驱动模块，其中：

所述电机驱动模块包括旋转变压器励磁电路、旋转变压器解码电路、换向时序电路、DAC转换电路、检测电路和电机驱动器，其中：

所述DSP通过SPI总线与所述CPLD连接；

所述旋转变压器励磁电路包括依次连接的波形发生器、仪表放大器和第一缓冲器，所述旋转变压器解码电路包括依次连接的第二缓冲器、旋变解码器和旋转变压器，所述第一缓冲器连接所述旋转变压器，所述波形发生器连接所述CPLD，以输出所述旋转变压器所需的正弦信号；所述第二缓冲器连接所述CPLD，以让DSP获得旋转变压器的角度信息，实现旋转变压器的角度信息解析；

所述换向时序电路包括第一数字隔离器，所述CPLD通过所述第一数字隔离器连接电机驱动器的三个霍尔传感器信号端，以提供电机的换向时序信号；

所述DAC转换电路包括依次连接的第二数字隔离器、DAC(数模转换器，又称D/A转换器)和差分放大器，所述第二数字隔离器连接所述CPLD，所述差分放大器连接电机驱动器的两个电压控制端口，以输出电机驱动器所需的控制电压；

所述检测电路包括第三数字隔离器和与该第三数字隔离器连接的仪表放大器，所述仪表放大器还连接电机驱动器的电流检测端，所述第三数字隔离器还连接所述DSP的ADC(模数转换器，又称D/A转换器)。

进一步，DSP的型号为TMS320F28335，旋变解码器的型号为AD2S82，波形发生器的型号为AD9834，DAC的型号为8734SPFB。

进一步，所述DSP的SCI模块通过串口驱动器与MEMS角速率陀螺相连，用于接收角速率信号。

进一步，所述DSP通过地址/数据总线与SRAM存储器相连，用于程序运行过程中临时变量的存储。

进一步，所述DSP还通过CAN总线连接CAN隔离收发器，所述CAN隔离收发器连接主控系统，所述主控系统用于发送控制、参数读写、目标角度信息命令给DSP，同时接收DSP返回的电机的俯仰和/或方位信息。

进一步，所述电机驱动模块为两组，两个所述旋转变压器分别用于获得电机的状态信息、俯仰信息或方位信息。

本发明的电源板包括电机驱动模块及为控制板提供所需电源供电电路组成。

按照本发明的另一个方面，还提供了一种位置随动伺服控制方法，包含以下步骤：

1)主控系统通过CAN总线发出目标失调角命令给所述DSP，然后所述DSP中断接收主控系统给出的控制命令；

2)定位模块以主控系统给出的目标失调角θ为输入量，以旋转变压器测得的角度值为反馈量，通过位置PID模块获得输出量速度值(输出量速度值是PID的计算结果，是速度量纲，这是控制工程领域的描述)；定位就是通过俯仰电机和方位电机分别控制俯仰和方位框架运行到命令位置，通过上位机发送命令位置。

速度PID模块以位置PID模块获得的输出量速度值为输入量，并利用旋转变压器测得的角度值获得电机的运行速度，再以电机的运行速度为反馈量获得电机驱动器控制电压信号并输送给DAC电路；同时，CPLD根据旋转变压器测得的当前角度值给出换向信号给电机驱动器，以控制电机旋转并实现电机旋转的闭环控制。换向时序信号HALL A、HALL B和HALLC。换向时序需要在控制电机运行前通过测试完成，具体方法包括以下步骤：

2.1)电机脱开所有负载，旋转变压器与电机同轴安装；

2.2)将电机每两相之间进行通电，为避免摩擦力矩或不平衡力矩影响，通电电压按电机供电电压90％供给。

2.3)电机通电顺序依次为AB-AC-BC-CA-CB-BA,该顺序为电机正向旋转，电机每两相绕组通电时，会使电机转子发生旋转然后静止，根据电机静止位置，向前推30°电角度做换向点，依次记录换向点位置P0-P6。

2.4)反向旋转时，按相反的通电顺序给电机的两相绕组通电，换向点的位置测试方法同步骤2.3)，依次记录换向点位置N0-N6。

2.5)根据步骤2.3)和步骤2.4)编写换向表，参见表1，并编写相应程序下载到CPLD中。

表1电机换向表

表1中，H指高电平，L指低电平。

3)稳像模块根据读取的MEMS角速率陀螺当前的角速率作为反馈值，以零作为命令值输入，通过速度PID模块获得电机驱动器控制电压信号，隔离载体扰动，实现稳像控制；此处的稳像模块的作用实际上就是起到隔离外界扰动，让观测装置获得的图像稳定。

4)稳像模块作为跟踪模块的内环，跟踪模块接收主控系统发送的目标失调角θ信息，以目标失调角θ信息作为目标和观测装置的光轴的角度差，通过位置PID模块获得输出量速度值；以位置PID模块获得的输出量速度值为输入量，以MEMS角速率陀螺输出的角速率为反馈量，获得电机驱动器控制电压信号，实现目标跟踪的同时隔离载体扰动，实现稳像控制；其中，目标指的是运行目标，如人、车，而载体是用于承载本位置随动伺服控制装置的物体。

电机在运行过程中，DSP的ADC通过电流检测电路采集电机驱动器的工作电流，当电流超过驱动器额定值时，DSP向主控系统返回电机过流状态信息。

所述的电源板包括电机驱动模块MSK4460及为控制板提供所需电源供电电路组成；

图2为本发明的定位模块控制原理框图，工作时，DSP处理器通过CAN隔离收发器接收主控系统发送的自检、定位、搜索等控制命令；图3、图4分别为本发明的稳像模块控制原理框图和跟踪模块控制原理框图。参照图4，工作时，稳像模块作为跟踪模块的内环，和观测装置(可为可见光相机、红外相机或激光探测器等)会测量出目标(这里的目标可以是人或车，一般通过图像处理、目标识别算法得到目标)和随动装置之间的角度差，该角度差由主控系统经过CAN隔离收发器发送给DSP，同时主控系统会向DSP发送跟踪命令，位置PID模块以目标和观测装置的角度差作为输入，获得输出量速度值；内环实时通过串口接收MESM陀螺反馈的观测装置抖动的速率值，以位置PID模块获得的输出量速度值为输入量，获得电机驱动器控制电压信号，实现目标跟踪的同时隔离载体扰动，实现稳像控制。

图5、图6为本发明的软件流程框图：控制算法依附于DSP处理器存在，软件主流程如图4所示，当DSP处理器通过CAN隔离收发器接收主控系统发送的自检、定位、搜索、跟踪运行等控制命令时，先进行命令解析，解析出目标角度值，然后使能驱动电机标志，并在后续的程序执行过程中，不断检测命令的执行情况，并将电机的命令执行状态相应的置为完成、超时或过流。当状态反馈计时时间到时，发送电机运行状态信息给主控系统。在程序运行过程中，会每2ms产生一次中断，中断处理流程如图5所示，每次定时中断，都会采集一次当前电机的角度值、速度值、MEMS陀螺速率值和电流值，当驱动电机标志置位时，根据主程序中收到的命令执行相应操作，由于自检、定位和搜索都属于角度定位，因此收到这些命令时调用定位模块，收到跟踪命令时时调用跟踪模块，经过定位和跟踪模块计算后，得到电机驱动数字控制电压值，DSP通过SPI总线将该电压值发送给DAC数模转换电路后，变为模拟控制电压值给电机驱动器，驱动电机运转。

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

Claims (10)

1.一种位置随动伺服控制装置，其特征在于，包括DSP、CPLD和电机驱动模块，所述DSP通过SPI总线和I/O口与所述CPLD连接，所述电机驱动模块包括旋转变压器励磁电路、旋转变压器解码电路、换向时序电路、DAC转换电路、检测电路和电机驱动器，其中：

所述旋转变压器励磁电路包括依次连接的波形发生器、仪表放大器和第一缓冲器，所述旋转变压器解码电路包括依次连接的第二缓冲器、旋变解码器和旋转变压器，所述第一缓冲器连接所述旋转变压器，所述波形发生器连接所述CPLD，以输出所述旋转变压器所需的正弦信号；所述第二缓冲器连接所述CPLD，以让DSP获得旋转变压器测量的电机转轴的角度信息；

所述换向时序电路包括第一数字隔离器，所述CPLD通过所述第一数字隔离器连接电机驱动器的三个霍尔传感器信号端，以提供电机的换向时序信号；

所述DAC转换电路包括依次连接的第二数字隔离器、DAC和差分放大器，所述第二数字隔离器连接所述CPLD，所述差分放大器连接电机驱动器的两个电压控制端口，以输出电机驱动器所需的控制电压；

所述检测电路包括第三数字隔离器和与该第三数字隔离器连接的仪表放大器，所述仪表放大器还连接电机驱动器的电流检测端，所述第三数字隔离器还连接所述DSP的ADC。

2.根据权利要求1所述的一种位置随动伺服控制装置，其特征在于，所述DSP的SCI模块通过串口驱动器与MEMS角速率陀螺相连，以获得MEMS角速率陀螺测量的电机转轴的角速率信号。

3.根据权利要求1所述的一种位置随动伺服控制装置，其特征在于，所述DSP通过地址/数据总线与SRAM存储器相连，用于程序运行过程中，临时变量的存储。

4.根据权利要求1所述的一种位置随动伺服控制装置，其特征在于，所述DSP还通过CAN总线连接CAN隔离收发器，所述CAN隔离收发器连接主控系统，所述主控系统用于发送控制、参数读写、目标角度信息命令给DSP，同时接收DSP返回的电机的俯仰和/或方位信息。

5.根据权利要求1所述的一种位置随动伺服控制装置，其特征在于，所述电机驱动模块为两组，两个所述旋转变压器分别用于获得电机的俯仰信息或方位信息。

6.根据权利要求1所述的一种位置随动伺服控制装置，其特征在于，所述旋转变压器为双通道旋转变压器，其包括粗机和单机，所述粗机和单机分别连接一旋变解码器，每个所述旋变解码器分别连接一所述第一缓冲器，每个所述第一缓冲器分别与所述CPLD连接。

7.权利要求1～6中任一权利要求所述的一种位置随动伺服控制装置的控制方法，其特征在于，包括以下步骤：

1)主控系统通过CAN总线发出目标失调角命令给所述DSP，然后所述DSP中断接收主控系统给出的控制命令；

2)定位模块以主控系统给出的目标失调角θ为输入量，以旋转变压器测得的角度值为反馈量，通过位置PID模块获得输出量速度值；

速度PID模块以位置PID模块获得的输出量速度值为输入量，并利用旋转变压器测得的角度值获得电机的运行速度，再以电机的运行速度为反馈量获得电机驱动器控制电压信号并输送给DAC电路；同时，CPLD根据旋转变压器测得的当前角度值给出换向信号给电机驱动器，以控制电机旋转并实现电机旋转的闭环控制。

8.权利要求7所述的一种位置随动伺服控制装置的控制方法，其特征在于，步骤2)中，换向时序在控制电机运行前通过测试完成，具体包括以下子步骤：

2.1)电机脱开所有负载，旋转变压器与电机同轴安装；

2.2)将电机每两相之间进行通电；

2.3)电机通电顺序依次为AB-AC-BC-CA-CB-BA,该顺序为电机正向旋转，电机每两相绕组通电时，会使电机转子发生旋转然后静止，根据电机静止位置，向前推30°电角度做换向点，依次记录换向点位置P0-P6；其中，A、B、C为三相；

2.4)电机反向旋转时，按与步骤2.3)相反的通电顺序给电机的两相绕组通电，换向点的位置测试方法同步骤2.3)，依次记录换向点位置N0-N6；

2.5)根据步骤2.3)和步骤2.4)的换向点位置编写换向表，并根据换向表编写相应换向程序下载到CPLD中。

9.权利要求7所述的一种位置随动伺服控制装置的控制方法，其特征在于，还包括步骤3)：稳像模块根据读取的MEMS角速率陀螺的当前角速率作为反馈值，以零作为输入量，通过速度PID模块获得电机驱动器控制电压信号并驱动电机向载体扰动相反的方向运行，以隔离载体扰动，实现观测装置稳像控制。

10.权利要求8所述的一种位置随动伺服控制装置的控制方法，其特征在于，还包括步骤4)：跟踪模块接收主控系统发送的目标失调角θ，以目标失调角θ作为反馈量，以零作为输入量，通过位置PID模块获得输出量速度值；再以位置PID模块获得的输出量速度值为输入量，以MEMS角速率陀螺获得的角速率为反馈量，获得电机驱动器控制电压信号，实现目标跟踪的同时隔离载体扰动，实现稳像控制。

Images