CN101819427A - On-chip control system of digital articulation based on FPGA (Field Programmable Gate Array) - Google Patents

Abstract

The FPGA-based digital joint on-chip control system relates to the field of robotics and solves the problem of low integration of existing robot joint control systems. The present invention comprises NIOSII soft-core processor, Avalon bus module, CAN bus soft-core controller, joint motor drive circuit, joint motor current vector control circuit and joint sensor information collection SPI interface circuit, the communication terminal of NIOSII soft-core processor passes Avalon The bus module is respectively connected to the communication end of the CAN bus soft-core controller, the communication end of the joint motor drive circuit, the communication end of the joint motor current vector control circuit and the communication end of the joint sensor information acquisition SPI interface circuit. The present invention is applicable to the mechanical arm Joint controller.

Description

On-Chip Control System of Digital Joint Based on FPGA

technical field

The invention relates to the field of robots, in particular to an FPGA-based digital joint on-chip control system.

Background technique

The development of robots has extended from the field of industrial robots in the past to the fields of service robots and space robots. The design goals of the new generation of robots are light weight, large output torque, and multi-perception and self-learning capabilities. The complexity of the control task puts forward more stringent requirements for the design of the joint servo control system. The motor is the executive part of the joint servo control system. With the rapid development of microelectronics technology and motor control, the control of the motor has gone through the analog control stage, the microprocessor-based control stage, the DSP+FPGA-based control stage, and the FPGA-based control stage. of the four stages.

Although the analog control scheme adopts the digital control strategy based on the microprocessor, the generation of the pulse width modulation PWM signal and the adjustment of the current loop are still realized by the analog circuit. The advantage of this scheme is fast response, but there are low anti-interference ability, Occupies a large space, complex circuit, single function and other disadvantages; DSP-based control scheme has the advantages of simple circuit, software control, high flexibility, and can be adapted to various occasions, but the generation of pulse width modulation PWM signal, the current loop Realization requires a higher sampling rate to obtain greater bandwidth; the hybrid control method based on DSP and FPGA has the advantages of higher flexibility and stronger functions, but the entire system is complex and the development is slow; FPGA with embedded microprocessor The control scheme implements a real system-on-chip, providing a control scheme with higher reliability, stronger performance, and quick development.

At present, robot joints generally adopt a hybrid control method based on DSP and FPGA to realize joint information acquisition and control, but the system is complex, the integration degree is low, and the development cycle is long.

Contents of the invention

In order to solve the problem of low integration of the existing robot joint control system, the present invention provides an FPGA-based digital joint on-chip control system.

The FPGA-based digital joint on-chip control system of the present invention includes a NIOSII soft-core processor, an Avalon bus module, a CAN bus soft-core controller, a joint motor drive circuit, a joint motor current vector control circuit and a joint sensor information acquisition SPI interface circuit , the CAN bus soft core controller includes the first CAN bus soft core controller, the joint motor drive circuit includes a sine wave SPWM generating circuit, the joint motor current vector control circuit includes a code disc interface circuit, a motor position coordinate transformation circuit, and a motor phase current acquisition circuit Interface circuit and motor current loop adjustment circuit; the communication end of the NIOSII soft-core processor is respectively connected to the communication end of the first CAN bus soft-core controller, the communication end of the joint motor drive circuit, and the joint motor current vector control circuit through the Avalon bus module The communication terminal of the joint sensor information acquisition SPI interface circuit communication terminal, the signal output terminal of the code disc interface circuit is connected to the signal input terminal of the motor position coordinate change circuit; the NIOSII soft-core processor is used for the joint action command issued by the host computer Calculation, realize the CAN bus communication protocol with the upper computer, and also be used to execute the calculation command, adjust the control parameters, realize the trajectory planning of the joint space, the management of the control parameters of the joint space, and the position, torque and impedance control of the joint space; CAN bus soft-core controller is used to transmit the joint space information to the host computer through the CAN bus interface circuit, and is also used to send the control commands and data information of the host computer to the NIOSII soft-core processor through the Avalon bus module; the joint motor drive The circuit is used to output the sine wave signal or square wave signal to drive the motor of the joint; the code disc interface circuit is used to transmit the measured motor position information and speed information; the motor position coordinate change circuit is used to change the motor position obtained by measurement The information is used for coordinate transformation; the motor phase current acquisition interface circuit is used to transmit the collected motor phase current; the joint sensor information acquisition SPI interface circuit is used to transmit joint space information including position and torque.

Beneficial effects of the present invention: the present invention provides an FPGA-based digital joint on-chip control system with high integration and short development cycle; the present invention uses a piece of FPGA chip to realize the collection of joint space information, joint motor drive, joint control and Contents such as CAN bus communication; The present invention has embedded NIOSII soft-core processor in FPGA, and described NIOSII soft-core processor has realized the visit to each interface circuit by Avalon bus module, realizes in described NIOSII soft-core processor simultaneously The calculation and processing functions such as trajectory planning, position control, and torque control of the joint space are realized; each interface circuit of the present invention realizes functions such as vector control of the motor, sensor acquisition, etc., and realizes a digital joint system-on-chip based on FPGA.

Description of drawings

Fig. 1 is a schematic structural diagram of the joint servo control system applying the FPGA-based digital joint on-chip control system of the present invention, wherein 8 is a joint, 8-1 is a motor, 8-2 is a reducer, 8-3 is a connecting rod, and 9 10 is the code disc signal circuit, 11 is the motor phase current acquisition circuit, 12 is the sensor acquisition circuit, 13 is the watchdog circuit, 14 is the CAN bus interface circuit and 15 is the Hall signal circuit. Figure 2 is the specific The schematic diagram of the principle of the motor position coordinate transformation circuit 5-2 in the fourth embodiment, FIG. 3 is the schematic diagram of the motor current loop regulation circuit 5-4 in the fourth embodiment, and FIG. 4 is the sine wave SPWM in the fourth embodiment Generate a schematic diagram of the circuit.

Detailed ways

Specific embodiment one: according to accompanying drawing 1 of description, illustrate this embodiment in detail, the digitized joint on-chip control system based on FPGA described in this embodiment, it comprises NIOSII soft-core processor 1, Avalon bus module 2, CAN bus soft-core control Device 3, joint motor drive circuit 4, joint motor current vector control circuit 5 and joint sensor information acquisition SPI interface circuit 6, CAN bus soft-core controller 3 includes the first CAN bus soft-core controller 3-1, joint motor drive circuit 4 includes sine wave SPWM generation circuit 4-1, joint motor current vector control circuit 5 includes code disc interface circuit 5-1, motor position coordinate transformation circuit 5-2, motor phase current acquisition interface circuit 5-3 and motor current loop adjustment Circuit 5-4; the communication end of the NIOSII soft-core processor 1 is respectively connected to the communication end of the first CAN bus soft-core controller 3-1, the communication end of the joint motor drive circuit 4, and the joint motor current vector through the Avalon bus module 2 The communication end of the control circuit 5 and the communication end of the joint sensor information acquisition SPI interface circuit 6, the signal output end of the code disc interface circuit 5-1 is connected to the signal input end of the motor position coordinate change circuit 5-2; NIOSII soft-core processor 1 , used to solve the joint action command issued by the upper computer U, realize the CAN bus communication with the upper computer U, and also be used to execute the calculated command, adjust the control parameters, realize the trajectory planning of the joint space, and the control parameters of the joint space Management and position, torque and impedance control of the joint space; CAN bus soft-core controller 3, used to transmit the joint space information to the host computer U through the CAN bus interface circuit 14, and also used to transmit the control commands and data of the host computer U The information is sent to the NIOSII soft-core processor 1 through the Avalon bus module 2; the joint motor drive circuit 4 is used to output the sine wave signal or square wave signal for driving the joint motor; the code disc interface circuit 5-1 is used to transmit the measured The motor position information and speed information; the motor position coordinate change circuit 5-2, used for coordinate transformation of the motor position information obtained by measurement; the motor phase current acquisition interface circuit 5-3, used to transmit the collected motor phase current; The joint sensor information collection SPI interface circuit 6 is used to transmit joint space information including position and torque.

In this specific embodiment, the acquisition of sensor information inside the joint, the motor vector control algorithm, the M/T speed measurement algorithm, the motor sine wave SPWM drive control, the trajectory planning of the joint space, the position control and the torque control are realized on a piece of FPGA. The CAN bus soft-core controller 3 is embedded in the FPGA, which realizes the CAN bus communication interface with the main control computer (upper computer U), so that the parameters of the joint controller can be adjusted online. The control system of this embodiment is highly integrated, and 32-bit NIOSII soft-core processor 1 and CAN bus soft-core controller 3 are embedded in the FPGA, which effectively reduces the area of the circuit board, and can customize the specific embodiment. Control the modules required by the system, cut unnecessary modules, and improve the integration of the system.

Specific embodiment two: the difference between this specific embodiment and specific embodiment one is that the CAN bus soft-core controller 3 also includes a second CAN bus soft-core controller 3-2, and the second CAN bus soft-core controller controls The communication end of the device 3-2 is connected to the communication end of the NIOSII soft-core processor 1 through the Avalon bus module 2.

In this specific embodiment, a second CAN bus soft-core controller 3-2 is added to the FPGA to realize redundant CAN bus communication and improve the reliability of the communication system.

Specific embodiment three: the difference between this specific embodiment and specific embodiment one or two is that the FPGA-based digital joint system-on-chip described in this specific implementation also includes a watchdog interface circuit 7, and the watchdog interface circuit The communication end of 7 connects the communication end of NIOSII soft-core processor 1 through Avalon bus module 2, and described watchdog interface circuit 7 is used for transmitting dog feeding signal or dog biting signal.

This specific embodiment, has increased watchdog interface circuit 7 in FPGA, cooperates external watchdog circuit 13 and can monitor the operation of NIOSII soft-core processor 1 in FPGA, prevents program from running away, has improved the reliability of system.

Embodiment 4: The difference between this embodiment and Embodiment 1, 2 or 3 is that the joint motor drive circuit 4 also includes a Hall interface circuit 4-2, a square wave RPWM generation circuit 4-3 and a multiplexer The switch circuit 4-4, the signal output end of the sine wave SPWM generation circuit 4-1 is connected to a signal input end of the multiplex switch circuit 4-4, and the signal output end of the Hall interface circuit 4-2 is connected to the square wave RPWM generation circuit The signal input terminal of 4-3, the signal output terminal of the square wave RPWM generation circuit 4-3 is connected to another signal input terminal of the multiplex switch circuit 4-4.

In this specific embodiment, the sine wave SPWM generation circuit 4-1 is to utilize the voltage value calculated by the current loop in the motor current loop regulation circuit 5-4 to compare with the triangular wave counter of 20KHz to realize the motor drive, when the voltage value is larger than the count value Output high level, and output low level when it is smaller than the count value;

The square wave RPWM generating circuit 4-3 is directly connected to the signal output terminal of the Hall interface circuit 4-2 for logic commutation to realize motor drive, wherein the input of the pulse width modulation PWM signal is calculated by the NIOSII soft-core processor 1, and the Hall interface circuit 4-2 is calculated. The signal input terminal of the Hall interface circuit 4-2 is connected to the Hall signal circuit 15; the two driving modes directly output the pulse width modulation PWM signal to the multi-way selection switch circuit 4-4, and the multi-way selection switch circuit 4-4 receives The control of the NIOSII soft-core processor 1 is to output the sine wave signal of the sine wave generating circuit 4-1 under the default condition; when the sine drive fails, the NIOSII soft-core processor 1 modifies the multiplexer circuit 4-4 To output the square wave signal of the square wave RPWM generating circuit 4-3.

In this specific embodiment, a square wave drive circuit is used for backup in the FPGA, which improves the reliability of the system.

In this specific embodiment, the CAN bus soft-core controller 3 is an independent IP core, which is embedded in the FPGA and realizes the bus protocol of the CAN bus 2.0B. The CAN bus soft-core controller 3 passes the information of the joint sensor through The CAN bus is sent to the host computer U, and the control commands and data information of the host computer U are sent to the NIOSII soft-core processor 1. In order to improve the reliability of the system, the CAN bus soft-core controller 3 adopts two, namely the first A CAN bus soft-core controller 3-1 and a second CAN bus soft-core controller 3-2.

In the present embodiment, motor phase current acquisition interface circuit 5-3 is the IR2175 current acquisition chip of IR company, and the expected current information is provided by NIOSII soft-core processor 1, and described NIOSII soft-core processor 1 is a 32-bit fixed-point processor , The motor current loop adjustment circuit 5-4 uses a PI regulator to adjust the current loop.

For this implementation mode, a case analysis is carried out:

In the actual control system, the frequency of the sine wave SPWM signal is 20KHz, and the frequency of the carrier signal of the IR2175 current acquisition chip is 130KHz. When a sine wave signal or square wave signal is generated each time, a synchronous signal is generated to control the current acquisition. After the synchronous signal , when the rising edges of the second and third pulses output by the IR2175 current acquisition chip come, the current signal acquisition is performed, and the average of the two acquisition results is used as the actual current value, and the sine wave SPWM control signal is generated by the regular sampling II method , sine wave SPWM generation circuit 4-1 shown in Figure 4.

到旋转坐标系的正变换和

到的反变换，其中正变换如下式(1)所示：The motor position coordinate transformation circuit 5-2 in this embodiment is shown in Figure 2, and the coordinate transformation includes three-phase static coordinate system

to the rotating coordinate system The positive transformation and

arrive The inverse transformation of , where the forward transformation is shown in the following formula (1):

                 (1)

(1)

In formula (1), there is the following relationship:

    

    

 

 

正弦表的信息实现。Therefore, in the realization of the coordinate transformation circuit, the operation under the root sign is realized by multiplication, addition and shifting operations, and the calculation of the positive and cosine angles needs to use the data stored in the memory of the FPGA.

Informational implementation of the sine table.

Motor current loop regulation circuit 5-4 is shown in Figure 3. The current loop uses a PI regulator. In order to prevent integral saturation, a method of weakening the integral with a limit is adopted.

In this embodiment, the code disc interface circuit 5-1 is mainly composed of a signal filter circuit, a quadruple frequency circuit and an M/T method speed measuring unit. The code disc adopts an incremental code disc. In order to overcome the interference on the signal line, digital filtering is required; then the filtered signal is sent to the quadruple frequency circuit for frequency multiplication processing, and the motor position information is obtained for coordinate transformation and Motor position measurement; at the same time, use the quadruple frequency signal and the M/T method to measure the motor speed information.

In this embodiment, the control system on the joint plate communicates with the upper computer U using the CAN bus, and the communication cycle is 250ms, while the control cycle of the joint is 2ms. Therefore, the joint on-chip control system needs to subdivide the expected joint position information provided by the upper computer U. In order to ensure the continuity of the acceleration, the fourth-order Paul programming is used to realize it; in order to realize the position control, torque control and impedance control of the joint space, through The joint sensor information collection SPI interface circuit 6 can obtain sensor information such as the position and torque of the joint space. Since the motor is controlled by a current loop, the motor is equivalent to a torque source. Therefore, a fourth-order state feedback controller is designed for the joint space, such as ( 2) as shown in the formula:

               (2)

(2)

、

分别为位置控制器调节参数；      In formula (2)

,

Adjust the parameters for the position controller respectively;

、

 分别为力矩控制器调节参数；

,

Adjust the parameters for the torque controller respectively;

, are position error and speed error respectively;

、

 分别为力矩和力矩微分信息。

,

are the moment and moment differential information, respectively.

 调节为零；实现力矩控制时，则需要将位置控制参数

、调节为零；而实现阻抗控制时，则需要综合调节位置和力矩控制参数。 By adjusting the parameters of each controller, different control performances can be obtained. For example, when realizing the position control of the joint space, the torque control parameters ,

Adjust it to zero; when realizing torque control, it is necessary to set the position control parameter

, Adjust to zero; and to achieve impedance control, you need to comprehensively adjust the position and torque control parameters.

Claims (4)

1. based on control system on the digital articulation sheet of FPGA, it is characterized in that it comprises NIOSII soft-core processor (1), Avalon bus module (2), the soft nuclear control device of CAN bus (3), joint motor driving circuit (4), joint motor Current Vector Control circuit (5) and joint sensors information acquisition SPI interface circuit (6), the soft nuclear control device of CAN bus (3) comprises the soft nuclear control device of a CAN bus (3-1), joint motor driving circuit (4) comprises sinusoidal wave SPWM generative circuit (4-1), and joint motor Current Vector Control circuit (5) comprises code-disc interface circuit (5-1), motor position coordinate conversion circuit (5-2), electric machine phase current acquisition interface circuit (5-3) and motor current ring regulating circuit (5-4);

The communication terminal of NIOSII soft-core processor (1) is connected respectively to the communication terminal of the soft nuclear control device of a CAN bus (3-1), the communication terminal of joint motor driving circuit (4), the communication terminal of joint motor Current Vector Control circuit (5) and the communication terminal of joint sensors information acquisition SPI interface circuit (6) by Avalon bus module (2), and the signal output part of code-disc interface circuit (5-1) connects the signal input part of motor position changes in coordinates circuit (5-2);

NIOSII soft-core processor (1), be used for resolving of joint action order that host computer (U) sends, realize CAN bus communication agreement with host computer (U), also be used to carry out the order of resolving, adjust controlled variable, realize trajectory planning, the management of joint space controlled variable and position, moment and the impedance Control of joint space of joint space;

The soft nuclear control device of CAN bus (3), be used for sending joint space information to host computer (U) by CAN bus interface circuit (14), also be used for the control command and the data message of host computer (U) are sent to NIOSII soft-core processor (1) by Avalon bus module (2);

Joint motor driving circuit (4) is used to export sine wave signal or the square-wave signal that drives joint motor work;

Code-disc interface circuit (5-1) is used to transmit measured motor position information and rotary speed information;

Motor position changes in coordinates circuit (5-2) is used for carrying out coordinate transform to measuring the motor position information that obtains;

Electric machine phase current acquisition interface circuit (5-3) is used to transmit the electric machine phase current of being gathered;

Joint sensors information acquisition SPI interface circuit (6) is used to transmit the joint space information that comprises position and moment.

2. according to claim 1 based on control system on the digital articulation sheet of FPGA, it is characterized in that the soft nuclear control device of CAN bus (3) also comprises the soft nuclear control device of the 2nd CAN bus (3-2), the communication terminal of the described soft nuclear control device of the 2nd CAN bus (3-2) is connected to the communication terminal of NIOSII soft-core processor (1) by Avalon bus module (2).

3. according to claim 1 and 2 based on control system on the digital articulation sheet of FPGA, it is characterized in that it also comprises house dog interface circuit (7), the communication terminal of described house dog interface circuit (7) is by the communication terminal of Avalon bus module (2) connection NIOSII soft-core processor (1), and described house dog interface circuit (7) is used to transmit feeding-dog signal or dog stings signal.

4. according to claim 3 based on control system on the digital articulation sheet of FPGA, it is characterized in that joint motor driving circuit (4) also comprises Hall interface circuit (4-2), square wave RPWM generative circuit (4-3) and multidiameter option switch circuit (4-4), the signal output part of sinusoidal wave SPWM generative circuit (4-1) connects a signal input part of multidiameter option switch circuit (4-4), the signal output part of Hall interface circuit (4-2) connects the signal input part of square wave RPWM generative circuit (4-3), and the signal output part of described square wave RPWM generative circuit (4-3) connects another signal input part of multidiameter option switch circuit (4-4).

Images