All-digital universal servo driving device, servo control method and detection method

Abstract

The invention discloses an all-digital universal servo driving device, a servo control method and a detection method. The device comprises an angle sensor, a motor, a current sensor, a central processing FPGA unit, a DSP processor, a power distribution unit, a driving unit, an angle resolving interface, a current detection interface and a motor driving interface. The servo control method comprises the following steps: when a load is a single-shaft motor and the system precision is required to be lower than a precision threshold value, adopting a single FPGA control method; when the load is a single-shaft motor and the precision requirement of the system is higher than a precision threshold value, a multi-loop closed-loop control method is realized by combining an FPGA (Field Programmable Gate Array) and a DSP (Digital Signal Processor); when the load is a multi-shaft motor, a multi-shaft motor synchronous control method is adopted; according to the current detection method, the A-type current detection interface or the B-type current detection interface is selected according to the amplitude of a voltage signal output by the current sensor, different calculation formulas are adopted, a current calculation value is obtained, and the current detection resolution can be improved through the method.

Description

Technical Field

[0001] The invention relates to a full-digital universal servo drive device, a servo control method and a detection method, belonging to the technical field of servo control. Background Art

[0002] As the control accuracy of various spatial mechanisms becomes higher and higher, the types and numbers of controlled objects are increasing, and the motor servo control system has higher and higher design requirements for various motor drivers. When designing servo drivers, not only high integration, high control accuracy, high reliability, and small size are required, but also the digitalization of the design and economic cost must be taken into account.

[0003] The traditional all-digital drive design is single-minded. It is necessary to design a DC motor drive or an AC motor drive according to the controlled object; design a single-axis motor drive, a dual-axis motor drive or a three-axis stand-alone drive according to the number of controlled objects; and use different main control chips and minimum systems according to the accuracy indicators to be achieved by the controlled object. This reduces the versatility of the servo drive and limits the application scenarios of the servo drive.

[0004] The traditional dual-axis motor drive system uses two servo drivers to control the two motors. This is costly, the control system is large, the control accuracy is low, and the coordinated response of the two degrees of freedom is poor. Therefore, some motor servo drive sensors are built with power modules, sampling modules, control modules, power amplifier modules, communication modules, etc. as a whole for drive control, but this type of driver is too large and the communication is easily disturbed, and the accuracy is not high. Some solutions have emerged, such as adding a synchronization timer to make the coordinated response between the servo motors faster, but it brings additional hardware resources and the topology structure is more complex. In response to the above problems, it is urgent to propose a universal, fully digital servo driver that can be applied to single motors, dual motors, and multiple motors. The servo driver can achieve high-precision single-axis or multi-axis servo control performance, has certain scalability, and can also take into account digital design, testing and economic costs. Summary of the invention

[0005] The technical problem solved by the present invention is: to overcome the deficiencies of the prior art, to provide a fully digital universal servo drive device, a servo control method and a detection method, and to achieve single-axis or multi-axis servo control performance with different precision requirements.

[0006] To achieve the above purpose, the technical solution adopted by the present invention is as follows:

[0007] The present invention discloses a fully digital universal servo drive device, comprising: an angle sensor, a motor, a current sensor, a central processing FPGA unit, a DSP processor, a power distribution unit, a drive unit, an angle calculation interface, a current detection interface and a motor drive interface; wherein:

[0008] The central processing FPGA unit receives the motion command sent by the integrated electronics through the communication interface, resolves the target position and forwards it to the DSP processor; stores the electrical angle sent by the angle resolution interface and the phase current of each motor sent by the current detection interface in the register; generates an external interrupt signal and sends it to the DSP processor; reads the motor control quantity sent by the DSP processor and forwards it to the motor drive interface;

[0009] The DSP processor receives an external interrupt signal generated by the central processing FPGA unit, reads the angle information and current information stored in the register; generates a motor control amount according to the target position, the electrical angle stored in the register and the phase current of each motor, and sends it to the central processing FPGA unit;

[0010] Angle calculation interface, which analyzes the angle information collected by the angle sensor in real time, obtains the electrical angle of each servo motor, and sends it to the central processing FPGA unit;

[0011] The current detection interface detects, filters, and amplifies the current signal of the motor collected by the current sensor in real time, generates the phase current of each motor, and sends it to the central processing FPGA unit;

[0012] The motor drive interface converts the motor control quantity into a PWM wave signal and sends it to the drive unit;

[0013] The driving unit receives the PWM wave signal, generates a motor driving signal, and sends it to the motor;

[0014] The motor rotates according to the motor drive signal;

[0015] The power distribution unit receives the primary power sent by the integrated electronics and converts it into the secondary power required by the above interfaces;

[0016] Furthermore, in the above device, it also includes:

[0017] The fault handling interface receives the electrical angle and phase current of each motor sent by the central processing FPGA unit, determines whether they are out of limit, and if so, handles the abnormal situation;

[0018] The communication interface receives the electrical angle and phase current of each motor sent by the central processing FPGA unit and sends them to the integrated electronics; the motion instructions sent by the integrated electronics are sent to the central processing FPGA unit;

[0019] The test interface is used to send important information such as motor control quantity, current signal and angle sent by the central processing FPGA unit to the host computer.

[0020] Furthermore, in the above device, the current detection interface includes an interface circuit A or an interface circuit B; wherein,

[0021] The interface circuit A includes a Hall current sensor A, a filter circuit A and an AD acquisition circuit A; wherein the Hall current sensor A is connected in series to the phase resistor of the motor, and converts the phase current signal of the motor into a corresponding voltage signal, and outputs it to the filter circuit A; the filter circuit A outputs the filtered voltage signal to the AD acquisition circuit A; the AD acquisition circuit A is used to perform analog-to-digital conversion on the filtered voltage signal, obtain a digital quantity and send it to the central processing FPGA unit;

[0022] The interface circuit B includes a Hall current sensor B, a subtraction circuit, an amplifier circuit, a filter circuit B and an AD acquisition circuit B; wherein the Hall current sensor B is connected in series to the phase resistor of the motor, and converts the phase current signal of the motor into a corresponding voltage signal, and outputs it to the subtraction circuit; the subtraction circuit is used to obtain a bias voltage by subtracting the center voltage value from the voltage signal, and the bias voltage is output to the amplifier circuit; the amplifier circuit is used to amplify the bias voltage and output it to the filter circuit B; the filter circuit B is used to output the filtered voltage signal to the AD acquisition circuit B; the AD acquisition circuit B is used to perform analog-to-digital conversion on the filtered voltage signal, obtain a digital quantity and send it to the central processing FPGA unit.

[0023] Furthermore, in the above-mentioned device, the motor is a direct motor or an AC motor, and the motor type is a voice coil motor, a permanent magnet synchronous torque motor or a brushless DC motor; the number of the angle solution interface, the current detection interface, the motor drive interface, the fault handling interface, and the drive unit are all consistent with the number of motors.

[0024] The present invention discloses a servo control method for a fully digital universal servo drive device, comprising:

[0025] When the load is a single-axis motor and the system accuracy requirement is lower than the accuracy threshold, a single FPGA control method is used to achieve drive control of the single-axis motor;

[0026] When the load is a single-axis motor and the system accuracy requirement is higher than the accuracy threshold, a multi-loop closed-loop control method is implemented by combining FPGA and DSP processors to achieve drive control of the single-axis motor;

[0027] When the load is a multi-axis motor, a multi-axis motor synchronous control method is used to realize the drive control of the multi-axis motor.

[0028] Furthermore, in the above control method, the single FPGA control method is specifically:

[0029] In time slice 1, the central processing FPGA unit receives motion instructions via the communication interface;

[0030] In time slice 2, the central processing FPGA unit collects the real-time electrical angle and phase current through the angle calculation interface and the current acquisition interface according to the motion instruction, and stores them in the register;

[0031] In time slice 3, the central processing FPGA unit generates the motor control quantity through single speed closed-loop control or single position closed-loop control;

[0032] In time slice 4, the central processing FPGA unit sends a PWM wave signal to the drive unit through the motor drive interface;

[0033] In time slice 5, the central processing FPGA unit determines and processes the abnormal information through the fault handling interface;

[0034] In time slice 6, the central processing FPGA unit sends the motor control quantity, current signal and angle information to the host computer through the test interface;

[0035] Time slices 1 to 6 constitute a control cycle, which is timed and controlled by the central processing FPGA unit. Each subsequent control cycle repeats time slices 1 to 6.

[0036] Furthermore, in the above control method, the FPGA and DSP processor implement a multi-loop closed-loop control method, specifically:

[0037] In time slice 1, the central processing FPGA unit receives motion instructions via the communication interface;

[0038] In time slice 2, the central processing FPGA unit collects the real-time electrical angle and phase current through the angle calculation interface and the current acquisition interface according to the motion instruction, and stores them in the register;

[0039] In time slice 3, the central processing FPGA unit generates an external interrupt signal to the DSP processor;

[0040] In time slice 4, the DSP processor implements dual closed-loop control of the position loop and the speed loop or triple closed-loop control of the position loop, the speed loop, and the current loop according to the received external interrupt signal to generate the motor control quantity;

[0041] In time slice 5, the DSP processor sends the motor control quantity to the central processing FPGA unit;

[0042] In time slice 6, the central processing FPGA unit reads the motor control quantity sent by the DSP processor;

[0043] In time slice 7, the central processing FPGA unit sends a PWM wave signal to the drive unit through the motor drive interface;

[0044] In time slice 8, the central processing FPGA unit determines and processes the abnormal information through the fault handling interface;

[0045] In time slice 9, the central processing FPGA unit sends the motor control quantity, current signal and angle information to the host computer through the test interface;

[0046] Time slices 1 to 9 constitute a control cycle, which is timed and controlled by the central processing FPGA unit. Each subsequent control cycle repeats time slices 1 to 9.

[0047] Furthermore, in the above control method, the multi-axis motor synchronous control method is specifically as follows:

[0048] In time slice 1, the central processing FPGA unit receives motion instructions for motors 1-n via the communication interface; n is the number of motors;

[0049] In time slice 2, the central processing FPGA unit simultaneously reads the angle information of motors 1-n through angle resolution interfaces 1-n and reads the phase current of motors 1-n through current detection interfaces 1-n;

[0050] In time slice 3, the central processing FPGA unit generates an external interrupt signal to the DSP processor;

[0051] In time slice 4, after receiving the external interrupt signal, the DSP processor reads the angle information and phase current of the n motors at the same time stored in the central processing FPGA unit, and uses the multi-axis synchronous control algorithm to generate 1-n motor control quantities;

[0052] In time slice 5, the DSP processor sends 1-n motor control quantities to the central processing FPGA unit;

[0053] In time slice 6, the central processing FPGA unit simultaneously reads 1-n motor control quantities sent by the DSP processor;

[0054] In time slice 7, the central processing FPGA unit sends the PWM wave signal of the motor 1-n to the drive unit 1-n through the motor drive interface 1-n;

[0055] In time slice 8, the central processing FPGA unit processes the fault of motor 1-n through fault processing interface 1-n;

[0056] In time slice 9, the central processing FPGA unit sends information such as 1-n motor control quantities, current signals, and angles to the host computer through the test interface;

[0057] Time slices 1 to 9 constitute a control cycle, which is timed and controlled by the central processing FPGA unit. Each subsequent control cycle repeats time slices 1 to 9.

[0058] The present invention discloses a current detection method using the current detection interface, specifically:

[0059] S91, read the voltage signal V output by the Hall current sensor A / B , if V A / B ≥50%M, interface circuit A is used and the process goes to step S92; otherwise, interface circuit B is used and the process goes to step S94;

[0060] S92, the voltage signal V A / B After passing through the filter circuit A, the filtered signal V is obtained A / B1 , the filtered signal V A / B1 Send it to AD acquisition circuit A to get digital quantity D A / B ;

[0061] S93, according to the digital quantity D A / B , get the current solution value I at high voltage A / B ;quit;

[0062] S94, the voltage signal V A / B Sent to the subtraction circuit, subtracting the center voltage value V ref , and get the bias voltage V A / B1 ;

[0063] S95, bias voltage V A / B1 Amplify and filter to obtain the amplified voltage V A / B2 , V A / B2 =KV A / B1 ; K is the amplification factor of the amplifier circuit;

[0064] S96, the amplified voltage V A / B2 It is sent to AD acquisition circuit B for acquisition and the digital quantity D is obtained. A / B ;

[0065] S97, according to the digital quantity D A / B and the center voltage value V ref , get the current solution value II at low voltage A / B . Further, in the above detection method,

[0066] I A / B =M I *D A / B / M D

[0067] II A / B =M I *DA / B / (K*M D )+V ref

[0068] Among them, I A / B Current solution value at high voltage; II A / B is the current solution value at low voltage; M is the full-scale voltage of the Hall current sensor, M I is the full-scale current of the Hall current sensor, M D is the full-scale digital quantity of the AD acquisition circuit, and K is the amplification factor of the amplifier circuit.

[0069] Compared with the prior art, the present invention has the following beneficial effects:

[0070] 1. The present invention can be applied to DC motors and AC motors, and in particular to a universal all-digital motor servo driver for voice coil motors and permanent magnet synchronous motors, and can be applied to the synchronous control of single-axis, two-axis, three-axis and multi-axis motors.

[0071] 2. The present invention uses FPGA as the main control chip to complete the timing control of the entire single-axis motor, thereby solving the cumulative error caused by the asynchronous dual processors; using the synchronous clock and parallel processing capabilities of FPGA, the coordinated response between multi-axis servo motors is faster, reaching the ns level, without adding additional hardware resources;

[0072] 3. The present invention can use a single FPGA or FPGA and DSP processor collaborative control to complete different control strategies according to different control accuracy requirements, taking into account both the digitalization of the design and the economic cost.

[0073] 4. The present invention proposes an improved current detection method, which can simply and effectively improve the resolution of current. BRIEF DESCRIPTION OF THE DRAWINGS

[0074] Figure 1 It is a structural schematic diagram of a universal all-digital motor servo driver of the present invention;

[0075] Figure 2 Schematic diagram of a current detection circuit of the present invention; (a) is a schematic diagram of interface circuit A; (b) is a schematic diagram of interface circuit B;

[0076] Figure 3 A schematic diagram of a low-precision single-axis motor servo control method of the present invention;

[0077] Figure 4 A schematic diagram of a high-precision single-axis motor servo control method of the present invention;

[0078] Figure 5 It is a schematic diagram of a multi-axis motor servo control method of the present invention. DETAILED DESCRIPTION

[0079] In order to make the purpose, technical solution and advantages of the present application clearer, the technical solution of the present application will be described in detail below. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all of the embodiments. Based on the embodiments in the present application, all other implementation methods obtained by ordinary technicians in the field without making creative work are within the scope of protection of the present application.

[0080] See also Figure 1 , the embodiment of the present application provides a universal all-digital motor servo driver, the driver comprising: an angle sensor 1, a motor 2, a current sensor 3, a central processing FPGA unit 4, a DSP processor 5, a power distribution unit 6, a drive unit 7, an angle solution interface 8, a current detection interface 9, a motor drive interface 10, a fault processing interface 11, a communication interface 12, and a test interface 13;

[0081] The central processing FPGA unit 4 receives the motion command sent by the integrated electronics through the communication interface 12, analyzes the target position and forwards it to the DSP processor 5, and stores the electrical angle sent by the angle resolution interface 8 and the phase current of each motor sent by the current detection interface 9 in the register; generates an external interrupt signal and sends it to the DSP processor 5; reads the motor control quantity sent by the DSP processor 5 and forwards it to the motor drive interface 10;

[0082] The DSP processor 5 receives an external interrupt signal, and reads the angle information and current information sent by the central processing FPGA unit 4 in the interrupt subroutine; generates a motor control amount according to the motion instruction, the angle information and the current information, and sends it to the central processing FPGA unit 4;

[0083] Angle calculation interface 8 analyzes the angle information collected by the angle sensor 1, obtains the electrical angle of each servo motor, and sends it to the central processing FPGA unit 4;

[0084] The current detection interface 9 detects, filters and amplifies the current signal collected by the current sensor 3, generates the phase current of each motor, and sends it to the central processing FPGA unit 4;

[0085] The motor drive interface 10 receives the motor control quantity, converts it into a PWM wave signal, and sends it to the drive unit 7;

[0086] The drive unit 7 receives the PWM wave signal sent by the motor drive interface 10, generates a motor drive signal, and sends it to each motor;

[0087] Power distribution unit 6, receives the primary power sent by the integrated electronics and converts it into the secondary power required by the above interfaces;

[0088] The fault processing interface 11 receives the electrical angle and phase current of each motor sent by the central processing FPGA unit 4, determines whether it exceeds the limit, and if so, performs abnormal situation processing;

[0089] The communication interface 12 receives the electrical angle and phase current information of each motor sent by the central processing FPGA unit 4, sends it to the integrated electronics, and forwards the motion instructions sent by the integrated electronics to the central processing FPGA unit 4;

[0090] The test interface 13 sends important information such as motor control quantity, current signal and angle signal sent by the central processing FPGA unit 4 to the host computer;

[0091] The motor 2 can be a DC current or AC motor, and is particularly suitable for voice coil motors, permanent magnet synchronous torque motors, and brushless DC motors; the number of angle calculation interfaces 8, current detection interfaces 9, motor drive interfaces 10, fault handling interfaces 11, and drive units 7 are consistent with the number of motors 2;

[0092] The angle sensor 1 can be a photoelectric encoder, an inductive synchronizer, an LVDT, a capacitive sensor, a rotary transformer, etc., and is particularly suitable for a photoelectric encoder and an inductive synchronizer;

[0093] The current sensor 3 may be a Hall current sensor;

[0094] Specifically, in the solution provided in the embodiment of the present application, the motor 4 is a permanent magnet synchronous torque motor, the angle sensor is a photoelectric encoder, and the current sensor is a Hall current sensor. There are two motors, which need to move synchronously. An external interrupt signal 4 is used between the DSP and the FPGA, and the EMIF interface is used to process the read and write requests of the DSP. The communication interface and the integrated electronics use the CAN bus communication, and the test interface and the host computer use the high-speed serial port RS232 communication.

[0095] See also Figure 2 The current detection interface includes two types: interface circuit A includes Hall current sensor A, filter circuit A, and AD acquisition circuit A; interface circuit B includes Hall current sensor B, subtraction circuit, amplifier circuit, filter circuit B, and AD acquisition circuit B;

[0096] The Hall current sensor A in the interface circuit A is connected in series to the phase resistor of the motor, converts the phase current signal into a phase voltage signal, and outputs it to the filter circuit A; the filter circuit A outputs the filtered voltage signal to the AD acquisition circuit A; the AD acquisition circuit A performs analog-to-digital conversion on the filtered voltage signal, obtains a digital quantity and sends it to the central processing FPGA unit;

[0097] The Hall current sensor B in the interface circuit B is connected in series to the phase resistance of the motor, converts the phase current signal into a phase voltage signal, and outputs it to the subtraction circuit; the subtraction circuit subtracts the center voltage value from the voltage signal to obtain a bias voltage, and the bias voltage is output to the amplifier circuit; the amplifier circuit is used to amplify the bias voltage and output it to the filter circuit B; the filter circuit B outputs the filtered voltage to the AD acquisition circuit B; the AD acquisition circuit B performs analog-to-digital conversion on the filtered voltage signal, obtains a digital quantity and sends it to the central processing FPGA unit.

[0098] The following is a further detailed description of a fully digital motor servo control method provided by the embodiment of the present application in conjunction with the accompanying drawings. The method is applied to Figure 1 The specific implementation method of the system is as follows:

[0099] See also Figure 3 ,When the load is a single-axis motor and the system accuracy requirement is lower than the accuracy threshold, a single FPGA control method is used to realize the drive control of the single-axis motor. The steps are as follows: In time slice 1, the FPGA receives the motion command sent by the integrated electronics through the communication interface; in time slice 2, the FPGA collects real-time angle information and current signals through the angle resolution interface and the current acquisition interface according to the motion command to ensure that the collected current and angle are at the same time, and store them in the register respectively; in time slice 3, the FPGA generates the motor control quantity through single speed closed loop or single position closed loop control; in time slice 4, the FPGA sends the PWM signal to the drive unit through the motor drive interface; in time slice 5, the FPGA judges and processes the abnormal information through the fault handling interface; in time slice 6, the FPGA sends important variables such as motor control quantity, current signal, angle information to the host computer through the test interface and stores and displays them. Then repeat time slices 1 to 6.

[0100] See also Figure 4When the load is a single-axis motor and the system accuracy requirement is higher than the accuracy threshold, the FPGA and DSP processors are used to coordinate control to implement a multi-loop closed-loop control algorithm to achieve drive control of the single-axis motor. The steps are as follows: In time slice 1, the FPGA receives the motion command sent by the integrated electronics through the communication interface; in time slice 2, the FPGA collects real-time angle information and current signals through the angle resolution interface and the current acquisition interface according to the motion command, and stores them in the register; in time slice 3, the FPGA generates an external interrupt signal to the DSP processor; in time slice 4, after the DSP processor receives the external interrupt signal, it implements in the interrupt subroutine The position loop and the speed loop are controlled in double closed loops or the position loop, speed loop, and current loop are controlled in three closed loops to generate the motor control quantity. In time slice 5, the DSP processor sends the motor control quantity to the FPGA through the bus interface. In time slice 6, the FPGA reads the motor control quantity sent by the DSP processor through the bus interface. In time slice 7, the FPGA sends the PWM signal to the drive unit through the motor drive interface. In time slice 8, the FPGA judges and processes the abnormal information through the fault processing interface. In time slice 9, the FPGA sends important variables such as the motor control quantity, current signal, and angle information to the host computer through the test interface and stores and displays them. Then repeat time slices 1 to 9.

[0101] See also Figure 5 When the load is a multi-axis motor, a multi-axis motor synchronous control method is used to realize the drive control of the multi-axis motor. The steps are as follows: in time slice 1, FPGA receives the motion instructions of each motor through the communication interface; in time slice 2, FPGA simultaneously reads the angle information of motor 1-n through angle resolution interface 1-n, and reads the phase current of motor 1-n through current detection interface 1-n; in time slice 3, FPGA unit generates an external interrupt signal to DSP processor; in time slice 4, after receiving the external interrupt signal, DSP reads the angle information of motor 1-n at the same time and the phase current of motor 1-n sent by FPGA in the interrupt subroutine The phase current at the same time, and the multi-motor synchronous control algorithm such as the deviation coupling algorithm is used to generate 1-n motor control quantities; in time slice 5, the DSP processor sends each motor control quantity to the FPGA; in time slice 6, the FPGA simultaneously reads the 1-n motor control quantities sent by the DSP; in time slice 7, the FPGA sends the PWM signal of each motor to the drive unit 1-n through the motor drive interface 1-n; in time slice 8, the FPGA handles the fault of each motor through the fault processing interface 1-n; in time slice 9, the FPGA sends each motor control quantity, current signal and angle information to the host computer through the test interface.

[0102] The following is a further detailed description of an improved current detection method provided by the embodiment of the present application in conjunction with the accompanying drawings. The method is applied to Figure 2On the current detection interface, the specific implementation method includes the following steps:

[0103] S1, read the voltage signal V output by the Hall current sensor A / B , if V A / B ≥50%M, interface circuit A is used and the process goes to step S92; otherwise, interface circuit B is used and the process goes to step S94;

[0104] S2, the voltage signal V A / B After passing through the filter circuit A, the filtered signal V is obtained A / B1 , the filtered signal V A / B1 Send it to AD acquisition circuit A to get digital quantity D A / B ;

[0105] S3, according to the digital quantity D A / B , get the current solution value I at high voltage A / B ;quit;

[0106] S4, the voltage signal V A / B Sent to the subtraction circuit, subtracting the center voltage value V ref , and get the bias voltage V A / B1 ;

[0107] S5, bias voltage V A / B1 Amplify and filter to obtain the amplified voltage V A / B2 , V A / B2 =KV A / B1 ; K is the amplification factor of the amplifier circuit;

[0108] S6, the amplified voltage V A / B2 It is sent to AD acquisition circuit B for acquisition and the digital quantity D is obtained. A / B ;

[0109] S7, according to the digital quantity D A / B and the center voltage value V ref , get the current solution value II at low voltage A / B .

[0110] Among them, I A / B =M I *D A / B / M D (1)

[0111] II A / B =M I *D A / B / (K*M D )+V ref (2)

[0112] The current detection method is further described in detail below using a specific example:

[0113] A space optical remote sensor uses a telescope overall rotation and half-angle reflector compensation imaging solution. The rotation speed of the telescope is 68° / s, and the rotation speed of the half-angle reflector is half of the telescope, performing synchronous follow-up movement. Both axes use Hall current sensors for current collection. Both use the model HBC004S / JN01. The sensor has a range of -4A to 4A, and the collected voltage range is 0 to 5V. The amplitude of the telescope phase current is about ±3A, and the amplitude of the half-angle reflector phase current is about 2.5A±0.25A. The voltage of the telescope current after the telescope phase current is collected by the Hall current sensor A is V A / B =0.625V~4.375V, M=5, V A / B =75%M, so the current detection interface A is used, and after passing through the filter circuit A, it is directly sent to the AD acquisition circuit A for solution. The digital quantity obtained is 512 to 3584, and the AD solution bit is 12 bits. Therefore, according to formula (1), M I =8A,M D =4096, we get I A / B = ±3A. The voltage of the half-angle current collected by the half-angle mirror phase current after passing through the Hall current sensor B is V A / B =2.6563V~2.3437V, V A / B =6.25%M, so the current detection interface B is used. First, the center value 2.5V is subtracted from the subtraction circuit to obtain the bias voltage V A / B1 = ±0.1563V, V A / B1 After passing through the amplifier circuit and filter circuit B, the voltage after amplification K = 30 times is V A / B2 = ±4.687V, V A / B2 After being solved by AD acquisition circuit B, the digital quantity obtained is D A / B 0 to 3840, according to formula (2), K = 30, V ref =2.5V, we get I A / B =2.5±0.25A, so the current resolution is improved from 1.95mA to 0.0651mA, an improvement of 30 times.

[0114] Although the content of the present invention has been described in detail through the above preferred embodiments, it should be appreciated that the above description should not be considered as a limitation of the present invention. After reading the above content, it will be apparent to those skilled in the art that various modifications and substitutions of the present invention will occur. Therefore, the protection scope of the present invention should be limited by the appended claims.

[0115] The contents not described in detail in the specification of the present invention belong to the common knowledge of the professionals in this field.

Claims

1. A fully digital universal servo drive device, characterized in that: include: Angle sensor, motor, current sensor, central processing FPGA unit, DSP processor, power distribution unit, drive unit, angle calculation interface, current detection interface and motor drive interface; among them, The central processing FPGA unit receives the motion command sent by the integrated electronics through the communication interface, resolves the target position and forwards it to the DSP processor; stores the electrical angle sent by the angle resolution interface and the phase current of each motor sent by the current detection interface in the register; generates an external interrupt signal and sends it to the DSP processor; reads the motor control quantity sent by the DSP processor and forwards it to the motor drive interface; The DSP processor receives an external interrupt signal generated by the central processing FPGA unit, reads the angle information and current information stored in the register; generates a motor control amount according to the target position, the electrical angle stored in the register and the phase current of each motor, and sends it to the central processing FPGA unit; The angle calculation interface analyzes the angle information collected by the angle sensor in real time, obtains the electrical angle of each servo motor, and sends it to the central processing FPGA unit; The current detection interface detects, filters, and amplifies the current signal of the motor collected by the current sensor in real time, generates the phase current of each motor, and sends it to the central processing FPGA unit; The motor drive interface converts the motor control quantity into a PWM wave signal and sends it to the drive unit; The driving unit receives the PWM wave signal, generates a motor driving signal, and sends it to the motor; The motor rotates according to the motor drive signal; The power distribution unit receives the primary power sent by the integrated electronics and converts it into the secondary power required by the angle resolution interface, the current detection interface and the motor drive interface.

2. A fully digital universal servo drive device according to claim 1, characterized in that: Also includes: The fault handling interface receives the electrical angle and phase current of each motor sent by the central processing FPGA unit, determines whether they are out of limit, and if so, handles the abnormal situation; The communication interface receives the electrical angle and phase current of each motor sent by the central processing FPGA unit and sends them to the integrated electronics; the motion instructions sent by the integrated electronics are sent to the central processing FPGA unit; The test interface is used to send the motor control quantity, current signal and angle information sent by the central processing FPGA unit to the host computer.

3. A fully digital universal servo drive device according to claim 1, characterized in that: The current detection interface includes an interface circuit A or an interface circuit B; wherein, The interface circuit A includes a Hall current sensor A, a filter circuit A and an AD acquisition circuit A; wherein the Hall current sensor A is connected in series to the phase resistor of the motor, and converts the phase current signal of the motor into a corresponding voltage signal, and outputs it to the filter circuit A; the filter circuit A outputs the filtered voltage signal to the AD acquisition circuit A; the AD acquisition circuit A is used to perform analog-to-digital conversion on the filtered voltage signal, obtain a digital quantity and send it to the central processing FPGA unit; The interface circuit B includes a Hall current sensor B, a subtraction circuit, an amplifier circuit, a filter circuit B and an AD acquisition circuit B; wherein the Hall current sensor B is connected in series to the phase resistor of the motor, and converts the phase current signal of the motor into a corresponding voltage signal, and outputs it to the subtraction circuit; the subtraction circuit is used to obtain a bias voltage by subtracting the center voltage value from the voltage signal, and the bias voltage is output to the amplifier circuit; the amplifier circuit is used to amplify the bias voltage and output it to the filter circuit B; the filter circuit B is used to output the filtered voltage signal to the AD acquisition circuit B; the AD acquisition circuit B is used to perform analog-to-digital conversion on the filtered voltage signal, obtain a digital quantity and send it to the central processing FPGA unit.

4. A fully digital universal servo drive device according to claim 1, characterized in that: The motor is a direct motor or an AC motor, and the motor type is a voice coil motor, a permanent magnet synchronous torque motor or a brushless DC motor; the number of the angle solution interface, the current detection interface, the motor drive interface, the fault handling interface, and the drive unit are all consistent with the number of motors.

5. A servo control method using the all-digital universal servo drive device according to any one of claims 1 to 4, characterized in that: include: When the load is a single-axis motor and the system accuracy requirement is lower than the accuracy threshold, a single FPGA control method is used to achieve drive control of the single-axis motor; When the load is a single-axis motor and the system accuracy requirement is higher than the accuracy threshold, a multi-loop closed-loop control method is implemented by combining FPGA and DSP processors to achieve drive control of the single-axis motor; When the load is a multi-axis motor, a multi-axis motor synchronous control method is used to realize the drive control of the multi-axis motor.

6. The servo control method according to claim 5, characterized in that: The single FPGA control method is specifically: In time slice 1, the central processing FPGA unit receives motion instructions via the communication interface; In time slice 2, the central processing FPGA unit collects the real-time electrical angle and phase current through the angle calculation interface and the current acquisition interface according to the motion instruction, and stores them in the register; In time slice 3, the central processing FPGA unit generates the motor control quantity through single speed closed-loop control or single position closed-loop control; In time slice 4, the central processing FPGA unit sends a PWM wave signal to the drive unit through the motor drive interface; In time slice 5, the central processing FPGA unit determines and processes the abnormal information through the fault handling interface; In time slice 6, the central processing FPGA unit sends the motor control quantity, current signal and angle information to the host computer through the test interface; Time slices 1 to 6 constitute a control cycle, which is timed and controlled by the central processing FPGA unit. Each subsequent control cycle repeats time slices 1 to 6.

7. The servo control method according to claim 5, characterized in that: The FPGA and DSP processors implement a multi-loop closed-loop control method, specifically: In time slice 1, the central processing FPGA unit receives motion instructions via the communication interface; In time slice 2, the central processing FPGA unit collects the real-time electrical angle and phase current through the angle calculation interface and the current acquisition interface according to the motion instruction, and stores them in the register; In time slice 3, the central processing FPGA unit generates an external interrupt signal to the DSP processor; In time slice 4, the DSP processor implements dual closed-loop control of the position loop and the speed loop or triple closed-loop control of the position loop, the speed loop, and the current loop according to the received external interrupt signal to generate the motor control quantity; In time slice 5, the DSP processor sends the motor control quantity to the central processing FPGA unit; In time slice 6, the central processing FPGA unit reads the motor control quantity sent by the DSP processor; In time slice 7, the central processing FPGA unit sends a PWM wave signal to the drive unit through the motor drive interface; In time slice 8, the central processing FPGA unit determines and processes the abnormal information through the fault handling interface; In time slice 9, the central processing FPGA unit sends the motor control quantity, current signal and angle information to the host computer through the test interface; Time slices 1 to 9 constitute a control cycle, which is timed and controlled by the central processing FPGA unit. Each subsequent control cycle repeats time slices 1 to 9.

8. The servo control method according to claim 5, characterized in that: The multi-axis motor synchronous control method is specifically as follows: In time slice 1, the central processing FPGA unit receives motion instructions for motors 1-n via the communication interface; n is the number of motors; In time slice 2, the central processing FPGA unit simultaneously reads the angle information of motors 1-n through angle resolution interfaces 1-n and reads the phase current of motors 1-n through current detection interfaces 1-n; In time slice 3, the central processing FPGA unit generates an external interrupt signal to the DSP processor; In time slice 4, after receiving the external interrupt signal, the DSP processor reads the angle information and phase current of the n motors at the same time stored in the central processing FPGA unit, and uses the multi-axis synchronous control algorithm to generate 1-n motor control quantities; In time slice 5, the DSP processor sends 1-n motor control quantities to the central processing FPGA unit; In time slice 6, the central processing FPGA unit simultaneously reads 1-n motor control quantities sent by the DSP processor; In time slice 7, the central processing FPGA unit sends the PWM wave signal of the motor 1-n to the drive unit 1-n through the motor drive interface 1-n; In time slice 8, the central processing FPGA unit processes the fault of motor 1-n through fault processing interface 1-n; In time slice 9, the central processing FPGA unit sends 1-n motor control quantities, current signals and angle information to the host computer through the test interface; Time slices 1 to 9 constitute a control cycle, which is timed and controlled by the central processing FPGA unit. Each subsequent control cycle repeats time slices 1 to 9.

9. A current detection method using a fully digital universal servo drive device as claimed in claim 3, characterized in that: include: S91, read the voltage signal V output by the Hall current sensor A / B , if V A / B ≥50%M, then use interface circuit A and go to step S92; Otherwise, interface circuit B is used and the process proceeds to step S94; S92, the voltage signal V A / B After passing through the filter circuit A, the filtered signal V is obtained A / B1 , the filtered signal V A / B1 Send it to AD acquisition circuit A to get digital quantity D A / B ; S93, according to the digital quantity D A / B , get the current solution value I at high voltage A / B ;quit; S94, the voltage signal V A / B Sent to the subtraction circuit, subtracting the center voltage value V ref , and get the bias voltage V A / B1 ; S95, bias voltage V A / B1 Amplify and filter to obtain the amplified voltage V A / B2 , V A / B2 =KV A / B1 ; K is the amplification factor of the amplifier circuit; S96, the amplified voltage V A / B2 It is sent to AD acquisition circuit B for acquisition and the digital quantity D is obtained. A / B ; S97, according to the digital quantity D A / B and the center voltage value V ref , get the current solution value II at low voltage A / B .

10. The current detection method according to claim 9, characterized in that: I A / B =M I *D A / B / M D II A / B =M I *D A / B / (K*M D )+V ref Among them, I A / B Current solution value at high voltage; II A / B is the current solution value at low voltage; M is the full-scale voltage of the Hall current sensor, M I is the full-scale current of the Hall current sensor, M D is the full-scale digital quantity of the AD acquisition circuit, and K is the amplification factor of the amplifier circuit.

Images