A robot fall detection and stand-up control method and system

Abstract

The application discloses a robot fall detection and standing up control method and system, and relates to the field of robot motion control.The method comprises the following steps: acquiring posture sensing data and joint motion data of a target robot; determining whether the robot enters a fall mode based on the data; if the robot enters the fall mode, performing communication link and hardware self-checking; if the self-checking fails, prohibiting standing up and alarming; if the self-checking passes, performing posture solving and classification, and obtaining fall state classification results such as lying flat without pressure, limbs under pressure, turning on one's side or turning over; and finally, calling corresponding standing up control strategies based on the classification results to perform adaptive recovery.The application solves the problems of low detection accuracy, inability to distinguish fall forms, single recovery strategy leading to secondary damage and lack of abnormal alarm through multi-sensor fusion, pre-execution self-checking and hierarchical adaptive recovery, and improves the survival ability and task reliability of the robot in a complex environment.

Description

Technical Field

[0001] This application relates to the field of robot motion control technology, and in particular to a method and system for robot fall detection and stand-up control. Background Technology

[0002] With the development of robotics technology, quadrupedal and quadwheeled robots (hereinafter referred to as "robots") are widely used in industries such as industrial inspection, medical care, and disaster relief. These robots typically possess high mobility and adaptability, enabling them to perform tasks in complex environments, such as railway inspection, power line maintenance, underground exploration, or logistics transportation. However, in actual operation, robots inevitably face the risk of falling, which can not only interrupt task execution but also potentially cause serious safety hazards. For example, in railway inspection scenarios, if a robot falls and lies on its side on the tracks due to uneven ground or external collisions, it may be run over by a high-speed train, resulting in complete damage to the robot and causing railway safety accidents, even affecting train scheduling and passenger safety. In power line maintenance, a robot falling may cause it to become stuck near high-voltage power lines, causing short circuits or arc discharges, threatening the safety of surrounding personnel and equipment. In chemical plant inspections, a fallen robot may obstruct passageways or cause a fire due to damage to the battery. Furthermore, in disaster relief such as earthquake sites, if a robot cannot recover quickly after falling, it may delay search and rescue time, leading to secondary disasters.

[0003] In related technologies, robot fall detection often relies on a single sensor, such as using only an IMU (Inertial Measurement Unit) to monitor angle changes, but neglecting joint encoders and torque feedback, resulting in low detection accuracy and an inability to distinguish different types of falls. Simultaneously, stand-up control often employs a fixed recovery strategy, ignoring complex situations such as leg compression or side falls; directly executing stand-up commands can lead to motor stalling, overheating, or even mechanical structural breakage. For falls caused by hardware damage, such as joint motor failure or communication link interruption, traditional methods lack abnormal alarm and manual intervention mechanisms, leaving the robot in a dangerous state for extended periods and unable to be maintained promptly.

[0004] Therefore, there is an urgent need for a robot fall detection and recovery control method to solve the problems of low fall detection accuracy, inability to distinguish different fall types, single recovery strategy leading to secondary damage, and lack of abnormal alarm and manual intervention mechanisms, thereby improving the robot's survivability and task reliability in complex environments. Summary of the Invention

[0005] The purpose of this application is to provide a method and system for robot fall detection and recovery control, which can improve the accuracy of fall detection, enhance the adaptability of recovery strategies, and improve execution safety.

[0006] To achieve the above objectives, this application provides the following solution: In a first aspect, this application provides a robot fall detection and standing control method, including: Acquire posture perception data and joint motion data of the target robot; the target robot includes a quadruped robot and a four-wheeled robot; Based on the posture perception data and joint motion data, determine whether the target robot has entered a fall mode; If the target robot enters the fall mode, a self-check is performed on the target robot's communication link and hardware status, and the self-check results are obtained. If the self-test result is unsuccessful, the target robot will be prohibited from executing the stand-up control strategy and an abnormal alarm will be triggered. If the self-check result is passed, then based on the positive kinematics solution method, the posture solution and classification operation are performed according to the posture perception data and joint motion data to obtain the fall state classification result; the fall state classification result includes at least one of the following: flat-lying fall without pressure, fall with limb pressure, side fall, and rollover. Based on the fall state classification results, an adaptive recovery control is performed through a stand-up control strategy to restore the target robot to a stable standing posture. The stand-up control strategy includes a direct stand-up recovery sub-strategy, an escape stand-up recovery sub-strategy, a first roll correction recovery sub-strategy, and a second roll correction recovery sub-strategy.

[0007] Secondly, this application provides a robot fall detection and standing control system, including: An inertial measurement unit is installed on the body of the target robot and is used to collect the attitude perception data of the target robot. Joint encoders are installed at each joint of the target robot to collect joint motion data of the target robot. The controller is communicatively connected to the inertial measurement unit and the joint encoder. The controller includes a memory and a processor. The memory stores a computer program. When the processor executes the computer program, it implements the above-described robot fall detection and stand-up control method.

[0008] According to the specific embodiments provided in this application, this application has the following technical effects: This application provides a method and system for robot fall detection and recovery control. By acquiring the target robot's posture perception data and joint motion data, it solves the problem of incomplete detection information caused by relying on a single sensor in related technologies, and realizes the data foundation of multi-sensor fusion. By determining whether the target robot has entered a fall mode based on the posture perception data and joint motion data, it solves the problem of low fall detection accuracy in related technologies, and realizes reliable identification of the fall state. By performing self-checks on the target robot's communication link and hardware status, it solves the problem of blind recovery in fault states due to the lack of pre-execution safety mechanisms in related technologies, and realizes safety verification before recovery actions. Based on the positive kinematics calculation method, posture calculation and classification operations are performed on the posture perception data and joint motion data to obtain fall state classification results including unpressurized flat-lying fall state, limb-pressurized fall state, side-lying fall state, and supine fall state. This solves the problem of not being able to distinguish different fall forms in related technologies and realizes refined classification of fall states. By calling the standing control strategy including the direct standing recovery sub-strategy, the escape standing recovery sub-strategy, the first roll correction recovery sub-strategy, and the second roll correction recovery sub-strategy based on the fall state classification results, adaptive recovery control is performed, which solves the problem of secondary damage caused by fixed recovery strategies in related technologies and realizes targeted recovery for different fall forms. Attached Figure Description

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

[0010] Figure 1 A flowchart illustrating a robot fall detection and stand-up control method according to an embodiment of this application; Figure 2 A schematic diagram of a robot in a crouching or lying-down mode according to an embodiment of this application; Figure 3 A schematic diagram of the robot in standing mode provided in an embodiment of this application; Figure 4 This is a schematic diagram of the motion mode of a robot provided in an embodiment of this application; Figure 5 A schematic diagram of a robot in a fallen state where one of its legs is trapped under the body, according to an embodiment of this application; Figure 6 A schematic diagram of a robot falling over with its two legs trapped under the body, provided in an embodiment of this application; Figure 7 A schematic diagram illustrating a robot in a fallen state where its three legs are trapped under the body, according to an embodiment of this application. Figure 8 A schematic diagram of a robot in a fallen state where its four legs are trapped under the body, according to an embodiment of this application; Figure 9 A schematic diagram of a robot in a flipping state provided in an embodiment of this application; Figure 10 A schematic diagram of a robot falling to its side, provided in an embodiment of this application; Figure 11 A functional framework diagram of a robot fall detection and recovery control system provided in another embodiment of this application; Figure 12 This is a schematic diagram of the structure of a computer device provided in an embodiment of this application.

[0011] Attached diagram labels: 1-Wheel; 2-Wheel motor; 3-Thigh joint motor; 4-Lower leg joint motor; 5-Hip joint motor; 6-Battery; 7-Lower leg; 8-Thigh; 9-Controller. Detailed Implementation

[0012] First, some technical terms involved in the embodiments of this application will be introduced.

[0013] IMU (Inertial Measurement Unit): A sensor that integrates a gyroscope, accelerometer, and magnetometer to measure attitude information such as attitude angles, angular velocities, and accelerations of quadrupedal and four-wheeled robots.

[0014] Joint encoder: A sensor installed on the joint motors of quadruped and four-wheeled robots to monitor the angular position and speed of change of the joint in real time.

[0015] Falling states: Classified according to the posture and limb position of quadruped and quadwheeled robots, including "limb-pressed fall" (at least one limb is pressed down by the robot body, causing joint jamming, such as one leg pressed down, two legs pressed down, three legs pressed down, or four legs pressed down), "side fall" (quadruped and quadwheeled robots roll over to the side, with the robot body tilted more than 45 degrees, corresponding to a side roll), "back roll fall" (quadruped and quadwheeled robots have their backs touching the ground, and all limbs are free), and "unpressed flat fall" (quadruped and quadwheeled robots lie flat on the ground, with all limbs free, corresponding to a squatting mode or a flat fall).

[0016] Abnormal situations: Unnatural causes of falls, such as "hardware damage" (joint motor failure or sensor malfunction, resulting in abnormal data) and "communication link failure" (loss of wireless connection, unable to receive remote commands).

[0017] Fall recovery control: An algorithm that dynamically adjusts the recovery strategy based on the detected fall pattern.

[0018] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0019] To make the above-mentioned objectives, features and advantages of this application more apparent and understandable, this application will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0020] In one exemplary embodiment, such as Figure 1 As shown, a method for robot fall detection and recovery control is provided. This method is executed by a computer device, specifically by a terminal or server alone, or by both a terminal and a server. In this embodiment, it includes steps 101 to 106. Wherein: Step 101: Obtain the posture perception data and joint motion data of the target robot; the target robot includes a quadruped robot and a four-wheeled robot.

[0021] Step 102: Based on the posture perception data and joint motion data, determine whether the target robot has entered a fall mode.

[0022] Step 103: If the target robot enters the fall mode, perform a self-check on the target robot's communication link and hardware status, and obtain the self-check results.

[0023] Step 104: If the self-test result is not passed, the target robot is prohibited from executing the stand-up control strategy and an abnormal alarm is triggered.

[0024] Step 105: If the self-check result is passed, then based on the forward kinematics calculation method, the posture calculation and classification operation is performed according to the posture perception data and joint motion data to obtain the fall state classification result; the fall state classification result includes at least one of the following: flat-lying fall without pressure, fall with limb pressure, side fall, and rollover.

[0025] Step 106: Based on the fall state classification results, adaptive recovery control is performed through the stand-up control strategy to restore the target robot to a stable standing posture; the stand-up control strategy includes a direct stand-up recovery sub-strategy, an escape stand-up recovery sub-strategy, a first roll correction recovery sub-strategy, and a second roll correction recovery sub-strategy.

[0026] By implementing steps 101 to 106 above, this application can acquire posture perception data and joint motion data through multi-sensor fusion to accurately detect whether the robot has entered a fall mode; by performing a self-check of the communication link and hardware status before recovery, and prohibiting the stand-up control strategy and issuing an alarm when the self-check fails, safety in abnormal situations is ensured; by performing fine posture classification through forward kinematics calculation, different fall states such as lying flat without pressure, limb pressure, side fall, and backward fall can be accurately distinguished; through an adaptive stand-up control strategy, the corresponding recovery sub-strategy is called according to the fall state classification result, realizing a graded recovery mechanism of first getting out of trouble and then standing up or first rolling correction and then standing up, solving the problems of low detection accuracy, inability to distinguish fall forms, single recovery strategy and lack of abnormal intervention in related technologies, avoiding secondary damage, and improving the robot's autonomous survival ability and environmental adaptability after a fall.

[0027] In another exemplary embodiment of this application, step 102 specifically includes: The target robot's body tilt angle and body acceleration are determined based on the attitude perception data, and the angular velocities and torques of each joint of the target robot are determined based on the joint motion data; the attitude perception data includes body attitude angle, body angular velocity, and body acceleration; the joint motion data includes the encoder angle and torque of each joint motor; If the abnormal triggering conditions are met, an abnormality is determined to have occurred, and the duration of the abnormal state is started to be timed. The abnormal triggering conditions include the joint angular velocity of any joint being greater than or equal to a preset angular velocity threshold and the fuselage tilt angle being greater than or equal to a preset safe tilt angle threshold; or, the rate of change of the joint torque of any joint being greater than or equal to a preset torque rate of change threshold and the fuselage tilt angle being greater than or equal to a preset safe tilt angle threshold. If the duration of the abnormal state is greater than or equal to the first preset time threshold, and at the end of the first preset time threshold, the difference between the three-axis resultant force modulus of the body acceleration and the gravitational acceleration is within the preset static tolerance range, then the target robot is determined to have entered the fall mode.

[0028] In another exemplary embodiment of this application, a self-test is performed on the communication link and hardware status of the target robot to obtain the self-test result, specifically including: If the heartbeat packet loss rate of the bus communication heartbeat packets between the target robot's controller 9 and each joint driver is greater than or equal to the preset loss rate threshold within the preset statistical period, then the communication link is determined to be faulty. If the heartbeat packet loss rate of the bus communication heartbeat packets between the target robot's controller 9 and each joint driver is less than the preset loss rate threshold within the preset statistical period, the communication link is determined to be normal. If at least one joint motor has an abnormal encoder angle data, or at least one joint motor has an open circuit in its current feedback, then the hardware status is determined to be abnormal; the abnormal data includes non-digital abnormalities or infinity abnormalities. If there are no abnormal data in the encoder angle of each joint motor and there is no open circuit in the current feedback of each joint motor, then the hardware status is considered normal. If the communication link is normal and the hardware is in normal condition, the self-test result is passed; If the communication link fails or the hardware status is abnormal, the self-test result will be "fail".

[0029] In another exemplary embodiment of this application, step 105 specifically includes: The body posture of the target robot is determined based on the posture angle data in the posture perception data; the body posture includes the roll angle and pitch angle of the body relative to the horizontal plane. If the absolute value of the roll angle is greater than or equal to a preset rollover angle threshold, and the normalized deviation of the roll angle relative to the preset rollover angle threshold is greater than or equal to the normalized deviation of the pitch angle relative to the preset pitch rollover angle threshold, then the fall state classification result is determined as a side fall state. If the absolute value of the pitch angle is greater than or equal to a preset pitch rollover angle threshold, and the normalized deviation of the pitch angle relative to the preset pitch rollover angle threshold is greater than the normalized deviation of the roll angle relative to the preset rollover angle threshold, then the fall state classification result is determined as a pitch rollover state. If the absolute value of the roll angle is less than a preset rollover angle threshold, and the absolute value of the pitch angle is less than a preset pitch rollover angle threshold, then the fall status classification result is determined through the following steps: Based on the encoder angle data of each joint motor in the joint motion data, the position of each foot relative to the center of the body is calculated based on the leg forward kinematics model of the target robot, and the three-dimensional spatial coordinates of each foot in the body coordinate system are obtained. Based on the fuselage attitude, fuselage geometry and current fuselage position, determine the projection boundary of the fuselage on the horizontal plane, and form the fuselage projection area by the projection boundary; Based on the three-dimensional spatial coordinates of each foot in the fuselage coordinate system and the current fuselage pose, coordinate transformation is performed to obtain the three-dimensional spatial coordinates of each foot in the world coordinate system, and the foot projection points on the horizontal plane of each foot are extracted respectively. Determine the positional relationship between each foot projection point and the body projection area, and determine whether each joint meets the pressure determination condition; the pressure determination condition is that the absolute value of the joint torque is continuously greater than or equal to the preset torque saturation threshold within a preset time period, and the absolute value of the change in the encoder angle data of the corresponding joint motor within the preset time period is less than the preset micro-motion threshold. If all foot projection points are located outside the fuselage projection area and all joints do not meet the pressure determination condition, then the fall state classification result is determined as a flat-lying fall state without pressure. If at least one foot projection point is located inside or on the boundary of the fuselage projection area, or if at least one joint meets the pressure determination condition, then the fall state classification result is determined to be a limb pressure fall state.

[0030] As an optional implementation, when a sideways or supine state and a limb compression state coexist, the sideways or supine state is prioritized, and after performing roll correction, it is then determined whether there is limb compression.

[0031] In another exemplary embodiment of this application, step 106 specifically includes: If the fall state classification result is a flat-lying fall without pressure, then the direct stand-up recovery sub-strategy is invoked to control the target robot to adjust the posture of each leg and enter a preset squatting mode. In the preset squatting mode, the stand-up action is performed to restore the robot to a stable standing posture. If the fall state classification result is a limb compression fall state, then the escape and stand-up recovery sub-strategy is invoked. The target robot is controlled to first perform an escape action to remove the restriction of the body or external environment on the compressed limb, and then the target robot is controlled to enter the preset squatting mode and perform a stand-up action to restore to a stable standing posture. If the fall state classification result is a side fall, then the first roll correction and recovery sub-strategy is invoked. Based on the positive or negative direction of the roll angle, the roll direction of the fuselage is determined. The leg group on the opposite side of the roll direction is designated as the first preparatory leg group, and the leg group on the same side of the roll direction is designated as the first active leg group. The hip pitch joint movement of the first preparatory leg group is controlled, so that the thigh 8 of the first preparatory leg group moves to a position basically flush with the lower surface of the fuselage. The knee joint flexion of the first preparatory leg group is controlled to reduce the obstruction of the first preparatory leg group to the fuselage roll. The hip abduction and hip pitch joint movements of the first active leg group are also controlled. The first active side leg group's leg or foot end is brought into contact with the ground and a force is applied to the ground. The reaction force of the ground on the target robot generates a rolling torque around the longitudinal axis of the body, driving the body to roll in the direction of the first prepared side leg group. After the roll is completed, the target robot's posture perception data and joint motion data are reacquired, and the fall state classification result of the target robot is re-determined based on the reacquired data. If the re-determined fall state classification result is a flat fall without pressure, the direct stand-up recovery sub-strategy is invoked. If the re-determined fall state classification result is a fall with limb pressure, the escape stand-up recovery sub-strategy is invoked. If the fall state classification result is a pitching fall, the second roll correction and recovery sub-strategy is invoked. Based on the positive or negative direction of the pitch angle, the fuselage is determined to be either pitching forward or backward. When the fuselage pitches forward, the rear leg group is designated as the second preparatory side leg group and the front leg group as the second active side leg group. When the fuselage rolls backward, the front leg group is designated as the second preparatory side leg group and the rear leg group as the second active side leg group. The hip pitch joint movement of the second preparatory side leg group is controlled, causing the thigh 8 of the second preparatory side leg group to move to a position approximately flush with the lower surface of the fuselage, and the knee joint flexion of the second preparatory side leg group is controlled. The hip pitch joint movement of the second active side leg group is also controlled. The knee joint movement causes the leg or foot of the second active side leg group to contact the ground and apply force to the ground. The reaction force of the ground on the target robot generates a rolling torque around the horizontal axis of the body, driving the body to roll in the direction of the second reserve side leg group. After the roll is completed, the posture perception data and joint motion data of the target robot are reacquired, and the fall state classification result of the target robot is re-determined based on the re-acquired data. If the re-determined fall state classification result is a flat fall without pressure, the direct standing recovery sub-strategy is invoked. If the re-determined fall state classification result is a fall with limb pressure, the escape standing recovery sub-strategy is invoked.

[0032] In another exemplary embodiment of this application, performing the standing action specifically includes: Obtain the current body pose of the target robot and the current position of each foot in the body coordinate system; the current body pose includes the current body position and the current body posture; Based on the current fuselage pose, the current foot position in the fuselage coordinate system is transformed to the world coordinate system to obtain the current foot position in the world coordinate system. The target fuselage attitude is determined based on the current fuselage attitude and preset stable standing parameters; the target fuselage attitude includes the target fuselage position and the target fuselage attitude; the vertical coordinate of the target fuselage position is the preset standing height, the roll angle and pitch angle of the target fuselage attitude are both zero, and the yaw angle of the target fuselage attitude is maintained as the yaw angle of the current fuselage attitude or is set to the preset yaw angle; The nominal standing position of each foot in the fuselage coordinate system is transformed according to the target fuselage pose to obtain the target standing position of each foot in the world coordinate system. Each foot is used as the starting control point in the world coordinate system and the target standing position of the corresponding foot in the world coordinate system as the ending control point. Based on the preset foot lifting height, starting velocity and acceleration boundary conditions and ending velocity and acceleration boundary conditions, four intermediate control points are determined, thereby generating a fifth-order Bézier curve determined by six control points. By traversing all feet, the fifth-order Bézier curve corresponding to each foot is obtained. Using model predictive control, the trajectory progress and trajectory progress change rate of each foot are optimized variables in the prediction time domain. The optimization objectives are to reduce the deviation between the current foot position and the target standing position, reduce the change of foot velocity and acceleration, and improve the stability of the fuselage attitude. The constraints are set as follows: trajectory progress is greater than or equal to 0 and less than or equal to 1, trajectory progress in adjacent control cycles is monotonically non-decreasing, foot velocity and acceleration do not exceed preset limits, and each joint does not exceed the joint range of motion. The optimal trajectory progress of each foot along the corresponding fifth-order Bézier curve is solved. Substitute the optimal trajectory progress of each foot and the first and second derivatives of the optimal trajectory progress with respect to time into the corresponding fifth-order Bézier curve, and parameterize the fifth-order Bézier curve with time to obtain the expected position, expected velocity and expected acceleration of each foot in the world coordinate system. Based on the current body pose of the target robot and the position of each hip joint relative to the center of the body, the desired position of each foot is transformed from the world coordinate system to the hip joint coordinate system of the corresponding leg, so as to obtain the desired coordinates of each foot relative to the corresponding hip joint. Based on the desired coordinates of each foot relative to the corresponding hip joint and the leg geometric parameters, the desired joint angles of each leg are calculated through inverse kinematics; the leg geometric parameters include hip abduction offset length, thigh length 8 and calf length 7; the desired joint angles include hip abduction joint angle, hip flexion joint angle and knee joint angle; Based on the kinematic Jacobian matrix of each leg and the expected velocity and expected acceleration of each foot, calculate the expected joint angular velocity and expected joint angular acceleration of each leg. The desired joint angle, desired joint angular velocity, and desired joint angular acceleration of each leg are input into the joint trajectory controller 9, driving each leg to perform a standing motion according to the corresponding fifth-order Bezier curve until the current body pose of the target robot reaches the target body pose and remains stable.

[0033] In another exemplary embodiment of this application, performing an escape action specifically includes: Based on the encoder angle data of the hip pitch joint motor, hip abduction joint motor and knee joint motor of each leg, the current angular position of each hip pitch joint, hip abduction joint and knee joint is determined. Lock the hip flexion and extension joints of each leg to the corresponding current angle position, control the knee joints of each leg to move to the preset maximum flexion angle position, and control the hip abduction joints of each leg to move to the preset maximum abduction angle position. Select two diagonally opposite legs from the four legs of the target robot as the first diagonal leg pair, release the position lock of the hip pitch joint of the first diagonal leg pair, control the knee joint of the first diagonal leg pair to move from the preset maximum flexion angle position to the preset maximum extension angle position, and control the hip pitch joint of the first diagonal leg pair to move to the preset hip joint neutral angle position. Based on the encoder angle data of each joint motor in the first diagonal leg pair, determine the knee joint angle and hip pitch joint angle of the first diagonal leg pair. When both knee joints of the first diagonal leg pair reach the preset maximum extension angle position and both hip flexion joints reach the preset hip joint neutral angle position, control the knee joints of the first diagonal leg pair to move back to the preset maximum flexion angle position, and lock the hip flexion joints of the first diagonal leg pair at the preset hip joint neutral angle position to complete the escape action of the first diagonal leg pair. The other two legs that are diagonally related are identified as the second diagonal leg pair. The position lock of the hip flexion-extension joint of the second diagonal leg pair is released. The knee joint of the second diagonal leg pair is controlled to move from the preset maximum flexion angle position to the preset maximum extension angle position. The hip flexion-extension joint of the second diagonal leg pair is controlled to move to the preset hip joint neutral angle position. Based on the encoder angle data of each joint motor in the second diagonal leg pair, determine the knee joint angle and hip pitch joint angle of the second diagonal leg pair; When both knee joints of the second diagonal leg pair reach the preset maximum extension angle position and both hip flexion joints reach the preset hip joint neutral angle position, control the knee joints of the second diagonal leg pair to move back to the preset maximum flexion angle position, and lock the hip flexion joints of the second diagonal leg pair at the preset hip joint neutral angle position to complete the escape action of the second diagonal leg pair. After the first and second diagonal leg pairs have completed their escape actions, the joint torque data and encoder angle data of each joint are reacquired. Based on the reacquired data, it is determined whether each joint still meets the pressure judgment condition. When all joints do not meet the pressure judgment condition, it is determined that the escape actions of all legs have been completed, and the target robot is controlled to enter the preset squatting mode.

[0034] In another exemplary embodiment of this application, the robot fall detection and stand-up control method further includes: performing anomaly protection measures during the invocation of the first roll correction recovery sub-strategy or the second roll correction recovery sub-strategy; the anomaly protection measures include joint angle limit protection, joint torque limit protection, joint speed smoothing protection, foot contact force buffering protection, and body posture and joint drive anomaly monitoring protection.

[0035] In one exemplary embodiment of this application, a world coordinate system W, a fuselage coordinate system B, and a hip joint coordinate system H corresponding to each leg are defined. i Where i represents the leg number. The origin of the world coordinate system W is located at the projection point of the robot's center of mass onto the horizontal plane when the target robot is in its initial stable standing state. The Z-axis is perpendicular to the horizontal plane and points upwards, the X-axis lies in the horizontal plane and points towards the robot's initial forward direction, and the Y-axis is determined according to the right-hand rule. The body coordinate system B is fixed to the body, with its origin located at the body's center of mass. The X-axis points along the body's longitudinal axis towards the robot's forward direction, the Z-axis is perpendicular to the body's reference plane and points upwards, and the Y-axis is determined according to the right-hand rule. The hip joint coordinate system H... i Fixed to the fuselage, its origin is located at the center of rotation of the hip abduction joint of the i-th leg. The Z-axis coincides with the axis of rotation of the hip abduction joint, the X-axis is along the nominal extension direction of the 8-link of the thigh when the i-th leg is in the preset zero position, and the Y-axis is determined according to the right-hand rule. Hip joint coordinate system H i The position and orientation relative to the body coordinate system B are predetermined based on the installation position and direction of the corresponding leg, and remain unchanged during the robot's movement, so as to be used for coordinate transformation, forward kinematics calculation and inverse kinematics calculation of each foot position.

[0036] The following example illustrates this application using a specific robot fall detection and recovery control process.

[0037] In one exemplary embodiment, such as Figures 2-10 The diagram shows various robot states. Figure 2 This diagram illustrates the crouching or lying-down mode, which is the mode when the robot is powered off. The legs can be in any position, but cannot be under the body. Therefore, this mode can also be understood as the "lying down" mode, which is a mode in which no limbs are compressed, the body is horizontal, and all joints meet the kinematic limit positions. Figure 3 This diagram illustrates the robot's standing mode, which is the robot's "preparatory mode" before movement, serving as an intermediate transition mode. The "falling and getting up" action described in this application's embodiments refers to this mode. Figure 4 This is a schematic diagram of the robot's movement mode. This mode is the basic mode for the robot to perform various tasks. In this mode, the robot can perform actions such as stepping, jumping, and climbing. It is the active movement mode of each joint. Figures 5-8 The diagrams show the robot in various states of falling, with one leg, two legs, three legs, and four legs trapped under the robot body. Figure 9 This is a diagram of the robot in a tilted position. In this mode, due to the kinematic constraints of the joints, the legs cannot be pressed under the robot body. (The kinematic constraints here can be understood as the reverse joints in a human). Figure 10 This is a diagram illustrating the robot's sideways fall.

[0038] In one exemplary embodiment, this application proposes a robot fall detection and stand-up control method, which specifically includes the following steps.

[0039] Step 1: Data Acquisition. Real-time acquisition of robot attitude angles, angular velocities, and acceleration data from the robot's inertial measurement unit (IMU), as well as encoder angle and torque data for each joint motor.

[0040] Step 2: Fall detection. Monitor the rate of change of the above data. When a sudden change occurs in the joint angle or torque, accompanied by an abnormal IMU attitude angle, start timing monitoring. If the duration of the abnormal state reaches the preset time threshold T, and the robot's acceleration approaches the gravitational acceleration constant, then the robot is determined to have entered fall mode.

[0041] As an optional implementation, the specific logic for determining whether the robot has entered the fall mode is as follows: set the fall determination time threshold T to 1 second; when the joint angular velocity is detected to be greater than the set threshold, or the tilt angle detected by the IMU exceeds the safety threshold, start timing; if the robot fails to recover to the safe posture angle range within the next 1 second, and the difference between the resultant force modulus of the three-axis acceleration and the gravitational acceleration g at the end of 1 second is within the static tolerance range, then it is confirmed that the robot has completely fallen and is in a static state.

[0042] Step 3: System self-check. Before executing the recovery action, enter the system self-check module to perform a self-check on the robot's communication link and hardware status; if the self-check passes, proceed to Step 4; if the self-check fails, the recovery action is prohibited and the abnormal alarm module is triggered.

[0043] As an optional implementation method, the system self-test and anomaly alarm specifically include: ① If the heartbeat packets of bus communication between the detection controller 9 and each joint driver are lost, it is determined to be a communication link failure if the loss rate exceeds the threshold. ② Check if the encoder value of the joint motor is non-digit (NaN, Not a Number) or infinite, and check if the motor current feedback is open-circuited. If so, it is determined to be a hardware failure. ③ When a communication link failure or hardware damage is detected, the robot enters the passive protection module, sets all joints to a high-damping state or mechanical brake locks, then enters the abnormal alarm module, sends a fault alarm signal with location information through the audible and visual alarm and the remote wireless communication module, and finally performs manual maintenance.

[0044] Step 4: Attitude calculation and classification. Based on forward kinematics and IMU data, the relative attitude of the fuselage to the ground and the position of the feet are calculated, and the fall state is classified into: flat fall without pressure, fall with limb pressure, side fall, and rollover.

[0045] As an optional implementation method, the method for determining limb compression and fall is as follows: using forward kinematics to calculate the coordinate position of each foot of the robot relative to the center of the body, constructing the projection contour area of ​​the body on the horizontal plane; if the horizontal coordinate of any foot falls inside the projection contour area, it is determined that the limb is compressed; or, if the output torque of a certain joint is detected to reach the saturation threshold, but the change in the angle of the joint within a preset time is lower than the micro-motion threshold, it is determined that the limb corresponding to the joint is in a compressed or stuck state.

[0046] The specific process of implementing this method, which uses forward kinematics to calculate the coordinate positions of each foot of the robot relative to the center of the body, and to construct the projected contour region of the body on the horizontal plane, is as follows: First, a kinematic model based on the DH parameter method is established for each mechanical leg of the robot. Coordinate systems are fixed at the center of the body, each joint, and the foot. Homogeneous coordinate transformation is used to describe the pose relationship between adjacent links. Then, based on the real-time collected angle values ​​of each joint, the corresponding homogeneous transformation matrix is ​​multiplied sequentially along the kinematic chain to calculate the three-dimensional spatial coordinates of the foot of each leg in the body coordinate system, obtaining the position vectors of the four foot ends: left front, right front, left rear, and right rear. After obtaining these three-dimensional coordinates, the height component (Z-axis) is ignored, and only the two-dimensional projection points on the horizontal plane (XY plane) are extracted. A minimum convex polygon that can contain all points is constructed using these four projection points as vertices. This convex polygon is defined as the projection contour region of the body on the horizontal plane, also known as the support polygon. This contour region is used for subsequent stability judgment—by calculating the projection point of the robot's overall center of gravity on the horizontal plane in real time, if the projection point falls outside the support polygon, it is determined that the robot has fallen due to limb compression, thus triggering the corresponding standing control process. This process transforms the abstract state of falling into explicit geometric judgment conditions, providing a quantifiable implementation basis for fall detection.

[0047] Step 5: Based on the classification results from Step 4, invoke the corresponding control strategy: ① If the fall is a flat, unpressed fall, execute the direct stand-up recovery sub-strategy, directly enter the squat mode, and then enter the exercise mode to perform the standing action.

[0048] ② If the fall is due to limb compression, first execute the escape and recovery sub-strategy. After the limb is released, enter the squatting mode and then enter the movement mode to perform the standing action. ③ If it is a side fall, execute the first roll correction recovery sub-strategy to turn into a flat fall without pressure or a fall with limb pressure; ④ If the fall is a supine fall, execute the second roll correction recovery sub-strategy to switch to a flat-lying fall without pressure or a fall with limb pressure.

[0049] As an optional implementation, the escape and recovery sub-strategy is as follows: First, read the current hip joint angle information and keep the hip joint position fixed. Then, contract all lower leg joints 7 to ensure that the legs do not interfere with space under the fuselage. Next, move all thigh joints to their extreme positions away from the fuselage interior, thereby removing the thighs 8 from under the fuselage. Then, select one pair of legs, extend their lower leg joints to move the lower leg 7 as far away from the fuselage as possible; then, move the hip joint of the opposite leg to the zero-degree position, i.e., the hip joint angle when standing normally, thus completing the escape of the opposite leg. Immediately afterwards, contract the lower leg joint of the opposite leg to make room for the escape of the other pair of legs. Then, perform the corresponding operation on the other pair of legs: first, extend its lower leg joint to move the lower leg 7 as far away from the fuselage as possible; then, move its hip joint to the zero-degree position as well; finally, contract the lower leg joint, thus completing the entire leg escape process. This strategy is applicable to falls where any leg is trapped, including falls with one leg trapped under the robot body, two legs trapped under the robot body, three legs trapped under the robot body, and four legs trapped under the robot body. The escape and recovery sub-strategy refers to a set of programmed joint movements executed when a quadruped or quadwheeled robot falls and experiences limb compression (i.e., one or more legs are trapped under the robot body, preventing the robot from standing up through normal movements). Its purpose is to gradually remove the trapped limb from under the robot body, restoring all legs to a freely movable state, creating the geometric space conditions for subsequent standing actions.

[0050] The core design principles of this strategy include: (1) Priority planning - Determine the order of release for each leg based on whether the leg is currently under pressure and the availability of movement space; (2) Interference avoidance - Ensure that there is no spatial collision or interference between joints and limbs when performing each step of the movement; (3) Step-by-step execution: Decompose the complex escape task into several independently verifiable sub-steps, and proceed to the next step only after each step is completed.

[0051] Specifically, the execution flow of the escape, stand-up, and recovery sub-strategy is as follows: Step S1: The controller 9 reads the current angle information of each hip joint and sends a position holding command to all hip joint motors 5 to fix the position of each hip joint, so as to avoid further deterioration of the body posture due to unexpected hip joint movement in subsequent actions.

[0052] Step S2: The controller 9 sends a retraction command (i.e., a rotation command to retract towards the body) to all the lower leg joint motors 4, so that all the lower legs 7 retract to the minimum length or minimum extension angle position to ensure that the legs do not interfere with the space under the body.

[0053] Step S3: The controller 9 sends an outward swing command to all thigh joint motors 3, causing all thigh joints to move to the extreme position away from the inside of the machine body (i.e., the thigh 8 is spread outward to the maximum angle), and the thigh 8 is pulled away from under the machine body by the outward swing of the thigh joints.

[0054] Step S4: The controller 9 selects one pair of legs (e.g., the left front leg and the right back leg) and sends an extension command to the lower leg joint motor 4 of the opposite leg, so that the lower leg joint of the opposite leg extends and the lower leg 7 is as far away from the machine body as possible; then, the controller 9 sends a position command to the hip joint motor 5 of the opposite leg, so that the hip joint of the opposite leg is executed to the zero-degree position (i.e., the hip joint angle when standing normally), thereby enabling the opposite leg to complete the escape and reach the ready standing posture.

[0055] Step S5: The controller 9 sends a retraction command to the lower leg joint motor 4 of the diagonal leg that has completed the escape, retracting the lower leg joint of the diagonal leg to make room for the other diagonal leg to escape.

[0056] Step S6: The controller 9 performs the same operation as steps four and five on the other diagonal leg—first, it sends an extension command to the lower leg joint motor 4 of the diagonal leg, so that the lower leg 7 is as far away from the machine body as possible; then it sends a position command to the hip joint motor 5 of the diagonal leg, so that the hip joint is executed to the zero-degree position; finally, it sends a contraction command to the lower leg joint motor 4 of the diagonal leg, so that the lower leg joint is contracted.

[0057] At this point, the entire leg freeing process is complete. This strategy is applicable to any number of limbs trapped in a fall, meaning that one, two, three, or even four legs trapped under the fuselage can all be freed using this strategy.

[0058] As an optional implementation, the first and second roll correction and recovery sub-strategies are as follows: The leg located on one side of the robot is designated as the preparatory side, with its thigh joint moved to a position roughly flush with the lower surface of the robot body, while the lower leg joint contracts to prepare for post-correction stability. Then, the hip and thigh joints on the other side are driven to quickly apply a thrust to the ground. This action generates a sufficiently large rollover torque under the robot body, supporting and propelling it to roll towards the preparatory side. After the robot body rolls back to its upright position through the above actions, the robot's state is determined based on the final sensor data. If the robot body has recovered to a flat, unpressurized fall posture, it enters the corresponding squatting recovery process. If the robot body still has limb compression, it switches to the corresponding limb compression fall recovery module and executes the corresponding escape and standing recovery sub-strategy. Each mechanical leg of the quadrupedal and four-wheeled robot is equipped with three independent joint motors, corresponding to the hip joint, thigh joint, and lower leg joint, respectively. The robot features a hip joint motor 5 that drives the robot's legs to swing back and forth and laterally around the hip axis; a thigh joint motor 3 that drives the thigh 8 to swing in a pitching motion around the knee axis; and a lower leg joint motor 4 that drives the lower leg to extend, contract, or pitch around the ankle axis. Each leg's three motors are independently controlled, with the controller 9 sending position or torque commands to each motor to achieve precise movement of the leg joints. The entire robot is equipped with twelve joint motors (four legs × three joints), each independently driving the degrees of freedom of its joints, providing ample driving power for obstacle avoidance and roll correction. Each leg of the four-wheeled robot corresponds to a wheel 1 and a wheel joint.

[0059] The first roll correction and recovery sub-strategy and the second roll correction and recovery sub-strategy refer to a set of control methods that are executed when a quadruped or quadwheeled robot falls and its body is in a side-flipped or overturned state (i.e. the body is in an abnormal posture and cannot stand up directly through simple leg movements). These methods utilize the interaction between the legs and the ground to generate a rolling torque, driving the body to roll back to its correct position around its longitudinal axis.

[0060] The tumbling torque is generated based on Newton's third law (the principle of action and reaction). Specifically, the controller 9 sends a rapid thrust command to the hip joint motor 5 and thigh joint motor 3 on one side (the active side) of the robot, causing the leg on that side to swing towards the ground with a certain speed and acceleration and apply thrust. At the same time, the ground applies an equal and opposite reaction force to the leg on that side. This reaction force does not act on the leg itself, but is transmitted to the body through the leg structure, generating an eccentric torque about the longitudinal axis of the body at the body's center of mass, i.e., the tumbling torque. When this tumbling torque is greater than the resisting torque generated by the weight of the body, the body gains sufficient angular momentum to tumble towards the intended side about its longitudinal axis.

[0061] The magnitude of the tumbling torque is determined by the following factors: (1) the magnitude of the thrust exerted by the active leg on the ground - the greater the thrust, the greater the reaction force of the ground on the fuselage, and the greater the torque generated; (2) the length of the lever arm from the point of application of the thrust (i.e. the foot contact point) to the center of mass of the fuselage in the horizontal direction - the longer the lever arm, the greater the torque generated by the same thrust; (3) the angle between the thrust direction and the longitudinal axis of the fuselage - the more perpendicular the thrust direction is to the lever arm direction, the higher the torque efficiency generated. The controller 9 calculates the minimum tumbling torque threshold (i.e. the minimum torque value required to overcome the gravitational torque) required in real time based on the current side roll angle of the fuselage, the fuselage mass and the position of the center of mass. Then, it converts the torque requirement into the torque command required to be output by each relevant joint motor through inverse dynamics calculation, and sends the corresponding torque command to the hip joint motor 5 and thigh joint motor 3 on the active side, so that the leg exerts a force on the ground with the calculated thrust, thereby generating a tumbling torque sufficient to push the fuselage to tumble.

[0062] In implementing this method, the specific execution flow of the first roll correction recovery sub-strategy and the second roll correction recovery sub-strategy is as follows: Step 1 (Preparatory Side Setting): Based on the current roll direction of the fuselage (determined by the fuselage roll angle obtained by the inertial measurement unit), the controller 9 sets the leg located on the opposite side of the roll direction (i.e., the side the fuselage will roll over) as the preparatory side. The controller 9 sends a position command to the thigh joint motor 3 on the preparatory side, causing the thigh joint to move to a position basically flush with the lower surface of the fuselage. At the same time, it sends a retraction command to the lower leg joint motor 4 on the preparatory side, causing the lower leg 7 to retract, preparing for post-correction posture stability.

[0063] Step Two (Active Side Force): Controller 9 sends a rapid thrust command to the hip joint motor 5 and thigh joint motor 3 on the other side (active side), causing the active side leg to swing towards the ground with a certain speed and acceleration, applying thrust. This action causes the active side foot to exert a downward force on the ground, and the ground subsequently exerts an upward reaction force on the robot. This reaction force generates a rollover torque around the longitudinal axis of the robot at its center of mass. When this rollover torque is greater than the resisting torque generated by the robot's gravity, the robot gains sufficient rollover angular momentum, thereby supporting and propelling the robot to roll towards the intended side. After the active side leg completes the thrust action, controller 9 sends position commands to each joint motor on the active side, causing the active side leg to retract to a position where it does not interfere with the ground.

[0064] Step 3 (Posture Determination and Subsequent Processing): After the robot body rolls back to its upright position through the above actions, the controller 9 determines the robot body status based on the data fed back by sensors such as the inertial measurement unit: If the robot body recovers to a flat, unpressurized fall posture (i.e., the robot body is in a horizontal or nearly horizontal flat position, and no limbs are pressed under the robot body), the controller 9 switches to the corresponding squatting mode recovery process, that is, by sending position commands to each joint motor to make the robot transition from a flat posture to a squatting posture, and then further to a normal standing posture; If there are still limbs pressed under the robot body (i.e., one or more legs are still pressed under the robot body), the controller 9 switches to the corresponding limb pressure fall recovery module and executes the above-mentioned escape and standing recovery sub-strategy.

[0065] The first and second roll correction recovery sub-strategies require determining the preparatory and active sides based on the current roll direction of the fuselage; therefore, the roll direction needs to be determined. The specific determination method is as follows: The controller 9 reads data in real time from the inertial measurement unit (IMU) installed at the center of the fuselage. This IMU includes a three-axis accelerometer and a three-axis gyroscope. The controller 9 calculates the static attitude angles of the fuselage based on the accelerometer data and the dynamic angular velocity of the fuselage based on the gyroscope data. It then obtains the real-time roll angle and pitch angle of the fuselage through a data fusion extended Kalman filter algorithm.

[0066] The logic for determining the roll direction is as follows: Determining the left and right roll direction: When the controller 9 detects that the absolute value of the roll angle of the fuselage is greater than the preset angle threshold (e.g., 30°), it determines that the fuselage is in a rollover state. If the roll angle is positive (according to the definition of the fuselage coordinate system, for example, a positive value indicates that the fuselage is tilted to the right), it determines that the fuselage is rolling to the right. At this time, the preparatory side is the left side (i.e., the fuselage needs to roll to the left to straighten), and the active side is the right side. If the roll angle is negative (indicating that the fuselage is tilted to the left), it determines that the fuselage is rolling to the left. At this time, the preparatory side is the right side, and the active side is the left side.

[0067] Determining the forward or backward roll direction: When the controller 9 detects that the absolute value of the fuselage's pitch angle is greater than a preset angle threshold (e.g., 30°), it determines that the fuselage is in a forward or backward roll state. If the pitch angle is positive (indicating that the fuselage is tilted forward or nose-down), it determines that the fuselage is rolling forward. In this case, the preparatory side is the rear (i.e., the fuselage needs to roll backward to right itself), and the active side is the front. If the pitch angle is negative (indicating that the fuselage is tilted backward or tail-down), it determines that the fuselage is rolling backward. In this case, the preparatory side is the front, and the active side is the rear.

[0068] Determining the Combined Roll Direction: When the fuselage simultaneously exhibits large roll and pitch angles, controller 9 determines the primary roll direction based on the relative magnitudes of the roll and pitch angles—the direction corresponding to the attitude angle with the larger absolute value is taken as the primary roll direction, and the preparatory side and active side are determined according to the above logic; simultaneously, when calculating the roll moment, controller 9 incorporates the attitude angles of both directions into the dynamic model, calculating the resultant moment requirement in the combined direction to ensure that the roll operation can be effectively executed under combined overturning conditions. In implementing this method, to avoid secondary damage to the robot during the roll correction process, the controller 9 simultaneously implements the following protective measures when executing the first roll correction recovery sub-strategy and the second roll correction recovery sub-strategy: (1) Joint angle limit protection: When the controller 9 generates position commands for each joint motor, it monitors the current angle value of each joint in real time to ensure that all command target angles are within the mechanical limit range of each joint (i.e., do not exceed the maximum extension angle and minimum retraction angle of each joint). If the command target angle of a certain joint exceeds the limit range, the controller 9 automatically clamps the command value at the limit boundary value and sends a limit warning signal to the system.

[0069] (2) Joint torque limiting protection: When generating torque commands for each joint motor, the controller 9 monitors the output torque of each joint motor in real time to ensure that all torque commands do not exceed the rated torque and peak torque limit of each joint motor. If the torque command of a certain joint exceeds the safety threshold, the controller 9 automatically reduces the command value to the safe range to avoid motor burnout or mechanical structure damage due to overload.

[0070] (3) Joint speed smoothing protection: When the controller 9 sends position commands or torque commands to each joint motor, it adopts trapezoidal speed planning or S-type speed planning algorithm to smoothly control the movement speed of the joint, so as to avoid damage to the fuselage structure and joint transmission mechanism caused by the impact load generated by the sudden speed change.

[0071] (4) Foot contact force buffer protection: The controller 9 monitors the contact force between each foot and the ground in real time through force sensors installed on the feet. During the roll, if the contact force of a certain foot exceeds the preset safety threshold (indicating that the foot may have an abnormal collision or get stuck with the ground), the controller 9 immediately pauses the current action and sends a reverse buffer command to the joint motor corresponding to that foot to alleviate the impact and readjust the posture before continuing to perform the roll.

[0072] (5) Monitoring and protection against abnormal fuselage attitude and joint drive: Throughout the execution of the first roll correction recovery sub-strategy and the second roll correction recovery sub-strategy, the controller 9 continuously monitors the rate of change of fuselage attitude angles fed back by the inertial measurement unit and the current feedback values ​​of each joint motor. If an abnormal rate of change of fuselage attitude angles is detected (indicating that the fuselage may roll or fall uncontrollably) or an abnormal increase in the current value of a certain joint motor (indicating that the joint may jam or overload), the controller 9 immediately interrupts the current roll correction process, switches all joint motors to the safety lock state, and issues a fault alarm signal.

[0073] The first and second roll correction and recovery sub-strategies refer to a set of control methods implemented when a quadruped or quadwheeled robot falls and its body is in a sideways or overturned state (i.e., the body is in an abnormal posture and cannot stand up directly through simple leg movements). These methods utilize the interaction between the legs and the ground to generate a rolling torque, driving the robot to roll back to its correct position around its longitudinal axis. The core principle of this strategy is: by generating an eccentric torque between the ground and the robot body through asymmetrical leg movements, the robot body gains sufficient angular momentum to overcome gravity and complete posture correction. Specifically, the execution flow of the first and second roll correction and recovery sub-strategies is as follows: First, the leg located on one side of the robot is designated as the preparatory side, with its thigh joint moved to a position roughly flush with the lower surface of the robot body. Simultaneously, the lower leg joint on that side is contracted to prepare support for post-correction posture stability. Then, the hip and thigh joints on the other side are driven to rapidly apply a thrust to the ground—this asymmetrical thrust generates a sufficiently large tumbling torque under the robot body, supporting and propelling it to roll towards the preparatory side. After the robot body rolls back to its upright position through the above actions, the system determines the robot's state based on the final sensor data: if the robot body recovers to a flat, unpressurized fall posture, it transitions to the corresponding squatting recovery mode; if the robot body still has limbs under pressure, it transitions to the corresponding limb pressure fall recovery module, executing the aforementioned escape and standing recovery sub-strategy. The core advantage of this strategy is its ability to adapt to different fall postures, transforming the robot from an unstable side-rolling state to a relatively stable flat state through a single roll, thus laying the foundation for subsequent standing recovery.

[0074] As an optional implementation, the standing action for the four-wheeled robot also includes a hub locking step: during the entire process of performing the standing action, a position locking command or a zero-speed control command is sent to the hub motor of the four-wheeled robot's foot to prevent the pivot point from slipping due to tire rolling during the standing process.

[0075] As an optional implementation, a method based on model predictive control (MPC) and Bézier curve trajectory planning is adopted to calculate the path of the foot moving from the current position to the target standing position. This path is used to generate the foot motion path of the robot during the process of recovering from a fall posture to a stable standing posture, and the inverse kinematics control of each joint angle is calculated in real time.

[0076] I. Coordinate system, leg numbering, and definition of state variables.

[0077] In one exemplary embodiment of this application, in order to uniformly describe the body pose, foot positions, leg joint states, and fall recovery process of the target robot, a world coordinate system W, a body coordinate system B, and a hip joint coordinate system corresponding to each leg are established. .

[0078] Define the set of leg numbers for: ; Among them, FL, FR, RL and RR represent the left foreleg, right foreleg, left hind leg and right hind leg respectively; This represents the i-th leg.

[0079] World coordinate system The reference coordinate system is fixed relative to the horizontal plane, with its origin located at the vertical projection point of the target robot's center of mass onto the horizontal plane when the robot is in its initial stable standing state; the world coordinate system... of The axis lies in the horizontal plane and points in the initial direction of movement of the target robot. The axis is located in the horizontal plane and points to the left of the target robot. The axis is perpendicular to the horizontal plane and points vertically upward.

[0080] The body coordinate system B is fixed to the target robot's body and moves synchronously with the robot, with its origin located at the body's center of mass; the body coordinate system B's... The axis is along the longitudinal axis of the robot body and points in the direction of the robot's movement. The axis points to the left side of the robot. The axis is perpendicular to the reference plane of the aircraft and points upwards from the aircraft.

[0081] Define the hip joint coordinate system for the i-th leg. Hip coordinate system Fixed to the body, its origin is located at the first The center of rotation of the hip abduction / adduction joint of the leg; The axis rotates along the hip abduction / adduction joint axis and points in front of the robot. The axis points to the right side of the robot. The axis points downwards.

[0082] Based on the above coordinate axis definition, the hip joint coordinate system The fixed rotation matrix relative to the body coordinate system B is: ; in, This indicates the hip joint coordinate system. The rotation matrix transforms the vector in the lower coordinate system B to the body coordinate system B; the superscript B indicates that the result is expressed in the body coordinate system B.

[0083] The physical state of the target robot Defined as: ; in, , Indicates the body's center of mass in the world coordinate system The lower position; , and These represent the longitudinal, lateral, and vertical positions of the body's center of mass, respectively. , Indicates the aircraft's attitude angle; Indicates the roll angle. Indicates pitch angle, Indicates the yaw angle; Indicates the body's center of mass in the world coordinate system The linear velocity of the line; This represents the angular velocity of the aircraft in the aircraft coordinate system B; the superscript T indicates the transpose of a vector or matrix.

[0084] Using the ZYX Euler angle order, the rotation matrix from the body coordinate system B to the world coordinate system W is: ; in, , and These represent the basic rotation matrices about the X-axis, Y-axis, and Z-axis, respectively; Used to transform vectors in the body coordinate system B to the world coordinate system W.

[0085] The position of the foot of the i-th leg at the start of the recovery trajectory in the world coordinate system W is denoted as: ; The target position of the foot of the i-th leg after the target robot regains its standing posture is denoted as: ; Wherein, the subscript f represents the foot end, the subscript i represents the leg number; the left superscript W represents the corresponding position expressed in the world coordinate system W; the right superscript 0 represents the starting state of the recovered trajectory; and the right superscript d represents the target state.

[0086] II. Target standing posture and target foot position.

[0087] When determining the target standing posture, the horizontal position of the machine at the start of control recovery is taken as the horizontal position of the target machine, and the height of the target machine is set to the preset normal standing height. ,Right now: ; in, Indicates the location of the target body's center of mass; and These represent the horizontal coordinates of the body's center of mass at the start of the recovery control; This indicates the preset height of the robot when it is standing normally.

[0088] Set the target roll and pitch angles to zero, and set the target yaw angle to the yaw angle at the start of recovery control: , , Therefore, the target aircraft's attitude angle is: ; in, This indicates the yaw angle of the robot at the start of control recovery. Maintaining a constant yaw angle prevents the robot from undergoing additional horizontal turning during the standing process.

[0089] With both the linear velocity and angular velocity of the target unit set to zero, the target unit's state is represented as follows: ; in, Indicates the target body's state; This represents the three-dimensional zero vector, corresponding to the target body's linear velocity and angular velocity, respectively.

[0090] Let the nominal position of the i-th leg relative to the body coordinate system B in a normal standing position be: ; in, This represents the nominal standing position of the i-th foot in the body coordinate system B; Indicates the position before and after the nominal value; Indicates the nominal lateral position; This indicates that when standing normally, the feet are located below the body's center of mass.

[0091] Since both the target roll angle and the target pitch angle are zero, the target aircraft rotation matrix is: ; No. The target's standing position in the world coordinate system W is: ; in, They respectively represent the first The nominal standing position of the foot is transformed from the body coordinate system B to the world coordinate system W; then added to the target body position to obtain the target world coordinates of the foot.

[0092] Third and fifth order Bessel foot path.

[0093] Regarding the first The foot of each leg is used as the starting point of the Bézier curve, with its current starting position as the ending point, and six control points are set: ; in, Indicates the first The j-th Bezier control point at the foot of one leg. ; Indicates the preset foot elevation height; Represents the vertically upward unit vector in the world coordinate system W. .

[0094] No. The fifth-order Bézier curve at the foot of the leg is: ; in, Indicates the first The trajectory progress variable of the foot tip of a leg; This indicates that the foot is located at the starting point of the trajectory; This indicates that the foot has reached the end of the trajectory; Represents the number of combinations: ,! represents factorial.

[0095] The fifth-order Bézier curve is expanded as follows: ; because ,and The first derivative of the Bézier curve with respect to the trajectory progress is zero at both the start and end points: ; The first derivative of the Bézier curve with respect to the trajectory progress is: ; The second derivative is: ; According to the chain rule, the velocity at the foot is: ; The foot acceleration is: ; in, This represents the first derivative of the trajectory progress with respect to time, i.e., the trajectory advancement speed; This represents the second derivative of the trajectory progress with respect to time, i.e., the trajectory acceleration. and These represent the velocity and acceleration of the foot in the world coordinate system W, respectively.

[0096] IV. Model Predictive Control Model.

[0097] Let the prediction step size be N, and the control period be... The predicted time is k=0,1,…,N.

[0098] Define the trajectory progress vectors of the four feet as follows: ; The trajectory progress velocity vector is: ; The augmented state variables for model predictive control are defined as follows: ; The control variables for model predictive control are defined as follows: ; in, This represents the augmented state at the k-th prediction time. This represents the control input at the k-th prediction time. This represents the acceleration along the reference line of the body's center of mass at the k-th prediction time. This represents the reference angular acceleration of the body at the k-th prediction time. This represents the trajectory progress acceleration vector of the four feet at the k-th prediction time.

[0099] The discrete update relationship between the body position and linear velocity is as follows: ; ; in, This indicates the time interval between two adjacent control cycles.

[0100] The relationship between the rate of change of Euler angles of the aircraft and the angular velocity of the aircraft is as follows: ; in: ; This represents the transformation matrix from the angular velocity to the Euler angle rate of change at the k-th prediction time.

[0101] The discrete update relationship between the body attitude angle and angular velocity is as follows: ; ; No. The trajectory progress of each foot end adopts a second-order discrete model: ; ; The above update relationship can be uniformly represented as: ; in, This represents a nonlinear discrete state update function that includes the body's position, attitude, velocity, and foot trajectory progress.

[0102] Performing first-order linearization near the current prediction working point yields: ; in, and These represent the state deviation and control deviation at the k-th prediction time, respectively. This represents the state Jacobian matrix at the k-th prediction time. Let Jacobian matrix represent the control input at the k-th prediction time.

[0103] V. Model Predictive Control Objective Function and Constraints.

[0104] The model predictive control objective function is defined as follows: ; The weighted quadratic norm is defined as: ; in, This represents the model's predictive control objective function; This represents the weighting matrix of the organism's state error; This represents the control input weight matrix; This represents the weighting matrix for the target position error at the foot. This represents the error weight matrix of the machine's terminal state; Indicates the weight of trajectory progress error; Indicates the trajectory progress speed weight; Indicates the acceleration weights of the trajectory progress; Indicates the weight of the terminal trajectory progress error; Indicates the terminal trajectory progress speed weight; This indicates the number of steps in the prediction time domain.

[0105] The model predictive control optimization problem is: .

[0106] The optimization process satisfies the following constraints: ; in, Indicates the maximum trajectory propulsion speed; This represents the maximum trajectory acceleration. and Indicates the allowable roll angle range; and Indicates the permissible range of pitch angles; and Indicates the range of aircraft height; Indicates the first A leg in the hip joint coordinate system The kinematics of the workspace are accessible. Indicates the maximum permissible speed at the foot; This indicates the maximum permissible acceleration at the foot.

[0107] VI. The expected state of the foot corresponding to the MPC output.

[0108] Within the control cycle, the MPC solution yields the [number]th [period]. The optimal trajectory progress, trajectory progress velocity, and trajectory progress acceleration of the foot tip of a leg: ; Among them, superscript This indicates that the corresponding variable is the optimal value obtained through MPC optimization.

[0109] The desired position of the foot is: ; The expected velocity at the foot is: ; The expected acceleration at the foot is: ; VII. Transformation of foot position to hip joint coordinate system.

[0110] No. The desired position of the foot of a leg is determined by the world coordinate system. Transform to body coordinate system : ; in, yes The inverse rotation matrix.

[0111] No. The origin of the hip joint of one leg in the body coordinate system The fixed installation position below is recorded as: ; Foot end relative to the first The positions of the origin points of the hip joints are: ; Further conversion to the hip joint coordinate system : ; After merging, we get: ; remember: ; in, This indicates the anteroposterior coordinates of the foot relative to the hip joint; This represents the lateral coordinate of the foot relative to the hip joint, with positive coordinates pointing to the right of the robot. This represents the vertical coordinate of the foot relative to the hip joint, with downward being positive.

[0112] 8. Leg analysis and inverse kinematics.

[0113] No. The joint angle vector of a leg is defined as: ; in, Indicates the hip abduction / adduction angle; Indicates the hip flexion-extension joint angle; This indicates the knee flexion angle.

[0114] Let the lateral offset length of the hip be... Thigh length is The length of the lower leg is Define the sign factor for the left and right leg directions: ; Calculate the radial distance of the foot tip in the transverse-vertical plane: ; The equivalent vertical distance after considering the lateral offset of the hip is: ; The equivalent distance from the tip of the foot in the sagittal plane of the leg to the center of the hip flexion-extension joint is: ; in, It represents the radial distance from the foot to the axis of the hip abduction joint within the hip abduction plane; This represents the equivalent vertical distance after deducting the lateral offset of the hip. It indicates the distance from the tip of the foot in the sagittal plane of the leg to the center of the hip flexion-extension joint.

[0115] The condition for inverse kinematics attainability is: ; ; The hip abduction / adduction angle is: ; in, This represents the arctangent function with quadrant determination.

[0116] definition: ; To avoid numerical errors, make: ; in: ; The knee flexion angle is: ; in, This represents pi; when this joint angle is defined, the leg is fully extended. .

[0117] Define the orientation angle of the foot in the sagittal plane of the leg: ; definition: ; ; ; in, It represents the angle between the line connecting the hip extension joint to the foot and the vertical downward direction; This indicates the angle between the thigh link and the line connecting the hip extension joint to the foot.

[0118] Choosing the inverse kinematic lateral movement with knee flexion, the hip extension / inversion angle is: ; The corresponding sagittal plane positive kinematic relationship of the leg is: ; ; 9. Joint command angle.

[0119] Taking into account the actual installation orientation of each joint motor and the encoder zero-position offset, the joint command angle is: ; ; ; in, , and All or , used to indicate the installation direction of the corresponding motor; , and These represent the encoder zero-position offsets of the three joints, respectively.

[0120] No. The joint command angle vector of a leg is: .

[0121] 10. Joint speed and control torque.

[0122] The origin of the hip joint corresponding to the i-th leg is in the world coordinate system. The following position is: ; Convert the body angular velocity to the world coordinate system The following is: ; No. The origin of the hip joint of one leg in the world coordinate system The speed at which it descends is: ; Among them, symbols This represents the cross product of three-dimensional vectors.

[0123] Hip coordinate system To the world coordinate system The rotation matrix is: ; The desired relative velocity of the foot with respect to the hip joint coordinate system is: ; in, Represents the foot relative to the hip joint coordinate system Expected speed; This represents the expected absolute velocity of the foot in the world coordinate system.

[0124] The differential kinematic relationship of the leg is as follows: ; in, Indicates the first A geometric Jacobian matrix for a single leg; Indicates the first The joint velocity vector of a leg.

[0125] When using the Jacobian matrix pseudo-inverse, the joint velocity command is: ; in, This represents the Moore–Penrose pseudoinverse of the Jacobian matrix.

[0126] To avoid abrupt changes in joint velocity near singular configurations, a damped least squares approach is preferred: ; in, Indicates the damping factor; Represents the identity matrix; superscript This represents finding the inverse of a matrix.

[0127] The joint control torque is: ; in, Indicates the first The joint control torque vector of a leg; Represents the position feedback gain matrix; This represents the velocity feedback gain matrix.

[0128] When model feedforward control is required, it can be written as: ; in, This represents the feedforward torque calculated through gravity compensation, inverse dynamics, or other models.

[0129] 11. Wheel joint control.

[0130] For quadrupedal wheeled robots with drive wheels mounted on their feet, during the fall recovery and standing phases, the first... The speed command for the wheel joint of one leg is set as follows: ; in, Indicates the first The desired angular velocity of each wheel joint.

[0131] After the robot regains a stable standing position and enters wheeled motion mode, the wheel joint speed command is as follows: ; in, Indicates the first The expected tangential velocity at each wheel end; Indicates the radius of the drive wheel.

[0132] No. The integrated control command for the single-leg wheel mechanism is expressed as follows: ; The first three components are leg joint position commands, and the fourth component is wheel joint velocity commands. Therefore, this vector belongs to a position-velocity hybrid control command, rather than a pure joint position vector.

[0133] 12. Overall recovery control process.

[0134] Step S501: Obtain the current body state of the target robot. Current body position Current body posture Current linear velocity of the machine body Current angular velocity of the machine body and the current position of each foot. .

[0135] Step S502, according to the preset normal standing height 1. Restore the yaw angle at the start of control and the nominal standing foot position of each leg. Determine the state of the target organism And calculate the target standing position at each foot. .

[0136] Step S503, based on the current position of each foot end Starting from the target's standing position As the endpoint, based on the preset foot lifting height Construct the fifth-order Bézier curves corresponding to each foot end.

[0137] Step S504: Establish an MPC optimization problem that includes body state recovery error, control input amplitude, foot target position error, trajectory progress error, trajectory progress velocity, and trajectory progress acceleration. Solve the optimal trajectory progress of each foot in real time within the prediction time domain. Optimal trajectory progress speed and optimal trajectory progress acceleration .

[0138] Step S505: Substitute the optimal trajectory progress, optimal trajectory progress velocity, and optimal trajectory progress acceleration into the corresponding fifth-order Bézier curve and its derivative to obtain the position of each foot end in the world coordinate system. The expected position below Expected speed and expected acceleration .

[0139] Step S506: Based on the target robot's current body position, current body posture, and the coordinates of each hip joint in the body coordinate system... The fixed installation position and the fixed rotation relationship of the hip joint coordinate system relative to the body coordinate system are used to change the desired foot position from the world coordinate system. Transform to the corresponding hip joint coordinate system .

[0140] Step S507, based on the foot in the hip joint coordinate system Lower position, lateral offset length of the hip Thigh length and calf length The hip abduction / adduction joint angle, hip pitch joint angle, and knee joint angle are calculated by analyzing inverse kinematics, and the joint command angle is obtained by combining the joint mounting direction coefficient and encoder zero position offset.

[0141] Step S508: Based on the expected foot velocity, body linear velocity, body angular velocity, and fixed installation position of the hip joint, calculate the expected relative velocity of the foot relative to the corresponding hip joint coordinate system, and calculate the joint velocity command using the Jacobian matrix or damped least squares method.

[0142] In step S509, the joint angle command and joint speed command are input into the joint controller 9, and each leg is driven to perform recovery actions through position control, speed control or torque control until the target robot returns to a stable standing posture.

[0143] Step S510: For a target robot with drive wheels installed on its feet, the wheel joint speed command is set to zero during the fall recovery phase; after the target robot recovers and stands stably, the wheel joint speed command is calculated according to the wheel motion requirements.

[0144] Based on the robot's current fall posture and the current positions of each foot, the target standing pose of the robot body and the target standing position of each foot are determined. For each foot, a fifth-order Bézier curve is constructed with the current position as the starting point and the target standing position as the ending point. Adjacent control points at the starting and ending points are set to ensure the foot trajectory has zero velocity at the starting and ending positions. Intermediate lift control points are set to ensure the foot has a preset lift height during movement. An MPC optimization problem is established, incorporating constraints on robot posture recovery error, foot target position error, control input amplitude, foot trajectory progress error, and foot motion smoothness. The optimal trajectory progress of each foot along the Bézier curve is calculated. The optimal trajectory progress is then substituted into the Bézier curve to obtain the desired position, desired velocity, and desired acceleration of the foot. The desired position of the foot is then transformed from the world coordinate system to the hip joint coordinate system. Based on the leg link length and hip abduction offset, the hip abduction / adduction joint angle, hip pitch joint angle, and knee joint angle are calculated through analytical inverse kinematics. Finally, the joint angles are used as the desired input of the joint controller to drive each leg to perform recovery actions, thereby realizing the recovery control of the quadrupedal, four-wheeled robot from a fallen state to a stable standing state.

[0145] Based on the same inventive concept, this application also provides a robot fall detection and standing control system for implementing the robot fall detection and standing control method described above. The solution provided by this system is similar to the solution described in the above method; therefore, the specific limitations of one or more robot fall detection and standing control system embodiments provided below can be found in the limitations of the robot fall detection and standing control method described above, and will not be repeated here.

[0146] In one exemplary embodiment, a robot fall detection and recovery control system is provided, comprising: An inertial measurement unit is installed on the body of the target robot and is used to collect the robot's attitude perception data.

[0147] Joint encoders are installed at each joint of the target robot to collect joint motion data of the target robot.

[0148] The controller 9 is communicatively connected to the inertial measurement unit and the joint encoder. The controller 9 includes a memory and a processor. The memory stores a computer program. When the processor executes the computer program, it implements the above-mentioned robot fall detection and stand-up control method.

[0149] In one exemplary embodiment, such as Figure 11 As shown, a robot fall detection and recovery control system is provided, including: The system self-test module includes IMU data detection, hardware detection, and communication detection, followed by attitude calculation and classification. Hardware includes joint motors, wheel motors 2, and a power control board. Communication includes communication between the motors and controller 9, communication between controller 9 and the power control board, and communication between battery 6 and the power control board.

[0150] Motion Module: Includes standing mode and motion mode, which can be executed sequentially.

[0151] Passive protection module: The quadruped and four-wheeled robots stop all movement and lock their joints to prevent further damage.

[0152] Anomaly alarm module: A mechanism to notify the operator via an audible and visual alarm or a remote wireless communication module.

[0153] Limb compression fall recovery module: Executes the escape and standing recovery sub-strategy.

[0154] The control method described above adopts robot kinematics control, namely joint angle closed-loop control.

[0155] In summary, this application has the following beneficial effects: 1) High Detection Accuracy. This application significantly improves the accuracy of fall detection, reducing false positives and false negatives compared to single IMU methods. This means the robot can more reliably identify genuine fall actions, avoiding misclassifying normal movements (such as rapid turns) as falls or missing complex fall patterns. Specifically, by monitoring the rate of change of multi-sensor data (such as the attitude angle, angular velocity, and acceleration of the IMU, as well as the angle data of the joint encoder and the torque data of the torque sensor), and combining this with a time threshold T (1 second) and the difference between the resultant force modulus of acceleration and the gravitational acceleration g, the judgment logic is rigorous, avoiding interference from single-sensor noise. Furthermore, forward kinematics is used to calculate the foot coordinate position and the projected contour area of ​​the body, combined with torque saturation thresholds and micro-motion thresholds, to classify fall patterns in detail (such as falls due to limb pressure, side falls, and flat falls without pressure). These technical points achieve multi-sensor fusion and intelligent classification, eliminating the defects of ignoring joint feedback and torque information, thereby improving accuracy.

[0156] 2) High recovery efficiency. This application optimizes the stand-up recovery time, far superior to the fixed recovery strategies of existing technologies. This allows for rapid restoration of the robot's standing posture, avoiding prolonged task interruptions or exposure to hazardous environments (such as being run over by a train during railway inspections). Specifically, the adaptive stand-up control invokes targeted strategies based on classification results: for falls due to limb compression, escape control is first implemented (e.g., using uncompressed limbs to lift the robot body, or using assisted oscillating torque for incline escape); for side falls, roll correction is implemented; for flat falls without compression, the robot stands up directly. Simultaneously, Model Predictive Control (MPC) and Bézier curve trajectory planning are used to calculate the inverse kinematic path in real time, and hub locking is added for four-wheeled robots to prevent slippage. These adaptive mechanisms avoid the single control strategy of existing technologies, dynamically adjusting the motion path for different types of falls, reducing ineffective attempts and secondary injuries, thereby significantly improving recovery efficiency.

[0157] 3) Enhanced Safety. This application improves the overall safety of the robot after a fall, enabling timely handling of abnormal situations and preventing secondary disasters. Furthermore, it significantly reduces the risk of secondary damage caused by motor overload and mechanical interference during the robot's recovery attempt, extending hardware lifespan. Related technologies typically execute a preset "standing sequence" directly after a fall. If a limb is unfortunately trapped under the robot's body, forcibly extending the leg can cause the motor to stall, instantly generating a large current that burns out the driver or breaks the mechanical linkage. This application, however, identifies the trapped limb through "projection area detection + torque saturation monitoring" and adopts a "first free, then stand" strategy, physically eliminating the risk of mechanical lock-up. In addition, the system's self-check and anomaly handling mechanisms detect the communication link (via heartbeat packet loss rate) and hardware status (encoder malfunction or current breakage) before recovery. If these fail, a passive protection mode (high damping or mechanical brake locking) is entered, triggering an alarm (audio-visual alarm and remote wireless module sending location information). This addresses the pain point of lacking fault intervention, ensuring that risky actions are prohibited in abnormal situations (such as hardware damage or communication link failure) and supporting manual maintenance. By placing security checks before the recovery process and combining real-time self-testing and protection modes, a multi-layered security network is constructed, improving system robustness.

[0158] 4) Strong environmental adaptability. This application solves the problem of robots being unable to recover from falls in unstructured terrain, significantly reducing the need for manual intervention and improving the robot's autonomy in complex environments (such as railway inspection and disaster relief). Traditional technologies often assume a standard flat-lying posture after a fall. However, if the robot rolls to its side or back, its joint configuration differs greatly from the standard flat-lying posture, causing conventional algorithms to fail. This application decouples the recovery process into three progressive stages: first, it addresses the problem of joint jamming (escape mode); then, it addresses the problem of abnormal posture (correcting side / back rolls to prone position through momentum rolling); and finally, it addresses the problem of standing up. This layered strategy, combined with multi-sensor data and adaptive algorithms, makes the method applicable to various terrains and fall patterns, thereby enhancing environmental adaptability and indirectly reducing operating costs.

[0159] In one exemplary embodiment, a computer device is provided, which may be a server or a terminal, and its internal structure diagram may be as follows. Figure 12 As shown, the computer device includes a processor, memory, input / output (I / O) interfaces, and a communication interface. The processor, memory, and I / O interfaces are connected via a system bus, and the communication interface is also connected to the system bus via the I / O interfaces. The processor provides computational and control capabilities. The memory includes non-volatile storage media and internal memory. The non-volatile storage media stores the operating system, computer programs, and a database. The internal memory provides the environment for the operation of the operating system and computer programs stored in the non-volatile storage media. The database stores data related to robot fall detection and recovery control. The I / O interfaces are used for information exchange between the processor and external devices. The communication interface is used for communication with external terminals via a network connection. When the computer program is executed by the processor, it implements a robot fall detection and recovery control method.

[0160] Those skilled in the art will understand that Figure 12 The structure shown is merely a block diagram of a portion of the structure related to the present application and does not constitute a limitation on the computer equipment to which the present application is applied. Specific computer equipment may include, for example, [the following is a list of possible additional structures]. Figure 12 The embodiments show more or fewer components, combinations of certain components, or different component arrangements. In one exemplary embodiment, a computer device is provided, including a memory and a processor, the memory storing a computer program, the processor executing the computer program to implement the steps in the above-described method embodiments.

[0161] In one exemplary embodiment, a computer-readable storage medium is provided storing a computer program that, when executed by a processor, implements the steps in the above-described method embodiments.

[0162] In one exemplary embodiment, a computer program product is provided, including a computer program that, when executed by a processor, implements the steps in the above-described method embodiments.

[0163] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data used for analysis, data stored, data displayed, etc.) involved in this application are all information and data authorized by the user or fully authorized by all parties, and the collection, use and processing of the relevant data must comply with relevant regulations.

[0164] Those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium, and when executed, it can include the processes of the embodiments described above. Any references to memory, databases, or other media used in the embodiments provided in this application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetic random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM can take many forms, such as Static Random Access Memory (SRAM) or Dynamic Random Access Memory (DRAM).

[0165] The databases involved in the embodiments provided in this application may include at least one type of relational database and non-relational database. Non-relational databases may include, but are not limited to, blockchain-based distributed databases. The processors involved in the embodiments provided in this application may be general-purpose processors, central processing units, graphics processing units, digital signal processors, programmable logic devices, quantum computing-based data processing logic devices, etc., and are not limited to these.

[0166] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0167] This document uses specific examples to illustrate the principles and implementation methods of this application. The descriptions of the above embodiments are only for the purpose of helping to understand the methods and core ideas of this application. Furthermore, those skilled in the art will recognize that, based on the ideas of this application, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of this application.

Claims

1. A robot fall detection and stand-up control method characterized by, include: Acquire posture perception data and joint motion data of the target robot; the target robot includes a quadruped robot and a four-wheeled robot; Based on the posture perception data and joint motion data, determine whether the target robot has entered a fall mode; If the target robot enters the fall mode, a self-check is performed on the target robot's communication link and hardware status, and the self-check results are obtained. If the self-test result is unsuccessful, the target robot will be prohibited from executing the stand-up control strategy and an abnormal alarm will be triggered. If the self-check result is passed, then based on the positive kinematics solution method, the posture solution and classification operation are performed according to the posture perception data and joint motion data to obtain the fall state classification result; the fall state classification result includes at least one of the following: flat-lying fall without pressure, fall with limb pressure, side fall, and rollover. Based on the fall state classification results, an adaptive recovery control is performed using a standing control strategy to restore the target robot to a stable standing posture. The stand-up control strategy includes a direct stand-up recovery sub-strategy, an escape stand-up recovery sub-strategy, a first roll correction recovery sub-strategy, and a second roll correction recovery sub-strategy.

2. The robot fall detection and stand-up control method according to claim 1, characterized by, Based on the posture perception data and joint motion data, determining whether the target robot has entered a fall mode specifically includes: The target robot's body tilt angle and body acceleration are determined based on the attitude perception data, and the angular velocities and torques of each joint of the target robot are determined based on the joint motion data; the attitude perception data includes body attitude angle, body angular velocity, and body acceleration; the joint motion data includes the encoder angle and torque of each joint motor; If the abnormal triggering conditions are met, an abnormality is determined to have occurred, and the duration of the abnormal state is started to be timed. The abnormal triggering conditions include the joint angular velocity of any joint being greater than or equal to a preset angular velocity threshold and the fuselage tilt angle being greater than or equal to a preset safe tilt angle threshold; or, the rate of change of the joint torque of any joint being greater than or equal to a preset torque rate of change threshold and the fuselage tilt angle being greater than or equal to a preset safe tilt angle threshold. If the duration of the abnormal state is greater than or equal to the first preset time threshold, and at the end of the first preset time threshold, the difference between the three-axis resultant force modulus of the body acceleration and the gravitational acceleration is within the preset static tolerance range, then the target robot is determined to have entered the fall mode.

3. The robot fall detection and standing control method according to claim 2, characterized in that, The communication link and hardware status of the target robot are self-checked, and the self-check results are obtained, including: If the heartbeat packet loss rate of the bus communication heartbeat packets between the target robot's controller and each joint driver is greater than or equal to the preset loss rate threshold within the preset statistical period, then the communication link is determined to be faulty. If the heartbeat packet loss rate of the bus communication heartbeat packets between the target robot's controller and each joint driver is less than the preset loss rate threshold within the preset statistical period, the communication link is determined to be normal. If at least one joint motor has an abnormal encoder angle data, or at least one joint motor has an open circuit in its current feedback, then the hardware status is determined to be abnormal; the abnormal data includes non-digital abnormalities or infinity abnormalities. If there are no abnormal data in the encoder angle of each joint motor and there is no open circuit in the current feedback of each joint motor, then the hardware status is considered normal. If the communication link is normal and the hardware is in normal condition, the self-test result is passed; If the communication link fails or the hardware status is abnormal, the self-test result will be "fail".

4. The robot fall detection and standing control method according to claim 1, characterized in that, Based on the forward kinematics calculation method, posture calculation and classification operations are performed on the posture perception data and joint motion data to obtain the fall state classification result, specifically including: The body posture of the target robot is determined based on the posture angle data in the posture perception data; the body posture includes the roll angle and pitch angle of the body relative to the horizontal plane. If the absolute value of the roll angle is greater than or equal to a preset rollover angle threshold, and the normalized deviation of the roll angle relative to the preset rollover angle threshold is greater than or equal to the normalized deviation of the pitch angle relative to the preset pitch rollover angle threshold, then the fall state classification result is determined as a side fall state. If the absolute value of the pitch angle is greater than or equal to a preset pitch rollover angle threshold, and the normalized deviation of the pitch angle relative to the preset pitch rollover angle threshold is greater than the normalized deviation of the roll angle relative to the preset rollover angle threshold, then the fall state classification result is determined as a pitch rollover state. If the absolute value of the roll angle is less than a preset rollover angle threshold, and the absolute value of the pitch angle is less than a preset pitch rollover angle threshold, then the fall status classification result is determined through the following steps: Based on the encoder angle data of each joint motor in the joint motion data, the position of each foot relative to the center of the body is calculated based on the leg forward kinematics model of the target robot, and the three-dimensional spatial coordinates of each foot in the body coordinate system are obtained. Based on the fuselage attitude, fuselage geometry and current fuselage position, determine the projection boundary of the fuselage on the horizontal plane, and form the fuselage projection area by the projection boundary; Based on the three-dimensional spatial coordinates of each foot in the fuselage coordinate system and the current fuselage pose, coordinate transformation is performed to obtain the three-dimensional spatial coordinates of each foot in the world coordinate system, and the foot projection points on the horizontal plane of each foot are extracted respectively. Determine the positional relationship between each foot projection point and the body projection area, and determine whether each joint meets the pressure determination condition; the pressure determination condition is that the absolute value of the joint torque is continuously greater than or equal to the preset torque saturation threshold within a preset time period, and the absolute value of the change in the encoder angle data of the corresponding joint motor within the preset time period is less than the preset micro-motion threshold. If all foot projection points are located outside the fuselage projection area and all joints do not meet the pressure determination condition, then the fall state classification result is determined as a flat-lying fall state without pressure. If at least one foot projection point is located inside or on the boundary of the fuselage projection area, or if at least one joint meets the pressure determination condition, then the fall state classification result is determined to be a limb pressure fall state.

5. The robot fall detection and standing control method according to claim 1, characterized in that, Based on the fall state classification results, an adaptive recovery control strategy is implemented to restore the target robot to a stable standing posture, specifically including: If the fall state classification result is a flat-lying fall without pressure, then the direct stand-up recovery sub-strategy is invoked to control the target robot to adjust the posture of each leg and enter a preset squatting mode. In the preset squatting mode, the stand-up action is performed to restore the robot to a stable standing posture. If the fall state classification result is a limb compression fall state, then the escape and stand-up recovery sub-strategy is invoked. The target robot is controlled to first perform an escape action to remove the restriction of the body or external environment on the compressed limb, and then the target robot is controlled to enter the preset squatting mode and perform a stand-up action to restore to a stable standing posture. If the fall state classification result is a side fall, then the first roll correction and recovery sub-strategy is invoked. Based on the positive or negative direction of the roll angle, the fuselage's roll direction is determined. The leg group on the opposite side of the roll direction is designated as the first preparatory leg group, and the leg group on the same side of the roll direction is designated as the first active leg group. The hip pitch joint movement of the first preparatory leg group is controlled, moving the thigh of the first preparatory leg group to a position approximately level with the lower surface of the fuselage. The knee joint of the first preparatory leg group is also controlled to flex, reducing the obstruction of the first preparatory leg group to the fuselage roll. The hip abduction and hip pitch joint movements of the first active leg group are also controlled. The first active side leg group's leg or foot end is brought into contact with the ground and a force is applied to the ground. The reaction force of the ground on the target robot generates a rolling torque around the longitudinal axis of the body, driving the body to roll in the direction of the first prepared side leg group. After the roll is completed, the target robot's posture perception data and joint motion data are reacquired, and the fall state classification result of the target robot is re-determined based on the reacquired data. If the re-determined fall state classification result is a flat fall without pressure, the direct stand-up recovery sub-strategy is invoked. If the re-determined fall state classification result is a fall with limb pressure, the escape stand-up recovery sub-strategy is invoked. If the fall state classification result is a pitching fall, the second roll correction and recovery sub-strategy is invoked. Based on the positive or negative direction of the pitch angle, the fuselage is determined to be either pitching forward or backward. When the fuselage pitches forward, the rear leg group is designated as the second preparatory side leg group and the front leg group as the second active side leg group. When the fuselage rolls backward, the front leg group is designated as the second preparatory side leg group and the rear leg group as the second active side leg group. The hip pitch joint movement of the second preparatory side leg group is controlled, causing the thigh of the second preparatory side leg group to move to a position approximately level with the lower surface of the fuselage, and the knee joint flexion of the second preparatory side leg group is controlled. The hip pitch joint movement of the second active side leg group is also controlled. The knee joint movement causes the leg or foot of the second active side leg group to contact the ground and apply force to the ground. The reaction force of the ground on the target robot generates a rolling torque around the horizontal axis of the body, driving the body to roll in the direction of the second reserve side leg group. After the roll is completed, the posture perception data and joint motion data of the target robot are reacquired, and the fall state classification result of the target robot is re-determined based on the re-acquired data. If the re-determined fall state classification result is a flat fall without pressure, the direct standing recovery sub-strategy is invoked. If the re-determined fall state classification result is a fall with limb pressure, the escape standing recovery sub-strategy is invoked.

6. The robot fall detection and standing control method according to claim 5, characterized in that, Performing the action of standing up specifically includes: Obtain the current body pose of the target robot and the current position of each foot in the body coordinate system; the current body pose includes the current body position and the current body posture; Based on the current fuselage pose, the current foot position in the fuselage coordinate system is transformed to the world coordinate system to obtain the current foot position in the world coordinate system. The target fuselage attitude is determined based on the current fuselage attitude and preset stable standing parameters; the target fuselage attitude includes the target fuselage position and the target fuselage attitude; the vertical coordinate of the target fuselage position is the preset standing height, the roll angle and pitch angle of the target fuselage attitude are both zero, and the yaw angle of the target fuselage attitude is maintained as the yaw angle of the current fuselage attitude or is set to the preset yaw angle; The nominal standing position of each foot in the fuselage coordinate system is transformed according to the target fuselage pose to obtain the target standing position of each foot in the world coordinate system. Each foot is used as the starting control point in the world coordinate system and the target standing position of the corresponding foot in the world coordinate system as the ending control point. Based on the preset foot lifting height, starting velocity and acceleration boundary conditions and ending velocity and acceleration boundary conditions, four intermediate control points are determined, thereby generating a fifth-order Bézier curve determined by six control points. By traversing all feet, the fifth-order Bézier curve corresponding to each foot is obtained. Using model predictive control, the trajectory progress and trajectory progress change rate of each foot are optimized variables in the prediction time domain. The optimization objectives are to reduce the deviation between the current foot position and the target standing position, reduce the change of foot velocity and acceleration, and improve the stability of the fuselage attitude. The constraints are set as follows: trajectory progress is greater than or equal to 0 and less than or equal to 1, trajectory progress in adjacent control cycles is monotonically non-decreasing, foot velocity and acceleration do not exceed preset limits, and each joint does not exceed the joint range of motion. The optimal trajectory progress of each foot along the corresponding fifth-order Bézier curve is solved. Substitute the optimal trajectory progress of each foot and the first and second derivatives of the optimal trajectory progress with respect to time into the corresponding fifth-order Bézier curve, and parameterize the fifth-order Bézier curve with time to obtain the expected position, expected velocity and expected acceleration of each foot in the world coordinate system. Based on the current body pose of the target robot and the position of each hip joint relative to the center of the body, the desired position of each foot is transformed from the world coordinate system to the hip joint coordinate system of the corresponding leg, so as to obtain the desired coordinates of each foot relative to the corresponding hip joint. Based on the desired coordinates of each foot relative to the corresponding hip joint and the leg geometric parameters, the desired joint angles of each leg are calculated through inverse kinematics; the leg geometric parameters include hip abduction offset length, thigh length, and calf length; the desired joint angles include hip abduction joint angle, hip flexion joint angle, and knee joint angle. Based on the kinematic Jacobian matrix of each leg and the expected velocity and expected acceleration of each foot, calculate the expected joint angular velocity and expected joint angular acceleration of each leg. The desired joint angle, desired joint angular velocity, and desired joint angular acceleration of each leg are input into the joint trajectory controller, which drives each leg to perform a standing motion according to the corresponding fifth-order Bézier curve until the current body pose of the target robot reaches the target body pose and remains stable.

7. The robot fall detection and standing control method according to claim 5, characterized in that, Performing escape maneuvers specifically includes: Based on the encoder angle data of the hip pitch joint motor, hip abduction joint motor and knee joint motor of each leg, the current angular position of each hip pitch joint, hip abduction joint and knee joint is determined. Lock the hip flexion and extension joints of each leg to the corresponding current angle position, control the knee joints of each leg to move to the preset maximum flexion angle position, and control the hip abduction joints of each leg to move to the preset maximum abduction angle position. Select two diagonally opposite legs from the four legs of the target robot as the first diagonal leg pair, release the position lock of the hip pitch joint of the first diagonal leg pair, control the knee joint of the first diagonal leg pair to move from the preset maximum flexion angle position to the preset maximum extension angle position, and control the hip pitch joint of the first diagonal leg pair to move to the preset hip joint neutral angle position. Based on the encoder angle data of each joint motor in the first diagonal leg pair, determine the knee joint angle and hip pitch joint angle of the first diagonal leg pair. When both knee joints of the first diagonal leg pair reach the preset maximum extension angle position and both hip flexion joints reach the preset hip joint neutral angle position, control the knee joints of the first diagonal leg pair to move back to the preset maximum flexion angle position, and lock the hip flexion joints of the first diagonal leg pair at the preset hip joint neutral angle position to complete the escape action of the first diagonal leg pair. The other two legs that are diagonally related are identified as the second diagonal leg pair. The position lock of the hip flexion-extension joint of the second diagonal leg pair is released. The knee joint of the second diagonal leg pair is controlled to move from the preset maximum flexion angle position to the preset maximum extension angle position. The hip flexion-extension joint of the second diagonal leg pair is controlled to move to the preset hip joint neutral angle position. Based on the encoder angle data of each joint motor in the second diagonal leg pair, determine the knee joint angle and hip pitch joint angle of the second diagonal leg pair; When both knee joints of the second diagonal leg pair reach the preset maximum extension angle position and both hip flexion joints reach the preset hip joint neutral angle position, control the knee joints of the second diagonal leg pair to move back to the preset maximum flexion angle position, and lock the hip flexion joints of the second diagonal leg pair at the preset hip joint neutral angle position to complete the escape action of the second diagonal leg pair. After the first and second diagonal leg pairs have completed their escape actions, the joint torque data and encoder angle data of each joint are reacquired. Based on the reacquired data, it is determined whether each joint still meets the pressure judgment condition. When all joints do not meet the pressure judgment condition, it is determined that the escape actions of all legs have been completed, and the target robot is controlled to enter the preset squatting mode.

8. The robot fall detection and standing control method according to claim 5, characterized in that, The robot fall detection and stand-up control method further includes: executing anomaly protection measures during the invocation of the first roll correction recovery sub-strategy or the second roll correction recovery sub-strategy; the anomaly protection measures include joint angle limit protection, joint torque limit protection, joint speed smoothing protection, foot contact force buffering protection, and body posture and joint drive anomaly monitoring protection.

9. The robot fall detection and standing control method according to claim 6, characterized in that, When the target robot is a four-wheeled legged robot, a position locking command or a zero-speed control command is sent to the wheel hub motor at the foot end throughout the entire process of performing the standing action.

10. A robot fall detection and standing control system, characterized in that, include: An inertial measurement unit is installed on the body of the target robot and is used to collect the attitude perception data of the target robot. Joint encoders are installed at each joint of the target robot to collect joint motion data of the target robot. A controller is communicatively connected to the inertial measurement unit and the joint encoder. The controller includes a memory and a processor. The memory stores a computer program. When the processor executes the computer program, it implements the robot fall detection and stand-up control method as described in any one of claims 1-9.

Images