CN114603563A

CN114603563A - Multi-screw rod motor bus type embedded control system of continuum mechanical arm

Info

Publication number: CN114603563A
Application number: CN202210439889.8A
Authority: CN
Inventors: 隆晨海; 李志广; 郭健; 李金亮; 杨淼; 陈思铭; 皮奥; 朱吉然; 邹妍晖; 刘蛟蛟
Original assignee: State Grid Corp of China SGCC; Electric Power Research Institute of State Grid Hunan Electric Power Co Ltd; State Grid Hunan Electric Power Co Ltd
Current assignee: State Grid Corp of China SGCC; Electric Power Research Institute of State Grid Hunan Electric Power Co Ltd; State Grid Hunan Electric Power Co Ltd
Priority date: 2022-04-25
Filing date: 2022-04-25
Publication date: 2022-06-10

Abstract

The invention discloses a multi-screw motor bus type embedded control system of a continuum mechanical arm, which comprises an upper computer, a control module and a plurality of motor driving modules, wherein the upper computer is in communication connection with the control module through an Ethernet, and the control module is in communication connection with the plurality of motor driving modules through a CAN bus; the upper computer sends the standard position and the standard speed of the motor to the control module through the Ethernet, the control module outputs a control instruction to the motor driving module according to the standard position and the standard speed, the motor driving module drives the motor to run according to the control instruction, and sends the real-time position and the real-time rotating speed of the motor to the control module so as to carry out double closed-loop control. The invention has the advantages of simple structure, small size, high control precision, quick response and the like.

Description

Multi-screw rod motor bus type embedded control system of continuum mechanical arm

Technical Field

The invention mainly relates to the technical field of mechanical arm motor control, in particular to a multi-lead-bar motor bus type embedded control system of a continuous mechanical arm.

Background

In the field of continuum mechanical arms, the mechanical arm needs to complete a series of tasks such as traveling, reaching a target position, grabbing or shearing. The tasks can be completed without a motor driving system, and the motor driving system has higher requirements on the size, the power consumption, the control precision and the like of the system. The line-driven continuum mechanical arm generally controls the motion of each section of mechanical arm by 3 lines or 4 lines, the motion control of a plurality of sections of mechanical arms needs a plurality of motors to drive the continuum mechanical arm at the same time, the requirement on the synchronism of the drive of the plurality of motors is high, and the volume of each driver is not too large due to the large number of the motors.

The existing motor control system of the continuum mechanical arm mostly adopts a single motor driver with larger volume and power, and because each section of the continuum mechanical arm needs a plurality of motors for driving, the large-volume motor driver inevitably causes the driving system to be too large; in addition, the existing continuum mechanical arm motor control system mostly adopts an I/O port of a microcontroller or a PWM wave mode to send a control signal to a driver, and has the defects of poor multi-motor drive synchronism, slow response speed, poor anti-jamming capability and the like.

Disclosure of Invention

The technical problem to be solved by the invention is as follows: aiming at the technical problems in the prior art, the invention provides a multi-lead-bar motor bus type embedded control system of a continuous mechanical arm, which has a simple structure and high control precision.

In order to solve the technical problems, the technical scheme provided by the invention is as follows:

a multi-screw rod motor bus type embedded control system of a continuum mechanical arm comprises an upper computer, a control module and a plurality of motor driving modules, wherein the upper computer is in communication connection with the control module through an Ethernet, and the control module is in communication connection with the plurality of motor driving modules through a CAN bus; the upper computer sends the standard position and the standard speed of the motor to the control module through the Ethernet, the control module outputs a control instruction to the motor driving module according to the standard position and the standard speed, the motor driving module drives the motor to run according to the control instruction, and sends the real-time position and the real-time rotating speed of the motor to the control module so as to carry out double closed-loop control.

Preferably, the control module includes a microcontroller U1, a minimum system circuit, a USB interface, a UART interface, a TF card interface, two CAN interfaces, an ETHERNET interface, and an RS422 interface, which are respectively connected to the microcontroller U1.

Preferably, the microcontroller U1 includes an STM32H743VIT6,

pins

8 and 9 of the microcontroller U1 are used as external crystal oscillator access terminals OSC32_ IN and OSC32_ OUT of an RTC clock, and

pins

12 and 13 are used as external crystal oscillator access terminals OSC _ IN and OSC _ OUT of a master clock;

pins

72 and 76 are used as download debugging signals JTMS _ SWDIO and JTCK _ SWCLK respectively;

pins

70 and 71 are used as USB driving signals USB _ D-and USB _ D + respectively;

pins

81 and 82 are used as the transceiving signals UART4_ RX and UART4_ TX of the serial port; pins 37 and 38 are used as the transceiving signals UART7_ RX and UART7_ TX of the serial port;

pins

65, 66, 78, 79, 80 and 83 are used as SDIO drive signals SDIO _ D0, SDIO _ D1, SDIO _ D2, SDIO _ D3, SDIO _ SCK and SDIO _ CMD of the TF card; pins 95 and 96 are used as transmitting and receiving signals CAN1_ RX and CAN1_ TX of CAN transceiver 1 #;

pins

91 and 92 are used as the transmitting and receiving signals CAN2_ RX and CAN2_ TX of CAN transceiver 2 #;

pins

15, 16, 23, 24, 31, 32, 33, 47, 51, 52 each serve as RMII interface signals ETH _ RESET, ETH _ MDC, RMII _ REF _ CLK, ETH _ MDIO, RMII _ CRS _ DV, RMII _ RXD0, RMII _ RXD1, RMII _ TX _ EN, RMII _ TXD0, RMII _ TXD1 for LAN8720A to communicate with the microcontroller;

pins

11, 27, 50, 75 and 100 are all connected with a direct current power supply VCC 3.3;

pins

10, 26, 49, 74 and 99 are all connected with GND;

pins

48 and 73 are connected to GND through a 2.2uF capacitor C27 and a C28 respectively.

Preferably, the minimum system circuit is used for providing the microcontroller with power supply, clock, reset and download debugging interfaces necessary for the normal operation of the microcontroller; an external power supply of the microcontroller is VCC5, and a voltage conversion chip AMS1117-3.3 is adopted to convert VCC5 into VCC 3.3; a VBAT pin of the microcontroller U1 is used for supplying power to a standby area thereof, and the mode of hybrid power supply of an external power supply VCC and a button cell CR1220 is adopted; when an external power supply VCC3.3 is available, CR1220 does not supply power to VBAT; when the external power supply is off, power is supplied by the CR1220, ensuring that VBAT is always powered to prevent the register contents from being lost.

Preferably, the microcontroller further comprises a crystal oscillator circuit for providing a microcontroller operating clock, the crystal oscillator circuit comprises two groups of external crystal oscillators for respectively providing clocks for a main clock and an RTC, the crystal oscillator Y1 of the main clock is connected between OSC _ IN and OSC _ OUT pins of U1, two ends of the crystal oscillator are grounded through a capacitor, and a resistor R5 is connected IN parallel; the crystal oscillator Y2 of the RTC clock is connected between the OSC32_ IN and the OSC32_ OUT pins of the U1, and two ends of the crystal oscillator are grounded through a capacitor.

Preferably, the system further comprises a download debugging circuit for downloading the running program of the driving motor, wherein the download debugging circuit adopts an SWD mode, a plug-in P1 is led out to serve as a download interface, a pin 1 of P1 is connected with GND, a pin 4 is connected with VCC3.3,

pins

2 and 3 are respectively connected with download debugging signals JTCK _ SWCLK and JTMS _ SWDIO of U1, and a pin 5 is connected with a RESET pin of U1; the RESET pin of U1 RESETs when it is low level, the design of RESET circuit follows the low level RESET principle, when the RESET button is not pressed, the DC power supply VCC3.3 is connected to the RESET pin through the resistor R1, making it keep high level; when the RESET button is pressed, the RESET pin is short-circuited with the power ground, and becomes a low level, and the microcontroller RESETs.

Preferably, the USB interface is connected to an upper computer, and is used to provide power for the microcontroller and update the firmware; the USB interface adopts a Micro USB interface, a pin D and a pin 3D + of the Micro USB interface are respectively connected with USB driving signals USB _ D and USB _ D + of a microcontroller U1 through resistors R2 and R3, pins 5 to 8 are all connected with GND, and a pin 1 VBUS of the Micro USB interface leads out a direct current power supply VCC 5;

the UART interface is used as a standby communication interface, an external plug-in P2 is led out, a pin 1 of the external plug-in P2 is connected with GND, and

pins

2 and 3 of the external plug-in P are respectively connected with a transceiving signal UART4_ RX and a UART4_ TX of a serial port 4# of a microcontroller U1;

the TF card interface is used for storing the position and speed information of the motor and the CAN communication address of each CAN bus interface stepping motor driver; the 7 pin, the 8 pin, the 1 pin, the 2 pin and the 3 pin of the TF card slot are respectively connected with SDIO drive signals SDIO _ D0, SDIO _ D1, SDIO _ D2, SDIO _ D3 and SDIO _ CMD of the microcontroller U1, and are respectively connected with pull-up resistors R10, R11, R7, R8 and R9, 5 pins of the TF card slot are connected with SDIO _ SCK of the U1, and 4 pins and 6 pins are respectively connected with VCC3.3 and GND.

Preferably, the number of the CAN interfaces is two, and the two CAN interfaces have the same circuit; each CAN interface comprises a digital isolation device U3, a CAN transceiver U4, and a power isolation device U5; the digital isolator U3 is used for protecting CAN transceiving signals from interference,

pins

2 and 3 of the digital isolator U3 are respectively connected with CAN transceiving signals CAN1_ RX and CAN1_ TX of the microcontroller U1,

pins

1 and 4 and pins 8 and pin 5 of the digital isolator U are respectively connected with two groups of different power supplies VCC3.3, GND and VO and GND2, and

pins

6 and 7 of the digital isolator U3 are connected with

pins

1 and 4 of the CAN transceiver U4; the CAN transceiver U4 is used for converting CAN control data of the microcontroller into an electric signal and sending the electric signal through a bus, receiving bus data and sending the data back to the microcontroller, wherein

pins

1 and 4 of the CAN transceiver U4 are connected with a send-receive signal isolated from U3,

pins

7 and 6 of the CAN transceiver U are led out to be CAN _ H and CAN _ L serving as CAN interfaces, resistors are connected between the

pins

7 and 6 of the CAN transceiver U and connected with GND2 and VO respectively, and pins 8 of the CAN transceiver U13 are connected with a pull-down resistor R13; the power supply isolation device U5 adopts a low-power DC/DC power supply isolation device for obtaining two groups of power supplies which are isolated from each other and used by the digital isolator U3, the input end of the power supply isolation device is connected with VCC3.3 and GND, and then a group of isolated power supplies VO and GND2 are output.

Preferably, the motor driving module comprises a plurality of CAN bus type drivers and a plurality of screw rod motors, and the CAN bus type drivers correspond to the screw rod motors one by one; the CAN bus type driver is used for analyzing the control instruction sent by the CAN bus and converting the control instruction into a corresponding control pulse to drive the screw rod motor.

Preferably, the CAN bus type driver is installed at the bottom of the lead screw motor.

Compared with the prior art, the invention has the advantages that:

according to the multi-screw motor bus type embedded control system, the control module receives the standard position and the standard speed of the motor sent by the upper computer through the Ethernet and receives the real-time position and the real-time speed of the motor, outputs an accurate control instruction to the motor driving module, performs double closed-loop feedback control on the speed and the position of the motor to realize accurate control of the motor and ensures that the motor reaches the expected position and speed; the invention adopts the CAN bus to realize the communication between the control module and the motor driving module, CAN simultaneously drive a plurality of motor driving modules, CAN ensure the control synchronism of a plurality of motors and improve the efficiency of controlling the motors.

Drawings

Fig. 1 is a block diagram of an embodiment of a multi-wire rod motor bus embedded control system according to the present invention.

Fig. 2 is a schematic diagram of a communication structure of a control module and a motor driving module according to an embodiment of the present invention.

Fig. 3 is a schematic structural diagram of a control module according to an embodiment of the present invention.

FIG. 4 is a block diagram of a dual closed loop control of motor position and speed in an embodiment of the present invention.

Fig. 5 is a schematic circuit diagram of a microcontroller according to an embodiment of the present invention.

Fig. 6 is a circuit schematic of a minimum system of microcontrollers in accordance with an embodiment of the invention.

FIG. 7 is a schematic circuit diagram of an embodiment of a USB interface according to the present invention.

FIG. 8 is a schematic circuit diagram of a UART interface according to an embodiment of the present invention.

FIG. 9 is a circuit diagram of an embodiment of the TF card interface of the present invention.

Fig. 10 is a schematic circuit diagram of a CAN interface according to an embodiment of the present invention.

Fig. 11 is a schematic circuit diagram of an ETHERNET interface according to an embodiment of the present invention.

Fig. 12 is a schematic circuit diagram of an RS422 interface according to an embodiment of the present invention.

Fig. 13 is a three-dimensional view of the structure of each set of the driver and the motor in the embodiment of the invention.

Detailed Description

The invention is further described below with reference to the figures and the specific embodiments of the description.

As shown in fig. 1, an embodiment of the present invention provides a multi-lead motor bus type embedded control system, which includes a three-layer architecture including an upper computer, a control module and a motor driving module, wherein the upper computer and the control module are in communication connection by using ETHERNET, and the control module and the motor driving module are in communication connection by using a CAN bus; the upper computer belongs to a man-machine interaction layer, the control module belongs to a control layer, the motor driving module belongs to an execution layer, and a three-level structure can guarantee safe and reliable operation of the system; the upper computer sends the standard position and the standard speed of the motor to the control module through the Ethernet, the control module outputs a control instruction to the motor driving module according to the standard position and the standard speed, the motor driving module drives the motor to run according to the control instruction, and sends the real-time position and the real-time rotating speed of the motor to the control module so as to carry out double closed-loop control.

In a specific embodiment, the main tasks completed by the upper computer are human-computer interaction, writing of a control program and sending of the control task to the control module. And completing a human-computer interaction function, namely completing the generation of a user graphical interface based on Qt, simultaneously completing an input function by utilizing a keyboard and a mouse externally connected with an upper computer, and displaying the target position and speed of the motor and the current position and speed information of the motor by using a display. The upper computer is provided with KEIL or IAR software for compiling, compiling and downloading the control program to the microcontroller. After the user inputs the expected position and speed of the motor, the upper computer sends the position and speed control task of the motor to the control module through an Ethernet based on a TCP/IP protocol.

In a specific embodiment, the control module mainly sends a control instruction to each motor driving module through the CAN bus according to the control task of the upper computer on the position and the speed of each motor. As shown in fig. 3, the control module includes a microcontroller U1, a minimum system circuit, a USB interface, a UART interface, a TF card interface, two CAN interfaces, an ETHERNET interface, and an RS422 interface, which are respectively connected to the microcontroller U1. The microcontroller U1 adopts STM32H743VIT6, and has the function of performing double closed-loop control on the position and the speed of the motor according to the expected position and the speed information of the motor sent by the upper computer and the current position and the speed information of the motor fed back by the motor driving module, the position loop adopts an absolute PID controller, the speed loop adopts an incremental PID controller, and a control block diagram is shown in FIG. 4.

IN order to realize the above functions, the microcontroller needs to rely on many peripheral interface circuits, as shown IN fig. 5,

pins

8 and 9 of the microcontroller U1 are used as external crystal oscillator access terminals OSC32_ IN and OSC32_ OUT of the RTC clock, and

pins

12 and 13 are used as external crystal oscillator access terminals OSC _ IN and OSC _ OUT of the master clock;

pins

72 and 76 of U1 are used as download debug signals JTMS _ SWDIO and JTCK _ SWCLK respectively;

pins

70 and 71 of U1 are used as USB driving signals USB _ D-and USB _ D +;

pins

81 and 82 of the U1 are used as the transceiving signals UART4_ RX and UART4_ TX of the serial port 4, and pins 37 and 38 are used as the transceiving signals UART7_ RX and UART7_ TX of the serial port 7;

pins

65, 66, 78, 79, 80, 83 of U1 are used as SDIO drive signals SDIO _ D0, SDIO _ D1, SDIO _ D2, SDIO _ D3, SDIO _ SCK, SDIO _ CMD of the TF card; pins 95 and 96 of U1 are used as transmitting and receiving signals CAN1_ RX and CAN1_ TX of CAN transceiver 1, and

pins

91 and 92 are used as transmitting and receiving signals CAN2_ RX and CAN2_ TX of CAN transceiver 2;

pins

15, 16, 23, 24, 31, 32, 33, 47, 51, 52 of U1 communicate with the microcontroller as, respectively, LAN8720A RMII interface signals ETH _ RESET, ETH _ MDC, RMII _ REF _ CLK, ETH _ MDIO, RMII _ CRS _ DV, RMII _ RXD0, RMII _ RXD1, RMII _ TX _ EN, RMII _ TXD0, RMII _ TXD 1;

pins

11, 27, 50, 75 and 100 of U1 are all connected with a direct current power supply VCC3.3,

pins

10, 26, 49, 74 and 99 are all connected with GND, and

pins

48 and 73 are connected with GND through a 2.2uF capacitor C27 and C28 respectively.

In one embodiment, the minimal system circuitry is primarily used to provide the microcontroller with the power, clock, reset and download debug interfaces necessary for its normal operation. As shown in fig. 6, the external power supply of the microcontroller is VCC5 (VBUS from USB interface), and VCC5 can be converted into VCC3.3 by using the voltage conversion chip AMS1117-3.3 (i.e., U2); the VBAT pin of the microcontroller U1 is used for supplying power to a backup area thereof, a mode of hybrid power supply of an external power supply VCC and a button cell CR1220 is adopted, when the external power supply VCC3.3 exists, the CR1220 does not supply power to the VBAT, and when the external power supply is disconnected, the CR1220 supplies power to the VBAT, so that the VBAT is always powered, and the content of the register is prevented from being lost. The crystal oscillator circuit is used for providing a microcontroller working clock and comprises two groups of external crystal oscillators which respectively provide clocks for a main clock and an RTC, wherein the crystal oscillator Y1 of the main clock is 25MHz and is connected between OSC _ IN and OSC _ OUT pins of U1, two ends of the crystal oscillator are grounded through a capacitor, and a 1M omega resistor R5 is connected IN parallel; the crystal oscillator Y2 of the RTC clock is 32.768KHz and is connected between the OSC32_ IN pin and the OSC32_ OUT pin of the U1, and two ends of the crystal oscillator are grounded through a capacitor. The download debugging circuit is used for downloading programs for driving a motor to run, a plug-in P1 is led out to serve as a download interface in an SWD mode, a pin 1 of a P1 is connected with GND, a pin 4 is connected with VCC3.3,

pins

2 and 3 are respectively connected with download debugging signals JTCK _ SWCLK and JTMS _ SWDIO of U1, and a pin 5 is connected with a RESET pin of U1. The RESET pin of the U1 is RESET when being at low level, so the design of the RESET circuit follows the principle of low level RESET, when the RESET button is not pressed, the DC power supply VCC3.3 is connected to the RESET pin through the resistor R1, so that the RESET pin keeps at high level; when the RESET button is pressed, the RESET pin is shorted to ground, and becomes low, and the microcontroller RESETs. The minimum system circuit structure is simple and easy to realize.

In a specific embodiment, the USB interface is connected to an upper computer for supplying power to the microcontroller and updating firmware. As shown in fig. 7, a Micro USB interface is selected, a 2 pin D-3 pin D + of the Micro USB interface is connected with a USB drive signal USB _ D-, USB _ D + of a microcontroller U1 through resistors R2 and R3, 5-8 pins of the Micro USB interface are all connected with GND, and a1 pin VBUS of the Micro USB interface leads out a dc power source VCC 5. The USB interface is simple in structure and easy to realize.

The UART interface serves as a backup communication interface. As shown in fig. 8, an external plug P2 is led out, pin 1 of which is connected to GND, and

pins

2 and 3 of which are connected to the UART4_ RX and UART4_ TX of the serial port 4 of the microcontroller U1, respectively. The UART interface is simple in structure and easy to realize.

The TF card interface is used for storing the position and speed information of the motor, CAN communication addresses of the CAN bus interface stepping motor drivers and the like. As shown in fig. 9,

pins

7, 8, 1, 2, and 3 of the TF card slot are respectively connected to SDIO drive signals SDIO _ D0, SDIO _ D1, SDIO _ D2, SDIO _ D3, SDIO _ CMD of the microcontroller U1, and are respectively connected to pull-up resistors R10, R11, R7, R8, and R9, pins 5 of which are connected to SDIO _ SCK of the U1, and

pins

4 and 6 of which are respectively connected to VCC3.3 and GND. The TF card interface is simple in structure and easy to realize.

The CAN interfaces are two in number, the circuits of the CAN interfaces are the same, and the CAN interfaces are used for the microcontroller to send a control instruction to the motor driving module and the motor driving module to feed back the current position and speed information of the motor to the microcontroller. As shown in fig. 10, each CAN interface mainly includes a digital isolation device U3, a CAN transceiver U4, and a power isolation device U5. The digital isolator U3 selects ADUM1201 to protect CAN transceiving signals from interference, pins 2 and 3 of the digital isolator are respectively connected with CAN transceiving signals CAN1_ RX and CAN1_ TX of the microcontroller U1, pins 1, 4, 8 and 5 of the digital isolator are respectively connected with two groups of different power supplies VCC3.3, GND and VO and GND2, and pins 6 and 7 of the digital isolator are connected with

pins

1 and 4 of the CAN transceiver U4. The CAN transceiver U4 selects TJA1050 to convert CAN control data of the microcontroller into electric signals and send the electric signals through a bus, and also receives bus data and sends the data back to the microcontroller, pins 1 and 4 of the CAN transceiver U4 are connected with transmitting and receiving signals isolated by U3, pins 7 and 6 of the CAN transceiver U are led out to form CAN _ H and CAN _ L which are CAN interfaces, 120 omega resistors are connected between the CAN _ H and CAN _ L in parallel, pins 2 and 3 of the CAN transceiver U4 are connected with GND2 and VO respectively, and pin 8 of the CAN transceiver U4 is connected with pull-down resistor R13. The power supply isolation device U5 adopts a low-power DC/DC power supply isolation device for obtaining two groups of power supplies which are isolated from each other and used by the digital isolator U3, the input end of the power supply isolation device is connected with VCC3.3 and GND, and then a group of isolated power supplies VO and GND2 are output. The CAN interface is simple in structure and easy to realize.

The ETHERNET interface is used for communication between the microcontroller and the upper computer, and is used for the upper computer to send expected position and speed information of the motor to the microcontroller and for the microcontroller to feed back the current position and speed of the motor to the upper computer. As shown in fig. 11, it mainly includes a PHY chip U6, an RJ45 header, and an inductor L1. PHY chip U6 employs LAN8720, communication with microcontroller U1 employs RMII interface, 12 pin of which is connected to ETH _ MDIO of microcontroller U1 via resistor R14, 13, 17, 18, 16, 8, 7, 11, 14, 15 pin of which is connected to ETH _ MDC, RMII _ TXD0, RMII _ TXD1, RMII _ TX _ EN, RMII _ RXD0, RMII _ RXD1, RMII _ CRS _ CLK, and ETH _ RESET of U1, 24 pin of which is connected to pull-down resistor R15, 21, 20, 23, 22 pin of which is used as signal TPTX +, TPTX-, TPRX +, TPRX-and is connected to 49.9 pull-up resistor R16-R19, 3 pin of which is used as signal of RJ 6342, and pin of which is connected to VCC _

LED

3827, 27, 3, 27, 3, 9, 3, 9, 3, 9. The RJ45 head is an ethernet port of the control module, and is of a model HR91105A, pins 1, 3, 4 and 6 of the RJ45 head are respectively connected with signals TPTX +, TPTX-, TPRX + and TPRX-from U6, and are respectively connected to GND by connecting a capacitor C20-C23 in series, pins 8 and 9 of the RJ45 head are respectively connected with signals LINK _ LED and SPEED _ LED from U6, pins 10 and 11 of the RJ45 head are respectively connected with pull-down resistors R24, R23, and pins 12-14 of the RJ are all grounded. Inductor L1 is used to convert power supply VCC3.3 to VCC3.3E. The ETHERNET interface is simple in structure and easy to realize.

The RS422 interface is used as a communication backup interface, and mainly comprises an RS422 transceiver U7 and a DB9 interface P5 as shown in fig. 12. The RS422 transceiver U7 selects MAX3490 for converting the transceiving signals of the serial port into the transceiving signals of RS422, pins 2 and 3 of the RS422 transceiver U7 are connected with serial port 7# transceiving signals UART7_ RX and UART7_ TX from the microcontroller U1, pins 1 and 4 of the RS transceiver U1 are respectively connected with VCC3.3 and GND, pins 5 to 8 of the RS transceiver U1 correspond to RS422_ RX +, RS422_ RX-, RS422_ TX + and RS422_ TX, and then are respectively connected with

pins

3, 4, 2 and 1 of a DB9 interface P5, and pin 5 of P5 is connected with GND; pins 2, 3, 5 and 8 of the U7 are also respectively connected with pull-up resistors R26-R28, pins 6 and 7 are connected with pull-down resistors R29 and R30, a 120 omega resistor R31 is connected between the pin 7 and the pin 8 in parallel, and a TVS tube D1-D4 is respectively connected between the pin 5-8 and GND in series. The RS422 interface is simple in structure and easy to realize.

In a specific embodiment, the motor driving module mainly has the task of driving the motor to run according to a control instruction of the control module and feeding back position and speed information of the motor to the control module, and specifically comprises a plurality of CAN bus interface stepping motor drivers (CAN bus type drivers 1) and a plurality of lead screw motors. Each motor corresponds to one driver (taking two-segment continuum mechanical arm as an example, 8 drivers and 8 motors are needed), and the drivers are installed at the bottom of the motors, so that the space of the whole driving device can be greatly saved, as shown in fig. 13.

As shown in fig. 13, the CAN bus driver 1 is configured to analyze a control command sent by a CAN bus, and convert the control command into a corresponding control pulse to drive the screw motor; the motor is 7TCSM4210Q, has an S-shaped acceleration and deceleration curve, can ensure the running stability of the motor, supports four working modes of a positioning mode, a forward and reverse rotation mode, a speed mode and an in-place mode, and can complete different task requirements; the size of the motor driving module is only 42.2mm multiplied by 14.5mm, and the motor driving module is arranged on the bottom surface of the screw rod motor through screws, so that the whole volume of the motor driving module is greatly reduced; the device can be subdivided by 32 at most, and the control precision can be greatly improved; by adopting a CAN2.0A protocol, a plurality of drivers can perform networking to perform synchronous driving.

As shown in fig. 13, the lead screw motor is composed of a motor 2, a lead screw 3 and a nut 4. The motor 2 adopts a 42 series motor, and the stepping angle of the motor is 1.8 degrees; the length of lead screw is 350mm, and lead 2mm, motor 2 rotates a week lead screw nut 4 and only removes 2mm promptly, and the highest 32 subdivision of cooperation driver 1 can reach very high precision. The driving ropes 5 of the continuum mechanical arm are fixedly connected to the nuts 4, the motor 2 drives the nuts 4 to move on the screw rod 3, the driving ropes 5 are further contracted or stretched, and every four driving ropes 5 control the motion of one section of mechanical arm. Taking two-segment continuum mechanical arm as an example, eight drivers and eight lead screw motors are needed in total, and the plurality of lead screw motors are synchronously controlled through a CAN bus.

The above is only a preferred embodiment of the present invention, and the protection scope of the present invention is not limited to the above-mentioned embodiments, and all technical solutions belonging to the idea of the present invention belong to the protection scope of the present invention. It should be noted that modifications and embellishments within the scope of the invention may be made by those skilled in the art without departing from the principle of the invention.

Claims

1. The multi-screw motor bus type embedded control system of the continuum mechanical arm is characterized by comprising an upper computer, a control module and a plurality of motor driving modules, wherein the upper computer is in communication connection with the control module through an Ethernet, and the control module is in communication connection with the plurality of motor driving modules through a CAN bus; the upper computer sends the standard position and the standard speed of the motor to the control module through the Ethernet, the control module outputs a control instruction to the motor driving module according to the standard position and the standard speed, the motor driving module drives the motor to run according to the control instruction, and sends the real-time position and the real-time rotating speed of the motor to the control module so as to carry out double closed-loop control.

2. The multi-wire motor bus embedded control system of the continuum robot arm of claim 1, wherein the control module comprises a microcontroller U1, minimum system circuits respectively connected to the microcontroller U1, a USB interface, a UART interface, a TF card interface, two CAN interfaces, an ETHERNET interface, and an RS422 interface.

3. The multi-wire rod motor bus embedded control system of the continuum robot arm of claim 2, wherein the microcontroller U1 comprises STM32H743VIT6, pin 8 and pin 9 of the microcontroller U1 are used as external crystal oscillator access terminals OSC32_ IN and OSC32_ OUT of RTC clock, and pin 12 and pin 13 are used as external crystal oscillator access terminals OSC _ IN and OSC _ OUT of master clock; pins 72 and 76 are used as download debugging signals JTMS _ SWDIO and JTCK _ SWCLK respectively; pins 70 and 71 are used as USB driving signals USB _ D-and USB _ D + respectively; pins 81 and 82 are used as the transceiving signals UART4_ RX and UART4_ TX of the serial port; pins 37 and 38 are used as the transceiving signals UART7_ RX and UART7_ TX of the serial port; pins 65, 66, 78, 79, 80 and 83 are used as SDIO drive signals SDIO _ D0, SDIO _ D1, SDIO _ D2, SDIO _ D3, SDIO _ SCK and SDIO _ CMD of the TF card; pins 95 and 96 are used as transmitting and receiving signals CAN1_ RX and CAN1_ TX of CAN transceiver 1 #; pins 91 and 92 are used as the transmitting and receiving signals CAN2_ RX and CAN2_ TX of CAN transceiver 2 #; pins 15, 16, 23, 24, 31, 32, 33, 47, 51, 52 each serve as RMII interface signals ETH _ RESET, ETH _ MDC, RMII _ REF _ CLK, ETH _ MDIO, RMII _ CRS _ DV, RMII _ RXD0, RMII _ RXD1, RMII _ TX _ EN, RMII _ TXD0, RMII _ TXD1 for LAN8720A to communicate with the microcontroller; pins 11, 27, 50, 75 and 100 are all connected with a direct current power supply VCC 3.3; pins 10, 26, 49, 74 and 99 are all connected with GND; pins 48 and 73 are connected to GND through a 2.2uF capacitor C27 and C28 respectively.

4. The multi-wire rod motor bus embedded control system of the continuum robot arm of claim 3, wherein the minimal system circuitry is configured to provide the microcontroller with power, clock, reset and download debug interfaces necessary for its normal operation; an external power supply of the microcontroller is VCC5, and a voltage conversion chip AMS1117-3.3 is adopted to convert VCC5 into VCC 3.3; a VBAT pin of the microcontroller U1 is used for supplying power to a standby area thereof, and the mode of hybrid power supply of an external power supply VCC and a button cell CR1220 is adopted; when an external power supply VCC3.3 is available, CR1220 does not supply power to VBAT; when the external power supply is turned off, power is supplied by the CR1220, ensuring that VBAT is always powered to prevent register contents from being lost.

5. The multi-wire rod motor bus embedded control system of the continuum robot arm of claim 4, further comprising a crystal oscillator circuit for providing a microcontroller operating clock, the crystal oscillator circuit comprising two sets of external crystal oscillators for respectively providing clocks to a main clock and an RTC, the crystal oscillator Y1 of the main clock is connected between OSC _ IN and OSC _ OUT pins of U1, and both ends of the crystal oscillator are grounded through a capacitor, and a resistor R5 is connected IN parallel; the crystal oscillator Y2 of the RTC clock is connected between the OSC32_ IN and the OSC32_ OUT pins of the U1, and two ends of the crystal oscillator are grounded through a capacitor.

6. The multi-screw motor bus embedded control system of the continuum robot arm according to claim 5, further comprising a download debug circuit for downloading the running program of the driving motor, wherein the download debug circuit adopts SWD mode, and uses a leading-out plug P1 as a download interface, pin 1 of P1 is connected to GND, pin 4 is connected to VCC3.3, pins 2 and 3 are respectively connected to download debug signals JTCK _ SWCLK and JTMS _ SWDIO of U1, and pin 5 is connected to RESET pin of U1; the RESET pin of U1 RESETs when it is low level, the design of RESET circuit follows the low level RESET principle, when the RESET button is not pressed, the DC power supply VCC3.3 is connected to the RESET pin through the resistor R1, making it keep high level; when the RESET button is pressed, the RESET pin is short-circuited with the power ground, and becomes a low level, and the microcontroller RESETs.

7. The multi-screw motor bus type embedded control system of the continuum mechanical arm according to any one of claims 3 to 6, wherein the USB interface is connected with an upper computer and is used for supplying power to the microcontroller and updating firmware; the USB interface adopts a Micro USB interface, a pin D and a pin 3D + of the Micro USB interface are respectively connected with USB driving signals USB _ D and USB _ D + of a microcontroller U1 through resistors R2 and R3, pins 5 to 8 are all connected with GND, and a pin 1 VBUS of the Micro USB interface leads out a direct current power supply VCC 5;

the UART interface is used as a standby communication interface, an external plug-in P2 is led out, a pin 1 of the external plug-in P2 is connected with GND, and pins 2 and 3 of the external plug-in P are respectively connected with a transceiving signal UART4_ RX and a UART4_ TX of a serial port 4# of a microcontroller U1;

8. The multi-screw motor bus embedded control system of the continuum mechanical arm according to any one of claims 3 to 6, wherein the number of the CAN interfaces is two, and the two CAN interface circuits are the same; each CAN interface comprises a digital isolation device U3, a CAN transceiver U4, and a power isolation device U5; the digital isolator U3 is used for protecting CAN transceiving signals from interference, pins 2 and 3 of the digital isolator U3 are respectively connected with CAN transceiving signals CAN1_ RX and CAN1_ TX of the microcontroller U1, pins 1 and 4 and pins 8 and pin 5 of the digital isolator U are respectively connected with two groups of different power supplies VCC3.3, GND and VO and GND2, and pins 6 and 7 of the digital isolator U3 are connected with pins 1 and 4 of the CAN transceiver U4; the CAN transceiver U4 is used for converting CAN control data of the microcontroller into an electric signal and sending the electric signal through a bus, receiving bus data and sending the data back to the microcontroller, wherein pins 1 and 4 of the CAN transceiver U4 are connected with a send-receive signal isolated from U3, pins 7 and 6 of the CAN transceiver U are led out to be CAN _ H and CAN _ L serving as CAN interfaces, resistors are connected between the pins 7 and 6 of the CAN transceiver U and connected with GND2 and VO respectively, and pins 8 of the CAN transceiver U13 are connected with a pull-down resistor R13; the power supply isolation device U5 adopts a low-power DC/DC power supply isolation device for obtaining two groups of mutually isolated power supplies for the digital isolator U3 to use, the input end of the power supply isolation device is connected with VCC3.3 and GND, and then a group of isolated power supplies VO and GND2 are output.

9. The multi-screw motor bus embedded control system of the continuum mechanical arm according to any one of claims 3 to 6, wherein the motor driving module comprises a plurality of CAN bus drivers and a plurality of screw motors, and the CAN bus drivers correspond to the screw motors one by one; the CAN bus type driver is used for analyzing the control instruction sent by the CAN bus and converting the control instruction into a corresponding control pulse to drive the screw rod motor.

10. The multi-lead motor bus embedded control system of a continuum robot arm of claim 9, wherein the CAN bus driver is mounted to the bottom of the lead screw motor.