CN117601152B - Mechanical arm driving control system and method based on closed-loop stepping motor - Google Patents
Mechanical arm driving control system and method based on closed-loop stepping motor Download PDFInfo
- Publication number
- CN117601152B CN117601152B CN202311672619.2A CN202311672619A CN117601152B CN 117601152 B CN117601152 B CN 117601152B CN 202311672619 A CN202311672619 A CN 202311672619A CN 117601152 B CN117601152 B CN 117601152B
- Authority
- CN
- China
- Prior art keywords
- loop
- closed
- microcontroller
- stepping motor
- motor
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 23
- 230000001133 acceleration Effects 0.000 claims description 10
- 230000008859 change Effects 0.000 claims description 10
- 238000004364 calculation method Methods 0.000 claims description 6
- 238000004458 analytical method Methods 0.000 claims description 3
- 230000003993 interaction Effects 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 13
- 238000013461 design Methods 0.000 description 11
- 238000004891 communication Methods 0.000 description 10
- 230000006870 function Effects 0.000 description 5
- 230000002093 peripheral effect Effects 0.000 description 4
- 238000006243 chemical reaction Methods 0.000 description 3
- 238000005259 measurement Methods 0.000 description 3
- 238000005070 sampling Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 235000015429 Mirabilis expansa Nutrition 0.000 description 1
- 244000294411 Mirabilis expansa Species 0.000 description 1
- 101000983338 Solanum commersonii Osmotin-like protein OSML15 Proteins 0.000 description 1
- 230000002159 abnormal effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 239000003990 capacitor Substances 0.000 description 1
- 230000001276 controlling effect Effects 0.000 description 1
- 239000013078 crystal Substances 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 229920005994 diacetyl cellulose Polymers 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 230000005611 electricity Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000007667 floating Methods 0.000 description 1
- 238000002955 isolation Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 235000013536 miso Nutrition 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000001105 regulatory effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000012827 research and development Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J13/00—Controls for manipulators
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- Artificial Intelligence (AREA)
- Evolutionary Computation (AREA)
- Fuzzy Systems (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Control Of Stepping Motors (AREA)
Abstract
The invention discloses a mechanical arm driving control system and a mechanical arm driving control method based on a closed-loop stepping motor, which belong to the technical field of mechanical arm driving control systems.
Description
Technical Field
The invention relates to a mechanical arm drive control system, in particular to a mechanical arm drive control system and method based on a closed-loop stepping motor, and belongs to the technical field of mechanical arm drive control systems.
Background
The utility model discloses a robotic arm drive module, robot configuration drive and control system as in application number 2015138684. X, concretely, include input interface module, motor drive module, sensor signal acquisition module and output interface module, input interface module includes the data transmission interface, chip power source and independent power source, motor drive module includes signal isolation circuit and the drive chip circuit of electricity connection, output interface module includes motor interface and sensor interface, a robotic arm configuration drive system includes module selection circuit, the chip power, independent power source, bus interface and two at least robotic arm drive modules, the quantity of robotic arm module is decided by the joint degree of freedom of control object, a robotic control system includes robotic configuration drive system and controller, it can satisfy the control requirement of different joint degree of freedom robots, characteristics such as have the commonality strong, moreover, the steam generator is simple in structure, driving capability is strong, can wide application in the robot control.
However, with the development of technology in the prior art, the desktop-level mechanical arm is widely applied to various fields, so that more and more research teams are attracted to be put into the research and development of a desktop-level mechanical arm driving system, however, the desktop-level mechanical arm driving system generally adopts a steering engine or a servo motor as an executing member, and the former has low cost, low precision and simple driving system; the latter has high precision and high cost, and the driving control system is also more complex.
Disclosure of Invention
The invention mainly aims to provide a mechanical arm driving control system and a mechanical arm driving control method based on a closed-loop stepping motor.
The aim of the invention can be achieved by adopting the following technical scheme:
The mechanical arm driving control system based on the closed-loop stepping motor comprises an upper computer, wherein the upper computer is coupled with an algorithm controller for kinematic calculation;
the upper computer is also coupled with a second microcontroller for assisting interaction;
The algorithm controller is connected with a plurality of groups of stepping motor closed-loop drivers by adopting a CAN bus, and is coupled with the MPU6050 motion tracking chip and the OLED display screen;
the second microcontroller is coupled with the LED lamp ring and the buzzer;
the stepping motor closed-loop driver comprises a motor driving chip, and a radial magnet and a magnetic encoder are arranged at the tail part of the stepping motor;
The motor driving chip is coupled with the magnetic encoder, and the motor driving IC is coupled with the stepping motor;
the input part of the motor drive IC is coupled with a reverse Park converter, and the input part of the reverse Park converter acquires the input information of a position ring and a speed ring and the decoding information of a magnetic encoder;
the decoding information of the magnetic encoder is fed back to the position loop and the speed loop for PID control and fed back to the reverse Park converter, and the position loop and the speed loop receive target values output by the algorithm controller.
Preferably, the upper computer comprises one or more of a computer, a mobile client, a cloud end or a rocker;
the upper computer is connected with the algorithm controller and the second microcontroller through a USB or WIFI serial port.
Preferably, the algorithm controller adopts an STM32F405RGT6 microcontroller;
the STM32F405RGT6 microcontroller is connected with the OLED screen by adopting an IIC bus;
And the STM32F405RGT6 microcontroller is connected with the MPU6050 motion tracking chip by adopting an IIC bus.
Preferably, the second microcontroller is an ESP32-PICO-D4 microcontroller;
the ESP32-PICO-D4 microcontroller is connected with the LED lamp ring through a GPI013 pin;
the ESP32-PICO-D4 microcontroller is connected with the buzzer through a GPI025 pin;
ESP32-PICO-D4 microcontroller is connected to NRF2401L via SPI bus.
Preferably, the motor driving chip adopts a driving chip with the model of MCU cks32F103CBT 6;
the magnetic encoder employs an absolute type magnetic encoder MT6816.
The mechanical arm driving control method based on the closed-loop stepping motor comprises the following steps:
step one: setting a current change rate, angular acceleration, upper and lower current limits and upper and lower speed limits through an upper computer and sending the current change rate, angular acceleration, upper and lower current limits and the upper and lower speed limits to an algorithm controller and a second microcontroller;
step two: the algorithm controller obtains current change rate, angular acceleration, upper and lower current limits and upper and lower speed limit instructions sent by the upper computer, and obtains feedback information of the magnetic encoder for calculation and analysis;
step three: sending a control instruction to a stepping motor closed-loop driver through a CAN transceiver;
step four: the stepping motor closed-loop driver controls the running motion of the stepping motor, and the motion condition of the stepping motor is detected through the magnetic encoder and fed back to the algorithm controller.
The beneficial technical effects of the invention are as follows:
The driving control system of the mechanical arm based on the closed-loop stepping motor and the method thereof have the advantages that the driving control system of the desktop mechanical arm based on the closed-loop stepping motor is more advantageous, the design adopts a distributed design, the communication between the kinematic algorithm controller and the motor driver is realized through the CAN bus, the communication line between the mechanical arm controller and the motor driver is greatly reduced, and the final control precision is obviously improved by programming and manufacturing a schematic diagram and a PCB.
Drawings
Fig. 1 is a physical topology of a CAN bus of a preferred embodiment of a closed loop stepper motor based mechanical arm drive control system and method in accordance with the present invention.
Fig. 2 is a circuit diagram of an MPU6050 gyroscope of a preferred embodiment of a closed-loop stepper motor-based mechanical arm drive control system and method according to the present invention.
Fig. 3 is a schematic diagram of an MCU master control circuit of a preferred embodiment of a closed loop stepper motor based mechanical arm drive control system and method according to the present invention.
Fig. 4 is a logic block diagram of a preferred embodiment of a closed loop stepper motor based robotic arm drive control system and method in accordance with the present invention, TB67H450 FNG.
Fig. 5 is a diagram of a stepper motor drive circuit of a preferred embodiment of a closed loop stepper motor based mechanical arm drive control system and method in accordance with the present invention.
Fig. 6 is a waveform diagram of a mechanical arm driving control system and method based on a closed loop stepper motor according to a preferred embodiment of the present invention, which is acquired during idle load.
Fig. 7 is a waveform diagram of a mechanical arm driving control system and method based on a closed-loop stepper motor according to a preferred embodiment of the present invention, which is acquired under load.
Fig. 8 is a schematic diagram of a PWM waveform data frame of a preferred embodiment of a closed-loop stepper motor based mechanical arm drive control system and method according to the present invention.
Fig. 9 is a circuit diagram of an MT6816 magnetic encoder in accordance with a preferred embodiment of the closed-loop stepper motor based robotic arm drive control system and method of the present invention.
FIG. 10 is a schematic diagram of a mechanism of a desktop level mechanical arm kinematic core algorithm controller according to a preferred embodiment of the closed-loop stepper motor-based mechanical arm drive control system and method according to the present invention
Fig. 11 is a block diagram of a closed-loop actuator for a table-top robot joint stepper motor in accordance with a preferred embodiment of the closed-loop stepper motor based robot drive control system and method of the present invention.
Fig. 12 is a diagram of a closed-loop control system of a stepper motor driver in accordance with a preferred embodiment of the closed-loop stepper motor based robotic arm drive control system and method of the present invention.
Detailed Description
In order to make the technical solution of the present invention more clear and obvious to those skilled in the art, the present invention will be described in further detail with reference to examples and drawings, but the embodiments of the present invention are not limited thereto.
As shown in fig. 1-12, the mechanical arm driving control system based on the closed-loop stepper motor provided by the embodiment comprises an upper computer, wherein the upper computer comprises one or more of a computer, a mobile client, a cloud end or a rocker, the upper computer is connected with an algorithm controller and a second microcontroller through a USB or WIFI serial port, the upper computer software of the design is opened to control, the upper computer is coupled with the algorithm controller for kinematic calculation, an STM32F405RGT6 microcontroller is adopted, the STM32F405RGT6 microcontroller is connected with an OLED screen through an IIC bus, and the STM32F405RGT6 microcontroller is connected with an MPU6050 motion tracking chip through the IIC bus;
STM32F405RGT6 is based on Cortex-M4 32 bit RISC kernel, and the running frequency is up to 168MHz.
The Cortex-M4 core has a single precision Floating Point Unit (FPU) supporting all Arm single precision data processing instructions and data types.
The STM32F405xx family contains 1MB FLASH, 192KB SRAM, and 4KB backup SRAM high speed embedded memory, as well as various enhanced I/O and peripheral buses connected to two APBs, three AHB buses, and a 32-bit multi-AHB bus matrix.
Three 12-bit ADCs, two DACs, one low-power RTC, twelve general 16-bit timers, including two PWM timers for motor control, two general 32-bit timers, which also have standard and advanced communication interfaces.
The resources of STM32F405RGT6 used in the design comprise a CAN, two IICs, a USB and two UARTs, wherein the CAN bus is used for communicating with a driver, the two IICs are respectively used for communicating with two slave devices of a gyroscope and a screen, one USB and one serial port are used for connecting an upper computer in a wired mode, and the other UART is used for communicating between the STM32 and the ESP 32.
The upper computer is also coupled with a second microcontroller for auxiliary interaction, the second microcontroller adopts an ESP32-PICO-D4 microcontroller, the ESP32-PICO-D4 microcontroller is connected with the LED lamp ring through a GPI013 pin, the ESP32-PICO-D4 microcontroller is connected with the buzzer through a GPI025 pin, and the ESP32-PICO-D4 microcontroller is connected with the NRF2401L through an SPI bus;
the ESP32-PICO-D4 chip is internally provided with 2.4GHz Wi-Fi and Bluetooth functions, a Wi-Fi and Bluetooth module is not needed to be additionally arranged, and peripheral components such as a crystal oscillator, a flash memory, a filter capacitor, a radio frequency matching link and the like are integrated in one package, and the volume is only 7mm by 1mm.
The resources of ESP32-PICO-D4 used in the design comprise Bluetooth, wi-Fi, two UART serial ports, an SPI bus and a plurality of GPIO ports, wherein Bluetooth is used for connecting a mobile phone client, wi-Fi is used for connecting equipment to a cloud to realize remote control, the two UART serial ports of the UART serial ports are respectively used for burning programs and communication with STM32, and the SPI bus is used for receiving instructions sent by terminal equipment through NRF2401L radio frequency communication.
Reserving a plurality of GPIO ports for controlling a buzzer, an LED lamp ring and an expansion function for standby, wherein GPIO0 is a boot starting selection pin, and when the pin is low level, the pin enters a downloading mode; when the pin is normally started at a high level, the power-on of the GPIO2 is 0 when the boot is started and the pin is selected for downloading, the power-on of the GPIO12 is 0 when the power-on is required, the power-on of the GPIO5 is required to be 1, the internal pull-up of the GPIO14 is required to be 1, and the power-on of the GPIO15 is required to be 1.
As shown in FIG. 1, the algorithm controller is connected with a plurality of groups of stepping motor closed-loop drivers by using a CAN bus, CAN is an abbreviation of Controller Area Network (hereinafter referred to as CAN), and is an ISO internationally standardized serial communication protocol.
As shown in fig. 1, each stepping motor closed-loop driver of the desktop-level mechanical arm only receives information of two ID numbers (own ID, no. 0 ID), no. 0 ID is used for information broadcasting and synchronization, and when the stepping motor closed-loop driver receives a motion instruction from the algorithm controller, the information is stored in the shadow register, and motion starts after receiving a broadcasted synchronization signal, so that the motor synchronism can be further ensured.
The algorithm controller is coupled with the MPU6050 motion tracking chip and the OLED display screen;
The MPU6050 is a six-axis (gyroscope+accelerometer+) MEMS motion tracking chip with an auxiliary IIC communication port for connection to a third party digital sensor (e.g., magnetometer). When the auxiliary IIC communication port is connected with a 3-axis magnetometer, the MPU-6050 can provide a complete 9-axis MOTIONFION output to the main IIC communication port.
As shown in fig. 2, CLKIN (pin 1) may be an optional external reference clock input, not connected to ground; AUX_DA (pin 6) and AUX_CL (pin 7) are auxiliary IIC port data and clock pins; AD0 (pin 9) is a slave mode address selection pin and is grounded; SCL (pin 23) and SDA (pin 24) are the primary IIC port clock and data pins; pin2,3,4,5, 14, 15, 16, 17 is an empty pin, other pin functions can be seen with reference to chip datasheet, when the IIC bus is idle, the pin level is pulled high with a 2k resistor, avoiding interference.
The second microcontroller is coupled with the LED lamp ring and the buzzer;
the stepping motor closed-loop driver comprises a motor driving chip, and the motor driving chip adopts an MCU cks32F103CBT6 driving chip;
The MCU of the driver adopts CKS32F103CBT6 of the middle core, 32 bit MCU products developed based on ARM Cortex-M3 inner cores, in view of supply shortage and price surge of imported chips, the adoption of the domestic MCU chips is a better choice, such as megainnovation, middle core, china, aviation, and the like, in terms of compatibility, the best is made of the middle core, in addition, the GD32 series of megainnovation can be considered in alternative schemes, the GD32F103CBT6 can realize pin to pin replacement, and compared with STM32F103CBT6, the GD32F103CBT6 has higher main frequency and own standard peripheral library.
As can be known by referring to a chip manual, CKS32F103CBT6 has the highest main frequency of 72MHz and two 12-bit ADC, the conversion time is 1us, and the conversion range is 0-3.6V; with 37I/O ports, all of which can be mapped to 16 external interrupts; almost all ports can withstand 5V signals; 2I 2C, 3 USART, 2 SPI, one CAN, one USB2.0; flash size is 128k, SRAM size is 20k, and definition of each I/O port used in the design is shown in FIG. 3.
The tail part of the stepping motor is provided with a radial magnet and a magnetic encoder, a motor driving chip is coupled with the magnetic encoder, an SPI bus is adopted between the MCU chip and the magnetic encoder, and a motor driving IC is coupled with the stepping motor;
the motor drive IC selects TB67H450FNG of Toshiba, the model chip has a working voltage range of 4.5V-44V wide voltage, the maximum rated specification is 50V/3.5A, the standby power consumption value can be as low as 1 mu A, and the chip has excellent low power consumption and low output on-resistance characteristics.
The logic block diagram of TB67H450FNG is shown IN FIG. 4, and when the IN_P and IN_M pins are simultaneously at a low level for 1 ms or more, the motor drive chip enters a standby mode from an operation mode; when either of the IN1, IN2 pins is set to a high level, the motor drive chip enters an operation mode from a standby mode, and the time required for releasing the return from standby is at most 30 mus.
The TB67H450FNG chip is provided with a built-in Thermal Shutdown (TSD) function, an overcurrent detection (ISD) function and an undervoltage lock (UVLO) function, so that the motor and the driving plate are effectively prevented from being damaged under abnormal conditions.
The stepping motor selected by the design is a two-phase four-wire stepping motor, two TB67H450FNG chips are required for driving one stepping motor to form a double H bridge control circuit, and an A phase and a B phase are respectively controlled, as shown in fig. 5, PWM waves output by two I/O ports (PB 11 and PB 10) of the MCU are filtered by RC to form two analog voltages, and the two analog voltages are respectively output to two driving chips.
The VREF pin of the TB67H450FNG controls the current of the A phase and the B phase, four I/O ports (PB 15, PB14, PB13 and PB 12) respectively output to the IN_P and IN_M pins of the two driving chips TB67H450 to control the levels of A+, A-, B+ and B-, so as to realize the chopper constant current control of the stepping motor, a thermistor is added IN the design of the board for protecting the motor and the driver, the MCU detects the voltage of a TEMP sampling point through the ADC to judge whether the temperature of the TB67H450FNG is too high, and the LSS pin of the TB67H450FNG is connected with the current sampling resistor, and the resistance value is 0.1 omega.
Waveforms of outa+ and outb+ acquired in the current mode are shown in fig. 6 and 7, and fig. 6 is a waveform acquired in no-load, and it can be seen that the phase current does not reach the set maximum current at this time; fig. 7 is a waveform acquired under load, and a waveform of a chopper constant current effect can be seen.
The input part of the motor drive IC is coupled with the reverse Park converter, the input part of the reverse Park converter acquires the input information of the position loop and the speed loop and the decoding information of the magnetic encoder, the decoding information of the magnetic encoder is fed back to the position loop and the speed loop for PID control and fed back to the reverse Park converter, and the position loop and the speed loop receive target values output by the algorithm controller.
And carrying out real-time motion planning according to the set current change rate, angular acceleration, upper and lower current limits and upper and lower speed limits, regulating according to deviation signals through a cascade PID controller, and outputting FOC (FOC) to a motor drive IC (integrated circuit) to control a stepping motor by a reverse Park converter, wherein a radial magnet and a magnetic encoder are arranged behind the stepping motor, the magnetic encoder senses magnetic field signals of the radial magnet on a motor shaft, encodes the magnetic signals, outputs the magnetic signals in an SPI (serial peripheral interface) digital signal mode, decodes the magnetic signals by a decoder and feeds back the magnetic signals to the PID controller and the reverse Park converter, so that multiple control modes such as speed mode closed-loop control, position speed double closed-loop and the like are realized.
The PID control used in the design is a position type PID algorithm, and the deduction flow is as follows:
The PID control law of a continuous system can be expressed in s-domain as:
;
Wherein the method comprises the steps of Is the proportional gain; /(I)Is the integral gain; /(I)Is a differential gain.
The inverse transformation by Laplace can be expressed as:
;
After discretizing it, it can be expressed in the time domain as:
;
Order the
;
Substituting equation 2-3 into equation 2-4 yields the discretized equation for the incremental PID:
;
the angular displacement signal of the joint motor can be obtained by sampling with a magnetic encoder, each position of which corresponds to a determined digital code, so that its indication is only related to the start and end positions of the measurement, irrespective of the intermediate course of the measurement.
When the power is turned off, the 360 ° absolute encoder is not separated from the actual position within a single turn. If the power is turned on again, the position reading is still accurate and valid; unlike incremental encoders, it is necessary to find a zero position mark, and there are modes such as ABZ, UVW, SPI, UART, PWM depending on the signal output mode.
The absolute magnetic encoder MT6816 adopted by the design has the following characteristics that the magnetic encoder chip consists of a pair of (AMR) Wheatstone bridges and a signal processing ASIC circuit.
Along with the rotation of a magnetic field parallel to the surface of the chip, the chip outputs corresponding encoded angle signals, the signal delay is as low as within 2us, the rotation speed of up to 25,000 revolutions per minute can be supported at the highest, meanwhile, a standard 3-wire or 4-wire SPI interface (highest clock 16 MHz) is provided for reading absolute angle data angle <0:13> of 14 bits in the chip, and a user reads angle data calculated in the chip through the high-speed SPI interface) The conversion formula is shown in the following formula 2-6:
;
in addition, the chip also provides a 12-bit digital Pulse Width Modulation (PWM) output, the duty cycle and the measurement angle of which are shown in equations 2-7.
;
One PWM output comprises a frame of 4119PWM clock cycles, as shown in fig. 8, where the angle data is represented in the frame at 12 bits resolution, one PWM clock cycle being 0.088 °, typically 250ns in duration;
the magnetic encoder employs an absolute type magnetic encoder MT6816.
Because the encoder MT6816 adopted by the design has 14bit resolution, the calibration data is saved by the data of the type of the uint16 when the power is on for the first time, and the total data is required to occupy 32k #) In addition, an SPI communication physical interface is needed, the difference between the two models of the CKS32F103CBT6 and the CKS32F103C8T6 is that the Flash size is different, the Flash size of the CKS32F103C8T6 is only 64k, the calibration data of 32k are removed, only the Flash storage space of 32k is left for program burning, and the Flash is not enough for storing user programs; and the CKS32F103CBT6 has a 128K Flash space, so that the model of the CKS32F103CBT6 is finally selected, as shown in FIG. 9, a chip selection signal CS pin of the magnetic encoder is connected to a PA15 port of the MCU, a clock signal SCK pin is connected to a PB3 port of the MCU, and two data pins MISO and MOSI are respectively connected to PB4 and PB5 ports of the MCU.
The mechanical arm driving control method based on the closed-loop stepping motor comprises the following steps:
step one: setting a current change rate, angular acceleration, upper and lower current limits and upper and lower speed limits through an upper computer and sending the current change rate, angular acceleration, upper and lower current limits and the upper and lower speed limits to an algorithm controller and a second microcontroller;
step two: the algorithm controller obtains current change rate, angular acceleration, upper and lower current limits and upper and lower speed limit instructions sent by the upper computer, and obtains feedback information of the magnetic encoder for calculation and analysis;
step three: sending a control instruction to a stepping motor closed-loop driver through a CAN transceiver;
step four: the stepping motor closed-loop driver controls the running motion of the stepping motor, and the motion condition of the stepping motor is detected through the magnetic encoder and fed back to the algorithm controller.
In the fourth step, the algorithm controller built-in control algorithm specifically comprises the following contents;
Establishing a connecting rod coordinate system;
The zi coordinate axis of each joint is along the axial direction of the i+1 joint;
the xi coordinate axis of each joint is a common vertical line of zi and zi-1;
After the x and z coordinates of each joint are determined, a y-axis coordinate is obtained according to a right hand rule;
Connecting rod parameters;
A distance;
Connecting rod length ai: along the xi axis, the distance moved by zi to zi-1 represents the distance between the two joint axes.
Link distance di: along the zi-1 axis, the distance from xi to xi-1 represents the distance between the two bars.
An angle;
Connecting rod torsion angle ai: the included angle between the zi axis and the zi-1 axis represents the included angle between the axes of the two joints;
connecting rod rotation angle θi: the included angle between the axis xi and the axis xi-1, i.e. the distance between the two connecting rods.
The D-H parameters of the robotic arm established by the link parameters are shown in the table below:
TABLE 3 DH parameters of each connecting rod
The above is merely a further embodiment of the present invention, but the protection scope of the present invention is not limited thereto, and any person skilled in the art will be able to apply equivalents and modifications according to the technical solution and the concept of the present invention within the scope of the present invention disclosed in the present invention.
Claims (6)
1. Mechanical arm drive control system based on closed loop step motor, its characterized in that: the system comprises an upper computer, a control unit and a control unit, wherein the upper computer is coupled with an algorithm controller for kinematic calculation;
the upper computer is also coupled with a second microcontroller for assisting interaction;
The algorithm controller is connected with a plurality of groups of stepping motor closed-loop drivers by adopting a CAN bus, and is coupled with the MPU6050 motion tracking chip and the OLED display screen;
the second microcontroller is coupled with the LED lamp ring and the buzzer;
the stepping motor closed-loop driver comprises a motor driving chip, and a radial magnet and a magnetic encoder are arranged at the tail part of the stepping motor;
The motor driving chip is coupled with the magnetic encoder, and the motor driving IC is coupled with the stepping motor;
the input part of the motor drive IC is coupled with a reverse Park converter, and the input part of the reverse Park converter acquires the input information of a position ring and a speed ring and the decoding information of a magnetic encoder;
the decoding information of the magnetic encoder is fed back to the position loop and the speed loop for PID control and fed back to the reverse Park converter, and the position loop and the speed loop receive target values output by the algorithm controller.
2. The closed-loop stepper motor based robotic arm drive control system of claim 1, wherein: the upper computer comprises one or more of a computer, a mobile client, a cloud end and a rocker;
the upper computer is connected with the algorithm controller and the second microcontroller through a USB or WIFI serial port.
3. The closed-loop stepper motor based robotic arm drive control system of claim 1, wherein: the algorithm controller adopts an STM32F405RGT6 microcontroller;
the STM32F405RGT6 microcontroller is connected with the OLED screen by adopting an IIC bus;
And the STM32F405RGT6 microcontroller is connected with the MPU6050 motion tracking chip by adopting an IIC bus.
4. The closed-loop stepper motor based robotic arm drive control system of claim 1, wherein: the second microcontroller adopts an ESP32-PICO-D4 microcontroller;
the ESP32-PICO-D4 microcontroller is connected with the LED lamp ring through a GPI013 pin;
the ESP32-PICO-D4 microcontroller is connected with the buzzer through a GPI025 pin;
ESP32-PICO-D4 microcontroller is connected to NRF2401L via SPI bus.
5. The closed-loop stepper motor based robotic arm drive control system of claim 1, wherein: the motor driving chip adopts an MCU cks32F103CBT6 driving chip;
the magnetic encoder employs an absolute type magnetic encoder MT6816.
6. The closed-loop stepping motor-based mechanical arm driving control method according to claim 1, wherein: the method comprises the following steps:
step one: setting a current change rate, angular acceleration, upper and lower current limits and upper and lower speed limits through an upper computer and sending the current change rate, angular acceleration, upper and lower current limits and the upper and lower speed limits to an algorithm controller and a second microcontroller;
step two: the algorithm controller obtains current change rate, angular acceleration, upper and lower current limits and upper and lower speed limit instructions sent by the upper computer, and obtains feedback information of the magnetic encoder for calculation and analysis;
step three: sending a control instruction to a stepping motor closed-loop driver through a CAN transceiver;
step four: the stepping motor closed-loop driver controls the running motion of the stepping motor, and the motion condition of the stepping motor is detected through the magnetic encoder and fed back to the algorithm controller.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311672619.2A CN117601152B (en) | 2023-12-07 | 2023-12-07 | Mechanical arm driving control system and method based on closed-loop stepping motor |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311672619.2A CN117601152B (en) | 2023-12-07 | 2023-12-07 | Mechanical arm driving control system and method based on closed-loop stepping motor |
Publications (2)
Publication Number | Publication Date |
---|---|
CN117601152A CN117601152A (en) | 2024-02-27 |
CN117601152B true CN117601152B (en) | 2024-05-07 |
Family
ID=89947929
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202311672619.2A Active CN117601152B (en) | 2023-12-07 | 2023-12-07 | Mechanical arm driving control system and method based on closed-loop stepping motor |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN117601152B (en) |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105215988A (en) * | 2015-10-31 | 2016-01-06 | 许志林 | A kind of mechanical arm driver module, robot configuration drive and control system |
CN105329399A (en) * | 2015-10-28 | 2016-02-17 | 合肥工业大学 | Control system and control method of scooter |
CN207027518U (en) * | 2017-06-06 | 2018-02-23 | 渭南师范学院 | A kind of intelligent body-sensing mechanical arm |
CN107804474A (en) * | 2017-09-29 | 2018-03-16 | 华南理工大学 | Carry more rotor flying robot Complete machine system design methods of redundancy mechanical arm |
CN109062115A (en) * | 2018-09-11 | 2018-12-21 | 长沙学院 | A kind of spin control method based on double-closed-loop control |
CN109240305A (en) * | 2018-10-19 | 2019-01-18 | 广州大学华软软件学院 | Coaxial two wheels robot kinetic control system and method based on complementary filter |
EP3974304A1 (en) * | 2020-09-28 | 2022-03-30 | Changzhou Taochen Electronic Technology Co., Ltd | Swing car controller |
CN115649268A (en) * | 2022-10-19 | 2023-01-31 | 湖北洋田塑料制品有限公司 | Intelligent baby carriage with control system and control method thereof |
-
2023
- 2023-12-07 CN CN202311672619.2A patent/CN117601152B/en active Active
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105329399A (en) * | 2015-10-28 | 2016-02-17 | 合肥工业大学 | Control system and control method of scooter |
CN105215988A (en) * | 2015-10-31 | 2016-01-06 | 许志林 | A kind of mechanical arm driver module, robot configuration drive and control system |
CN207027518U (en) * | 2017-06-06 | 2018-02-23 | 渭南师范学院 | A kind of intelligent body-sensing mechanical arm |
CN107804474A (en) * | 2017-09-29 | 2018-03-16 | 华南理工大学 | Carry more rotor flying robot Complete machine system design methods of redundancy mechanical arm |
CN109062115A (en) * | 2018-09-11 | 2018-12-21 | 长沙学院 | A kind of spin control method based on double-closed-loop control |
CN109240305A (en) * | 2018-10-19 | 2019-01-18 | 广州大学华软软件学院 | Coaxial two wheels robot kinetic control system and method based on complementary filter |
EP3974304A1 (en) * | 2020-09-28 | 2022-03-30 | Changzhou Taochen Electronic Technology Co., Ltd | Swing car controller |
CN115649268A (en) * | 2022-10-19 | 2023-01-31 | 湖北洋田塑料制品有限公司 | Intelligent baby carriage with control system and control method thereof |
Non-Patent Citations (2)
Title |
---|
基于FreeRTOS的移动机器人控制***设计;曹俊;软件工程;20230731;第第26卷卷(第第7期期);第25-28页 * |
基于改进粒子群算法的多电机智能控制研究;曾宪荣;机床与液压;20230731;第第51卷卷(第第14期期);第75-80页 * |
Also Published As
Publication number | Publication date |
---|---|
CN117601152A (en) | 2024-02-27 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107901044A (en) | The drive control integral system of joint of robot | |
CN101729002A (en) | SOPC-based remote monitoring system of no-position sensor brushless DC motor | |
CN117601152B (en) | Mechanical arm driving control system and method based on closed-loop stepping motor | |
CN106052689A (en) | IMU (inertial measurement unit) signal acquisition system based on C8051F064 single chip microcomputer | |
CN208999805U (en) | Quadrotor system based on Embedded micro-system | |
CN109001628B (en) | Micro asynchronous motor set monitoring system and method based on synchronous phasor data | |
CN206505335U (en) | A kind of temperature and humidity controller based on LPC1768 | |
CN112721634B (en) | New energy automobile intelligent instrument control system | |
RU160905U1 (en) | MULTIFUNCTIONAL CONTROLLER OF WIDE APPLICATION | |
CN107328431A (en) | Timing orientation sensor carrying platform and its measuring method | |
CN104501791A (en) | Novel north seeker | |
CN112678158A (en) | Four-rotor aircraft based on embedded technology | |
CN114594695A (en) | Inertial device simulation control system based on FPGA and DSP | |
CN212541063U (en) | Multifunctional robot development control system with 16-path steering engine expansion control | |
CN208224851U (en) | Control circuit takes aim at control system with thick | |
CN206670611U (en) | The Angle Measurement Module of serial ports output | |
CN209928284U (en) | Six-degree-of-freedom platform | |
CN215120627U (en) | Stepping motor closed-loop control system based on single-chip microcomputer technology | |
Lu et al. | The research and design of auto-leveling control system for vehicle-borne radar platform based on AVR | |
CN209001864U (en) | Revolving speed position real-time detecting system | |
CN108724193A (en) | A kind of multi-joint bio-robot control system | |
CN204967774U (en) | Sinusoidal drive signal source | |
CN209980072U (en) | Unmanned warning monitoring module bearing platform electrical system | |
Yifei et al. | Design of dual-PMSM synchronous control drive | |
CN202978800U (en) | ARM-based stepper motor controller |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |