A hierarchical decoupling control system and control method of a multi-degree-of-freedom force feedback master hand

Abstract

The application relates to the technical field of remote operation robot force feedback interaction, in particular to a layered decoupling control system and a control method of a multi-degree-of-freedom force feedback master hand, the control system comprising a feedback force solving layer, an electric control driving layer, a first single-chip microcomputer module, a second single-chip microcomputer module and a power conversion and monitoring circuit module, in combination with a corresponding control method, the application divides the control architecture into the feedback force solving layer and the electric control driving layer, adopts a distributed hardware architecture to independently drive the parallel mechanism, the series mechanism and the clamping mechanism; the top control software SDK adopts a double-thread architecture of a main thread solving and a secondary thread receiving and sending, in combination with a bottom single-chip microcomputer timing synchronization mechanism, relies on an SPl bus and a DMA cyclic transmission to realize data interaction, completes real-time decoupling and output of three-dimensional space feedback force, so that the multi-degree-of-freedom force feedback master hand has the significant advantages of high real-time response, high-precision control and strong system stability.

Description

Technical Field

[0001] This invention relates to the field of force feedback interaction technology for teleoperated robots, and in particular to a hierarchical decoupling control system and control method for a multi-degree-of-freedom force feedback master hand. Background Technology

[0002] Force feedback teleoperation technology integrates visual and force perception, and relies on the master hand to realize remote control and real-time feedback of environmental forces. It can complete safe, precise and efficient long-distance operations and has broad application prospects in fields such as minimally invasive medicine, aerospace missions and precision assembly.

[0003] Existing force feedback actuators are mainly classified into three configurations: series, parallel, and hybrid. Series actuators offer convenient kinematic calculations, large workspace, and simple control, but suffer from low stiffness, large inertia, weak force feedback, and poor accuracy. Parallel actuators offer high accuracy, high stiffness, and large feedback force, but are structurally complex, have limited workspace, restricted attitude, complex position calculations, and are difficult to control. Hybrid actuators combine the advantages of series and parallel mechanisms, balancing large workspace and high precision, but existing control systems still have the following problems: High degree of freedom coupling: Series structures have a large number of drives, and parallel structures have large joint motion coupling, resulting in complex dynamic calculations and insufficient real-time performance; Limited control computing power: Multiple motor drives, multiple encoder acquisitions, and high-speed communication are concentrated in a single controller, and the computing power bottleneck leads to low control frequency and large delay; Insufficient force control accuracy: Position closed loops are mostly used, making it difficult to directly control the output force, resulting in distorted force feedback; Poor real-time communication: Traditional serial communication has a low rate, and the communication frequency is generally no more than 1kHz, while high-speed bus drives are complex, consuming a lot of microcontroller resources, and it is difficult to balance speed and performance; Lack of compensation mechanism: Gravity, inertial force, and spring force are not effectively compensated, resulting in poor naturalness of force feedback.

[0004] Therefore, the core challenges for master control systems to achieve highly accurate force feedback and remote operation lie in complex structural dynamics modeling, synchronous decoupling of multi-degree-of-freedom force signals, precise motor output force control, and ensuring system stability under high real-time conditions. Summary of the Invention

[0005] The purpose of this invention is to provide a hierarchical decoupling control system and control method for a multi-degree-of-freedom force feedback master, which achieves high real-time, high precision, and high stability force feedback teleoperation function through reasonable design of the system's hardware and software architecture.

[0006] To achieve the above objectives, this invention provides a hierarchical decoupling control system for a multi-degree-of-freedom force feedback master hand, used in a series-parallel hybrid teleoperation master hand. The teleoperation master hand includes a parallel mechanism with three degrees of freedom of translation, a series mechanism with three degrees of freedom of rotation, and an end-effector clamping mechanism. The system includes: The feedback force calculation layer, as the top layer of the hardware architecture, includes the control software SDK deployed on a general-purpose computer. The feedback force calculation layer adopts a dual-thread architecture, in which the main thread is configured to perform the decoupling and calculation of three-dimensional spatial feedback force to multi-joint torque, and the serial port transceiver thread is configured to perform high-speed data communication interaction. The electronic control drive layer, as the underlying hardware architecture, includes: The first microcontroller module is configured to control the first motor, the second motor and the third motor of the parallel mechanism, and to collect the position data of the first encoder, the second encoder and the third encoder corresponding to each motor. The second microcontroller module is configured to control the fourth motor of the serial mechanism and collect the position data of the corresponding fourth, fifth, sixth and seventh encoders. The second microcontroller module and the first microcontroller module interact with each other through the SPI communication interface. The power conversion and monitoring circuit module is configured to convert the input DC power into multiple voltage levels to power each motor, each microcontroller and corresponding auxiliary circuit, and to monitor the voltage and current of the input power in real time. When abnormal voltage or current fluctuations are detected to exceed preset thresholds (voltage preset threshold is 21V-29.4V, current preset threshold is 0-12A), power-off protection is triggered.

[0007] For a teleoperated master hand with a series-parallel hybrid structure, the parallel mechanism is a three-degree-of-freedom translational mechanism, with the three degrees of freedom being active. It includes three drive motors: a first motor, a second motor, and a third motor, enabling translational motion in three spatial directions. Each motor is equipped with a position encoder: a first encoder, a second encoder, and a third encoder, capable of acquiring position data in all three spatial directions. The series mechanism of the teleoperated master hand consists of a three-degree-of-freedom rotational mechanism and an end effector. The end effector is an active degree of freedom, containing a fourth motor and equipped with a fourth encoder, capable of simulating the opening and closing motion of the gripper and acquiring the gripper's opening and closing distance data. The three degrees of freedom of rotation are passive degrees of freedom, with only one position encoder per degree of freedom: a fifth encoder, a sixth encoder, and a seventh encoder, capable of acquiring rotational attitude data in all three spatial directions. Since the output torque of the motor is proportional to the output current, the encoder position can be converted into the rotational angle of each joint of the master hand. Therefore, the key points of the control system of this invention can be briefly summarized as follows: the control system is divided into two layers. The electric control drive layer, as the bottom layer control hardware, adopts a dual single-chip microcomputer distributed architecture. The first single-chip microcomputer module controls the parallel mechanism, and the second single-chip microcomputer module controls the series and clamping mechanism. The two achieve data interaction without CPU intervention through SPI interface and DMA loop transmission. The feedback force calculation layer, as the top layer, is deployed on the host computer after the control software SDK is deployed. It unifies the control cycle and uses a multi-source torque fusion compensation algorithm to calculate and output the feedback force in real time, realizing precise feedback force control. This solves the technical problems of strong degree of freedom coupling, insufficient computing power, poor real-time performance, and low force control accuracy of existing master control systems. It has high real-time performance, high precision, and strong stability, and is suitable for remote operation scenarios such as medical surgery, aerospace operations, and industrial precision assembly.

[0008] Furthermore, the control software SDK includes: The system initialization module is configured to configure and initialize various parameters of the remote control master and check various statuses of the master; The virtual serial communication module is configured to perform high-speed data interaction with the first microcontroller, receive data collected from the first microcontroller and the second microcontroller, and issue generator control commands to both of them. The kinematics solution module is configured to map angular positions in joint space to poses in Cartesian space; The dynamics calculation module is configured to calculate the feedback torque that each joint of the parallel mechanism needs to output based on the feedback force that needs to be output. The feedback force output module is configured to convert the calculated joint output torque into a target current value and send the encapsulated control data packet to the first microcontroller module and the second microcontroller module via a serial port. The system status monitoring module is configured to monitor the various operating parameters and working status of the control system in real time, and trigger protection mechanisms and alarm prompts when an anomaly is detected.

[0009] The control software SDK provides interfaces for core algorithms such as kinematics, dynamics, and feedback force calculation for the main hand. Because the calculation of the main hand's feedback force is highly complex, the microcontroller's computing power cannot meet real-time requirements; therefore, the control software SDK needs to be deployed on a general-purpose computer. This general-purpose computer can be a desktop computer, laptop computer, embedded industrial computer, or other common computer devices, and the operating system can be a common operating system such as Windows or Linux.

[0010] The main function of the system initialization module is to configure and initialize various parameters of the master control, and at the same time check whether the status of the master is normal, so as to ensure the smooth operation of subsequent operations.

[0011] The virtual serial communication module is mainly used for high-speed data interaction with the first microcontroller module. While commonly used serial communication is simple to program and easy to configure, its communication rate is generally no more than 6Mbps, which is insufficient for the high-speed communication needs of the master unit. Direct USB communication requires writing dedicated drivers, which is complex to configure. Using a virtual serial port combines the advantages of both, meeting high-speed communication requirements while reducing program complexity and simplifying the configuration process. By configuring the high-speed USB interface (USB-HS) of the microcontroller in the electronic control driver layer as a virtual serial port, the control software SDK package can achieve high-speed USB communication through simple serial communication programming, effectively ensuring real-time communication. This module is responsible for receiving encoder position, speed, and system status data from each joint collected by the first and second microcontroller modules, and issuing generator control commands to the two microcontrollers.

[0012] The kinematics calculation module is responsible for mapping the angular positions in joint space to poses in Cartesian space using the master hand kinematic model. Specifically, it uses the collected current joint angular position data and the master hand kinematic forward kinematics calculation formula to calculate the master hand's Cartesian space position and pose in the current joint space. Since the three-dimensional translational and three-dimensional rotational degrees of freedom of the hybrid configuration master hand are realized by parallel and series mechanisms respectively, the kinematic calculation formulas of the parallel and series mechanisms can be called separately to achieve decoupling of the translational and rotational degrees of freedom.

[0013] The dynamics calculation module is responsible for using the master hand's dynamics model to calculate the actual feedback torque required by the three joints of the parallel mechanism based on the desired three-dimensional spatial feedback force. Besides the feedback force, the master hand is also affected by gravity, friction, and spring forces during movement, thus requiring real-time calculation and dynamic compensation. This is achieved by applying corresponding compensation torques to the joint motors to counteract interference from gravity, friction, and spring forces. Since the joints of the master hand are assembled using precision bearings, joint friction is relatively small and its influence is usually ignored in actual calculations.

[0014] The mapping from the desired three-dimensional spatial feedback force to the joint spatial joint feedback torque can be calculated using the Jacobian matrix of the parallel mechanism.

[0015] The gravity compensation torque consists of two parts. First, since the rotating mechanism and the end clamping mechanism are fixed to the linkage's moving platform, their gravity magnitude and direction remain unchanged. Using the Jacobian matrix, this gravity can be mapped onto the three joints of the parallel mechanism. Second, the connecting rods of the parallel mechanism have constant mass and center of gravity position, and rotate around the joints, which can be directly converted into joint torque.

[0016] The spring is directly connected to the joint linkage and extends and retracts as the joint rotates. The spring torque can be calculated based on the angle of joint rotation.

[0017] Therefore, the final calculated joint output torque is composed of multiple components, including joint feedback torque, gravity compensation torque, and joint spring torque, thereby completing the synchronous decoupling of multi-degree-of-freedom force signals and realizing the multi-source torque fusion compensation algorithm.

[0018] In addition, the feedback force on the end-gripping mechanism of the master hand can be directly calculated based on the parameters of the mechanism to determine the target output torque of the fourth motor of the gripping mechanism.

[0019] The feedback force output module is responsible for converting the calculated joint output torque into the target current that the motor actually needs to output. It also packages this calculated data and sends it to two microcontrollers via serial port. By adjusting the motor's output current, the microcontrollers allow the operator to perceive the actual external feedback force.

[0020] The system status monitoring module is responsible for real-time monitoring of various operating parameters and working status of the master system. When an anomaly occurs, it can be handled in a timely manner and an alarm will be issued.

[0021] Furthermore, the first microcontroller module includes: The first timer group is configured to read the position data of the first encoder, the second encoder and the third encoder in real time in encoder input mode; The first analog-to-digital converter group is configured to synchronously read the output current of the first motor, the second motor and the third motor in real time in ADC sampling mode; The USB communication interface is configured to connect to an external USB conversion chip and its associated circuitry; and... The first SPI communication interface is configured to interact with the second microcontroller module.

[0022] The first microcontroller module is the core circuit module for controlling the parallel mechanism. It is responsible for tasks such as motor control, encoder and current data acquisition, and data transmission.

[0023] In one embodiment, the first microcontroller module uses a Cortex-M4 core microprocessor. It synchronously controls three motors in real time. Each motor has an independent drive circuit: a first motor drive circuit, a second motor drive circuit, and a third motor drive circuit. The three motor drive circuits operate on the same principle, using an H-bridge drive circuit. The first microcontroller module can output three PWM waves and, in conjunction with corresponding GPIO interfaces, can control the current and rotation direction of the three motors in real time.

[0024] The three timers inside the first microcontroller module are configured for encoder input mode, enabling real-time reading of the positions of the three encoders. Each encoder has an independent encoder conversion circuit: a first encoder conversion circuit, a second encoder conversion circuit, and a third encoder conversion circuit. These three encoder conversion circuits operate on the same principle, being differential-to-single-ended circuits. Their main function is to convert the differential signals of the encoder's A and B phases into single-ended signals for easier microcontroller processing.

[0025] The first microcontroller module contains three analog-to-digital converters configured in a triple ADC sampling mode, capable of synchronously reading the output current of the three motors in real time. Each motor has an independent current acquisition circuit, namely, a first motor current acquisition circuit, a first motor current acquisition circuit, and a first motor current acquisition circuit. These three current acquisition circuits operate on the same principle, using differential operational amplifier circuits. Their main function is to proportionally amplify the voltage across the sampling resistors, facilitating data acquisition by the microcontroller's ADC module.

[0026] The first microcontroller module has an internal high-speed USB interface (USB-HS), which connects to an external high-speed USB converter chip and its associated circuitry to enable high-speed USB communication. The first microcontroller module acts as a USB slave device and is configured in virtual serial port mode. Since USB communication drivers are relatively complex, while serial communication drivers are simpler, a virtual serial port mode is chosen. This approach not only meets the high-speed communication requirements of up to 480Mbps between the microcontroller and a general-purpose computer but also simplifies the communication program in the master SDK, facilitating the integration and invocation of master control algorithms.

[0027] The SPI interface inside the first microcontroller module is connected to the SPI interface inside the second microcontroller module for data transmission between the two microcontrollers. Since the first microcontroller module needs to control three motors and three encoders, and also perform USB and SPI communication functions, its CPU resource consumption is higher than that of the second microcontroller module. To balance the resource consumption of the two microcontroller CPUs, the SPI of the first microcontroller module is set to slave mode, and the SPI of the second microcontroller module is set to master mode. Both SPIs are configured in full-duplex synchronous communication mode. Because the SPI of the first microcontroller module is in slave mode, in conjunction with the corresponding DMA transfer channel, when a transmission request is received from the SPI master, the first microcontroller module can quickly synchronize data transmission and reception without any CPU operation, saving CPU resource consumption and ensuring the real-time communication between the two microcontrollers.

[0028] The first microcontroller module is also equipped with a key detection and status indicator circuit for key detection and display of system status.

[0029] Furthermore, the second microcontroller module includes: The second timer group is configured to read the position data of the fourth encoder, the fifth encoder, the sixth encoder and the seventh encoder in real time in encoder input mode; The second analog-to-digital converter group is configured to acquire sensor signals; The second SPI communication interface is configured in master mode and connected to the first SPI communication interface to realize data transmission.

[0030] The second microcontroller module is the core circuit module for controlling the series mechanism. This module uses a Cortex-M4 core microprocessor and is responsible for tasks such as motor control, encoder and current data acquisition, and data transmission.

[0031] The second microcontroller module, in conjunction with the fourth motor drive circuit, can control the fourth motor in real time. The fourth motor drive circuit is an H-bridge drive circuit. The PWM wave output by the second microcontroller module, along with the corresponding GPIO interface, can control the current and rotation direction of the fourth motor in real time.

[0032] The second microcontroller module contains four timers configured for encoder input mode, enabling real-time reading of the positions of the four encoders. Each encoder has an independent encoder conversion circuit: the fourth, fifth, sixth, and seventh encoder conversion circuits. These four circuits operate on the same principle, being differential-to-single-ended circuits. Their main function is to convert the differential signals of the encoder's A and B phases into single-ended signals for easier microcontroller processing.

[0033] Furthermore, the second analog-to-digital converter group includes: The first conversion channel is configured to read the output current of the fourth motor in real time; The second conversion channel is configured to work in conjunction with the power conversion and monitoring circuit module to acquire power voltage values ​​in real time.

[0034] The first conversion channel inside the second microcontroller module can read the output current of the fourth motor in real time. The current acquisition circuit of the fourth motor is a differential operational amplifier circuit, whose main function is to proportionally amplify the voltage across the sampling resistor to facilitate data acquisition by the microcontroller ADC module. The second conversion channel inside the second microcontroller module, in conjunction with the power conversion and monitoring module, can acquire the power supply voltage value in real time for monitoring the power supply status.

[0035] The SPI interface inside the second microcontroller module is connected to the first microcontroller module for data transmission between them. The SPI of the second microcontroller module is set to master mode, and in conjunction with its internal timer, it periodically initiates transmission requests to the slave device, achieving fast synchronous data transmission and reception. This configuration, with the second microcontroller module acting as the SPI master and the first microcontroller module as the SPI slave, effectively allocates the computing resources of the two microcontrollers, improving the system's communication capabilities.

[0036] The second microcontroller module also includes a key detection and status indicator circuit for additional key detection and system status display.

[0037] This invention also discloses a hierarchical decoupling control method for a multi-degree-of-freedom force feedback master, applied to the control system described above, comprising the following steps: S1: The main thread's main hand pose and feedback force calculation steps decouple the three-dimensional spatial feedback force to multi-joint torque; S2: The control steps of the serial port transceiver thread are to periodically receive data from the first microcontroller module and the second microcontroller module, and send the latest control data to both. S3: Control steps of the first microcontroller module: The first microcontroller module acts as an SPI slave, performs current closed-loop regulation and sensor data aggregation of the parallel mechanism, and uploads the aggregated parallel and series status data to the serial port transceiver thread. S4: Control steps of the second microcontroller module. The second microcontroller module acts as the SPI master, responds to the timing synchronization signal, performs current closed-loop regulation and sensor data aggregation on the serial mechanism side, and uploads the aggregated sensor data to the serial port transceiver thread of the feedback force calculation layer.

[0038] This invention discloses a hierarchical decoupling control system and method for a multi-degree-of-freedom force feedback master hand. The control system is configured with hardware in two layers. The bottom layer of control hardware uses a distributed architecture to control parallel mechanisms, as well as series and clamping mechanisms. The top layer of control software SDK adopts a dual-thread architecture, with the main thread performing calculations in conjunction with the secondary thread transmitting and receiving data. Combined with the corresponding control method, the timing of the bottom layer control hardware is synchronized. With the help of SPI bus and DMA loop transfer mechanism, the feedback force is calculated in real time and output, thereby achieving precise feedback force control. This enables the multi-degree-of-freedom force feedback master hand to have the advantages of real-time performance, high precision, and strong stability. Attached Figure Description

[0039] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0040] Figure 1 This is a schematic diagram of the electronic drive layer of a hierarchical decoupling control system for a multi-degree-of-freedom force feedback master hand according to the present invention.

[0041] Figure 2 This is a hardware architecture diagram of a hierarchical decoupling control system for a multi-degree-of-freedom force feedback master hand according to the present invention.

[0042] Figure 3 This is a flowchart illustrating the hierarchical decoupling control method for a multi-degree-of-freedom force feedback master hand according to the present invention.

[0043] Figure 4 This is a flowchart of the serial port transceiver thread of the control method for a multi-degree-of-freedom force feedback master hand according to the present invention.

[0044] Figure 5 This is a flowchart of the first microcontroller module of the control method for a multi-degree-of-freedom force feedback master hand according to the present invention.

[0045] Figure 6This is a flowchart of the second microcontroller module of the control method for a multi-degree-of-freedom force feedback master hand according to the present invention. Detailed Implementation

[0046] Embodiments of the present invention are described in detail below, examples of which are illustrated in the accompanying drawings. The embodiments described below with reference to the accompanying drawings are exemplary and intended to explain the present invention, and should not be construed as limiting the present invention.

[0047] In one embodiment, the formula for calculating the forward kinematics of the dominant hand is as follows: The three joint rotation angles of the three-degree-of-freedom parallel translation mechanism are θ1, θ2, and θ3. The actual θ1, θ2, and θ3 can be calculated by collecting the current position data of the first encoder, the second encoder, and the third encoder.

[0048] The three branches of the three-degree-of-freedom parallel translational mechanism are evenly distributed on the circumferential plane, with a fixed included angle. ,in This is the branch number.

[0049] Let L1 be the length of the driving arm, L2 be the length of the driven arm, and R be the difference between the circumradii of the stationary and moving platforms of the parallel mechanism, and make the following stipulations: Where i = 1, 2, 3 are the branch numbers.

[0050] Further provisions are made as follows: When C=0, the current position of the main hand in Cartesian space is: When C≠0, the current position of the main hand in Cartesian space is: The three joint angles of the three-degree-of-freedom rotary mechanism are θ4, θ5, and θ6. The actual θ4, θ5, and θ6 can be calculated by collecting the current position data from the fifth, sixth, and seventh encoders. Since the three degrees of freedom of the rotary mechanism are independent, the current posture of the master hand in Cartesian space is as follows: In addition, the clamping distance of the master end gripping mechanism is defined as L. C The actual L can be calculated by collecting the position data from the fourth encoder. C value.

[0051] Therefore, when data from all seven encoders is collected at once, the current pose of the master hand can be calculated. .

[0052] In the above formula, the calculation formula for parallel mechanisms is more complex, while the calculation formula for series mechanisms is very simple. This method of calculating the two separately can reduce the computational complexity to a certain extent and improve the real-time performance of the program.

[0053] In another embodiment, the calculation process for the joint output torque of the parallel mechanism is as follows: Let the gravity generated by the end mass of the parallel mechanism be , where m e Let g be the sum of the masses of the parallel mechanism's moving platform, the three-degree-of-freedom rotating mechanism, and the end-effector's clamping mechanism, and g be the acceleration due to gravity.

[0054] The expected three-dimensional spatial feedback force output by the parallel mechanism is .

[0055] The gravitational torque generated by the mass of the connecting rod in the parallel mechanism at each joint is: The torque generated by the joint spring of the parallel mechanism is , τ si k is the initial tension of the joint spring. si Let i be the spring constant of the joint spring, i = 1, 2, 3.

[0056] The Jacobian matrix of a parallel mechanism is The Jacobian matrix can be calculated using the kinematic parameters of the parallel mechanism and the current joint position.

[0057] Let the target output torque of the three active joints be The final calculation formula is as follows: Therefore, using the above formula, the actual output torque required by the three joints of the parallel mechanism can be calculated based on the desired three-dimensional feedback force.

[0058] In another embodiment, the formula for calculating the target output torque of the end-effector is: Where τ4 is the clamping joint torque, l c The rotation radius of the clamping mechanism, F c The desired clamping feedback force.

[0059] In another embodiment, the formula for calculating the target output current of the motor is as follows: Where j=1, 2, 3, 4 are the motor numbers, I j Let τ be the target output current of the j-th motor.j For the target output torque of the j-th joint, k j Let r be the torque constant of the j-th motor. j Let be the transmission ratio of the joint corresponding to the j-th motor.

[0060] Therefore, by using the relevant formulas in the above embodiments, the desired feedback force can be calculated in real time. Under the given conditions, the target output current of the four motors.

[0061] The master SDK program is divided into two threads: the main thread and the serial port transceiver thread. The program has serial port receive and transmit buffers, and the data of the two threads are exchanged in these buffers.

[0062] Please see Figure 3 The specific process of the main thread is as follows: First, after the program starts, it initializes various parameters of the master hand, including master hand mechanism parameters, serial port parameters, system status parameters, etc.

[0063] Then, open the serial port and connect to the master device. Once connected to the master device, start the serial port transmit / receive thread. At this point, you can receive various master data from the first microcontroller module and the second microcontroller module.

[0064] Then, the main thread directly calls the system status monitoring module to check whether the various states of the master system are normal. If the state is abnormal, an alarm is issued, the serial port transceiver thread is exited, the device serial port is closed, and finally the program exits. The program is restarted after the master system is in normal condition.

[0065] If all states of the master system are normal, the main thread reads and decompresses a data packet from the serial port receive buffer.

[0066] Then, the kinematics calculation module is called to calculate the current pose of the dominant hand.

[0067] Furthermore, if the desired feedback force is set... The dynamics calculation module is called to calculate the target output torque of each joint of the main hand.

[0068] Finally, the feedback force output module is invoked to convert the target output torque of the joint into the target output current of each motor. All data is then encapsulated and stored in the serial port transmission buffer, awaiting transmission from the serial port transceiver thread. This completes one calculation of the master hand pose and feedback force. The main thread then invokes the system status monitoring module again and enters the next round of loop calculation.

[0069] Since the main SDK program runs on a general-purpose computer, and the CPU of a general-purpose computer usually has strong computing power, combined with the aforementioned algorithm formula, the program's single loop calculation time generally does not exceed 200 microseconds, thus ensuring strong real-time computing capabilities.

[0070] Please see Figure 4 The specific process of the serial port send / receive thread is as follows: The main function of the serial port transceiver thread is to continuously receive data from the microcontroller and send the latest control data back to the microcontroller.

[0071] After the serial port transceiver thread starts, it first needs to initialize some data and the buffer. Secondly, a high-precision timer needs to be started to achieve timed data transmission and reception. Since the main hand uses virtual serial communication and the microcontroller is equipped with a high-speed USB communication module, the underlying data transmission is still implemented through high-speed USB communication. Considering that the minimum frame interval of high-speed USB is 125 microseconds, the timing interval can be set to 250 microseconds to ensure that data can be received and sent stably in real time.

[0072] When the timer expires, the data in the serial port transmit buffer is updated, and the latest calculation result is sent to the first microcontroller module and the second microcontroller module. Simultaneously, it checks whether the transmission was successful; if transmission fails, the failure count is incremented; if transmission succeeds, the failure count is reset to zero.

[0073] Next, resume data reception and perform the same success check again. If reception fails, increment the failure count; if reception succeeds, reset the failure count to zero. Then, update the serial port receive buffer with the latest received data for the main thread to read.

[0074] Thus, one timeout period constitutes one transmit / receive cycle. Setting the timeout period to 250 microseconds ensures the top-level program runs at a 4kHz control cycle.

[0075] Before the scheduled timeout period, the serial port is in an idle state. The program can use this idle time to check the previous transmission and reception status. If the number of transmission and reception failures exceeds the set value of 10, it indicates that the serial port transmission has suffered multiple consecutive packet losses, and the serial communication has a serious fault. At this time, the serial port needs to be set to an abnormal state, an alarm message needs to be output to the main thread, and the current thread needs to be exited.

[0076] Please see Figure 5 The specific control flow of the first microcontroller module is as follows: After the first microcontroller module powers on, it first initializes the system. Simultaneously, it configures the SPI of the first microcontroller module as a slave model and the DMA channel corresponding to the SPI transmit / receive mechanism as a circular transfer mode. In this way, the SPI of the first microcontroller module, acting as a slave, automatically reads data from the SPI transmit buffer and automatically updates the data in the SPI receive buffer under the control of the master (the SPI of the second microcontroller module), in conjunction with the DMA circular transfer function, achieving automatic data transmission and reception without consuming the CPU resources of the first microcontroller module.

[0077] Next, the high-speed USB interface inside the first microcontroller module is configured as a virtual serial port, and an attempt is made to receive data. The received data frame contains two pieces of information: one is control data from the first microcontroller module, and the other is control data from the second microcontroller module.

[0078] If a new data frame is received, the data segment from the second microcontroller module is updated in the SPI transmit buffer, and this data segment will be automatically transmitted. Simultaneously, the data segment from the first microcontroller module is calculated as the target output current for the first, second, and third motors, serving as the input for motor PID control in subsequent steps. If no new data is received, the previous data segment is directly calculated as the target output current for the first, second, and third motors, serving as the input for motor PID control in subsequent steps.

[0079] In addition, it is also necessary to collect the actual output current of the three motors. Since the three ADCs of the first microcontroller module are configured in triple ADC sampling mode, the actual output current of the three motors can be read synchronously in real time, with a sampling period of approximately 23 microseconds.

[0080] Then, the target output current and actual output current of the three motors are compared. A PID control algorithm is used to precisely control the output current of the first, second, and third motors by adjusting the duty cycle of the PWM wave in the corresponding motor drive circuit, thereby completing the feedback force output of the parallel mechanism. Since the position loop calculation is completed in the top-level SDK, the first and second microcontroller modules are only responsible for controlling the motor current loop. A single current loop adjustment process takes approximately 100 microseconds. Including data processing time, the control frequency of the current loop can reach up to 8kHz, thus ensuring high-precision real-time control of the feedback force.

[0081] Finally, it is necessary to collect the current position data of the first encoder, the second encoder, and the third encoder, package the data, and send it to the master SDK program via serial port. The data frame sent by the serial port of the first microcontroller module also contains two segments of information: one segment is the status data of the first microcontroller module, mainly the position data of the first encoder, the second encoder, and the third encoder; the other segment is the status data of the second microcontroller module, mainly the position data of the fourth encoder, the fifth encoder, the sixth encoder, and the seventh encoder, read from the SPI receive buffer of the first microcontroller module.

[0082] Thus, the first microcontroller module completes one control cycle. Since the serial port transmission and reception of the first microcontroller module is controlled by the serial port transmission and reception thread of the host SDK, the communication control cycle of the first microcontroller module is also 250 microseconds, ensuring the synchronization of data transmission.

[0083] Please see Figure 6 The specific control flow of the second microcontroller module is as follows: After the second microcontroller module powers on, it first performs system initialization settings. It configures the SPI to master mode and sets up the DMA channels and data transmit / receive buffers for SPI transmission and reception.

[0084] Then, start a timer. To keep the control cycle synchronized, the timer duration is set to the timer duration of the main hand SDK serial port transceiver thread, which is 250 microseconds.

[0085] Once the timer expires, an SPI transmission is initiated, with data automatically received and sent via the DMA channel. After the transmission is complete, the received data is automatically updated in the SPI receive buffer. Due to the high transmission rate and small data volume of SPI, the time consumed for a single SPI transmit / receive operation is extremely short, approximately 10 microseconds. Then, using the latest received data, the target output current of the fourth motor can be calculated. If the timer has not yet expired, the previous data is directly used to calculate the target output current of the fourth motor.

[0086] Then, the actual output current of the fourth motor is acquired. Based on the target and actual output current of the fourth motor, a PID control algorithm is used to adjust the duty cycle of the PWM wave in the fourth motor drive circuit, achieving precise control of the fourth motor's output current and thus completing the feedback force output of the clamping mechanism. Similarly, the second microcontroller module is only responsible for controlling the motor current loop. A single current loop adjustment process does not exceed 100 microseconds. Including data processing time, the control frequency of the current loop can reach up to 8kHz, thereby ensuring high-precision real-time control of the feedback force.

[0087] Finally, the current position data of the fourth, fifth, sixth, and seventh encoders needs to be collected and packaged to update the SPI transmit buffer. When SPI transmission is initiated next time, this data will be automatically sent to the receive buffer of the first microcontroller module. Since the timing of the second microcontroller module is also set to 250 microseconds, its completion of one communication control cycle is consistent with the top layer, ensuring data transmission synchronization.

[0088] Thus, the top layer of the entire master control system is responsible for position and feedback force calculation, with a control frequency of up to 4kHz, while the bottom layer is responsible for the precise control of the motor output force, with a control frequency of up to 8kHz, fully meeting the requirements of high real-time and high-precision control feedback force.

[0089] The above-disclosed embodiments are merely preferred embodiments of the present invention and should not be construed as limiting the scope of the invention. Those skilled in the art will understand that implementing all or part of the above-described embodiments and making equivalent changes in accordance with the claims of the present invention are still within the scope of the invention.

Claims

1. A hierarchical decoupling control system for a multi-degree-of-freedom force feedback master hand, used for a teleoperation master hand with a series-parallel hybrid structure, the teleoperation master hand comprising a parallel mechanism for three-degree-of-freedom translation, a series mechanism for three-degree-of-freedom rotation, and an end-effector clamping mechanism, characterized in that, The system includes: The feedback force calculation layer, as the top layer of the hardware architecture, includes the control software SDK deployed on a general-purpose computer. The feedback force calculation layer adopts a dual-thread architecture, in which the main thread is configured to perform the decoupling and calculation of three-dimensional spatial feedback force to multi-joint torque, and the serial port transceiver thread is configured to perform high-speed data communication interaction. The electronic control drive layer, as the underlying hardware architecture, includes: The first microcontroller module is configured to control the first motor, the second motor and the third motor of the parallel mechanism, and to collect the position data of the first encoder, the second encoder and the third encoder corresponding to each motor. The second microcontroller module is configured to control the fourth motor of the serial mechanism and collect the position data of the corresponding fourth, fifth, sixth and seventh encoders. The second microcontroller module and the first microcontroller module interact with each other through the SPI communication interface. The power conversion and monitoring circuit module is configured to convert the input DC power into multiple voltage levels to power each motor, each microcontroller and corresponding auxiliary circuit, and to monitor the voltage and current of the input power in real time. When abnormal voltage or current fluctuations are detected to exceed a preset threshold, power-off protection is triggered.

2. The hierarchical decoupling control system for a multi-degree-of-freedom force feedback master as described in claim 1, characterized in that, The control software SDK includes: The system initialization module is configured to configure and initialize various parameters of the remote control master and check various statuses of the master; The virtual serial communication module is configured to perform high-speed data interaction with the first microcontroller, receive data collected from the first microcontroller and the second microcontroller, and issue generator control commands to both of them. The kinematics solution module is configured to map angular positions in joint space to poses in Cartesian space; The dynamics calculation module is configured to calculate the feedback torque that each joint of the parallel mechanism needs to output based on the feedback force that needs to be output. The feedback force output module is configured to convert the calculated joint output torque into a target current value and send the encapsulated control data packet to the first microcontroller module and the second microcontroller module via a serial port. The system status monitoring module is configured to monitor the various operating parameters and working status of the control system in real time, and trigger protection mechanisms and alarm prompts when an anomaly is detected.

3. A hierarchical decoupling control system for a multi-degree-of-freedom force feedback master hand as described in claim 1 or 2, characterized in that, The first microcontroller module includes: The first timer group is configured to read the position data of the first encoder, the second encoder and the third encoder in real time in encoder input mode; The first analog-to-digital converter group is configured to synchronously read the output current of the first motor, the second motor and the third motor in real time in ADC sampling mode; The USB communication interface is configured to connect to an external USB conversion chip and its associated circuitry; and... The first SPI communication interface is configured to interact with the second microcontroller module.

4. The hierarchical decoupling control system for a multi-degree-of-freedom force feedback master as described in claim 3, characterized in that, The second microcontroller module includes: The second timer group is configured to read the position data of the fourth encoder, the fifth encoder, the sixth encoder and the seventh encoder in real time in encoder input mode; The second analog-to-digital converter group is configured to acquire sensor signals; The second SPI communication interface is configured in master mode and connected to the first SPI communication interface to realize data transmission.

5. The hierarchical decoupling control system for a multi-degree-of-freedom force feedback master as described in claim 4, characterized in that, The second analog-to-digital converter group includes: The first conversion channel is configured to read the output current of the fourth motor in real time; The second conversion channel is configured to work in conjunction with the power conversion and monitoring circuit module to acquire power voltage values ​​in real time.

6. A hierarchical decoupling control method for a multi-degree-of-freedom force feedback master, applied to the control system described in any one of claims 1 to 5, characterized in that, Includes the following steps: S1: The main thread's main hand pose and feedback force calculation steps decouple the three-dimensional spatial feedback force to multi-joint torque; S2: The control steps of the serial port transceiver thread are to periodically receive data from the first microcontroller module and the second microcontroller module, and send the latest control data to both. S3: Control steps of the first microcontroller module: The first microcontroller module acts as an SPI slave, performs current closed-loop regulation and sensor data aggregation of the parallel mechanism, and uploads the aggregated parallel and series status data to the serial port transceiver thread. S4: Control steps of the second microcontroller module. The second microcontroller module acts as the SPI master, responds to the timing synchronization signal, performs current closed-loop regulation and sensor data aggregation on the serial mechanism side, and uploads the aggregated sensor data to the serial port transceiver thread of the feedback force calculation layer.

7. The hierarchical decoupling control method for a multi-degree-of-freedom force feedback master hand as described in claim 6, characterized in that, Step S1, the main thread's main hand pose and feedback force calculation steps, specifically include the following steps: S11: After startup, initialize the parameters of the master hand and connect the remote master hand to start the serial port technique thread; S12: Call the system status monitoring module to check the status of the master system. If the status is abnormal, trigger an alarm and exit the program. If the status is normal, read and decompress the data packet from the serial port receive buffer. S13: Call the kinematics solution module to calculate the current pose of the main hand, and call the dynamics solution module to calculate the target output torque of each joint based on the desired feedback force; S14: Call the feedback force output module to convert the target output torque into the target output current of each motor, and encapsulate all the data and store it in the serial port transmission buffer, waiting for the serial port transceiver thread to send it. S15: Recall the system status monitoring module and execute steps S12~S14 to enter the next round of loop calculation.

8. The hierarchical decoupling control method for a multi-degree-of-freedom force feedback master hand as described in claim 6, characterized in that, Step S2, the control steps of the serial port transceiver thread, specifically include the following steps: S21: Initialize the data and buffer, and start a high-precision timer to realize the timed transmission and reception of underlying data based on the USB communication protocol; S22: When the timer expires, update the serial port transmit buffer to send the latest control data to the first microcontroller module and the second microcontroller module, and receive data from both. S23: Determine the data sending and receiving status. If sending fails, increment the sending failure count by one; if sending succeeds, set the sending failure count to zero. If receiving fails, increment the receiving failure count by one; if receiving succeeds, set the receiving failure count to zero. S24: Update the latest received data to the serial port receive buffer for the main thread to read; S25: Check the transmit / receive status before the timeout period. If the number of transmit / receive failures exceeds the set value, the serial communication is determined to be abnormal. An alarm message is output to the main thread and the current thread is exited. If the number of failures does not exceed the set value, return to step S22.

9. A hierarchical decoupling control method for a multi-degree-of-freedom force feedback master hand as described in any one of claims 6 to 8, characterized in that, Step S3, the control steps of the first microcontroller module, specifically include the following steps: S31: After power-on, the system is initialized, SPI is configured as slave mode, and the DMA channel corresponding to SPI transmission and reception is configured as cyclic transfer mode. S32: Configure the internal high-speed USB interface to virtual serial port mode to receive data frames containing the first microcontroller module control data segment and the second microcontroller module control data segment; S33: If a new data frame is received, the control data segment of the second microcontroller module is updated to the SPI transmit buffer, and the control data segment of the first microcontroller module is calculated as the target output current of the first motor, the second motor and the third motor; if no new data frame is received, the target output current is calculated using the data segment of the previous cycle. S34: Utilizes a triple ADC sampling mode to synchronously read the actual output current of the first motor, second motor, and third motor in real time; S35: Compare the target output current with the actual output current, and use the PID control algorithm to adjust the duty cycle of the PWM wave of the corresponding motor drive circuit. The current closed-loop control of the parallel mechanism is completed in a single adjustment process of about 100 microseconds to realize feedback force output. S36: Collect the current position data of the first encoder, the second encoder and the third encoder, and send the status data packet containing the position data of the first encoder, the second encoder and the third encoder and the position data of the fourth encoder, the fifth encoder, the sixth encoder and the seventh encoder read from the SPI receive buffer to the serial port transceiver thread of the feedback force calculation layer through the serial port; S37: Repeat steps S31 to S36 in the control cycle to ensure data transmission synchronization.

10. The hierarchical decoupling control method for a multi-degree-of-freedom force feedback master hand as described in claim 9, characterized in that, In step S4, the control steps of the second microcontroller module specifically include the following steps: S41: After power-on, initialize the system, configure SPI to master mode, and set the DMA channel and data transmission / reception buffer corresponding to SPI transmission and reception. S42: Start the timer to keep it synchronized with the control cycle of the serial port transceiver thread; S43: When the timer expires, initiate an SPI transmission, automatically receive and send data through the DMA channel, and automatically update the received data to the SPI receive buffer. S44: Calculate the target output current of the fourth motor using the latest data in the SPI receive buffer. If the timing time has not been reached, use the data from the previous cycle to calculate the target output current. S45: Collect the actual output current of the fourth motor, and adjust the PWM wave duty cycle of the fourth motor drive circuit according to the target output current and the actual output current using the PID control algorithm to achieve feedback force output; S46: Collect the current position data of the fourth, fifth, sixth, and seventh encoders, and package the data to update the SPl transmission buffer so that it can be sent to the first microcontroller module the next time SPl transmission is started; S47: Repeat steps S41 to S46 in the control cycle to ensure data transmission synchronization.

Images