Multi-core heterogeneous control-drive integrated internal and external shaft motor cooperative control method

Abstract

The invention discloses a multi-core heterogeneous control-drive integrated internal and external shaft motor cooperative control method, and belongs to the technical field of motor cooperative control method.The control-drive integrated system is connected with an external shaft through a field bus, mechanical motion parameters of the external shaft are obtained, and the coordinate relation between a robot and the external shaft is established; an operation control system carries out unified planning on an internal shaft and an external shaft of a robot, an interpolation track is generated, a master station program clock is taken as a reference, a master station can obtain numerical values of a global counter of an arm core and a microsecond counter of an FPGA core at the starting moment of each period, and the numerical values of the global counter of the arm core and the microsecond counter of the FPGA core are calculated through a master station program, a middle layer program and a servo control program. An FPGA program needs to be subjected to layer-by-layer handshake and bus stability detection and enters a clock calibration link after handshake passes, and a master station program obtains a counting value of an ARM core global counter at the starting moment of thread execution.

Description

Technical Field

[0001] The present invention relates to a motor collaborative control method, and in particular to a multi-core heterogeneous control-drive integrated body and external axis motor collaborative control method, belonging to the technical field of motor collaborative control methods. Background Art

[0002] Based on the measured clock offset and transmission delay, the master station compensates the clock of each slave station to ensure that they remain synchronized with the reference clock.

[0003] During the periodic operation phase, the master station will continue to send synchronization data frames to dynamically compensate for clock drift and ensure the clock synchronization accuracy of the entire network.

[0004] In many industrial applications, such as robots and CNC machine tools, multiple external motors are often required to work together to achieve complex motion control. For example, in welding applications, robots need to be equipped with dual-axis positioners to weld complex workpieces. Traditional multi-axis motor control systems usually use independent control units to achieve coordination between axes through bus or network communication. The internal axis motor driver of the robot is connected through the commonly used EtherCAT bus. If you need to add external axes, you only need to purchase a driver that supports the EtherCAT bus and connect it. The distributed clock mechanism in the EtherCAT protocol will help synchronize the internal and external axis motor control data.

[0005] However, for the integrated control and drive type controller, the driver of the internal shaft motor is not controlled through the commonly used bus, but through the shared memory and AXI bus inside the chip for data exchange. In order to reduce costs and development and maintenance difficulties, the EtherCAT type protocol stack will not be added. When an external axis needs to be added, there are two options. One is to expand the drive module inside the controller, but our controller is a standard product, and it is difficult to adapt to the ever-changing market. The second is to add an external driver with a bus protocol. Since the EtherCAT protocol is based on the first slave station as the benchmark, this will involve the problem of control data synchronization in some processes that require collaborative applications. The internal control clock of the integrated control and drive needs to be synchronized with the first slave station.

[0006] At the same time, similar products such as integrated drive and control often use multi-core heterogeneous chips or multiple chips. This will result in inconsistent clock frequencies of each core or chip. Over time, cumulative errors will occur, leading to clock offsets. Periodic clock calibration is required for each part of the program to ensure synchronization.

[0007] No other relevant solutions have been found so far. The purpose of the present invention is mainly to synchronize the control clocks of the bus protocol master station in the A core (such as EtherCAT protocol stack with slave clock synchronization function), motion planning system; servo control system in the R core; motor drive system in the FPGA, so as to achieve precise coordinated control of internal and external axes and meet the high-precision requirements in applications. Summary of the invention

[0008] The main purpose of the present invention is to provide a method for coordinated control of external axis motors in a multi-core heterogeneous control-drive system.

[0009] The purpose of the present invention can be achieved by adopting the following technical solutions:

[0010] A method for coordinated control of external shaft motors in a multi-core heterogeneous control-drive system, including a control system for the control method;

[0011] The control system includes an operation control system, which shares memory with a master station program and a servo control program;

[0012] The master station program is connected to the external drive module through the industrial bus, and is connected to the external motor shaft through the external drive module;

[0013] The servo control program is connected to the FPGA module through the AXI bus, and is connected to the internal axis of the robot through the FPGA module.

[0014] Preferably, the method comprises the following steps:

[0015] Step 1: Connect the control-drive system to the external axis via the fieldbus;

[0016] Step 2: Obtain the mechanical motion parameters of the external axis and establish the coordinate relationship between the robot and the external axis;

[0017] Step 3: The operation control system uniformly plans the internal and external axes of the robot and generates interpolation trajectories;

[0018] Step 4: Based on the master program clock, the master will obtain the values ​​of the global counter of the arm core and the microsecond counter of the FPGA core at the beginning of each cycle;

[0019] Step 5: Through the master program, middle layer program, servo control program, and FPGA program, the handshake and bus stability tests are performed layer by layer;

[0020] Step 6: After the handshake is passed, the clock calibration phase begins. The master program obtains the count value of the ARM core global counter at the start time of the thread execution.

[0021] Step 7: The master station program triggers the FPGA to latch the clock counter value cnt at the start time of the master station by adding 1 to the cycle count value, and defines the count value of the FPGA at the end of the current cycle as FPGAEndCnt, and the cycle count digit CycleCnt, then:

[0022] FPGAEndCnt=cnt+CycleCnt;

[0023] Step 8: In the servo control program running and interrupt function of the R core, it is triggered by the FPGA and uses its own counter to record the current count value ServoCnt, and calculates the end count value of the current cycle as ServoEndCnt;

[0024] Step 9: Each part of the program calculates the Tus moment of the current cycle through the known end time of the cycle, and defines the Tus moment of the master program as MasterT, the middle layer as MidT, the FPGA as FPGAT, and the servo control program as ServoT. Then:

[0025] MasterT = Time + T * global counter microsecond count;

[0026] MidT = Time + T * global counter microsecond count;

[0027] FPGAT = cnt + T * FPGA clock counter microsecond count;

[0028] ServoT = ServoCnt + T*R core counter microsecond count;

[0029] Step 10: When reaching the Tus moment of the current cycle, each part of the program first reads the instruction information and feedback information from the corresponding memory or register, and then executes the corresponding motion planning program and motor control driver. After the execution is completed, the corresponding control instruction and feedback information are written to the memory, bus or register;

[0030] Step 11: After waiting for the cycle time to end, jump to clock calibration and cycle periodically to achieve synchronization between internal and external drive control instructions and feedback information.

[0031] Preferably, in step three, the middle layer is responsible for the scheduling of the operation and control system, the master station program receives the instructions of the operation and control system, and then sends them to the external driver through the bus. The external driver receives the control instructions and controls the external axis; the servo control program receives the instructions of the operation and control system, and sends them to the FPGA through the AXI bus. The FPGA receives the instructions to control the internal axis.

[0032] Preferably, step four also includes transmitting the count value to the middle layer program and the FPGA core respectively, and they also use the moment as the starting moment to calculate the cycle end moment, and achieve the purpose of synchronization through repeated calibration of each cycle.

[0033] Preferably, in step 6, the stability detection requires that the master station program adds 1 to its own cycle count value at the initial stage of each cycle and writes it into the shared memory ocm4. The servo calculates the time difference between two cycle counts plus 1 and stores it in the circular array DIF[N]. Considering the time fluctuation and the existence of the allowable error, the time unit is 62.5us, that is, the time deviation is allowed to be within 62.5us.

[0034] The stability check passes when all values ​​in the ring array DIF[N] are equal.

[0035] Preferably, step six further includes writing the count value Time into ocm6, and the middle layer calculates the end time of the current cycle by obtaining the count value, and defines the end time of the cycle as MidEndTimer and the cycle time as CycleTime, then:

[0036] MidEndTimer=Time+CycleTime.

[0037] Beneficial technical effects of the present invention:

[0038] The present invention provides a method for coordinated control of external axis motors in a multi-core heterogeneous control-drive integrated body. The control-drive integrated body is connected to the external axis through a field bus, the mechanical motion parameters of the external axis are obtained, and the coordinate relationship between the robot and the external axis is established. The operation control system uniformly plans the internal axis and the external axis of the robot, generates an interpolation trajectory, and takes the master station program clock as a reference. The master station will obtain the global counter of the ARM core and the microsecond counter value of the FPGA core at the start of each cycle. The master station program, the middle layer program, the servo control program, and the FPGA program need to undergo layer-by-layer handshake and bus stability detection. After the handshake passes, the clock calibration link is entered. The master station program obtains the count value of the global counter of the ARM core at the start of the thread execution.

[0039] Step 7: The master station program triggers the FPGA to latch the clock counter value cnt at the start time of the master station by adding 1 to the cycle count value, and defines the count value of the FPGA at the end of the current cycle as FPGAEndCnt, and the cycle count digit CycleCnt, then:

[0040] FPGAEndCnt=cnt+CycleCnt. In the servo control program running and interrupt function of R core, it is triggered by FPGA and records the current count value ServoCnt through its own counter. The end count value of the current cycle is calculated as ServoEndCnt. Each part of the program calculates the Tus moment of the current cycle through the known end time of the cycle. The Tus moment of the master station program is defined as MasterT, the middle layer as MidT, the FPGA as FPGAT, and the servo control program as ServoT. Then:

[0041] MasterT = Time + T * global counter microsecond count;

[0042] MidT = Time + T * global counter microsecond count;

[0043] FPGAT = cnt + T * FPGA clock counter microsecond count;

[0044] ServoT = ServoCnt + T*R core counter microsecond count. When reaching the Tus moment of the current cycle, each part of the program first reads the instruction information and feedback information from the corresponding memory or register, and then executes the corresponding motion planning program and motor control driver. After the execution is completed, the corresponding control instructions and feedback information are written to the memory, bus or register. After the waiting cycle time ends, it jumps to clock calibration and cycles periodically to achieve synchronization of internal and external drive control instructions and feedback information. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 The timing diagram of a preferred embodiment of a method for coordinated control of internal and external axis motors in a multi-core heterogeneous control drive system according to the present invention.

[0046] Figure 2 The clock calibration relationship diagram is a preferred embodiment of a method for coordinated control of internal and external axis motors in a multi-core heterogeneous control drive system according to the present invention.

[0047] Figure 3 A control block diagram of a preferred embodiment of a method for coordinated control of internal and external shaft motors in a multi-core heterogeneous control drive system according to the present invention. DETAILED DESCRIPTION

[0048] In order to make the technical solution of the present invention more clear and specific to those skilled in the art, the present invention is further described in detail below in conjunction with embodiments and drawings, but the implementation manner of the present invention is not limited thereto.

[0049] like Figure 1-Figure 3As shown, this embodiment provides a method for coordinated control of external shaft motors in a multi-core heterogeneous control-drive integrated body, connecting the control-drive integrated body to an external shaft through a field bus.

[0050] Obtain the mechanical motion parameters of the external axis and establish the coordinate relationship between the robot and the external axis.

[0051] In this system, the operation and control system uniformly plans the internal and external axes of the robot and generates interpolation trajectories. The middle layer is responsible for the scheduling of the operation and control system. The master station program receives instructions from the operation and control system and then sends them to the external driver through the bus. The external driver receives the control instructions and controls the external axis. The servo control program receives instructions from the operation and control system and sends them to the FPGA through the AXI bus. The FPGA receives the instructions to control the internal axis.

[0052] This solution will use the master program clock as the benchmark. At the start of each cycle, the master will obtain the global counter of the arm core and the microsecond counter of the FPGA core, and pass the count value to the middle-layer program and FPGA core respectively. They will also use this moment as the start time to calculate the end time of the cycle, and achieve synchronization through repeated calibration of each cycle.

[0053] First, the master program, middle-layer program, servo control program, and FPGA program need to undergo layer-by-layer handshake and bus stability testing.

[0054] The stability test requires the master program to add 1 to its own cycle count value at the initial stage of each cycle and write it into the shared memory ocm4. The servo calculates the time difference between two cycle counts plus 1 and stores it in the ring array DIF[N]. Considering the time fluctuation and the existence of allowable error, the time unit is 62.5us, that is, the time deviation is allowed to be within 62.5us. When all values ​​in the ring array DIF[N] are equal, the stability test passes.

[0055] After the handshake is passed, the clock calibration phase begins. The master program obtains the count value of the ARM core global counter at the start time of the thread execution, and writes the count value Time into ocm6. The middle layer obtains the count value to calculate the end time of the current cycle, and defines the end time of the cycle as MidEndTimer and the cycle time as CycleTime. Then:

[0056] MidEndTimer=Time+CycleTime;

[0057] The master station program triggers the FPGA to latch the clock counter value cnt at the start time of the master station by adding 1 to the cycle count value, and defines the count value at the end of the current cycle of the FPGA as FPGAEndCnt, and the cycle count digit CycleCnt, then:

[0058] FPGAEndCnt=cnt+CycleCnt;

[0059] In the servo control program running and interrupt function of the R core, it is triggered by the FPGA and uses its own counter to record the current count value ServoCnt, and calculates the end count value of the current cycle as ServoEndCnt.

[0060] Each part of the program calculates the Tus moment of the current cycle through the known end time of the cycle, and defines the Tus moment of the master program as MasterT, the middle layer as MidT, the FPGA as FPGAT, and the servo control program as ServoT. Then:

[0061] MasterT = Time + T * global counter microsecond count;

[0062] MidT = Time + T * global counter microsecond count;

[0063] FPGAT = cnt + T * FPGA clock counter microsecond count;

[0064] ServoT = ServoCnt + T*R core counter microsecond count;

[0065] When the Tus moment of the current cycle is reached, each part of the program first reads the instruction information and feedback information from the corresponding memory or register, and then executes the corresponding motion planning program and motor control driver. After the execution is completed, the corresponding control instructions and feedback information are written to the memory, bus or register.

[0066] After the waiting cycle time is over, jump to clock calibration and cycle periodically to achieve synchronization between internal and external drive control instructions and feedback information.

[0067] The above description is only a further embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can make equivalent replacements or changes according to the technical solutions and concepts of the present invention within the scope disclosed by the present invention, which belong to the protection scope of the present invention.

Claims

1. A method for coordinated control of external shaft motors in a multi-core heterogeneous control-drive system, characterized in that: A control system including a control method for use with the control method; The control system includes an operation control system, which shares memory with a master station program and a servo control program; The master station program is connected to the external drive module through the industrial bus, and is connected to the external motor shaft through the external drive module; The servo control program is connected to the FPGA module through the AXI bus, and is connected to the internal axis of the robot through the FPGA module.

2. The method for coordinated control of external shaft motors in a multi-core heterogeneous control-drive system according to claim 1, characterized in that: The steps include: Step 1: Connect the control-drive system to the external axis via the fieldbus; Step 2: Obtain the mechanical motion parameters of the external axis and establish the coordinate relationship between the robot and the external axis; Step 3: The operation control system uniformly plans the internal and external axes of the robot and generates interpolation trajectories; Step 4: Based on the master program clock, the master will obtain the values ​​of the global counter of the arm core and the microsecond counter of the FPGA core at the beginning of each cycle; Step 5: Through the master program, middle layer program, servo control program, and FPGA program, the handshake and bus stability tests are performed layer by layer; Step 6: After the handshake is passed, the clock calibration phase begins. The master program obtains the count value of the ARM core global counter at the start time of the thread execution. Step 7: The master station program triggers the FPGA to latch the clock counter value cnt at the start time of the master station by adding 1 to the cycle count value, and defines the count value of the FPGA at the end of the current cycle as FPGAEndCnt, and the cycle count digit CycleCnt, then: FPGAEndCnt=cnt+CycleCnt; Step 8: In the servo control program running and interrupt function of the R core, the program is triggered by the FPGA and uses its own counter to record the current count value ServoCnt, and calculates the end count value of the current cycle as ServoEndCnt; Step 9: Each part of the program calculates the Tus moment of the current cycle through the known end time of the cycle, and defines the Tus moment of the master program as MasterT, the middle layer as MidT, the FPGA as FPGAT, and the servo control program as ServoT. Then: MasterT = Time + T * global counter microsecond count; MidT = Time + T * global counter microsecond count; FPGAT = cnt + T * FPGA clock counter microsecond count; ServoT = ServoCnt + T*R core counter microsecond count; Step 10: When reaching the Tus moment of the current cycle, each part of the program first reads the instruction information and feedback information from the corresponding memory or register, and then executes the corresponding motion planning program and motor control driver. After the execution is completed, the corresponding control instruction and feedback information are written to the memory, bus or register; Step 11: After waiting for the cycle time to end, jump to clock calibration and cycle periodically to achieve synchronization between internal and external drive control instructions and feedback information.

3. The method for coordinated control of external shaft motors in a multi-core heterogeneous control-drive system according to claim 2 is characterized in that: In step three, the middle layer is responsible for the scheduling of the operation and control system. The master station program receives instructions from the operation and control system, and then sends them to the external driver through the bus. The external driver receives the control instructions and controls the external axis. The servo control program receives instructions from the operation and control system, and sends them to the FPGA through the AXI bus. The FPGA receives the instructions to control the internal axis.

4. The method for coordinated control of external shaft motors in a multi-core heterogeneous control-drive system according to claim 3 is characterized in that: Step 4 also includes passing the count value to the middle layer program and FPGA core respectively. They also use this moment as the starting moment to calculate the end moment of the cycle, and achieve the purpose of synchronization through repeated calibration of each cycle.

5. The method for coordinated control of external shaft motors in a multi-core heterogeneous control-drive system according to claim 4 is characterized in that: In step 6, the stability test requires the master program to add 1 to its own cycle count value at the initial stage of each cycle and write it into the shared memory ocm4. The servo calculates the time difference between two cycle counts plus 1 and stores it in the circular array DIF[N]. Considering the time fluctuation and the existence of allowable error, the time unit is 62.5us, that is, the time deviation is allowed to be within 62.5us. The stability check passes when all values ​​in the ring array DIF[N] are equal.

6. The method for coordinated control of external shaft motors in a multi-core heterogeneous control-drive system according to claim 5, characterized in that: Step 6 also includes writing the count value Time into ocm6. The middle layer calculates the end time of the current cycle by obtaining the count value, defines the end time of the cycle as MidEndTimer, and the cycle time as CycleTime, then: MidEndTimer=Time+CycleTime.

Images