Robot joint control method and robot joint control system

Abstract

The application discloses a kind of robot joint control method and robot joint control system, it is related to joint control technical field, main controller reads the identity information corresponding to multiple domain controllers, and loads the joint parameters of each domain controller;When main controller determines that identity information satisfies preset safety condition, the joint parameters corresponding to identity information are output to corresponding domain controller;Domain controller obtains the output result of first encoder, second encoder, torque sensor and inertial measurement unit, and executes sensor topology detection action based on output result;When the sensor topology of access matches with preset target topology, main controller executes communication link detection action, and in the case where communication link state is normal after executing communication link detection action, control domain controller executes sensor consistency check action, to obtain check result.The application aims to improve the running stability, control accuracy and safety of multi-joint robot.

Description

Technical Field

[0001] This invention relates to the field of joint control technology, and in particular to a robot joint control method and a robot joint control system. Background Technology

[0002] With the rapid development of humanoid robots, higher demands are being placed on the integration, response speed, and intelligence level of robot joints. Robotic suits typically contain dozens of degrees of freedom. To reduce design complexity and production costs, a general-purpose domain controller solution is often used. However, this hardware-generalized design lacks hardware identification and parameter matching mechanisms. During assembly or maintenance, if the domain controller is incorrectly installed in an incompatible joint position or loaded with incorrect kinematic parameters, it will directly lead to abnormal joint movement or even equipment damage. Summary of the Invention

[0003] The main objective of this invention is to provide a robot joint control method and control device, which aims to improve the operational stability, control accuracy and safety of multi-joint robots.

[0004] To achieve the above objectives, this invention proposes a robot joint control method, characterized in that it is applied to a robot joint control system. The robot joint control system includes a main controller, multiple domain controllers, a first encoder, a second encoder, a torque sensor, and an inertial measurement unit. Each domain controller is used to control the enable state of a joint. The first encoder is used to detect the motor rotation angle, the second encoder is used to detect the joint rotation angle, the torque sensor is used to detect the joint's output torque, and the inertial measurement unit is used to detect the joint's pose. The robot joint control method includes: The main controller reads the identity information corresponding to the multiple domain controllers and loads the joint parameters of each domain controller based on the identity information; When the main controller determines that the identity information meets the preset security conditions based on the identity information, it outputs the joint parameter corresponding to the identity information to the corresponding domain controller. The domain controller acquires the output results corresponding to the first encoder, the second encoder, the torque sensor, and the inertial measurement unit, and performs sensor topology detection based on the output results to determine whether the sensor topology connected to each joint matches the preset target topology. If the accessed sensor topology matches the preset target topology, the main controller performs a communication link detection action. After performing the communication link detection action and determining that the communication link status is normal, the main controller controls the domain controller to perform a sensor consistency verification action to obtain the verification result. If the verification result indicates that the verification passed, the domain controller enters the closed-loop enable mode; If the verification result indicates a failure, the domain controller enters a fault protection mode. The closed-loop enabling mode includes outputting corresponding motor control signals based on the output results of the first encoder, the second encoder, the torque sensor, and the inertial measurement unit; the fault protection mode includes, in order of risk level from low to high, the observation limiting state, the degraded closed-loop state, and the fault incapacitance state.

[0005] In one embodiment, the sensor consistency verification action includes: Multi-source sensor data is obtained based on the output results of the first encoder, the second encoder, the torque sensor, and the inertial measurement unit, as well as the status of the communication link. Feature extraction is performed on the multi-source sensor data to obtain multiple feature quantities; The multiple features are normalized, and the roles of each sensor in the current joint are weighted to obtain a consistency score. The corresponding verification result is determined based on the consistency score. The feature quantities include at least one of the following: the angle deviation between the joint angle calculated from the motor rotation angle and the joint rotation angle; the cycle jump variable between the current cycle and the previous cycle; the difference between the current domain control time and the most recent effective sampling time; the difference between the output torque and the estimated torque of the motor current; and the difference between the joint pose and the preset pose.

[0006] In one embodiment, under the observation limiting state, the domain controller downweights the role of the sensor corresponding to the abnormal data source and limits the slope of the output torque; In the degraded closed-loop state, the domain controller stops using the abnormal data source output by the faulty sensor, enables the data source of the non-faulty sensor, and reduces the position loop gain or limits the output torque. In the fault-incapacitated state, the domain controller prohibits output control actions to latch the fault and sends a system protection request or emergency stop request to the main controller.

[0007] In one embodiment, the step of the domain controller entering fault protection mode when the verification result indicates a verification failure includes: When the consistency score indicates a single sensor jumps in a single cycle while the other sensors remain consistent over multiple cycles, the domain controller enters the observation limiting state. When the consistency score characterization angle deviation maintains an upward trend within a preset first time period and the speed change trends of the output results of the first encoder and the output results of the second encoder are inconsistent, the domain controller enters a degraded closed-loop state. If the output torque of the joint detected by the torque sensor is opposite to the direction of the torque estimated by the motor current, or if the difference between the output torque of the joint detected by the torque sensor and the torque estimated by the motor current is greater than a preset difference, the domain controller enters a degraded closed-loop state. If the output results of two or more sensors are all abnormal data sources within a preset second time period, the domain controller enters the fault-disabled state.

[0008] In one embodiment, the sensor consistency verification action includes: Obtain its own identity information, sensor topology, and joint parameter verification code; Based on the identity information, the sensor topology, and the joint parameter check code, if the preset verification conditions are met, the verification result is determined to be a successful verification. If the preset verification conditions are not met based on the identity information, the sensor topology, and the joint parameter check code, the verification result is determined to be a verification failure. The preset verification conditions include: The identity information satisfies at least one of the following: the number of connected sensors matches the number of connected targets; the type of connected sensors matches the type of connected targets; and the joint parameter check code is loaded correctly.

[0009] In one embodiment, after the domain controller enters fault protection mode, the robot joint control method further includes: When multiple features meet their corresponding preset security ranges within a preset third time period, the domain controller switches from its current state to an adjacent low-risk level state. When the domain controller operates in the adjacent low-risk level state for a preset fourth time period, the relationship between the multiple features and their corresponding preset security ranges is determined. If the domain controller is in a faulty or disabled state, the process of switching from the current state to an adjacent low-risk level state shall be stopped. If the domain controller is in a faulty or disabled state and an external reset command is received, a reset action is performed.

[0010] In one embodiment, the robot joint control method further includes: After the domain controller enters fault protection mode, the domain controller generates fault alarm information and uploads it to the main controller; The fault alarm information includes at least one of the following: fault code, its own identity information, and timestamp.

[0011] In one embodiment, the robot joint control method further includes: When the main controller receives the fault alarm information, it performs a whole machine status assessment and gait replanning action based on the fault code and the timestamp to determine the whole machine control strategy. The overall machine control strategy includes any one of the following: continued operation, degraded operation, and emergency stop.

[0012] This application also proposes a robot joint control system based on the robot joint control method described above. The robot joint control system includes a robot body, and a main controller, multiple domain controllers, a first encoder, a second encoder, a torque sensor, and an inertial measurement unit disposed on the robot body. Each domain controller is used to control the enable state of a joint. The first encoder is used to detect the motor rotation angle, the second encoder is used to detect the joint rotation angle, the torque sensor is used to detect the joint output torque, and the inertial measurement unit is used to detect the joint pose.

[0013] In one embodiment, the robot joint control system further includes an EtherCAT slave communication interface, through which the master controller communicates with each of the domain controllers.

[0014] In practical applications, automatic identification and parameter mapping enable automatic identification of each joint module, allowing joint control to be achieved using the same set of hardware. Different limb positions can be configured and adapted using identification information, reducing hardware costs and maintenance complexity. Simultaneously, sensor topology detection and sensor consistency verification actions can identify hardware defects, wiring errors, or assembly deviations, reducing the risk of joint control anomalies. Furthermore, a graded fault protection strategy can be executed based on the verification results, enabling dynamic response from limited observation under low-risk anomalies to emergency stop and disability under high-risk anomalies. This effectively improves the safety hazards of motion anomalies caused by sensor failures or communication anomalies in traditional robot joint control systems, enhancing the operational stability, control accuracy, and safety of multi-joint robots. Attached Figure Description

[0015] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with the invention and, together with the description, serve to explain the principles of the invention.

[0016] 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, for those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0017] Figure 1 This is a flowchart illustrating the first embodiment of the robot joint control method of the present invention. Figure 2 This is a flowchart illustrating the second embodiment of the robot joint control method of the present invention. Figure 3 This is a flowchart illustrating the third embodiment of the robot joint control method of the present invention. Figure 4 This is a flowchart illustrating the fourth embodiment of the robot joint control method of the present invention. Figure 5 This is a flowchart illustrating the fifth embodiment of the robot joint control method of the present invention. Figure 6 This is a flowchart illustrating the sixth embodiment of the robot joint control method of the present invention. Figure 7 This is a flowchart illustrating the seventh embodiment of the robot joint control method of the present invention. Figure 8 This is a flowchart illustrating the eighth embodiment of the robot joint control method of the present invention. Figure 9 This is a flowchart illustrating the ninth embodiment of the robot joint control method of the present invention. Figure 10 This is a flowchart illustrating the tenth embodiment of the robot joint control method of the present invention. Figure 11 This is a flowchart illustrating the eleventh embodiment of the robot joint control method of the present invention. Figure 12 This is a flowchart illustrating the twelfth embodiment of the robot joint control method of the present invention. Figure 13 This is a flowchart illustrating the thirteenth embodiment of the robot joint control method of the present invention.

[0018] The objectives, features, and advantages of this invention will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0019] It should be understood that the specific embodiments described herein are merely illustrative of the technical solutions of the present invention and are not intended to limit the present invention.

[0020] To better understand the technical solution of the present invention, a detailed description will be provided below in conjunction with the accompanying drawings and specific embodiments.

[0021] With the rapid development of humanoid robots, higher demands are being placed on the integration, response speed, and intelligence level of robot joints. Robotic suits typically contain dozens of degrees of freedom. To reduce design complexity and production costs, a general-purpose domain controller solution is often used. However, this hardware-generalized design lacks hardware identification and parameter matching mechanisms. During assembly or maintenance, if the domain controller is incorrectly installed in an incompatible joint position or loaded with incorrect kinematic parameters, it will directly lead to abnormal joint movement or even equipment damage.

[0022] Therefore, refer to Figure 1 This invention proposes a robot joint control method applied to a robot joint control system. The robot joint control system includes a main controller, multiple domain controllers, a first encoder, a second encoder, a torque sensor, and an inertial measurement unit. Each domain controller is used to control the enable state of a joint. The first encoder is used to detect the motor rotation angle, the second encoder is used to detect the joint rotation angle, the torque sensor is used to detect the joint output torque, and the inertial measurement unit is used to detect the joint pose. The robot joint control method includes: Step S100: The main controller reads the identity information corresponding to the multiple domain controllers and loads the joint parameters of each domain controller based on the identity information; Step S200: When the main controller determines that the identity information meets the preset security conditions based on the identity information, the joint parameter corresponding to the identity information is output to the corresponding domain controller. Step S300: The domain controller acquires the output results corresponding to the first encoder, the second encoder, the torque sensor, and the inertial measurement unit, and performs a sensor topology detection action based on the output results to determine whether the sensor topology connected to each joint matches the preset target topology. Step S400: When the accessed sensor topology matches the preset target topology, the main controller performs a communication link detection action, and after performing the communication link detection action and determining that the communication link status is normal, it controls the domain controller to perform a sensor consistency verification action to obtain the verification result. Step S500: If the verification result indicates that the verification passed, the domain controller enters the closed-loop enable mode. Step S600: If the verification result indicates a verification failure, the domain controller enters a fault protection mode. The closed-loop enabling mode includes outputting corresponding motor control signals based on the output results of the first encoder, the second encoder, the torque sensor, and the inertial measurement unit; the fault protection mode includes, in order of risk level from low to high, the observation limiting state, the degraded closed-loop state, and the fault incapacitance state.

[0023] In this embodiment, the robot joint control method of the present invention can be applied to a control device for a multi-joint robot or a robot joint control system. The control device includes a memory, a processor, and a computer program stored in the memory and executable on the processor. The computer program is configured to implement the steps of the robot joint control method. The control device can be implemented using a main controller, such as an MCU (Micro Controller Unit), a DSP (Digital Signal Processor), an FPGA (Field Programmable Gate Array), a PLC (Programmable Logic Controller), or a SOC (System on Chip). In this embodiment, the control device includes the main controller and domain controller proposed in this application.

[0024] In this embodiment, the main controller, in joint control, is responsible for reading the identity of each joint, issuing matching joint parameters, verifying communication links, and coordinating the overall safety actions of the machine (such as requesting an emergency stop) when a fault occurs. The domain controller is an independent control node installed at each joint (i.e., a "limb domain controller"). It has local computing capabilities, is responsible for receiving joint parameters issued by the main controller, collecting and processing various sensor data in real time, executing the underlying motor closed-loop control algorithm, and performing local fault protection actions when an anomaly occurs. The first encoder (built-in encoder in the motor) can be installed at the tail of the motor to detect the rotation angle and speed of the motor rotor itself. The second encoder (external encoder) can be installed at the joint output end to directly measure the actual physical angle of the joint linkage, providing high-precision position feedback and having a power-off position memory function. The torque sensor can be connected in series in the joint drivetrain to directly sense the actual output torque of the joint and external collision force. It is the core component for achieving compliant control (such as softening after a collision) and overload protection. An inertial measurement unit (IMU) can be installed on the limb where the joint is located to measure the absolute attitude (tilt angle) and acceleration of the limb in three-dimensional space, providing a global reference and preventing the robot from losing attitude or falling during movement.

[0025] In this embodiment, reference Figure 5 and Figure 7After the robot powers on, both the main controller and each domain controller perform initialization actions. Then, each domain controller reads the level state of the hardware ID configuration circuit (e.g., a combination of high and low levels from a 4-bit DIP switch or resistor array) via its own GPIO pins, converts it into an identification information, such as a digital ID (e.g., 0001 representing the left knee joint), and writes its own digital ID into a specific register via the EtherCAT slave interface, and sends it to the main controller along with periodic messages. Upon power-on, the main controller obtains the identification information uploaded by each domain controller and retrieves the joint parameters (including but not limited to reduction ratio, motor direction, PID gain, torque limit, etc.) corresponding to each ID from the pre-stored "Joint Configuration Mapping Table" in its internal memory. The main controller determines whether the identification information meets preset security conditions, such as checking if the read ID is within a legal range (e.g., 0-15) and checking for ID conflicts on the bus (e.g., both joints reporting 0001). The domain controllers then determine whether the sensor topology connected to each joint matches a preset target topology based on the received joint parameters. The preset target topology may include the theoretical number and type of sensors that can be connected. For example, if the joint parameters are: the joint location should include 1 motor, 1 external encoder, 1 torque sensor, and 1 IMU; the domain controller corresponding to this joint location can sequentially scan the communication connection terminals (including but not limited to CANFD, RS485, and ADC interfaces) connected to each sensor. For instance, it can send a broadcast request to the CANFD bus and wait for a response from the motor driver; send a command to the RS485 bus to read the absolute encoder; or read the voltage value of the ADC pin to determine if the torque sensor is connected. In this way, the domain controller can count the number and type of sensors that actually respond and communicate normally, compare them with the preset target topology, and if it is determined that 1 motor, 1 external encoder, 1 torque sensor, and 1 IMU are actually connected, it means that the connected sensor topology matches the preset target topology, and reports the corresponding matching information to the main controller. When the main controller determines that the sensor topology connected to the joint matches the preset target topology based on the matching information reported by the domain controller, it performs communication link detection actions, such as determining whether the EtherCAT, CANFD, RS485, and other communication buses / interfaces are normal. If communication is normal, the main controller can send a "self-test enabled" command to the corresponding domain controller. Upon receiving the self-test enabled command, the domain controller can execute a preset control algorithm locally. For example, it can read the absolute angle of the second encoder, read the angle of the first encoder, convert it to joint angles based on the reduction ratio, and calculate the angle deviation. For instance, if the angle deviation is less than a preset safety deviation threshold, and the torque sensor and IMU data do not show any abnormal jumps while stationary, the verification is considered successful.The domain controller can utilize the first encoder to perform high-frequency motor field-oriented control current and speed loops; use the second encoder as a reference for the position loop to reduce gearbox errors; utilize torque sensor data to perform impedance control or gravity compensation; and utilize IMU data for attitude constraint. Simultaneously, the domain controller can output corresponding PWM signals (i.e., motor control signals) to drive the inverter, supply current to the motor, release the mechanical brake, and allow the joints to begin controlled movement. Conversely, if the angle deviation is greater than or equal to a preset safety deviation threshold, or if the torque sensor or IMU data shows an abnormal jump while stationary, the verification is deemed a failure. In this case, the domain controller enters fault protection mode and, based on the current risk level, enters the corresponding fault state (i.e., observation limiting state, degraded closed-loop state, and fault incapacity state).

[0026] Understandably, if a minor anomaly of low risk occurs, the system enters an observation and limiting state, which can reduce the weight of suspected sensors and limit the power output speed. If a medium-risk fault occurs, the system enters a degraded closed-loop state, which can isolate the faulty sensor, switch to a backup solution, reduce output torque, and maintain a basic safe posture. If a high-risk fault occurs, the system enters a fault-disabling state, which can immediately cut off power, latch the fault, and request an emergency stop from the main controller.

[0027] In practical applications, automatic identification and parameter mapping enable automatic identification of each joint module, allowing joint control to be achieved using the same set of hardware. Different limb positions can be configured and adapted using identification information, reducing hardware costs and maintenance complexity. Simultaneously, sensor topology detection and sensor consistency verification actions can identify hardware defects, wiring errors, or assembly deviations, reducing the risk of joint control anomalies. Furthermore, a graded fault protection strategy can be executed based on the verification results, enabling dynamic response from limited observation under low-risk anomalies to emergency stop and disability under high-risk anomalies. This effectively improves the safety hazards of motion anomalies caused by sensor failures or communication anomalies in traditional robot joint control systems, enhancing the operational stability, control accuracy, and safety of multi-joint robots.

[0028] In another embodiment, reference Figure 2 The sensor consistency verification action includes: Step S410: Obtain multi-source sensor data based on the output results of the first encoder, the second encoder, the torque sensor, and the inertial measurement unit, as well as the communication link status; Step S420: Extract features from the multi-source sensor data to obtain multiple feature quantities; Step S430: Normalize the multiple feature quantities and weight the roles of each sensor in the current joint to obtain a consistency score. Step S440: Determine the corresponding verification result based on the consistency score; The feature quantities include at least one of the following: the angle deviation between the joint angle calculated from the motor rotation angle and the joint rotation angle; the cycle jump variable between the current cycle and the previous cycle; the difference between the current domain control time and the most recent effective sampling time; the difference between the output torque and the estimated torque of the motor current; and the difference between the joint pose and the preset pose.

[0029] In this embodiment, the domain controller can timestamp-align the raw data of the first encoder, second encoder, torque sensor, and IMU within each control cycle using a high-precision local clock or bus synchronization clock, eliminating phase delay caused by different sampling frequencies. Based on the aligned data, the domain controller extracts and calculates multiple feature quantities in real time. These include features such as inter-source deviation, cycle jump variables, communication age, dynamic residuals, and trend consistency. Inter-source deviation refers to the angular deviation eθ between the joint angle calculated by the motor's built-in encoder (first encoder) and the joint angle of the external encoder (second encoder), as well as the speed deviation eω between them. Cycle jump variables refer to the angular jump variables Jθ, Jτ, and torque jump variables Jimu between the current and previous cycles. Communication age is the difference between the current domain control time and the most recent effective sampling time of each sensor, used to monitor communication packet loss or sensor status. Dynamic residuals refer to the difference between the measured load (output torque) of the torque sensor and the torque estimated based on the motor current, also known as torque residual eτ, used to detect abnormal force in the drivetrain or sensor calibration failure. Trend / posture consistency refers to the difference between the actual posture of a limb measured by the IMU and the posture predicted based on joint kinematics. It is used to detect overall posture loss of control or whether the IMU is loose.

[0030] refer to Figure 13 This embodiment does not directly use a single feature quantity to trigger a fault, but instead normalizes each feature quantity to... And assign weights based on the sensor’s role in the current joint. For example, the consistency score can be calculated using the following formula: ; in To address the combined anomaly penalty, when a single sensor exhibits a short-term jump while other sensors remain continuously consistent, When the error is small, it only enters the observation and limiting state; when the encoder deviation continues to increase and is accompanied by inconsistent speed trends, Increase and enter a degraded closed-loop state; when the encoder, torque, and IMU are simultaneously mismatched and the communication age exceeds the limit. It may further increase to the point of triggering a failure and incapacitation state.

[0031] Optionally, under the observation limiting state, the domain controller downweights the role of the sensor corresponding to the abnormal data source and limits the slope of the output torque; In the degraded closed-loop state, the domain controller stops using the abnormal data source output by the faulty sensor, enables the data source of the non-faulty sensor, and reduces the position loop gain or limits the output torque. In the fault-incapacitated state, the domain controller prohibits output control actions to latch the fault and sends a system protection request or emergency stop request to the main controller.

[0032] In this embodiment, the domain controller can execute a hierarchical action strategy based on the verification result. Optionally, refer to... Figure 11 The hierarchical action strategy can include four levels, C0-C3. C0 is normal enable, where the domain controller enters closed-loop enable mode, using a complete multi-sensor closed loop. C1 is observation / limiting, where the domain controller enters observation / limiting mode, reducing the weight of suspicious sources and limiting the torque slope without immediately cutting off the joint control channel; for example, in a data fusion algorithm, the weight of the suspicious sensor is reduced. For instance, if the second encoder skips a beat, the weight of the data from the first encoder and IMU increases, while the weight of the second encoder data decreases. Limiting the torque slope can limit the rate of change of the joint output torque. C2 is degraded closed loop, where the domain controller enters degraded closed loop mode, isolating suspicious sources based on the anomaly category, switching to a reliable angle source, reducing the position loop gain, or limiting the output torque; for example, directly removing the data from the control loop of a faulty sensor and activating a backup scheme. For instance, if the first encoder fails, only the data from the second encoder is used to estimate the joint angle. Reducing gain / limiting torque refers to reducing the controller's "stiffness" (position loop gain) and limiting the maximum output torque within a safe range. This prevents the joint from performing high-precision or high-force movements. C3 indicates a fault-induced disability. The domain controller enters a fault-induced disability state, prohibiting the joint from outputting new active movements, latching the fault, and requesting the main controller to perform overall protection or emergency stop. For example, the domain controller immediately cuts off the motor's power output, allowing the joint to enter a free or braked state, records the fault code, and does not allow automatic recovery. Simultaneously, it reports the highest-level error to the main controller, requesting the robot to enter an emergency stop state.

[0033] Under observation and limiting conditions, by reducing the weight of abnormal data sources and limiting the torque slope, system oscillations caused by data jumps were effectively suppressed. Under degraded closed-loop conditions, by isolating fault sources and enabling backup data, combined with gain reduction or torque limiting measures, basic operational capabilities were maintained under abnormal conditions, improving the reliability of the multi-joint robot. Under fault-incapacitated conditions, by disabling outputs and latching faults, rapid fault isolation and safety protection were achieved, preventing the fault range from expanding. In this way, while ensuring control accuracy, the fault tolerance and safety of the robot joint control system were also taken into account, reducing the risk of unexpected downtime and improving the overall operational stability of the robot.

[0034] In one embodiment, reference Figure 3 The step of the domain controller entering fault protection mode when the verification result indicates a verification failure includes: Step S610: When the consistency score indicates a single sensor jump in a single cycle and the other sensors are continuously consistent over multiple cycles, the domain controller enters the observation limiting state. Step S620: When the consistency score characterization angle deviation maintains an upward trend within a preset first time period and the speed change trends of the output results of the first encoder and the output results of the second encoder are inconsistent, the domain controller enters a degraded closed-loop state. Step S630: If the output torque of the joint detected by the torque sensor in the consistency score is opposite to the direction of the torque estimated by the motor current, or if the difference between the output torque of the joint detected by the torque sensor and the torque estimated by the motor current is greater than a preset difference, the domain controller enters a degraded closed-loop state. Step S640: If the output results of two or more sensors within a preset second time period are all abnormal data sources, the domain controller enters the fault-disabled state.

[0035] It should be noted that each type of anomaly can have an observation window (N_observe), a fault confirmation window (N_fault), and a recovery window (N_clear). The duration of each window is pre-set by the R&D personnel and stored in the internal or external memory of the control device. The observation window indicates that when an anomaly is detected for the first time, an alarm will not be triggered immediately; instead, an observation period will begin. Within this window, the data will be continuously observed. If the anomaly is merely intermittent electromagnetic interference and returns to normal within the observation period, it will not be counted as a suspected fault event. Conversely, if the fault persists for longer than N_observe, it will be considered a suspected fault event. The fault confirmation window indicates that if the anomaly does not disappear within the observation window, a confirmation period will begin. Only when the anomaly persists for multiple consecutive cycles (reaching a preset number) is it confirmed as a fault event, and the domain controller enters fault protection mode. The recovery window limits the recovery time required to downgrade from the current fault state to the next fault state.

[0036] In this embodiment, a short-term jump must remain abnormal for multiple consecutive cycles to escalate to the next level of fault. Recovery also requires continuous fulfillment of the following conditions: deviation regression to a safe range, no data jumps, no communication timeouts, and the inter-source residual decreasing to within the hysteresis interval before fault degradation is allowed. Specifically, all of the following conditions must be continuously met: deviation regression to a safe range, no data jumps, no communication timeouts, and the inter-source residual decreasing to within the hysteresis interval before degradation processing can proceed. For example, the fault is degraded from C2 to closed-loop recovery to C1 for observation, and then back to the normal enabled state of C0. It should be noted that the C3 fault inoperability state will not recover automatically; it requires master control confirmation or manual fault clearing.

[0037] In this embodiment, when the domain controller detects a jump in data from a sensor (e.g., the first encoder) that far exceeds physical limits within the current control cycle, while the data from the second encoder, torque sensor, and IMU remain highly consistent across multiple consecutive cycles, this jump may be caused by electromagnetic interference or occasional packet loss in communication. The domain controller enters the C1 observation and limiting state, reduces the weight of the first encoder in the data fusion algorithm, and limits the rate of change (slope) of the joint output torque, thus slowing down the joint movement, and continues to observe the data for the next cycle. If the domain controller continues to monitor the data from the first and second encoders, and finds that the angle deviation between the two not only fails to converge within a preset first time period but continues to widen; simultaneously, the speed change at the motor end and the speed change at the joint end show a divergence in trend (e.g., the motor is accelerating, but the joint end response is lagging or even reversed), the domain controller enters the C2 degraded closed-loop state, immediately isolates the abnormal encoder in the algorithm, relies solely on another reliable source for estimation control, and significantly reduces the position loop gain or limits the maximum output torque to prevent the joint from losing control due to transmission failure, thereby enabling the compliance protection mechanism. If the domain controller detects that not only are the data from the first and second encoders mismatched, but the data from the torque sensor or IMU also shows serious anomalies, and this abnormal state persists for a preset second duration (i.e., the duration corresponding to the fault confirmation time window), this indicates that the problem is no longer a single sensor failure, but may be due to a calculation error by the domain controller itself, a serious power supply anomaly, or the entire wiring harness being torn. The domain controller then enters the highest-level C3 fault incapacity state, immediately cutting off the PWM signal, latching the fault code to prevent automatic recovery, and sending a highest-level emergency stop request to the main controller, requesting the entire system to enter a safety protection state. Both the preset first and second durations can be pre-set and stored in the control device's memory by the developers.

[0038] Steps S610 to S640 establish a fault classification response mechanism based on the consistency scoring of multi-source heterogeneous data. This mechanism effectively filters out interference while ensuring control continuity, guaranteeing equipment compliance and operational safety under mechanical wear or external collision fault conditions. Simultaneously, for severe domain control or wiring harness-level faults with concurrent anomalies of multiple sensors, the highest level of disability protection and emergency stop requests are executed. This improves the fault detection accuracy of the robot control system.

[0039] In one embodiment, reference Figure 4 After the domain controller enters fault protection mode, the robot joint control method further includes: Step S700: When multiple feature quantities meet their corresponding preset security ranges within a preset third time period, the domain controller switches from the current state to an adjacent low-risk level state, and when the domain controller operates in the adjacent low-risk level state for a preset fourth time period, the relationship between the multiple feature quantities and their corresponding preset security ranges is determined. Step S800: If the domain controller is in a fault-incapacitated state, stop the process of switching from the current state to the adjacent low-risk level state. Step S900: If the domain controller is in a fault-incapacitated state and an external reset command is received, a reset action is performed.

[0040] In this embodiment, the preset third duration and preset fourth duration can be set in advance by the R&D personnel and stored in the domain controller's memory.

[0041] Based on the above embodiments, when a domain controller is in a certain fault-degraded state (such as C2 limit or C1 observation), if all relevant characteristic quantities (such as deviation, residual, jump, etc.) are monitored and remain stable within a safe range for a relatively long period of time (preset third time interval), it indicates that the fault may have been eliminated. However, at this time, the domain controller will not directly jump back to the normal C0 state, but will first switch to an adjacent low-risk level state (such as switching from C2 to C1). Simultaneously, this intermediate state (such as C1) must operate stably for a preset fourth time interval, and only after confirming that the data is still normal can it be allowed to continue upward recovery or be confirmed as safe. When the domain controller is in a fault-incapacitated state (C3), it usually means that a serious fault has occurred, such as a domain controller crash, wiring harness breakage, or simultaneous failure of multiple sensors, and automatic recovery logic is prohibited. Even if the sensor data is normal at this time, it will be locked in the incapacitated state. In the C3 fault-incapacitated state, the only way to recover is to receive an external reset command. External reset commands usually come from the host computer main controller, physical button operation by maintenance personnel, or dedicated debugging software commands. Only upon receiving an external reset command will the domain controller execute a reset action, clear the fault latch, and return to the initialization process, standby state, or C0-C2 closed-loop enabled mode, which can be determined based on the external reset command.

[0042] By constructing a fault classification and recovery mechanism, a safe and smooth transition of robot joint control from a fault state to a normal state is achieved, effectively preventing state oscillations and false recovery caused by transient signal fluctuations. At the same time, by setting a forced reset logic triggered by external commands, it is ensured that the resolution and recovery of serious faults must be confirmed by humans or the host computer, further ensuring the operational safety and reliability of multi-joint robots.

[0043] In another embodiment, the sensor consistency verification action includes: Obtain its own identity information, sensor topology, and joint parameter verification code; Based on the identity information, the sensor topology, and the joint parameter check code, if the preset verification conditions are met, the verification result is determined to be a successful verification. If the preset verification conditions are not met based on the identity information, the sensor topology, and the joint parameter check code, the verification result is determined to be a verification failure. The preset verification conditions include: The identity information satisfies at least one of the following: the number of connected sensors matches the number of connected targets; the type of connected sensors matches the type of connected targets; and the joint parameter check code is loaded correctly.

[0044] Based on the above embodiments, refer to Figure 9 The domain controller obtains its own identification information, such as hardware ID or firmware version, to confirm its identity. For example, it determines whether the domain controller is installed on the left lower leg or the right upper leg to prevent interchangeable installations during production or maintenance. If the domain controller for the left leg is mistakenly installed on the right leg, the ID will not match, and the preset safety conditions will not be met. The system will then disable the action and report the issue directly to the main controller. Sensor topology includes the number and type of connected sensors (e.g., encoders, IMUs, torque sensors, FSRs, etc.) and their connection status to prevent incorrect or missing wiring harnesses. For example, if the design requires two encoders and one IMU, but only one encoder is detected, or an incorrect sensor is detected, it indicates an incorrect wiring harness connection or hardware damage. Joint parameter checksums include, but are not limited to, CRC checksums of key control parameters such as direction, limit switches, reduction ratio, and gain, to confirm whether the loaded joint parameters match the current hardware and prevent parameter writing errors. For example, if a motor with a reduction ratio of 100:1 is loaded with a parameter file of 50:1, the robot will run away or become uncontrollable upon movement. Only when the information in the above three dimensions simultaneously meets the preset verification conditions (e.g., valid ID and correct location + complete match of sensor quantity and type + passed parameter CRC check), will the domain controller output a verification result indicating successful verification, allowing the joint to enter the enabled state and begin receiving control commands. If any dimension does not match (e.g., the ID is correct but one sensor is missing, or the sensors match but the parameter CRC error occurs), the domain controller immediately determines that the verification has failed. This forces the joint to remain in an disabled state (prohibiting motor torque output) and reports the specific error code to the main controller, prompting maintenance personnel to check the assembly or rewrite the parameters.

[0045] refer to Figure 6 and Figure 10The multi-joint robot is equipped with a first encoder, a second encoder, a torque sensor, and an IMU, enabling closed-loop control during shutdown. Each domain controller can perform closed-loop control based on the outputs of the first encoder, second encoder, torque sensor, and IMU, constraining joint outputs through a combination of motor inner loop, actual joint angle, torque load, and limb posture. The domain controller can also feed back the execution results to the first encoder, second encoder, torque sensor, and IMU, achieving joint closed-loop control of the motor inner loop, joint outer loop, and posture safety loop.

[0046] Understandably, this embodiment allows the same domain-controlled PCBA to be assembled to different limb positions via hardware IDs, automatically loading corresponding parameters and interface topology relationships, thus reducing BOM (Bill of Materials) costs. The PCBA can have one MCU, as well as EtherCAT x2, CANFD x2 / RS485 x5, torque analog-to-digital converter interface / sensor interface, etc. Only four resistor bits or DIP switches need to be reserved on the circuit board; by changing the high and low level combinations of four pins, 16 different joint identities can be defined. The domain controller can load its own parameters based on the parameter mapping table stored in its internal memory or the joint parameter data issued by the main controller. For example, ID=0001 (left upper arm): loads the upper arm-specific PID gain and gravity compensation parameters, and enables the corresponding CAN node to read the upper arm-specific sensors. ID=0010 (left forearm): parameters automatically switch to forearm mode; the load parameters may decrease, and the control stiffness may be adjusted. ID=0101 (back) and ID=1000 (right lower leg) will also load the corresponding joint parameters. In this way, multi-joint control can be achieved using the same hardware configuration, reducing assembly and maintenance costs.

[0047] In one embodiment, reference Figure 11 The robot joint control method further includes: After the domain controller enters fault protection mode, the domain controller generates fault alarm information and uploads it to the main controller; The fault alarm information includes at least one of the following: fault code, its own identity information, and timestamp.

[0048] In this embodiment, when a domain controller enters any level of fault protection mode (C1 to C3), it generates fault alarm information, packages it into a fault alarm message, and sends it to the master controller via a high-speed communication bus (such as EtherCAT). In this way, the domain controller can execute local fault protection actions without waiting for instructions from the master controller, and simultaneously report the fault to the master controller so that the master controller can perform subsequent system cadence adjustments, etc.

[0049] It is understood that fault alarm information includes, but is not limited to, fault codes, the node's own identification information, and timestamps. The main controller can determine the fault type by parsing the fault codes, and based on this, decide whether to continue slow operation or perform an emergency shutdown. The node's own identification information, i.e., its node ID, allows the main controller to pinpoint the specific physical location of the fault (e.g., "left knee joint" or "right wrist joint"). The main controller can determine the time of the fault occurrence using the timestamp.

[0050] Optional, see reference Figure 12 The robot joint control method further includes: When the main controller receives the fault alarm information, it performs a whole machine status assessment and gait replanning action based on the fault code and the timestamp to determine the whole machine control strategy. The overall machine control strategy includes any one of the following: continued operation, degraded operation, and emergency stop.

[0051] Based on the above embodiments, when a sensor or motor malfunctions, the domain controller does not need to wait for instructions from the main controller and immediately executes protective actions locally (such as cutting off torque output, entering compliant mode, and locking joints). After receiving fault alarm information reported by the domain controllers, the main controller can perform an overall system status assessment by combining the status of other joints (reported by the domain controllers of other joints). For example, it can determine the severity and type of the fault based on the fault code and the temporal correlation of the fault based on the timestamp. Based on the assessment results, the main controller determines the overall system control strategy and issues corresponding control commands to each domain controller.

[0052] Optionally, if the domain controller reports intermittent interference (such as a single-cycle jump) that has been filtered or suppressed locally and does not affect actual motion performance, the main controller can determine that the entire robot continues to operate. If a joint partially fails (e.g., the torque sensor is faulty, but the encoder is normal), the main controller determines to degrade operation. This can involve modifying the gait algorithm (e.g., switching from dynamic running to static walking) or adjusting the center of gravity distribution to avoid the stress limits of the damaged joint. By sacrificing some performance (speed, load), the task can be completed and the equipment preserved. However, if a critical support joint is completely disabled (e.g., C3 level), or if multiple joints fail simultaneously, causing the multi-joint robot to lose balance, the main controller immediately triggers the emergency stop logic, controlling all non-faulty joint domain controllers to execute controlled fall actions or emergency braking actions to prevent secondary damage and protect personnel and equipment safety.

[0053] In this way, the domain controller possesses local fault diagnosis and immediate handling capabilities. When a fault occurs, it can first be handled within the limb domain controller to prevent the fault from spreading to other joints. At the same time, it reports to the main controller, facilitating cross-limb coordination and recovery management. The two form a layered safety mechanism. This further improves the reliability and safety of multi-joint robot operation.

[0054] This application also proposes a robot joint control system based on the robot joint control method described above. The robot joint control system includes a robot body, and a main controller, multiple domain controllers, a first encoder, a second encoder, a torque sensor, and an inertial measurement unit disposed on the robot body. Each domain controller is used to control the enable state of a joint. The first encoder is used to detect the motor rotation angle, the second encoder is used to detect the joint rotation angle, the torque sensor is used to detect the joint output torque, and the inertial measurement unit is used to detect the joint pose.

[0055] Optionally, the robot joint control system further includes an EtherCAT slave communication interface, through which the master controller communicates with each of the domain controllers.

[0056] In this embodiment, the master controller and each domain controller can communicate using the EtherCAT slave communication interface, ensuring that the master controller can synchronously read the status of all domain controllers at a high frequency and issue control commands to each domain controller.

[0057] To achieve high-precision joint control, each joint can be equipped with multiple sensors. In this embodiment, the first encoder (motor end) is responsible for detecting the position of the motor rotor, used for motor commutation and speed loop control. The second encoder (joint end / external) is responsible for detecting the actual angle of the joint, thereby achieving high-precision position control. The torque sensor is used to measure the physical torque output by the joint, providing core data for achieving compliant control and collision detection. The inertial measurement unit is responsible for detecting the spatial attitude and acceleration of the joint, assisting in judging the motion trend and abnormal impact motion.

[0058] In this embodiment, the domain controller, in addition to the main controller (such as an MCU), may also be configured with a CAN FD interface, an RS485 interface, an analog-to-digital converter, an ID configuration circuit, and a debugging interface. The domain controller can acquire the internal status of the motor (such as current, temperature, and speed) via the CAN FD interface. The domain controller can read an external high-precision encoder (such as a second encoder) via the RS485 interface and acquire the analog voltage signal from the torque sensor through the analog-to-digital converter. In this embodiment, 1.25V can be set as the zero point (i.e., the state without torque output). 0V ~ 1.25V is mapped to the maximum reverse torque, and 1.25V ~ 2.5V is mapped to the maximum forward torque. That is, 0V to 2.5V is mapped to the maximum torque from reverse to forward. This differential voltage design can effectively distinguish the direction of force and improve noise immunity.

[0059] Understandably, the raw ADC data acquired by the analog-to-digital converter often contains noise. The domain controller can also use its internal filtering circuit to filter the raw data first, and then use it as the basis for impedance control and overload protection.

[0060] In practical applications, the robot joint control system allows the same set of domain controller hardware to be reused at different joint positions, reducing hardware design costs. At the same time, the dual verification mechanism of identification information and sensor topology detection reduces the risk of assembly errors. In addition, the setting of the first encoder, second encoder, torque sensor and IMU improves the joint control accuracy of the robot joint control system for multi-joint robots, thereby improving the operational safety and power failure recovery capability of multi-joint robots.

[0061] The robot joint control system provided by this invention is based on the robot joint control method described above. Compared with the prior art, the beneficial effects of the robot joint control system provided by this invention are the same as those of the robot joint control method provided in the above embodiments, and other technical features in the robot joint control system are the same as those disclosed in the methods of the above embodiments, and will not be repeated here.

[0062] The above description is only a part of the embodiments of the present invention and does not limit the patent scope of the present invention. All equivalent structural transformations made under the technical concept of the present invention using the contents of the present invention specification and drawings, or direct / indirect applications in other related technical fields, are included within the patent protection scope of the present invention.

Claims

1. A robot joint control method, characterized in that, This invention relates to a robot joint control system, which includes a main controller, multiple domain controllers, a first encoder, a second encoder, a torque sensor, and an inertial measurement unit. Each domain controller is used to control the enable state of a joint. The first encoder is used to detect the motor rotation angle, the second encoder is used to detect the joint rotation angle, the torque sensor is used to detect the joint output torque, and the inertial measurement unit is used to detect the joint pose. The robot joint control method includes: The main controller reads the identity information corresponding to the multiple domain controllers and loads the joint parameters of each domain controller based on the identity information; When the main controller determines that the identity information meets the preset security conditions based on the identity information, it outputs the joint parameter corresponding to the identity information to the corresponding domain controller. The domain controller acquires the output results corresponding to the first encoder, the second encoder, the torque sensor, and the inertial measurement unit, and performs sensor topology detection based on the output results to determine whether the sensor topology connected to each joint matches the preset target topology. If the accessed sensor topology matches the preset target topology, the main controller performs a communication link detection action. After performing the communication link detection action and determining that the communication link status is normal, the main controller controls the domain controller to perform a sensor consistency verification action to obtain the verification result. If the verification result indicates that the verification passed, the domain controller enters the closed-loop enable mode; If the verification result indicates a failure, the domain controller enters a fault protection mode. The closed-loop enabling mode includes outputting corresponding motor control signals based on the output results of the first encoder, the second encoder, the torque sensor, and the inertial measurement unit; the fault protection mode includes, in order of risk level from low to high, the observation limiting state, the degraded closed-loop state, and the fault incapacitance state.

2. The robot joint control method as described in claim 1, characterized in that, The sensor consistency verification action includes: Multi-source sensor data is obtained based on the output results of the first encoder, the second encoder, the torque sensor, and the inertial measurement unit, as well as the status of the communication link. Feature extraction is performed on the multi-source sensor data to obtain multiple feature quantities; The multiple features are normalized, and the roles of each sensor in the current joint are weighted to obtain a consistency score. The corresponding verification result is determined based on the consistency score. The feature quantities include at least one of the following: the angle deviation between the joint angle calculated from the motor rotation angle and the joint rotation angle; the cycle jump variable between the current cycle and the previous cycle; the difference between the current domain control time and the most recent effective sampling time; the difference between the output torque and the estimated torque of the motor current; and the difference between the joint pose and the preset pose.

3. The robot joint control method as described in claim 2, characterized in that, Under the observation limiting state, the domain controller downweights the role of the sensor corresponding to the abnormal data source and limits the slope of the output torque; In the degraded closed-loop state, the domain controller stops using the abnormal data source output by the faulty sensor, enables the data source of the non-faulty sensor, and reduces the position loop gain or limits the output torque. In the fault-incapacitated state, the domain controller prohibits output control actions to latch the fault and sends a system protection request or emergency stop request to the main controller.

4. The robot joint control method as described in claim 2, characterized in that, When the verification result indicates a failure, the domain controller entering fault protection mode includes: When the consistency score indicates a single sensor jumps in a single cycle while the other sensors remain consistent over multiple cycles, the domain controller enters the observation limiting state. When the consistency score characterization angle deviation maintains an upward trend within a preset first time period and the speed change trends of the output results of the first encoder and the output results of the second encoder are inconsistent, the domain controller enters a degraded closed-loop state. If the output torque of the joint detected by the torque sensor is opposite to the direction of the torque estimated by the motor current, or if the difference between the output torque of the joint detected by the torque sensor and the torque estimated by the motor current is greater than a preset difference, the domain controller enters a degraded closed-loop state. If the output results of two or more sensors are all abnormal data sources within a preset second time period, the domain controller enters the fault-disabled state.

5. The robot joint control method as described in claim 1, characterized in that, The sensor consistency verification action includes: Obtain its own identity information, sensor topology, and joint parameter verification code; Based on the identity information, the sensor topology, and the joint parameter check code, if the preset verification conditions are met, the verification result is determined to be a successful verification. If the preset verification conditions are not met based on the identity information, the sensor topology, and the joint parameter check code, the verification result is determined to be a verification failure. The preset verification conditions include: The identity information satisfies at least one of the following: the number of connected sensors matches the number of connected targets; the type of connected sensors matches the type of connected targets; and the joint parameter check code is loaded correctly.

6. The robot joint control method as described in claim 2, characterized in that, After the domain controller enters fault protection mode, the robot joint control method further includes: When multiple features meet their corresponding preset security ranges within a preset third time period, the domain controller switches from its current state to an adjacent low-risk level state. When the domain controller operates in the adjacent low-risk level state for a preset fourth time period, the relationship between the multiple features and their corresponding preset security ranges is determined. If the domain controller is in a faulty or disabled state, the process of switching from the current state to an adjacent low-risk level state shall be stopped. If the domain controller is in a faulty or disabled state and an external reset command is received, a reset action is performed.

7. The robot joint control method according to any one of claims 1 to 6, characterized in that, The robot joint control method further includes: After the domain controller enters fault protection mode, the domain controller generates fault alarm information and uploads it to the main controller; The fault alarm information includes at least one of the following: fault code, its own identity information, and timestamp.

8. The robot joint control method as described in claim 7, characterized in that, The robot joint control method further includes: When the main controller receives the fault alarm information, it performs a whole machine status assessment and gait replanning action based on the fault code and the timestamp to determine the whole machine control strategy. The overall machine control strategy includes any one of the following: continued operation, degraded operation, and emergency stop.

9. A robot joint control system, characterized in that, Based on the robot joint control method according to any one of claims 1-8, the robot joint control system includes a robot body, and a main controller, multiple domain controllers, a first encoder, a second encoder, a torque sensor and an inertial measurement unit disposed on the robot body. Each domain controller is used to control the enable state of a joint. The first encoder is used to detect the motor rotation angle, the second encoder is used to detect the joint rotation angle, the torque sensor is used to detect the joint output torque, and the inertial measurement unit is used to detect the joint pose.

10. The robot joint control system as described in claim 9, characterized in that, The robot joint control system also includes an EtherCAT slave communication interface, through which the master controller communicates with each of the domain controllers.

Images