Body-equipped intelligent robot joint module system adopting force-position hybrid control algorithm and control method

Abstract

The invention discloses an intelligent robot joint module system adopting a force-position hybrid control algorithm and a control method. The intelligent robot joint module system with the body comprises a joint, a robot joint execution part, a joint driving motor, a harmonic reducer, a magnetic encoder, an inductance type encoder, a strain type torque sensor module, a six-dimensional force sensor module, an imu sensor module, an embedded processor, a high-speed communication interface, a safety circuit and a whole machine central controller. According to the invention, high-precision force-position cooperative control is realized, accurate control of contact force + / -0.1 N is achieved, control period delay is guaranteed to be within 500 microseconds, force control precision and motion response speed are taken into consideration, system expansibility, maintainability and control stability are improved, reliable operation under a load with a peak value of 150Nm is supported, an integrated safety protection mechanism is provided, and the system is suitable for large-scale popularization and application. According to the invention, the operation requirements of various body-equipped intelligent robots such as precise assembly, material carrying, man-machine cooperation and the like are met, and the universality and the practicability are high.

Description

Technical Field

[0001] This invention relates to a joint module system and control method for an embodied intelligent robot that employs a force-position hybrid control algorithm. Background Technology

[0002] In the field of embodied intelligent robots, the joint module, as the core execution unit, directly determines the overall robot's performance through its force control accuracy, motion compliance, and environmental adaptability. Existing embodied intelligent robot joint modules rely solely on encoders to collect position data, employing pure position closed-loop control without torque detection units. This results in a lack of real-time force feedback, making them prone to rigid collisions with the environment, damaging the work object or joint components. They are unsuitable for compliant operation scenarios such as precision assembly and human-machine interaction. Furthermore, they lack contact state recognition, failing to switch control modes based on different states. The absence of feedforward compensation based on dynamic models leads to position overshoot, force control fluctuations, and insufficient control accuracy and response speed under high loads or high-speed movements. Sensor fusion capabilities are weak, relying solely on force and torque data without incorporating IMU attitude and dual encoder position data. This results in incomplete motion state estimation, delayed collision detection, and insufficient safety protection. Control mode switching lacks clear state judgment criteria, easily causing switching stutters and response delays, preventing seamless switching. The absence of safety protections (overcurrent, overtemperature, position over-limit, and collision detection linkage) leads to low safety and a short lifespan. Summary of the Invention

[0003] The purpose of this invention is to overcome the shortcomings of the prior art and provide an embodied intelligent robot joint module system and control method that employs a force-position hybrid control algorithm.

[0004] A embodied intelligent robot joint module system employing a force-position hybrid control algorithm includes a joint, a robot joint actuator, a joint drive motor, a harmonic reducer, a magnetic encoder, an inductive encoder, a strain gauge torque sensor module, a six-dimensional force sensor module, an IMU sensor module, an embedded processor, a high-speed communication interface, a safety circuit, and a central controller for the entire machine. The output shaft of the joint drive motor is connected to the input end of the harmonic reducer, and the output end of the harmonic reducer is connected to the joint through the robot joint actuator. The magnetic encoder is mounted on the joint drive motor, the inductive encoder is mounted on the output end of the harmonic reducer, the strain gauge torque sensor module is mounted on the output end of the harmonic reducer, the six-dimensional force sensor module is mounted on the end effector side of the joint, and the IMU sensor module is mounted on the joint. The magnetic encoder, inductive encoder, strain gauge torque sensor module, six-dimensional force sensor module, IMU sensor module, and joint drive motor are all electrically connected to the embedded processor. The embedded processor communicates bidirectionally with the central controller for the entire machine through the high-speed communication interface, and the safety circuit is connected to the embedded processor.

[0005] Preferably, the magnetic encoder is used to collect the angular position data of the output shaft of the joint drive motor, the inductive encoder is used to collect the final output angular position data of the joint, and the strain gauge torque sensor module is used to collect the torque data during joint movement.

[0006] Preferably, the six-dimensional force sensor module is used to collect the linear forces (Fx, Fy, Fz) along the X / Y / Z axes and the rotational torques (Mx, My, Mz) around the X / Y / Z axes when the joints are in contact.

[0007] Preferably, the IMU sensor module is used to collect the three-dimensional attitude angles of the joint, angular velocity data around the X / Y / Z axes, and linear acceleration data along the X / Y / Z axes. The three-dimensional attitude angles are roll angle, pitch angle, and yaw angle.

[0008] Preferably, the safety circuit includes an overcurrent circuit, an overtemperature circuit, and a position limiting circuit. The overcurrent circuit is used for overcurrent protection, the overtemperature circuit is used for overtemperature protection, and the position limiting circuit is used for position over-limit protection.

[0009] Preferably, the software architecture of an embedded processor includes a top-down control layer, driver layer, and firmware layer.

[0010] The control layer includes a force-position hybrid controller layer, a state observation layer, and a fault detection layer. The force-position hybrid controller layer is used to run the force-position hybrid control algorithm to calculate the joint target torque and position control quantity. The state observation layer is used to infer the real-time motion state of the joint and detect the state based on the data collected by each sensor module. The fault detection layer is used to detect system hardware / software abnormalities and trigger fault handling procedures.

[0011] The drive layer includes a motor driver, a sensor interface layer, and a safety monitoring layer. The motor driver receives control signals from the control layer and converts them into torque and position control commands that the joint drive motor can execute. The sensor interface layer enables data transmission between the magnetic encoder, inductive encoder, strain gauge torque sensor module, six-dimensional force sensor module, IMU sensor module, and embedded processor, and synchronously completes data filtering and calibration preprocessing. The safety monitoring layer is used to link with the safety circuit, monitor overcurrent, overtemperature, and position over-limit states in real time, and feed them back to the control layer.

[0012] The firmware layer includes a real-time operating system, a hardware abstraction layer, and a bootloader.

[0013] The real-time operating system is used to control the real-time scheduling of tasks, ensuring that the control cycle delay is within 500μs. The hardware abstraction layer is used to realize the resource abstraction and calling of the embedded processor and various hardware components, reducing the module coupling. The bootloader is used to trigger hardware self-test and initialization after the system is powered on, and to build the basic operating environment.

[0014] Preferably, the states include collision state, contact state, free space state, and transition region state. The real-time joint motion states include joint kinematic state, joint posture state, joint contact trend state, and environmental interaction correlation state. The joint kinematic state includes motor shaft speed, joint final output speed, and joint translational acceleration information. The joint posture state includes the joint's posture orientation and posture change rate in space. The joint contact trend state is a prediction of whether the joint is about to enter a collision state, contact state, free space state, or transition region state. The environmental interaction correlation state is an online acquisition and real-time update of environmental stiffness values ​​in the range of 0.1-10000 N / mm, while simultaneously determining whether the environment is rigid or flexible.

[0015] Preferably, the collision state is an unintended contact state in which the joint and the contact surface are excessively compressed; the contact state is a state in which the joint and the contact surface are in stable contact; the free space state is an unconstrained state in which the joint is far away from the contact surface and no contact occurs; and the transition region state is an intermediate state between no contact and stable contact.

[0016] A control method for a force-position hybrid control joint module of an embodied intelligent robot includes the following steps:

[0017] Step S1: Power on the system;

[0018] Step S2: System initialization. The firmware layer triggers a hardware self-test to complete the initial adaptation between the embedded processor and each hardware component.

[0019] Step S3: Load the preset parameters and system configuration file for joint control in the firmware layer to complete the parameter initialization of the control task;

[0020] Step S4: The driver layer receives control commands from the central controller of the whole machine through the high-speed communication interface;

[0021] Step S5: The control layer identifies and parses the control command to determine whether the command type is trajectory tracking, direct control, or teaching mode;

[0022] Step S6: If the instruction type is trajectory tracking, the control layer plans the joint motion path; if it is direct control, the control layer sets the force control target and position control target of the joint; if it is teaching mode, enter compliant teaching mode, that is, when the human body pulls the joint, the joint drive motor passively follows the direction of the human hand pulling.

[0023] Step S7: The firmware layer schedules and controls tasks to enter the main control loop through the real-time operating system.

[0024] Step S8: The sensor interface module of the driving layer collects sensing unit data and performs filtering and calibration preprocessing on the data. The sensing unit data includes: the joint drive motor output shaft angular position data collected by the magnetic encoder, the joint final output angular position data collected by the inductive encoder, the joint motion torque data collected by the strain torque sensor module, the X / Y / Z axis linear forces Fx, Fy, Fz and rotational torques Mx, My, Mz and imu around the X / Y / Z axis when the joint is in contact collected by the six-dimensional force sensor module, and the joint three-dimensional attitude angle, angular velocity data around the X / Y / Z axis and linear acceleration data along the X / Y / Z axis collected by the sensor module 9.

[0025] Step S9: The state observation layer calculates the real-time motion state of the joint based on the preprocessed sensor data. The real-time motion state includes the joint kinematic state, joint posture state, joint contact trend state, and environmental interaction correlation state. First, it determines whether the current working environment is rigid or flexible. If the environmental stiffness value is ≥500N / mm, it is a rigid environment; otherwise, it is a flexible environment, thereby completing the identification of the rigid and flexible characteristics of the environment.

[0026] Step S10: The state observation layer detects the joint contact state and matches the control strategy with the rigidity and flexibility characteristics of the environment: If it is a collision state, regardless of whether it is a rigid or flexible environment, the force-position hybrid controller layer performs a safety reflection, immediately cuts off the torque output and reverses to avoid collision; if it is a contact state, in a rigid environment, the force-position hybrid controller layer increases the joint stiffness to 1000-10000 N / m and reduces the damping to 50-100 N·s / m, performing high-rigidity torque control; in a flexible environment, the force-position hybrid controller layer reduces the joint stiffness to 0.1-1000 N / m and increases the damping to 100-200 N·s / m, performing compliant torque control; if it is a free space state, the force-position hybrid controller layer performs position control to maintain high-rigidity positioning; if it is a transition region state, in a rigid environment, the force-position hybrid controller layer matches high-rigidity impedance control; in a flexible environment, the force-position hybrid controller layer matches low-rigidity impedance control, achieving precise adaptation of "rigid to rigid, flexible to flexible";

[0027] Step S11: The force-position hybrid controller layer runs the force-position hybrid control algorithm according to the current control mode to calculate the target torque and target position of the joint;

[0028] Step S12: The force-position hybrid controller layer combines the joint dynamics model to perform feedforward compensation on the target torque and target position to obtain the compensated target control quantity.

[0029] Step S13: The safety monitoring module of the drive layer sets the upper and lower limits of torque and current, and at the same time detects whether overcurrent, overtemperature or position limit protection is triggered. If it is not triggered, proceed to step S14; otherwise, perform a safety shutdown and exit the main control loop.

[0030] Step S14: The motor drive module of the drive layer converts the compensated target control quantity into an execution command and sends it to the joint drive motor to drive the joint movement.

[0031] Step S15: The safety monitoring module of the driver layer and the fault detection module of the control layer monitor the system's operating status in real time and detect whether there are any hardware or software abnormalities.

[0032] Step S16: If an anomaly is detected, the control layer executes the fault handling procedure, saves the system safety status, and reports the fault information to the central controller of the whole machine through the high-speed communication interface, and then returns to step S4; if no anomaly is detected, it is determined whether the current control task is completed. If it is completed, it returns to the status observation layer to the idle state and enters step S4; otherwise, it returns to step S7.

[0033] The beneficial effects of this invention are as follows:

[0034] 1. This invention achieves high-precision force-position coordinated control. By integrating multiple sensors and feeding forward compensation for the target torque and target position, it achieves precise control of the contact force within ±0.1N, while ensuring that the control cycle delay is within 500μs, thus balancing force control accuracy and motion response speed.

[0035] 2. This invention has full-scene adaptive control capability. It can automatically match four modes—safety reflection, torque control, position control, and impedance control—based on four states: collision, contact, free space, and transition area, to achieve seamless switching and smooth interaction.

[0036] 3. This invention constructs a clear three-layer software architecture (control layer, driver layer, firmware layer), and achieves software and hardware decoupling through the hardware abstraction layer and real-time operating system, thereby improving system scalability, maintainability and control stability, and supporting reliable operation under peak loads of 150Nm.

[0037] 4. This invention provides an integrated safety protection mechanism that combines hardware protection against overcurrent, overtemperature, and position over-limit with collision detection and fault handling software protection, providing comprehensive safety for joint modules, work objects, and human-machine interaction.

[0038] 5. This invention supports wide-range environmental stiffness adaptation, can acquire and update environmental stiffness values ​​from 0.1 to 10000 N / mm online, automatically adjust joint impedance parameters, achieve precise matching of "rigid to rigid, flexible to flexible", and adapt to complex and ever-changing working environments.

[0039] 6. This invention integrates multi-mode operation capabilities, supports trajectory tracking, direct control and compliant teaching, and meets the needs of various embodied intelligent robot operations such as precision assembly, material handling and human-machine collaboration. It has strong versatility and practicality. Attached Figure Description

[0040] Figure 1 This is a system block diagram of the present invention;

[0041] Figure 2 This is a module connection diagram for a safety circuit;

[0042] Figure 3 This is a software architecture diagram of an embedded processor. Detailed Implementation

[0043] The technical solution of the present invention will be further described below with reference to the accompanying drawings:

[0044] like Figure 1 As shown, an embodied intelligent robot joint module system employing a force-position hybrid control algorithm includes a joint 1, a robot joint actuator 2, a joint drive motor 3, a harmonic reducer 4, a magnetic encoder 5, an inductive encoder 6, a strain gauge torque sensor module 7, a six-dimensional force sensor module 8, an IMU sensor module 9, an embedded processor 11, a high-speed communication interface 13, a safety circuit 12, and a central controller 14. The output shaft of the joint drive motor 3 is connected to the input end of the harmonic reducer 4, and the output end of the harmonic reducer 4 is connected to the joint 1 through the robot joint actuator 2. The magnetic encoder 5 is mounted on the joint drive motor 3. The inductive encoder 6 is installed at the output end of the harmonic reducer 4, the strain gauge torque sensor module 7 is installed at the output end of the harmonic reducer 4, the six-dimensional force sensor module 8 is installed on the end actuation side of the joint 1, and the IMU sensor module 9 is installed on the joint 1. The magnetic encoder 5, the inductive encoder 6, the strain gauge torque sensor module 7, the six-dimensional force sensor module 8, the IMU sensor module 9, and the joint drive motor 3 are all electrically connected to the embedded processor 11. The embedded processor 11 communicates bidirectionally with the central controller 14 of the whole machine through the high-speed communication interface 13. The safety circuit 12 is connected to the embedded processor 11.

[0045] like Figure 1As shown, the magnetic encoder 5 is used to collect the angular position data of the output shaft of the joint drive motor 3, the inductive encoder 6 is used to collect the final output angular position data of the joint 1, and the strain gauge torque sensor module 7 is used to collect the torque data of the joint 1 during movement. The six-dimensional force sensor module 8 is used to collect the X / Y / Z axis linear forces Fx, Fy, Fz and the rotational torques Mx, My, Mz around the X / Y / Z axes when the joint 1 is in contact. The IMU sensor module 9 is used to collect the three-dimensional attitude angles of the joint 1, the angular velocity data around the X / Y / Z axes, and the linear acceleration data along the X / Y / Z axes. The three-dimensional attitude angles are roll angle, pitch angle, and yaw angle.

[0046] like Figure 2 As shown, the safety circuit 12 includes an overcurrent circuit 121, an overtemperature circuit 122, and a position limiting circuit 123. The overcurrent circuit 121 is used for overcurrent protection, the overtemperature circuit 122 is used for overtemperature protection, and the position limiting circuit 123 is used for position over-limit protection.

[0047] like Figure 3 As shown, the software architecture of the embedded processor 11 includes, from top to bottom, a control layer 15, a driver layer 16, and a firmware layer 17.

[0048] The control layer 15 includes a force-position hybrid controller layer 151, a state observation layer 152, and a fault detection layer 153. The force-position hybrid controller layer 151 is used to run a force-position hybrid control algorithm to calculate the target torque and position control quantity of joint 1. The state observation layer 152 is used to infer the real-time motion state of joint 1 and detect the state based on the data collected by each sensor module. The fault detection layer 153 is used to detect system hardware / software abnormalities and trigger fault handling procedures.

[0049] The drive layer 16 includes a motor driver 161, a sensor interface layer 162, and a safety monitoring layer 163. The motor driver 161 receives control signals from the control layer 15 and converts them into torque and position control commands that the joint drive motor 3 can execute. The sensor interface layer 162 enables data transmission between the magnetic encoder 5, inductive encoder 6, strain gauge torque sensor module 7, six-dimensional force sensor module 8, IMU sensor module 9, and embedded processor 11, and synchronously completes data filtering and calibration preprocessing. The safety monitoring layer 163 is used to link with the safety circuit 12, monitor overcurrent, overtemperature, and position over-limit states in real time, and feed them back to the control layer 15.

[0050] The firmware layer 17 includes a real-time operating system 171, a hardware abstraction layer 172, and a bootloader 173.

[0051] The real-time operating system 171 is used to control the real-time scheduling of tasks and ensure that the control cycle delay is within 500μs. The hardware abstraction layer 172 is used to realize the resource abstraction and calling between the embedded processor 11 and various hardware components, reducing the module coupling. The boot program 173 is used to trigger hardware self-test and initialization after the system is powered on, and to build the basic operating environment.

[0052] The states include collision state, contact state, free space state, and transition region state. The real-time motion state of joint 1 includes joint kinematic state, joint posture state, joint contact trend state, and environmental interaction correlation state. The joint kinematic state includes motor shaft speed, joint final output speed, and joint translational acceleration information. The joint posture state includes the posture orientation and posture change rate of joint 1 in space. The joint contact trend state is the prediction of whether joint 1 is about to enter a collision state, contact state, free space state, or transition region state. The environmental interaction correlation state is the online acquisition and real-time updating of environmental stiffness values ​​in the range of 0.1-10000 N / mm, while determining whether the environment is rigid or flexible.

[0053] The collision state is an unexpected contact state in which joint 1 is excessively squeezed against the contact surface; the contact state is a state in which joint 1 is in stable contact with the contact surface; the free space state is an unconstrained state in which joint 1 is far away from the contact surface and no contact occurs; the transition region state is an intermediate state between joint 1 being in no contact and stable contact.

[0054] A control method for a joint module of an embodied intelligent robot employing a force-position hybrid control algorithm includes the following steps:

[0055] Step S1: Power on the system;

[0056] Step S2: System initialization. The firmware layer 17 triggers a hardware self-test to complete the initial adaptation between the embedded processor 11 and each hardware component.

[0057] Step S3: The firmware layer 17 loads the preset parameters, stiffness threshold and system configuration file of the joint 1 control, and completes the parameter initialization of the control task. The stiffness threshold is 500 N / mm.

[0058] Step S4: The drive layer 16 receives control commands issued by the central controller 14 of the whole machine through the high-speed communication interface 13;

[0059] Step S5: The control layer 15 identifies and parses the control command, and determines the command type as trajectory tracking, direct control, or teaching mode;

[0060] Step S6: If the instruction type is trajectory tracking, the control layer 15 plans the motion path of joint 1; if it is direct control, the control layer 15 sets the force control target and position control target of joint 1; if it is teaching mode, enter the compliant teaching mode, that is, when the human body pulls joint 1, the joint drive motor 3 passively follows the direction of the human hand pulls.

[0061] Step S7: The firmware layer 17 schedules and controls tasks to enter the main control loop through the real-time operating system 171;

[0062] Step S8: The sensor interface module of the drive layer 16 collects sensing unit data and performs filtering and calibration preprocessing on the data. The sensing unit data includes: the output shaft angular position data of the joint drive motor 3 collected by the magnetic encoder 5, the final output angular position data of the joint 1 collected by the inductive encoder 6, the motion torque data of the joint 1 collected by the strain torque sensor module 7, the X / Y / Z axis linear forces Fx, Fy, Fz and rotational torques Mx, My, Mz and imu of the joint 1 when in contact collected by the six-dimensional force sensor module 8, and the three-dimensional attitude angle, angular velocity data around the X / Y / Z axis and linear acceleration data along the X / Y / Z axis of the joint 1 collected by the sensor module 9.

[0063] Step S9: The state observation layer 152 calculates the real-time motion state of joint 1 based on the preprocessed sensor data. The real-time motion state includes joint kinematic state, joint posture state, joint contact trend state, and environmental interaction correlation state. It first determines whether the current working environment is rigid or flexible. If the environmental stiffness value is ≥500N / mm, it is a rigid environment; otherwise, it is a flexible environment. This completes the identification of the rigid and flexible characteristics of the environment.

[0064] Step S10: The state observation layer 152 detects the contact state of joint 1 and matches the control strategy with the rigidity and flexibility characteristics of the environment: If it is a collision state, regardless of whether it is a rigid or flexible environment, the force-position hybrid controller layer performs a safety reflection, immediately cuts off the torque output and reverses to avoid collision; if it is a contact state, in a rigid environment, the force-position hybrid controller layer increases the joint stiffness to 1000-10000 N / m and reduces the damping to 50-100 N·s / m, performing high-rigidity torque control; in a flexible environment, the force-position hybrid controller layer reduces the joint stiffness to 0.1-1000 N / m and increases the damping to 100-200 N·s / m, performing compliant torque control; if it is a free space state, the force-position hybrid controller layer performs position control to maintain high-rigidity positioning; if it is a transition region state, in a rigid environment, the force-position hybrid controller layer matches high-rigidity impedance control; in a flexible environment, the force-position hybrid controller layer matches low-rigidity impedance control, achieving precise adaptation of "rigid to rigid, flexible to flexible";

[0065] Step S11: The force-position hybrid controller layer runs the force-position hybrid control algorithm according to the current control mode to calculate the target torque and target position of joint 1;

[0066] Step S12: The force-position hybrid controller layer combines the joint 1 dynamic model to perform feedforward compensation on the target torque and target position, and obtains the compensated target control quantity.

[0067] Step S13: The safety monitoring module of the drive layer 16 sets the upper and lower limits of torque and current, and at the same time detects whether overcurrent, overtemperature or position limit protection is triggered. If it is not triggered, proceed to step S14; otherwise, perform a safety shutdown and exit the main control loop.

[0068] In step S14, the motor drive module of the drive layer 16 converts the compensated target control quantity into an execution command and sends it to the joint drive motor 3 to drive the joint 1 to move.

[0069] Step S15: The security monitoring module of the driver layer 16 and the fault detection module of the control layer 15 monitor the system operation status in real time and detect whether there are any hardware or software abnormalities.

[0070] Step S16: If an anomaly is detected, the control layer 15 executes the fault handling procedure, saves the system safety status, and reports the fault information to the central controller 14 of the whole machine through the high-speed communication interface 13, and then returns to step S4; if no anomaly is detected, it is determined whether the current control task is completed. If it is completed, it returns to the status observation layer 152 to the idle state and enters step S4; otherwise, it returns to step S7.

[0071] I. Overall Working Principle

[0072] This invention adopts the core logic of "multi-sensor fusion perception + three-layer software architecture + state-driven four-mode force-position hybrid control + environmental rigid-flexible adaptive matching + all-dimensional safety protection". Through the collaborative acquisition of hardware components, the hierarchical scheduling of the software architecture, and the dynamic adaptation of control algorithms, it realizes the high precision, high real-time performance, compliant interaction and safe and reliable operation of the embodied intelligent robot joint module. The system is based on an embedded processor and relies on multi-source data fusion from magnetic encoders, inductive encoders, strain gauge torque sensors, six-dimensional force sensors, and IMU sensors to accurately calculate the real-time motion state of the joint and the rigidity and flexibility characteristics of the environment. Combined with the contact state detection results, it dynamically matches four modes: safety reflection, torque control, position control, and impedance control. It improves control accuracy through dynamic feedforward compensation and links hardware safety circuits and software fault handling mechanisms to achieve precise "rigid-to-rigid" and "flexible-to-flexible" adaptation in a wide range of stiffness environments from 0.1 to 10000 N / mm. This ensures that the joint can quickly position itself in free space, maintain stable force control in contact state, smoothly transition in transition areas, and provide emergency protection in collision state. It meets the needs of various scenarios such as precision assembly, human-machine collaboration, and flexible operation. The core control cycle delay is ≤500μs, and the contact force control accuracy reaches ±0.1N.

[0073] II. Working Principle of Hardware System

[0074] The hardware system provides physical support for the joint module's sensing, driving, control, and safety. All components work together to complete the entire process of "power output - data acquisition - command execution - safety protection."

[0075] 1. Power transmission link: The joint drive motor serves as the power source, and its output shaft is connected to the input end of the harmonic reducer. The harmonic reducer improves the output torque and positioning accuracy by reducing speed and increasing torque. Its output end is connected to the joint through the robot joint actuator, realizing efficient power transmission and precise joint movement, supporting stable operation under a peak load of 150Nm.

[0076] 2. Multi-sensor perception chain: A magnetic encoder is installed on the joint drive motor to collect the angular position data of the motor output shaft, realizing motor-level position feedback; an inductive encoder is installed at the output end of the harmonic reducer to collect the final output angular position data of the joint, forming a dual-position closed loop of "motor shaft - joint output end" to eliminate transmission errors; a strain gauge torque sensor module synchronously collects joint motion torque data to realize real-time monitoring of joint torque; a six-dimensional force sensor module is installed on the end effector side of the joint to collect the X / Y / Z axial linear forces (Fx, Fy, Fz) and rotational torques around the axes (Mx, My, Mz), accurately capturing the end contact force / torque; an IMU sensor module is installed on the joint to collect three-dimensional attitude angles (roll angle, pitch angle, yaw angle), angular velocity and linear acceleration data, supplementing the joint spatial attitude and motion dynamic information. All sensor data are transmitted to the embedded processor in real time, providing full-dimensional perception support for state estimation and control decision-making.

[0077] 3. Control and Communication Link: The embedded processor, as the core control unit, receives all sensor data, runs the force-position hybrid control algorithm to generate control commands, and sends them to the joint drive motor to execute actions; it communicates bidirectionally with the central controller of the whole machine through a high-speed communication interface, receives upper-level control commands and reports operating status and fault information, so as to realize the collaborative operation of the joint module and the whole machine.

[0078] 4. Safety Protection Link: The safety circuit integrates overcurrent circuit, overtemperature circuit, and position limiting circuit, which monitor motor current, system temperature, and joint position respectively. When overcurrent, overtemperature, or position over-limit abnormalities occur, hardware-level protection is directly triggered. At the same time, the safety circuit is electrically connected to the embedded processor, which feeds back abnormal signals to the software layer in real time, linking the software safety mechanism to form a "hardware + software" dual-layer safety protection.

[0079] III. Working Principles of Software Architecture

[0080] The embedded processor adopts a top-down three-layer software architecture consisting of a control layer, a driver layer, and a firmware layer. This architecture achieves layered decoupling and efficient collaboration between "algorithm decision-making, instruction translation, and hardware scheduling," ensuring system real-time performance, scalability, and stability.

[0081] Firmware layer: As the foundation of the software system, the real-time operating system is responsible for the real-time scheduling of control tasks. Through priority allocation and time slice management, it ensures that the execution cycle of core control tasks is ≤500μs, meeting the high real-time requirements. The hardware abstraction layer abstracts and encapsulates the resources of the embedded processor and various hardware components (sensors, motors, safety circuits), shielding hardware differences, reducing module coupling, and improving system scalability and maintainability. The bootloader triggers hardware self-test after the system is powered on, completes the initialization and adaptation of various hardware components, builds the basic operating environment, and ensures reliable system startup.

[0082] The driver layer, acting as an intermediary bridge between the hardware and control layers, enables data transmission between various sensors and the embedded processor. It simultaneously performs data filtering and calibration preprocessing to eliminate noise and errors and improve the accuracy of the sensed data. The motor driver receives control signals from the control layer and converts them into torque / position control commands that the joint drive motor can execute, thus achieving precise execution of the control commands. The safety monitoring layer, in conjunction with the safety circuit, monitors overcurrent, overtemperature, and position over-limit states in real time, feeds back abnormal signals to the control layer, and simultaneously executes torque / current upper and lower limit constraints to ensure the safe operation of the motor.

[0083] Control Layer: As the core decision-making unit of the software system, the force-position hybrid controller layer runs the force-position hybrid control algorithm, calculates the target control quantity and completes feedforward compensation in combination with the joint dynamics model, and realizes high-precision force-position coordinated control; the state observation layer, based on preprocessed sensor data, infers the real-time motion state of the joint (kinematics, posture, contact trend, environmental interaction correlation), solves the environmental stiffness and determines the rigid-flexible characteristics, and detects the contact state (collision, contact, free space, transition region), providing a basis for control mode switching; the fault detection layer monitors the operating status of the system hardware / software in real time, triggers the fault handling program when an anomaly is detected, saves the safe state and reports the fault information, ensuring safe shutdown and fault tracing when the system is abnormal.

[0084] IV. Detailed Control Method Flow

[0085] The control method follows a closed-loop logic of "power-on initialization - command reception - status awareness - decision control - command execution - anomaly monitoring". The specific steps and working principle are as follows:

[0086] Steps S1-S3 (System Startup and Initialization): After the system is powered on, the firmware layer triggers a hardware self-test, completes the initial adaptation between the embedded processor and each hardware component, loads the joint control preset parameters, the 500N / mm stiffness threshold and the system configuration file, completes the initialization of control task parameters, and ensures that the system has the basic conditions for stable operation after startup.

[0087] Steps S4-S6 (Command Reception and Mode Parsing): The drive layer receives control commands from the central controller of the whole machine through the high-speed communication interface. The control layer identifies and parses the command type. For trajectory tracking commands, it plans the joint motion path. For direct control commands, it sets the force / position control target. For teaching mode commands, it enters the compliant teaching state. When the human body pulls the joint, the motor passively follows the direction of the human hand to meet the needs of multi-mode operation.

[0088] Steps S7-S8 (Main Loop Startup and Data Acquisition): The firmware layer schedules and controls the task to enter the main control loop through the real-time operating system. The sensor interface module of the driver layer collects data from all sensing units, completes filtering and calibration preprocessing, eliminates data noise and errors, and provides accurate sensing input for subsequent state estimation.

[0089] Steps S9-S10 (State Calculation and Rigid-Flexible Adaptive Control): Based on preprocessed data, the state observation layer calculates the real-time motion state of the joint (kinematics, posture, contact trend, and environmental interaction correlation), and calculates the environmental stiffness value in the range of 0.1-10000 N / mm online based on the environmental interaction correlation state. A threshold of 500 N / mm is used to determine whether the environment is rigid or flexible. Subsequently, the contact state is detected, and a control strategy is matched to the rigid-flexible characteristics of the environment: In the event of a collision, regardless of whether the environment is rigid or flexible, a safety reflection is executed, immediately cutting off the torque output and reversing to avoid damage to the joint and the work object; in the event of contact, the rigid environment is enhanced. With joint stiffness up to 1000-10000 N / m and damping reduced to 50-100 N·s / m, high-rigidity torque control is implemented to ensure contact force is stable within ±0.1 N. In flexible environments, joint stiffness is reduced to 0.1-1000 N / m and damping increased to 100-200 N·s / m, and compliant torque control is implemented to avoid squeezing flexible objects. In free space, position control is implemented to maintain high-rigidity positioning and achieve rapid and accurate load transfer. In transition regions, high-rigidity impedance control is matched to rigid environments, and low-rigidity impedance control is matched to flexible environments, achieving precise adaptation and seamless transition between rigid and rigid environments.

[0090] Steps S11-S12 (Control Quantity Calculation and Feedforward Compensation): The force-position hybrid controller layer calculates the target torque and target position by running the force-position hybrid control algorithm according to the current control mode; combined with the joint dynamics model, it performs full-dimensional feedforward compensation of the target control quantity for gravity, friction, inertia and Coriolis force to offset the inherent interference of the system and further improve the control accuracy and response speed.

[0091] The joint dynamics model is τ_dynamics=M(θ)ṯ+V(θ,ṯ)+G(θ), where M(θ)ṯ corresponds to inertial compensation, which counteracts the inertial resistance of the joint's own mass and load during acceleration / deceleration (such as the impulse during high-speed start-stop).

[0092] V(θ,ṯ): Corresponds to Coriolis force compensation, which cancels out coupling interference during multi-joint motion (such as the additional torque of one joint rotation driving another joint).

[0093] G(θ): Corresponds to gravity compensation, which counteracts the torque of gravity on the joint under different postures (such as gravity sagging when lifting a load).

[0094] Additional friction model: Corresponds to friction compensation, which offsets the static friction (starting resistance) and dynamic friction (motion resistance) of the motor and reducer.

[0095] Feedforward compensation logic: First, calculate the "total compensation torque" (the sum of inertia, Coriolis force, gravity, and friction) in real time through the dynamic model.

[0096] The compensation torque is directly superimposed on the original target torque, and the target position is finely adjusted according to the compensation amount (to eliminate position deviation caused by interference).

[0097] Finally, the "compensated target control quantity" is obtained and then sent down to the driver layer.

[0098] Steps S13-S14 (Safety Detection and Command Execution): The safety monitoring module sets the upper and lower limits of torque / current, and detects whether overcurrent, overtemperature, and position limit protection are triggered. If not triggered, the compensated target control quantity is converted into an execution command and sent to the joint drive motor to drive the joint to move according to the preset strategy. If the protection is triggered, a safety stop is executed and the main control loop is exited.

[0099] Steps S15-S16 (Anomaly Monitoring and Process Closure): The driver layer and control layer monitor the system's operating status in real time. When a hardware / software anomaly is detected, the control layer executes the fault handling procedure, saves the safety status and reports the fault information, and then returns to the instruction receiving step. If no anomaly is detected, it determines whether the task is completed. If completed, it returns to the status observation layer to the idle state; if not completed, it re-enters the main control loop, forming a complete control closure.

[0100] Environmental stiffness-flexibility adaptive matching principle: Through multi-source data fusion of six-dimensional force sensors, dual encoders, and IMU, the environmental stiffness (0.1-10000N / mm) is calculated in real time based on the environmental interaction and correlation status, and rigid / flexible environments are divided with 500N / mm as the threshold. The joint impedance parameters (stiffness and damping) are dynamically adjusted for different environments. For rigid environments, high stiffness and low damping parameters are matched with high stiffness torque / impedance control to ensure positioning accuracy and contact stability. For flexible environments, low stiffness and high damping parameters are matched with compliant torque / impedance control to achieve flexible fit and object protection. Finally, a precise "rigid-to-rigid, flexible-to-flexible" fit is achieved to adapt to complex and ever-changing working environments.

[0101] Force-position hybrid control and mode switching principle: Based on the precise detection of contact states (collision, contact, free space, transition region), it realizes seamless switching of four control modes: collision state triggers safety reflection to ensure emergency protection; contact state executes torque control to achieve stable force control; free space state executes position control to achieve rapid positioning; transition region state executes impedance control to achieve smooth transition; mode switching is based on the real-time judgment of the state observation layer, combined with the rigidity and flexibility characteristics of the environment, to ensure that the switching process is smooth and without delay, taking into account both motion efficiency and interaction smoothness.

[0102] Principle of dynamic feedforward compensation:

[0103] Based on the joint dynamics model, interference factors such as gravity, static / dynamic friction, inertia, and Coriolis force in joint movement are predicted and offset in advance. The feedforward compensation is superimposed on the original target control quantity to form the final control command. The compensation covers all-dimensional interference, effectively improving the control accuracy under high load and high speed, and ensuring a control accuracy of ±0.1N for contact force and a response speed of 500μs.

[0104] The principle of all-dimensional safety protection is to build a three-layer protection system of "hardware safety circuit + software safety reflection + fault handling". The hardware layer realizes hardware-level emergency protection through overcurrent, overtemperature and position limiting circuits. The software layer triggers safety reflection through collision state detection. The fault detection layer monitors system anomalies in real time and performs fault handling, which comprehensively protects the safety of joint modules, working objects and human-machine interaction, and avoids equipment damage and safety risks caused by anomalies such as collision, overload and exceeding limits.

[0105] It should be noted that the above examples are merely one specific embodiment of the present invention. Obviously,

[0106] This invention is not limited to the above embodiments, and many variations are possible. In short, those skilled in the art...

[0107] All modifications that a person skilled in the art can directly derive or conceive from the content disclosed in this invention should be applied.

[0108] This is considered to be within the scope of protection of this invention.

Claims

1. A embodied intelligent robot joint module system employing a force-position hybrid control algorithm, characterized in that, The system includes a joint (1), a robot joint actuator (2), a joint drive motor (3), a harmonic reducer (4), a magnetic encoder (5), an inductive encoder (6), a strain gauge torque sensor module (7), a six-dimensional force sensor module (8), an IMU sensor module (9), an embedded processor (11), a high-speed communication interface (13), a safety circuit (12), and a central controller (14). The output shaft of the joint drive motor (3) is connected to the input end of the harmonic reducer (4), and the output end of the harmonic reducer (4) is connected to the joint (1) through the robot joint actuator (2). The magnetic encoder (5) is mounted on the joint drive motor (3), and the inductive encoder (6) is mounted on the harmonic reducer (4). The output end of the harmonic reducer (4) is equipped with the strain torque sensor module (7), the six-dimensional force sensor module (8) is installed on the end execution side of the joint (1), the IMU sensor module (9) is installed on the joint (1), the magnetic encoder (5), the inductive encoder (6), the strain torque sensor module (7), the six-dimensional force sensor module (8), the IMU sensor module (9), and the joint drive motor (3) are all electrically connected to the embedded processor (11). The embedded processor (11) communicates bidirectionally with the central controller (14) of the whole machine through the high-speed communication interface (13), and the safety circuit (12) is connected to the embedded processor (11).

2. The embodied intelligent robot joint module system employing a force-position hybrid control algorithm according to claim 1, characterized in that, The magnetic encoder (5) is used to collect the angular position data of the output shaft of the joint drive motor (3), the inductive encoder (6) is used to collect the final output angular position data of the joint (1), and the strain torque sensor module (7) is used to collect the torque data of the joint (1) during movement.

3. The embodied intelligent robot joint module system employing a force-position hybrid control algorithm according to claim 1, characterized in that, The six-dimensional force sensor module (8) is used to collect the linear forces (Fx, Fy, Fz) along the X / Y / Z axes and the rotational torques (Mx, My, Mz) around the X / Y / Z axes when the joints (1) are in contact.

4. The embodied intelligent robot joint module system employing a force-position hybrid control algorithm according to claim 1, characterized in that, The IMU sensor module (9) is used to collect the three-dimensional attitude angles, angular velocity data around the X / Y / Z axes and linear acceleration data along the X / Y / Z axes of the joint (1). The three-dimensional attitude angles are roll angle, pitch angle and yaw angle.

5. The embodied intelligent robot joint module system employing a force-position hybrid control algorithm according to claim 1, characterized in that, The safety circuit (12) includes an overcurrent circuit (121), an overtemperature circuit (122), and a position limiting circuit (123). The overcurrent circuit (121) is used for overcurrent protection, the overtemperature circuit (122) is used for overtemperature protection, and the position limiting circuit (123) is used for position over-limit protection.

6. The embodied intelligent robot joint module system employing a force-position hybrid control algorithm according to claim 1, characterized in that, The software architecture of the embedded processor (11) includes a top-down control layer (15), a driver layer (16), and a firmware layer (17). The control layer (15) includes a force-position hybrid controller layer (151), a state observation layer (152), and a fault detection layer (153). The force-position hybrid controller layer (151) is used to run the force-position hybrid control algorithm and calculate the target torque and position control quantity of the joint (1). The state observation layer (152) is used to calculate the real-time motion state of the joint (1) and detect the state based on the data collected by each sensor module. The fault detection layer (153) is used to detect system hardware / software abnormalities and trigger fault handling procedures. The drive layer (16) includes a motor driver (161), a sensor interface layer (162), and a safety monitoring layer (163). The motor driver (161) is used to receive control quantities issued by the control layer (15) and convert them into torque and position control commands that can be executed by the joint drive motor (3). The sensor interface layer (162) is used to realize the data transmission between the magnetic encoder (5), the inductive encoder (6), the strain torque sensor module (7), the six-dimensional force sensor module (8), the IMU sensor module (9), and the embedded processor (11), and synchronously complete data filtering and calibration preprocessing. The safety monitoring layer (163) is used to link the safety circuit (12), monitor the overcurrent, overtemperature, and position over-limit status in real time, and feed back to the control layer (15). The firmware layer (17) includes a real-time operating system (171), a hardware abstraction layer (172), and a bootloader (173). The real-time operating system (171) is used to control the real-time scheduling of tasks and ensure that the control cycle delay is within 500μs. The hardware abstraction layer (172) is used to realize the resource abstraction call between the embedded processor (11) and each hardware component, and reduce the module coupling. The boot program (173) is used to trigger hardware self-test and initialization after the system is powered on, and to build the basic operating environment.

7. The embodied intelligent robot joint module system employing a force-position hybrid control algorithm according to claim 6, characterized in that, The states include collision state, contact state, free space state, and transition region state. The real-time motion state of the joint includes joint kinematic state, joint posture state, joint contact trend state, and environmental interaction and correlation state. The joint kinematic state is the motor shaft speed, the final output speed of the joint, and the joint translational acceleration information. The joint posture state is the posture orientation and posture change rate of the joint (1) in space. The joint contact trend state is the prediction of whether the joint (1) is about to enter the collision state, contact state, free space state, or transition region state. The environmental interaction and correlation state is the online acquisition and real-time updating of the environmental stiffness value in the range of 0.1-10000 N / mm, while judging whether the environment is rigid or flexible.

8. The embodied intelligent robot joint module system employing a force-position hybrid control algorithm according to claim 7, characterized in that, The collision state is an unexpected contact state in which the joint (1) and the contact surface are excessively squeezed; the contact state is a state in which the joint (1) and the contact surface are in stable contact; the free space state is an unconstrained state in which the joint (1) is far away from the contact surface and no contact occurs; the transition region state is an intermediate state between the joint (1) and the stable contact state.

9. A control method for a joint module of an embodied intelligent robot employing a force-position hybrid control algorithm, characterized in that, A embodied intelligent robot joint module system based on the force-position hybrid control algorithm described in claims 1 to 8 is characterized by comprising the following steps: Step S1: Power on the system; Step S2: System initialization, firmware layer (17) triggers hardware self-test, completes the initial adaptation of embedded processor (11) and each hardware component; Step S3: Firmware layer (17) loads the preset parameters and system configuration file of joint (1) control, and completes the parameter initialization of the control task; Step S4: The driver layer (16) receives control commands from the central controller (14) of the whole machine through the high-speed communication interface (13); Step S5: The control layer (15) identifies and parses the control command, and determines the command type as trajectory tracking, direct control or teaching mode; Step S6: If the instruction type is trajectory tracking, the control layer (15) plans the motion path of the joint (1); if it is direct control, the control layer (15) sets the force control target and position control target of the joint (1); if it is teaching mode, enter the compliant teaching mode, that is, when the human body pulls the joint (1), the joint drive motor (3) passively follows the direction of the human hand pulling. Step S7: The firmware layer (17) schedules and controls the task to enter the main control loop through the real-time operating system (171); Step S8: The sensor interface module of the drive layer (16) collects the sensor unit data and performs filtering and calibration preprocessing on the data. The sensor unit data includes: the output shaft angular position data of the joint drive motor (3) collected by the magnetic encoder (5), the final output angular position data of the joint (1) collected by the inductive encoder (6), the motion torque data of the joint (1) collected by the strain torque sensor module (7), the X / Y / Z axis linear forces Fx, Fy, Fz and rotational torques Mx, My, Mz and imu of the joint (1) when in contact collected by the six-dimensional force sensor module (8), and the three-dimensional attitude angle, angular velocity data around the X / Y / Z axis and linear acceleration data along the X / Y / Z axis collected by the sensor module (9). Step S9: The state observation layer (152) calculates the real-time motion state of the joint (1) based on the preprocessed sensor data. The real-time motion state includes the joint kinematic state, joint posture state, joint contact trend state, and environmental interaction correlation state. It first determines whether the current working environment is rigid or flexible. If the environmental stiffness value is ≥500N / mm, it is a rigid environment; otherwise, it is a flexible environment. Thus, the identification of the rigid and flexible characteristics of the environment is completed. Step S10: The state observation layer (152) detects the contact state of joint (1) and combines the rigidity and flexibility characteristics of the environment to match the control strategy: If it is a collision state, regardless of whether it is a rigid environment or a flexible environment, the force-position hybrid controller layer (151) performs a safety reflection, immediately cuts off the torque output and reverses the retreat; if it is a contact state, in a rigid environment, the force-position hybrid controller layer (151) increases the joint stiffness to 1000-10000 N / m and reduces the damping to 50-100 N・s / m, and performs high-rigidity torque control; in a flexible environment, the force-position hybrid controller layer (151) increases the joint stiffness to 1000-10000 N / m and reduces the damping to 50-100 N・s / m, and performs high-rigidity torque control. The force-position hybrid controller layer (151) reduces the joint stiffness to 0.1-1000 N / m and increases the damping to 100-200 N·s / m, and performs compliant torque control. In the free space state, the force-position hybrid controller layer (151) performs position control to maintain high stiffness positioning. In the transition region state, the force-position hybrid controller layer (151) matches high stiffness impedance control in the rigid environment and low stiffness impedance control in the flexible environment, so as to achieve precise "rigid to rigid, flexible to flexible" adaptation. Step S11: The force-position hybrid controller layer (151) runs the force-position hybrid control algorithm according to the current control mode to calculate the target torque and target position of the joint (1); Step S12: The force-position hybrid controller layer (151) combines the joint dynamics model to perform feedforward compensation on the target torque and target position to obtain the compensated target control quantity. Step S13: The safety monitoring module of the drive layer (16) sets the upper and lower limits of torque and current, and at the same time detects whether overcurrent, overtemperature or position limit protection is triggered. If it is not triggered, proceed to step S14; otherwise, perform a safety shutdown and exit the main control loop. Step S14: The motor drive module of the drive layer (16) converts the compensated target control quantity into an execution command and sends it to the joint drive motor (3) to drive the joint (1) to move. Step S15: The safety monitoring module of the driver layer (16) and the fault detection module of the control layer (15) monitor the system operation status in real time and detect whether there are any hardware or software abnormalities. Step S16: If an anomaly is detected, the control layer (15) executes the fault handling procedure, saves the system security status, and reports the fault information to the central controller (14) of the whole machine through the high-speed communication interface (13), and then returns to step S4; if no anomaly is detected, it determines whether the current control task is completed. If it is completed, it returns to the status observation layer (152) to the idle state and enters step S4; otherwise, it returns to step S7.

Images