Precise torque output device of upper limb rehabilitation robot based on vector control and space vector pulse width modulation
By using a precision torque output device for upper limb rehabilitation robots based on vector control and spatial vector pulse width modulation, the problems of insufficient torque control accuracy and response speed are solved, achieving high-precision torque output and system stability, expanding the application range, reducing energy consumption, and enhancing system integration and reliability.
Patent Information
- Application Number
- CN202410992634.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-23
- Publication Date
- 2024-11-12
AI Technical Summary
Existing upper limb rehabilitation robots have shortcomings in torque control precision and response speed, which affect the rehabilitation training effect and system reliability.
A precise torque output device based on vector control and space vector pulse width modulation is adopted, including an MCU unit, a sensor subsystem, a motor control subsystem, and a power supply subsystem. It achieves precise control of the motor through vector control algorithms and SVPWM technology, integrates WiFi and serial communication, expands GPIO control sensor units, uses a high-efficiency LDO power supply subsystem, and combines AS5600 and INA240A2PWR sensors for rotor position and current detection.
It achieves high-precision torque control, improves motor voltage utilization and system stability, expands the application range, reduces energy consumption, and enhances system integration and reliability.
Smart Images

Figure CN118944516A_ABST
Abstract
Description
Technical Field
[0001] The invention relates to the field of output torque control of an upper limb rehabilitation robot actuator, in particular to a precise torque output device of an upper limb rehabilitation robot based on vector control and space vector pulse width modulation. Background Art
[0002] In recent years, rehabilitation robot technology has been widely used in the rehabilitation process of different parts of the body. There are more subdivisions of upper limb rehabilitation robots, which can be roughly divided into: full upper limb rehabilitation, wrist rehabilitation, hand rehabilitation, finger rehabilitation, shoulder rehabilitation, elbow rehabilitation, full arm rehabilitation and forearm rehabilitation robots according to the targeted rehabilitation parts. By providing precise control, standardized and personalized rehabilitation training, it helps patients restore the limb function and motor ability of their upper limbs. Therefore, the technical research on the precise control of the actuators of upper limb rehabilitation robots is crucial in clinical practice.
[0003] Upper limb rehabilitation robots need to be able to move at precise linear and angular velocities to perform specific rehabilitation motion paths. Upper limb rehabilitation robot end effectors are often driven by brushless motors. Brushless motors (taking three-phase motors as an example) usually require three symmetrical sinusoidal currents with a spatial difference of 120° to drive. If the square wave PWM is modulated in a sinusoidal manner, although the stator sinusoidal current is obtained, only the current size can be controlled, not the direction of the current. Field vector control (FOC) is a control strategy for controlling brushless DC motors and permanent magnet synchronous motors. The three-phase AC signal is transformed into a DC controllable two-phase orthogonal current through a series of coordinate transformations, which can realize three closed-loop controls of torque (current), speed, and position. Space vector pulse width modulation (SVPWM) is an advanced PWM technology for three-phase inverters. It uses space vector theory to control the switching state of the inverter so that the output voltage vector approaches the target reference voltage vector, thereby achieving precise control of the brushless motor. Space vector pulse width modulation generates efficient PWM to control the output state of the inverter, and magnetic field vector control provides independent control of physical parameters such as brushless motor torque and flux. Summary of the invention
[0004] The present invention aims to solve the deficiencies of existing upper limb rehabilitation robots in torque control accuracy and response speed, and to provide an accurate torque output device based on vector control and space vector pulse width modulation (SVPWM), which can significantly improve the rehabilitation training effect and system reliability.
[0005] In order to solve the above technical problems, the present invention provides an upper limb rehabilitation robot precise torque output device based on vector control and space vector pulse width modulation, including: an MCU unit, a sensor subsystem, a motor control subsystem, a power supply subsystem and a host computer communication subsystem. Among them, the sensor subsystem is composed of a rotor position detection subsystem and a current detection subsystem, and the motor control system is composed of a FOC control subsystem and a SVPWM control subsystem. The MCU acquires the position and speed data of the brushless motor rotor by expanding the GPIO control sensor unit, and controls the motor to output precise torque through the motor driver. The system uses a vector control algorithm to control the motor, and realizes efficient voltage modulation through SVPWM to ensure the accuracy and stability of the motor torque output. The device communicates with the host computer through WIFI and serial communication protocol.
[0006] The MCU unit is built around the ESP32-D0WDQ6 chip, with a preset clock frequency of 160MHz, up to 240MHz, and an operating voltage of 3.0-3.6V. The unit can be powered by 5V via Micro USB, and the voltage is reduced to 3.3V via an LDO low-dropout linear regulator. The MCU unit has 4MB Flash and 520KB SRAM, connects to a wireless network via built-in WiFi to build a host computer web service, and uses Bluetooth 4.2BR / EDR and UART for basic function debugging. The unit can be configured with two ADCs, ADC1 with 6 channels and ADC2 with 10 channels. In addition, the MCU unit also has IIC and SPI ports for information communication with general sensors.
[0007] The power supply subsystem uses an efficient LDO (low dropout regulator) to ensure a stable output voltage and reduce the impact of voltage fluctuations on the algorithm and motor operation. Sufficient filter capacitors are set at the power input and output to smooth power supply noise and transient voltage changes. The FOC and SVPWM algorithms have high requirements for the response speed of current and voltage changes, so the power supply subsystem uses capacitors with low equivalent series resistance (ESR) to improve the filtering effect and response speed. The power supply subsystem has a voltage protection circuit that automatically cuts off the power supply when the voltage exceeds the set value, and uses a power monitoring IC to monitor the power supply status in real time and adjust and protect it as needed.
[0008] The sensor subsystem mainly includes the rotor position detection subsystem and the motor working current detection subsystem, which are designed using AS5600 and INA240A2PWR respectively. The AS5600 angle sensor accurately detects the real-time position of the rotor through non-contact magnetic induction technology and outputs high-resolution angle information to support the precise control of the FOC (field-oriented control) algorithm. The INA240A2PWR current sensing amplifier has high precision and high-speed response characteristics, can accurately monitor the current of each phase of the motor, and provide current data in real time to ensure the efficient operation of the SVPWM (space vector pulse width modulation) algorithm.
[0009] The motor control subsystem is capable of calculating and executing FOC and SVPWM (space vector pulse width modulation) algorithms in real time. The FOC control subsystem is designed around EG2133 and NMOS. EG2133 is a high-performance FOC (field oriented control) dedicated control chip with powerful computing power and multi-channel PWM output function. NMOS (N-channel metal oxide semiconductor field effect transistor) is used as a power drive element to control the phase current of the motor, and achieves precise control of the motor torque and speed through precise switching operations.
[0010] A control method for an upper limb rehabilitation robot precision torque output device based on vector control and space vector pulse width modulation comprises the following steps:
[0011] First, a simple model of a brushless motor in FOC control and the basic process of its coordinate transformation are given. The A, B, and C coordinate systems where the three-phase symmetrical sinusoidal current is located are converted into a two-phase motor model of α and β through Clarke transformation, which are 90° apart in space. The mapping relationship between the q-axis and d-axis coordinate systems that follow the rotor movement and the stator coordinate systems α and β is established through Park transformation. Clarke transformation keeps the front and rear magnetomotive forces the same, so that the air gap magnetic field of the motor remains consistent. The orthogonal windings are passed through two-phase currents, simulating the situation where the three-phase windings originally arranged symmetrically in space are passed through three-phase currents. The Park transformation maps the points in the stationary coordinate system (α and β coordinate systems) one by one to the rotating coordinate system (d and q coordinate systems), which is essentially the mathematical expression of the same two-dimensional space vector in two different coordinate systems. The outermost part of the FOC control architecture is the speed loop, which obtains the current position information, rotor angular velocity, and electrical angle θ of the rotor through the position sensor installed at the bottom of the motor or the sensorless observer algorithm, compares the calculated feedback speed with the given speed, and controls the torque current output through the PI controller. If Id=0, Iq_ref enters the current loop torque PI controller, and the voltage in the dq rotating coordinate system is converted to the voltage in the α-β two-phase stationary coordinate system through Park inverse transformation, and then the voltage in the equivalent α-β stationary two-phase coordinate system is converted back to the voltage in the ABC three-phase coordinate system through Clarke inverse transformation. The three-phase voltage is then processed by SVPWM to obtain the PWM duty cycle, and the inverter is controlled for output.
[0012] The matrix form of Clark's equal amplitude transform and the form of Clark's inverse transform are given as follows:
[0013]
[0014] The matrix forms of Park transform and inverse Park transform are given as follows, where θ is the electrical angle.
[0015]
[0016] Among them, the current sampling of the d-axis and q-axis is combined and filtered by hardware filtering and software filtering. In the hardware filtering, a low-pass RC filter is used to filter out high-frequency noise and retain low-frequency signals. In the software filtering, three filtering methods, namely, mean filtering, exponentially weighted moving average filtering and Kalman filtering, are designed according to actual needs for actual adaptive use. The formula for mean filtering is: Where I[n] is the nth current sample value, N is the window size of the moving average filter (i.e. the number of samples considered), and I avg [n] is the average value of the nth sample point. The exponentially weighted moving average (EWMA) smoothes the signal and reduces noise by giving greater weight to new sample values and gradually reducing the weight to old sample values. Its formula is:ewma [n] = α·I[n] + (1-α)·I ewma [n-1] where α is the smoothing coefficient, ranging from 0<α≤10. A larger α value will make the new data more influential and respond faster; a smaller α value will make the filter more influential on historical data and respond slower. I[n] is the nth current sampling value. I ewma [n-1] is the EWMA filter value of the n-1th sampling point. For current sampling, simplify the Kalman filter, assume that both the state and the measurement are scalars, 1) predict the current current state estimate: 2) Predict the current error covariance: P k - =P k-1 +Q, 3) Calculate the Kalman gain: 4) Update state estimate: 5) Update error covariance: where x^ k is the state estimate (current estimate) at the kth moment, is the predicted state estimate at the kth moment, P k is the error covariance at the kth moment, is the forecast error covariance at the kth moment, K k is the Kalman gain at the kth moment, z k is the measured value at the kth moment (actual current measurement value), Q is the process noise covariance, and R is the measurement noise covariance.
[0017] The beneficial effects of the present invention are as follows: (1) High-precision torque control: The present invention utilizes a vector control method to independently control the motor magnetic field and torque, thereby achieving high-precision output of the torque of the upper limb rehabilitation robot, and ensuring precise torque control during rehabilitation training. (2) Efficient voltage modulation: The present invention uses space vector pulse width modulation (SVPWM) technology to achieve precise control of the motor output voltage and current, effectively improving the voltage utilization of the motor, reducing harmonic interference, thereby improving the efficiency and stability of the system, and being suitable for operation on embedded platforms. (3) Expanded application scope: The device can be integrated and applied on embedded platforms, and is not only suitable for various upper limb rehabilitation training, but can also be expanded to other robot systems that require precise torque control, greatly broadening the application scope of the device. The device integrates peripherals such as WiFi, serial ports, and multi-channel ADCs, and has powerful computing power and multiple communication interfaces, which facilitates information interaction with the host computer and various sensors, thereby improving the system's integration and reliability. (4) Low energy consumption and high efficiency: The present invention adopts an efficient LDO (low dropout regulator) power supply subsystem to ensure stable power output, reduce the impact of voltage fluctuations on the algorithm and motor operation, and reduce energy consumption. At the same time, SVPWM technology improves the operating efficiency of the motor and reduces energy loss by optimizing voltage modulation. BRIEF DESCRIPTION OF THE DRAWINGS
[0018] Figure 1 It is a schematic diagram of the structure of the device of the present invention.
[0019] Figure 2 It is a schematic diagram of the MCU structure of the present invention.
[0020] Figure 3 It is a schematic diagram of the circuit structure of the FOC integrated control subsystem of the present invention.
[0021] Figure 4 Schematic diagram of the circuit structure of the current detection subsystem of the present invention.
[0022] Figure 5 This is a schematic diagram of the circuit structure of the power supply subsystem of the present invention.
[0023] Figure 6 This is a control algorithm implementation block diagram of the FOC integrated control subsystem of the present invention.
[0024] Figure 7 Schematic diagram of low-side current detection of the brushless motor of the present invention.
[0025] Figure 8 It is a schematic diagram of the software operation flow of the device of the present invention. DETAILED DESCRIPTION
[0026] like Figure 1As shown in the figure, the torque output device of the upper limb rehabilitation robot based on vector control mainly includes: microcontroller unit, power supply subsystem, rotor position detection subsystem, motor working current detection subsystem, FOC integrated control subsystem. The device receives the control signal from the host computer, outputs the corresponding torque, and outputs the motor operation information to the server via WIFI. Figure 1 As shown, the device mainly includes: 1) Central controller, connected to different subsystems through various pins, power supply pins (VP, VN, EN): These pins are used to power the MCU and external circuits. Control pins (M0_EN, M1_EN, M0_OUT_CS, M1_OUT_CS): Used to control the start, stop and output of the FOC motor control system and the SVPWM electric control system. Communication pins (SDA_0, SDA_1, SCL_0, SCL_1, RXD, TXD): Used to exchange data with the sensor system and the host computer communication system (including WIFI and serial port communication), 2) Sensor system, the sensor system includes a current sensor and a rotor position sensor. 3) Power supply system, 4) Host computer communication system, 5) Extended GPIO, used to connect other peripheral devices or expansion modules to increase the flexibility and scalability of the system.
[0027] like Figure 2 As shown, the MCU unit is built around the ESP32-D0WDQ6 chip, with a preset clock frequency of 160Mhz, up to 240Mhz, and an operating voltage of 3.0-3.6V. It can be powered by 5V via Micro USB and reduced to 3.3V via an LDO low-dropout linear regulator. The MCU unit has 4MB Flash and 520KB SRAM. It connects to the wireless network through the built-in WIFI to build the host computer web service, and uses Bluetooth 4.2BR / EDR and UART for basic function debugging. It can configure two ADCs, ADC1 has 6 channels, and ADC2 has 10 channels. The MCU unit also has IIC and SPI ports for information communication with general sensors.
[0028] like Figure 3As shown in the figure, the FOC integrated control subsystem is constructed around the EG2133 three-phase independent half-bridge driver chip. Its main functions include: logic signal input processing: the high level threshold is above 2.5V and the low level threshold is below 1.0V. Dead time control: ensure that the upper and lower bridge arms are not turned on at the same time during switching. Level conversion: convert the level of the logic signal to a level suitable for driving the power MOS tube. Suspended bootstrap power supply: provide power to the high-side driver through the bootstrap circuit, so that it can drive the N-channel MOSFET. Upper and lower bridge totem pole output: provide drive signals for the upper and lower bridge arms to control the connected power MOS tube. Take the two N-channel MOSFETs (Q1 and Q2, model WSD4070DN) in the three-phase drive circuit as an example. They are connected to the power ground (PGND) through the source (S) and the drain (D) is connected to the load respectively. The input signal (M0_GH1 and M0_GL1) comes from the output of EG2133 to control the MOSFET on and off. When M0_GH1 is high, Q1 is turned on; when M0_GL1 is high, Q2 is turned on. The gate resistors (R27 and R26) are connected between the gate (G) of each MOSFET and the input signal to limit the gate current and protect the MOSFET from high current damage. The pull-up resistors (R28 and R29) are used to ensure that the gate of the MOSFET is in a high-impedance state when the control signal is not activated to avoid accidental turn-on due to floating inputs. VIN is the input power supply, which provides the operating voltage for the MOSFET. The output M0_OUT1 is connected to the load, and when one of the MOSFETs is turned on, the current will flow from VIN through the MOSFET to the load. Bypass capacitors C7 and C11 are placed on the power line to suppress voltage spikes and noise on the power line to prevent these voltage spikes and noise from affecting the performance of the MOSFET. Through FOC and SVPWM technology, the output torque can be accurately controlled using a brushless DC motor, and then the speed and direction of each motor can be controlled, so that the rehabilitation robot can make smooth and precise movements in any direction.
[0029] like Figure 4 As shown, the present invention detects the current of the d-axis and q-axis of the brushless motor through a current loop, and the current detection subsystem is designed around the INA240A2PWR current sensing amplifier, such as Figure 4As shown, the power supply side and the load side are connected to the IN+ and IN- pins respectively, REF1 is shorted to the GND terminal, and REF2 is shorted to the VS terminal, where the power supply voltage range of the VS terminal is 2.7 to 5.5V. The sensor outputs an analog signal at the OUT terminal, which is connected to the ADC1 channel 6 and ADC1 channel 7 of the MCU unit, and the analog-to-digital conversion is performed in the MCU unit. The current detection subsystem calibrates the ADC offset by reading the ADC value multiple times and averaging the value to determine the voltage offset at zero current. In the case of non-zero current measurement, the actual current value is obtained by reading the various current values of the motor, subtracting the offset, and multiplying by the corresponding gain.
[0030] like Figure 5 As shown in the figure, the function of the power supply subsystem is to convert the input voltage of 12V or 24V into a stable 5V output. It is designed around the power management buck converter chip TPS54331DDAR (U3): the input voltage is 12V or 24V, and the input filter capacitors C8 and C3 are used to filter out the high-frequency noise in the input voltage to ensure the stability of the input voltage. R4 and R5 form a voltage divider to ensure that the voltage of the EN pin is within the appropriate range to control the enablement of the power supply. The capacitor C12 connected to the SS pin is used to set the soft start time of the power supply. C14, C15, and R6 form a compensation network (COMP) to prevent the power supply from having a current surge at startup, and to stabilize the feedback loop of the power supply to ensure the stability of the output voltage. The switch node (PH) is controlled by the switch inductor (L1) and the freewheeling diode (D2) to convert the high-frequency switch voltage into a stable DC voltage. The inductor L1 is used to smooth the current of the switch node and store and release energy. When the switch tube is turned off, the freewheeling diode D2 provides a current path to ensure continuous current output. The two capacitors C1 and C2 are used to filter out high-frequency noise in the output voltage to ensure the stability of the 5V output. R9 and R10 are feedback resistors connected to the VSENSE pin, and the feedback voltage is used to adjust the output voltage. The power supply subsystem converts the 12V or 24V input voltage into a 5V output voltage stably through the DC-DC buck converter circuit, input filtering, enable control, soft start, buck conversion and output filtering.
[0031] The present invention uses the Hall principle to detect the position information of the brushless motor rotor. A small magnet is installed on the rotating shaft connected to the motor, and the rotation angle is determined by measuring the change of the magnetic field. The position detection module is designed around the AS5600 rotary magnetic position sensor. A 3.3V DC power supply is used to drive the AS5600 to non-contactly measure the absolute angle of the radially magnetized coaxial magnet. The measured data is output in 12-bit analog format, and the SCL pin is connected to the MCU unit I 2 C clock signal line, connect the SDA pin to the MCU unit I 2C data signal, read the measured angle data through the IIC bus, and convert it into 0° to 360° angle data and 0-2π radian data through analog-to-digital conversion. The position detection subsystem also has zero point calibration and range setting functions. The specific steps are as follows: 1. Turn the motor to the zero position. 2. Write the zero point register (ZPOS) through IIC. 3. Turn the motor to the maximum position. 4. Through I 2 C writes to the maximum position register (MPOS).
[0032] Figure 6 A control block diagram of the FOC integrated control subsystem of the present invention is given, in which the implementation process of the FOC speed control loop, torque and weak magnetic control loop, position control loop, and current loop of the motor is given, and the execution steps of the corresponding software algorithm are as follows:
[0033] 1) Speed control loop: The target speed ω of the motor is input through the host computer or real-time user feedback * , the motor speed is adjusted by the speed PI controller to reach the target value, and the speed error is ω e =ω * -ω, the output of the PI controller is I q_ref =K p_speed ·ω e +K i_speed ∫ω e dt, where ω is the actual speed, K p_speed and K i_speed is the gain of the speed PI controller.
[0034] 2) Torque and weak magnetic control loop: The q-axis current reference value I corresponding to the target torque of the motor is input through the host computer or real-time user feedback q_ref Used to control torque. Under high-speed conditions, weak magnetic control is required. Usually set to 0 or a negative value. Input the d-axis current reference value I d_ref .
[0035] In the torque PI controller, the torque current error is: I q_e =I q_ref -I q . PI controller output: V q =K p_torque I qe +K i_torque ∫I qe dt. Among them, I q is the actual q-axis current, K p_torque and K i_torque is the gain of the torque PI controller.
[0036] In the field-weakening PI controller, the torque current error is: I d_e =Id_ref -I d . PI controller output: V d =K p_flux I d_e +K i_flux ∫I d_e dt. Among them, I d is the actual d-axis current, K p_flux and K i_flux is the gain of the field weakening PI controller.
[0037] 3) Position control loop: The desired motor is rotated to the target position θ through the host computer or real-time user feedback input * The corresponding angle, given in radians or degrees, is the target position θ * The error between the actual position θ and the e =θ * -θ. Position controller: The position error is calculated by the PD position controller to obtain the speed reference value: Among them, K p_pos and K d_pos are the proportional and derivative gains of the position controller.
[0038] 4) Park transformation: convert the three-phase current (Ia, Ib, Ic) to a two-phase stationary coordinate system (α, β), and then to a rotating coordinate system (d, q). Clarke transformation form (ABC to αβ): I α =I α , Transformation (αβ to dq): I d =I α cos(θ)+I β sin(θ)I q =-I α sin(θ)+I β cos(θ)
[0039] 4. SVPWM (Space Vector Pulse Width Modulation): Generates PWM signals to control the switching of the inverter, realizes the three-phase voltage output to the motor, calculates the reference voltage vector position, selects the appropriate voltage vector and switching time, and finally generates PWM signals to control the three-phase inverter.
[0040] 5. Low-side current sensing: In the FOC system, the current sensing resistor (shunt resistor) is usually placed between the motor phase line and the ground, that is, at the low side. The current flowing through the current sensing resistor produces a voltage drop. By detecting the voltage drop, the current flowing through the resistor can be calculated. Since the voltage drop across the current sensing resistor is usually very small, a high-precision differential amplifier is required to amplify the voltage drop, and the amplified voltage signal is sent to the ADC (analog-to-digital converter) for digitization. By using a known current sensing resistor value (R shunt ) and the measured voltage drop (V shunt ), the corresponding current (I) can be calculated. The formula is: Current sampling is performed when the low-side MOSFET is turned on to ensure that the current passing through the sampling resistor is the phase current. ADC sampling needs to be synchronized with the PWM signal, and the PWM frequency is usually 20 to 50kHz to ensure that the phase current can be accurately collected when the low-side MOSFET switches 20,000 to 50,000 times per second.
[0041] like Figure 7 As shown, the low-side current detection adopted by the present invention can realize the following functions to ensure the implementation of the FOC algorithm: hardware configuration, ADC resolution and frequency, automatic zero offset detection, drive synchronization, ADC acquisition event triggering, adaptive alignment with the drive phase, reading phase current, calculating the current vector size, and calculating the current of the FOC d and q axes.
[0042] The low-side current sensing algorithm consists of the following steps:
[0043] 1. Use the shunt resistor value, current sensing op amp gain, and phase of the ADC pin (phA, phB, phC)
[0044] Initialize the current sensor C++ instance. If you only want to measure 2 phase currents, you can mark the unused phases as _NC in the constructor.
[0045] 2. Use C++ to instantiate the following hardware association configurations: associate the current sensor with the driver, initialize the current sensor and calibrate it, and configure the ADC hardware to read the current and find the zero offset of each channel. Associate the current sensor with the motor.
[0046] 3. The phase function of the current sensor and driver is called through C++ instantiation. Then the phase current, current vector magnitude and the current of the d-axis and q-axis in the FOC driver are read.
[0047] The present invention uses Kalman filtering to filter the detected brushless motor phase current. The main process includes: 1) Prediction: predict the current state according to the system state transition equation. 2) Update: Combined with the actual measurement value, the predicted state is corrected according to the measurement equation. The specific implementation method is as follows:
[0048] 1) Assume that the time-varying model of the system state (current value) is a linear model: k =A·x k-1 +B·u k +w k Among them, x k is the current state (current value). A is the state transfer matrix. B is the control input matrix. u k is the control input. k is the process noise, which is assumed to be zero-mean Gaussian distributed. The measurement model is z k =H·x k +v k Among them, z k is the measured value (current measurement value). H is the measurement matrix. v k is the measurement noise, which is assumed to follow a zero-mean Gaussian distribution.
[0049] 2) Initialize the state estimate x^0, estimated covariance matrix P0, state transfer matrix A, control input matrix B, measurement matrix H, process noise covariance Q and measurement noise covariance R
[0050] 3) Predict the current state and covariance matrix:
[0051]
[0052] 4) Update the state estimate and covariance matrix based on the measurements:
[0053]
[0054] In current detection, Kalman filter can be used to predict and correct current signals, thereby effectively removing noise. is the updated state estimate, which is the best estimate of the state at the current moment. is the predicted state estimate, which is based on the prediction of the state at the previous moment. k is the actual measurement data at the current moment, K k stands for Kalman gain, which is a coefficient used to weigh the confidence between prediction and measurement. is the predicted covariance matrix, which represents the estimate of the prediction error. H is the measurement matrix, H T is the transpose of the measurement matrix, R is the measurement noise covariance matrix, P kis the updated covariance matrix, which represents the updated state estimation error.
[0055] 5) The above steps are performed in each control cycle to filter the detected phase current to obtain a smooth current signal for the FOC control algorithm.
[0056] like Figure 8 As shown, after the device of the present invention starts working, MCU1 first starts the hardware self-check to detect whether the corresponding power management, drive circuit, position sensor and communication interface can work normally. If there is a problem with the self-check, the device will enter the program to initialize the hardware environment; after the self-check is successful, MCU1 drives the position sensor unit to start working, and starts the motor unit to perform vector control with the initial position. At this time, the system determines whether the position feedback signal is successfully received. If the reception fails, the MCU1 response is returned to receive the position feedback signal again; after successfully receiving the position feedback signal data, MCU1 starts to use the FOC (field oriented control) algorithm to calculate The target torque and current vector are generated, and the corresponding PWM signal is generated through SVPWM (space vector pulse width modulation) to drive the motor, the torque output is adjusted in real time and the relevant data is stored in the database; after further optimizing the torque output using the error compensation algorithm, it is determined whether the preset rehabilitation training requirements are met. If the standard requirements are met, the storage display unit displays the current working status and data, and returns the control result. If the torque output does not meet the requirements, the system allows 3 cycle timeout detections and returns to MCU1 to respond and receive the position feedback signal again; at this point, the process of a rehabilitation training torque output is completed, and the device enters the cycle of reading the position sensor unit.
Claims
1. An upper limb rehabilitation robot precision torque output device based on vector control and space vector pulse width modulation, characterized in that: include: MCU unit, sensor subsystem, motor control subsystem, power supply subsystem and host computer communication subsystem; the MCU unit is based on vector control and space vector pulse width modulation. It controls the sensor subsystem through expanding GPIO to obtain the brushless motor position and speed data, and controls the motor output precise torque through the motor control subsystem to achieve precise control of the upper limb rehabilitation robot. The device realizes data communication with the host computer through WIFI and serial port communication protocol.
2. The upper limb rehabilitation robot precise torque output device according to claim 1, characterized in that: The sensor subsystem includes a position sensor, a speed sensor, and a current sensor, which can collect the position and speed data of the brushless motor rotor and the current value output by the speed loop in real time, and transmit them to the MCU for processing.
3. The upper limb rehabilitation robot precise torque output device according to claim 1, characterized in that: The motor drive unit includes a three-phase inverter, which uses SVPWM technology to achieve efficient voltage modulation to ensure accurate and stable motor output.
4. The upper limb rehabilitation robot precise torque output device according to claim 1, characterized in that: The sensor unit mainly includes a rotor position detection subsystem and a motor operating current detection subsystem, which are designed using AS5600 and INA240A2PWR respectively. The AS5600 angle sensor accurately detects the real-time position of the rotor through non-contact magnetic induction technology and outputs high-resolution angle information to support precise control of the FOC (field-oriented control) algorithm. The INA240A2PWR current sensing amplifier has high precision and high-speed response characteristics, can accurately monitor the current of each phase of the motor, and provide current data in real time to ensure the efficient operation of the SVPWM (space vector pulse width modulation) algorithm.
5. The upper limb rehabilitation robot precise torque output device according to claim 1, characterized in that: The MCU unit is built around the ESP32-D0WDQ6 chip, with a preset clock frequency of 160MHz, up to 240MHz, and an operating voltage of 3.0-3.6V. The unit is powered by 5V via Micro USB, and the voltage is reduced to 3.3V via an LDO low-dropout linear regulator. The MCU unit has 4MB Flash and 520KB SRAM, and is connected to a wireless network via built-in WiFi to build a host computer web service, and uses Bluetooth 4.2BR / EDR and UART for basic function debugging. The unit can be configured with two ADCs, ADC1 has 6 channels and ADC2 has 10 channels. The MCU unit also has IIC and SPI ports for information communication with general sensors.
6. A control method for the upper limb rehabilitation robot precision torque output device based on vector control and space vector pulse width modulation as claimed in claims 1 to 5, comprising the following steps: (1) Data acquisition: Obtain the brushless motor rotor position and speed data and the current value output by the speed loop; (2) Clarke transform and Park transform: convert motor current and voltage vectors; (3) Current control: Regulate the d-axis and q-axis currents through the PI controller; (4)SVPWM: Generates efficient PWM signals to control the motor.
7. The control method of the upper limb rehabilitation robot precise torque output device based on vector control and space vector pulse width modulation as claimed in claim 6, characterized in that: In step (1), the data acquisition step includes the installation and calibration of the position sensor and the speed sensor to ensure the accuracy of the position and speed data of the brushless motor rotor.
8. The control method of the upper limb rehabilitation robot precise torque output device based on vector control and space vector pulse width modulation as claimed in claim 6, characterized in that: In step (2), Clarke transformation converts the three-phase current into two-phase stationary coordinate system current, and Park transformation converts the two-phase stationary coordinate system current into rotating coordinate system current, thereby realizing independent control of the motor flux and torque. The three-phase current I a ,I b ,I c and the two-phase stationary coordinate system current I α ,I β The Clarke transformation formula is: a =I α , Then Park transforms the two-phase stationary coordinate system current I α ,I β Converted to rotating coordinate system current I d ,I q , the specific transformation formula is: Where θ is the electrical angle of the motor rotor detected by the sensor subsystem.
9. The control method of the upper limb rehabilitation robot precise torque output device based on vector control and space vector pulse width modulation as claimed in claim 6, characterized in that: In step (3), the current control is performed by adjusting the d-axis and q-axis currents respectively through the PI controller. The specific implementation of the PI controller is as follows: V d =K pd (I dref -I d )+K id ∫(I dref -I d )dt,V q =K pq (I qref -I q )+K iq ∫(I qref -I q )dt, where K pd , K id , K pq , K iq are the proportional and integral coefficients of the PI controller. Accurate current control can be achieved through the above adjustment to ensure that the torque output by the motor is accurate and stable.
10. The control method of the upper limb rehabilitation robot precise torque output device based on vector control and space vector pulse width modulation as claimed in claim 6, characterized in that: In step (4), SVPWM first calculates the reference voltage vector V ref The position in the three-phase stationary coordinate system, and then according to V ref The corresponding sector is determined by the position of the inverter, and then the action time T1, T2 of each vector and the zero vector time T0 in the sector are calculated. Finally, the corresponding PWM signal is generated to ensure the precise control of the motor output voltage and current by controlling the switching state of the inverter. The feedback control unit monitors the torque output by the motor and the position of the brushless motor rotor in real time, makes error adjustments, realizes adaptive control of the system, and improves the performance and reliability of the rehabilitation robot.
Citation Information
Cited By
Motor torque dynamic adjustment method and system based on analog input
CN119891881A
Joint motor driving control system of lower limb rehabilitation robot
CN120034040A
Lower limb rehabilitation robot joint motor drive control system
CN120034040B
