Control methods, apparatus, equipment, computer-readable storage media and program products
By acquiring robot measurement data, performing estimation data processing and multi-layer progressive control, the dynamic stability problem of a humanoid upper body robot with a four-way omnidirectional wheel X-shaped chassis under various working conditions was solved, achieving efficient anti-tipping control.
Patent Information
- Application Number
- CN202611613455.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-10-19
- Publication Date
- 2026-11-17
AI Technical Summary
In the existing technology, robots with humanoid upper bodies and four omnidirectional wheels in an X-shaped chassis face dynamic stability problems in service, logistics, and inspection fields. These problems include high center of mass and low inertia, unstructured terrain disturbances, dynamic load uncertainty, wheel-ground nonlinear coupling, and non-cooperative external impacts, which lead to robot tipping over and low anti-tipping control efficiency.
By acquiring measurement data from the robot and performing estimation data processing, the effective support polygon and zero-moment point are determined. Variable-weight convex optimization planning and active momentum game execution are used, combined with robust compensation processing, to generate drive signals to counteract the overturning moment, thus achieving five-layer progressive anti-overturning control.
It achieves full-coverage anti-tipping control for robots under various working conditions, improves the efficiency of anti-tipping control, and covers typical instability scenarios such as flat ground, slopes, load, slippage, and impact.
Smart Images

Figure CN123113403A_ABST
Abstract
Description
Technical Field
[0001] This disclosure relates to the field of computer technology, and more specifically, to a control method, apparatus, device, computer-readable storage medium, and program product. Background Technology
[0002] In existing technologies, robots with a humanoid upper body and an X-shaped chassis with four omnidirectional wheels have broad application prospects in service, logistics, and inspection fields. However, the dynamic stability of these robots faces the following challenges: (1) High center of mass and low inertia: When the lifting column is raised and the double arms are extended, the center of mass of the system rises significantly, the moment of inertia is large, and the overturning moment is easily generated when the acceleration and deceleration are rapid. (2) Unstructured terrain disturbance: Slopes, steps and other ground surfaces cause the gravity vector to no longer be perpendicular to the supporting plane, and the effective projected area of the supporting polygon is reduced; (3) Dynamic load uncertainty: During the dual-arm handling process, the load mass and center of mass change in real time, and the system dynamic parameters change over time; (4) Wheel-ground nonlinear coupling: When the omnidirectional wheel turns or moves laterally at high speed, it generates non-negligible longitudinal / lateral slip, causing the actual acceleration to deviate from the expected value; (5) Non-cooperative external impact: In a human-machine collaborative environment, accidental collisions or shovings generate instantaneous impact force pulses.
[0003] The above (1)-(5) often lead to robot tipping over, and the efficiency of robot anti-tipping control in the prior art is low. Summary of the Invention
[0004] This disclosure addresses the shortcomings of existing methods by proposing a control method, apparatus, device, computer-readable storage medium, and computer program product to solve the problem of how to improve the efficiency of robot anti-tipping control.
[0005] Firstly, this disclosure provides a control method, including: Acquire measurement data from the robot; The measurement data is processed to determine the robot's estimated data, which includes at least one of the following: the robot's local terrain slope angle, the robot's load mass change, the robot's center of gravity offset, the robot's wheel slip parameters, and scene labels. Based on the estimated data, the effective support polygon and zero moment point of the robot are determined through scene adaptive prediction processing. If the distance between the zero-moment point and the boundary of the effective supporting polygon is less than a preset distance threshold, the initial driving signal is determined through variable weight convex optimization programming. Based on the initial drive signal, the virtual inertial field parameters and virtual compensation torque are determined through active momentum game processing. Based on the virtual inertial field parameters, virtual compensation torque and initial drive signal, the drive command with active damping is obtained. Based on the drive command with active damping, the sliding mode compensation torque is determined through robust compensation processing, and the sliding mode compensation torque is fed forward and superimposed on the drive command with active damping to obtain the compensated drive signal. The robot is driven to move based on the compensated drive signal.
[0006] In one embodiment, the measurement data includes the gravity direction and body attitude rotation matrix in the body coordinate system measured by the robot's inertial measurement unit, and the estimated data includes the robot's local terrain slope angle. The measurement data is processed to determine the robot's estimated data, including: Based on the gravity direction and body attitude rotation matrix measured by the inertial measurement unit in the body coordinate system, the local slope angle of the robot is determined by the terrain local slope angle identification. The local slope angle of the terrain includes pitch slope angle and roll slope angle.
[0007] In one embodiment, the measurement data includes the robot's actual generalized momentum and observed generalized momentum, and the estimation data includes the robot's load mass change and center of mass shift. The measurement data is processed to determine the robot's estimated data, including: Based on the residual between the actual generalized momentum and the observed generalized momentum, the load mass change and centroid shift are determined by identifying the load parameters.
[0008] In one embodiment, the measurement data includes the robot's wheel encoder data, and the estimation data includes the robot's wheel-to-ground slip parameters; The measurement data is processed to determine the robot's estimated data, including: Based on wheel encoder data, wheel-ground slip parameters are identified and determined through wheel-ground slip parameter identification. The wheel-ground slip parameters include the longitudinal slip rate and lateral slip angle. The wheel-ground slip parameters are used to construct a modified Jacobian matrix that includes slip compensation.
[0009] In one embodiment, the estimated data includes at least one scenario label, which includes at least one of flat conditions, slope conditions, load conditions, slip conditions, and impact conditions.
[0010] In one embodiment, based on estimated data, the effective support polygon of the robot is determined through scene adaptive prediction processing, including: By projecting the ground contact points of each wheel of the robot onto a plane perpendicular to the direction of the equivalent gravity, multiple effective support points are obtained. Based on multiple effective support points, construct the robot's effective support polygon.
[0011] In one embodiment, the initial driving signal is determined through variable-weight convex optimization programming, including: Obtain available decision-making time; If the available decision time is greater than or equal to the preset time threshold, then the optimal control time series is determined through variable weight convex optimization programming, and the optimal control time series is used as the initial driving signal.
[0012] In one embodiment, if the available decision time is less than a time threshold, an emergency soft landing process is performed by calling a preset limit boundary soft landing library to obtain an emergency command for the minimum damage trajectory, and the emergency command for the minimum damage trajectory is used as the initial drive signal.
[0013] In one embodiment, based on the initial driving signal, processing is performed through active momentum game to determine the virtual inertial force field parameters and virtual compensation torque, including: Determine the robot's current actual overturning moment; Determine the power spectrum of the current actual overturning moment and extract the dominant frequency from the power spectrum; Based on the dominant frequency and optimal control time series, the virtual inertial force field parameters and virtual compensation torque generated by the robot's dual arms and lifting column motion are determined.
[0014] In one embodiment, based on a drive command with active damping, the sliding mode compensation torque is determined through robust compensation processing, including: In response to the active damping drive command, a sliding surface is obtained, which is used to characterize the difference between the robot's actual generalized velocity and a reference generalized velocity; Based on the sliding surface, the external impact force of the robot is estimated by using a super-helical sliding mode observer; Based on the estimation of external impact force, the preset Jacobian matrix of the external force application point and the sliding surface, the sliding compensation torque is determined.
[0015] Secondly, this disclosure provides a control device, comprising: The acquisition module is used to acquire the robot's measurement data; The perception and identification processing module is used to process the measurement data and determine the robot's estimated data. The estimated data includes at least one of the following: the robot's local terrain slope angle, the robot's load mass change, the robot's center of gravity offset, the robot's wheel-ground slip parameters, and scene labels. The scene adaptive prediction processing module is used to determine the robot's effective support polygon and zero moment point based on the estimated data through scene adaptive prediction processing. The variable weight convex optimization planning processing module is used to determine the initial driving signal through variable weight convex optimization planning processing if the distance between the zero moment point and the boundary of the effective support polygon is less than a preset distance threshold. The active momentum game execution processing module is used to determine the virtual inertial field parameters and virtual compensation torque based on the initial drive signal through active momentum game execution processing, and to obtain the drive command with active damping based on the virtual inertial field parameters, virtual compensation torque and initial drive signal. The robust compensation processing module is used to determine the sliding mode compensation torque based on the drive command with active damping through robust compensation processing, and feed forward and superimpose the sliding mode compensation torque onto the drive command with active damping to obtain the compensated drive signal. The drive module is used to drive the robot to move based on the compensated drive signal.
[0016] In one embodiment, when determining the initial driving signal through variable-weight convex optimization programming, the variable-weight convex optimization programming module is specifically used for: Obtain available decision-making time; If the available decision time is greater than or equal to the preset time threshold, then the optimal control time series is determined through variable weight convex optimization programming, and the optimal control time series is used as the initial driving signal. In one embodiment, when the active momentum game execution processing module determines the virtual inertial force field parameters and virtual compensation torque based on the initial driving signal through active momentum game execution processing, it is specifically used for: Determine the robot's current actual overturning moment; Determine the power spectrum of the current actual overturning moment and extract the dominant frequency from the power spectrum; Based on the dominant frequency and optimal control time series, the virtual inertial force field parameters and virtual compensation torque generated by the robot's dual arms and lifting column motion are determined.
[0017] Thirdly, this disclosure provides an electronic device, including: a processor, a memory, and a bus; A bus is used to connect the processor and memory; Memory, used to store operation instructions; A processor is configured to execute the control method of the first aspect of this disclosure by invoking operation instructions.
[0018] Fourthly, this disclosure provides a computer-readable storage medium storing a computer program that is used to execute the control method of the first aspect of this disclosure.
[0019] Fifthly, this disclosure provides a computer program product, including a computer program that, when executed by a processor, implements the steps of the control method in the first aspect of this disclosure.
[0020] The technical solutions provided in this disclosure have at least the following beneficial effects: The process involves acquiring measurement data from the robot; processing the measurement data to determine estimated robot data; based on the estimated data, determining the robot's effective support polygon and zero-moment point through scene adaptive prediction processing; if the distance between the zero-moment point and the boundary of the effective support polygon is less than a preset distance threshold, determining the initial drive signal through variable-weight convex optimization programming; based on the initial drive signal, determining the virtual inertial field parameters and virtual compensation torque through active momentum game execution processing, and obtaining a drive command with active damping based on the virtual inertial field parameters, virtual compensation torque, and initial drive signal; based on the drive command with active damping, determining the sliding mode compensation torque through robust compensation processing, and feeding the sliding mode compensation torque forward and superimposing it onto the drive command with active damping to obtain the compensated drive signal; and driving the robot to move based on the compensated drive signal. Thus, the process involves "determining the robot's estimated data," "scene adaptive prediction processing," "variable-weight convex optimization programming processing," "active momentum game execution processing," and "robust compensation processing." The five-layer progressive anti-tipping control achieves full-condition coverage of the robot (such as covering five typical robot instability scenarios: flat ground, slope, load, slippage, and impact), thereby improving the efficiency of robot anti-tipping control. Attached Figure Description
[0021] To more clearly illustrate the technical solutions in the embodiments of this disclosure, the accompanying drawings used in the description of the embodiments of this disclosure will be briefly introduced below.
[0022] Figure 1 A flowchart illustrating a control method provided in an embodiment of this disclosure; Figure 2 A schematic diagram of a five-layer progressive anti-rollover control system provided in an embodiment of this disclosure; Figure 3 This is a schematic diagram of the structure of a control device provided in an embodiment of the present disclosure; Figure 4 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this disclosure. Detailed Implementation
[0023] The embodiments of this disclosure are described below with reference to the accompanying drawings. It should be understood that the embodiments described below with reference to the accompanying drawings are exemplary descriptions for explaining the technical solutions of the embodiments of this disclosure, and do not constitute a limitation on the technical solutions of the embodiments of this disclosure.
[0024] Those skilled in the art will understand that, unless specifically stated otherwise, the singular forms “a,” “an,” “the,” and “the” used herein may also include the plural forms. It should be further understood that the terms “comprising” and “including” as used in embodiments of this disclosure mean that the corresponding feature can be implemented as the presented feature, information, data, step, operation, element, and / or component, but do not exclude implementation as other features, information, data, step, operation, element, component, and / or combinations thereof supported by the art. It should be understood that when we say that an element is “connected” or “coupled” to another element, the one element can be directly connected or coupled to the other element, or it can mean that the one element and the other element are connected through an intermediate element. Furthermore, “connected” or “coupled” as used herein can include wireless connection or wireless coupling. The term “and / or” as used herein indicates at least one of the items defined by the term; for example, “A and / or B” indicates implementation as “A,” or implementation as “B,” or implementation as “A and B.”
[0025] It is understood that in the specific embodiments of this disclosure, which involve control-related data, when the above embodiments of this disclosure are applied to specific products or technologies, user permission or consent is required, and the collection, use and processing of related data must comply with the relevant laws, regulations and standards of the relevant countries and regions.
[0026] To make the objectives, technical solutions, and advantages of this disclosure clearer, the embodiments of this disclosure will be described in further detail below with reference to the accompanying drawings.
[0027] This disclosure provides a control method for a control system, which relates to fields such as control.
[0028] To better understand and explain the solutions of the embodiments of this disclosure, some technical terms involved in the embodiments of this disclosure will be briefly explained below.
[0029] ZMP (Zero Moment Point): A point on the ground where the resultant moment in the horizontal direction is zero; the ZMP being located within the supporting polygon is a necessary condition for anti-overturning stability.
[0030] Supporting polygon: The convex hull on the ground at all wheel-ground contact points of the robot; the effective half-width shrinks after projection along the direction of gravity under slope conditions.
[0031] Attraction domain: The region in the state space where the system can autonomously converge back to its equilibrium posture; exceeding the boundary triggers replanning.
[0032] hp adaptive LGR pseudospectral method: A numerical method for discretizing optimal control problems using Legendre-Gauss-Radau collocations and adaptively adjusting the number of collocations p or interval subdivision h according to the interval residuals.
[0033] Differential flatness: The property that the system state and input can be algebraically expressed by a set of flat outputs and their finite-order derivatives, used to transform nonlinear optimal control problems into convex ones.
[0034] ATMD (Active Tuned Mass Damper): A mechanism in which two arms and a lifting column act as controllable mass blocks, generating a virtual inertial force field through opposite-phase motion to counteract the overturning moment.
[0035] Superhelical sliding mode observer: A second-order sliding mode observer that can reconstruct unmodeled external impact forces and feedforward compensation within a finite time.
[0036] Scene labels: Working condition classifications (flat / slope / load / slippage / impact) generated from online identification results, supporting multiple label overlays, used to select and optimize the weight matrix.
[0037] Joint torque sensor: A joint torque sensor is a force / torque measuring device installed at the rotary joint of a robot to measure the actual torque or moment output by the joint.
[0038] Wheel encoder: A wheel encoder is an angular displacement / angular velocity sensor mounted on a wheel axle or drive motor to measure the wheel's rotational speed, angle, and direction of rotation.
[0039] Based on this, the present disclosure provides a control method, apparatus, device, computer-readable storage medium, and program product, and the specific technical solutions will be described below.
[0040] The solutions provided in this disclosure relate to control technology. The technical solutions of this disclosure will be described in detail below with reference to specific embodiments. These specific embodiments can be combined with each other, and the same or similar concepts or processes may not be described again in some embodiments. The embodiments of this disclosure will now be described with reference to the accompanying drawings.
[0041] See Figure 1 , Figure 1 A flowchart illustrating a control method provided in an embodiment of this disclosure is shown. This method can be executed by any electronic device, such as a robot. As an optional implementation, the method can be executed by a robot. For ease of description, in the following description of some optional embodiments, the execution by a robot will be used as an example. Figure 1 As shown, the control method provided in this embodiment includes the following steps: S201, Obtain the robot's measurement data.
[0042] Specifically, for example, the robot's measurement data includes the direction of gravity in the body coordinate system, the body attitude rotation matrix, the robot's actual generalized momentum, the robot's observed generalized momentum, and the robot's wheel encoder data, etc., as measured by the robot's inertial measurement unit; among which, the wheel encoder data includes: the robot's rotation angle, angular velocity, direction, number of pulses, wheel linear velocity, longitudinal velocity component, lateral velocity component, chassis generalized velocity, etc.
[0043] S202, process the measurement data to determine the robot's estimated data, which includes at least one of the following: the robot's local terrain slope angle, the robot's load mass change, the robot's center of gravity offset, the robot's wheel-ground slip parameters, and scene labels.
[0044] Specifically, for example, the estimated data includes the robot's local terrain slope angle, the robot's load mass variation, the robot's center of gravity offset, the robot's wheel-ground slip parameters, scene labels, etc.
[0045] For example, a five-layer integrated anti-rollover control architecture includes: (1) Layer 0 (Perception and Identification Layer): Identify terrain slope, load parameters and wheel-ground friction coefficient online; (2) Layer 1 (Scene Adaptive Prediction Layer): Based on the identification results, in Constructing dynamically stable attraction domains on manifolds; (3) Second layer (variable weight convex optimization planning layer): The cost function weights are adaptively adjusted according to the scene label, using... The adaptive pseudospectral method is used to solve for the optimal control sequence. (4) The third layer (active momentum game execution layer): a virtual inertial force field is generated by the active tuned mass damper (ATMD) mechanism to cancel residual disturbances in real time; (5) The fourth layer (robust compensation layer): based on the super-helical sliding mode disturbance observer to compensate for unmodeled external shocks.
[0046] It should be noted that the five-layer progressive fusion control architecture forms a closed-loop progressive control chain of perception, prediction, planning, game theory, and compensation. The five layers are: Layer 0 (perception and identification layer), Layer 1 (scene adaptive prediction layer, i.e., dynamic attraction domain prediction layer), Layer 2 (variable weight convex optimization planning layer, i.e., variable weight pseudospectral method planning layer), Layer 3 (active momentum game execution layer), and Layer 4 (robust compensation layer, i.e., super-spiral sliding mode robust compensation layer). The five layers work together under a unified Lagrange dynamics framework, covering five types of instability conditions with a single control framework: flat rapid acceleration and deceleration, sloping terrain, sudden load change, slip steering, and external impact, without the need to switch control laws between different conditions.
[0047] S203, based on estimated data, determines the robot's effective support polygon and zero-moment point through scene adaptive prediction processing.
[0048] Specifically, for example, the effective support polygon is the projection of each grounding point onto a plane perpendicular to the direction of the equivalent gravity. For example, the zero moment point ZMP is the point on the ground that makes the resultant moment in the horizontal direction zero, and the zero moment point ZMP being located within the support polygon is a necessary condition for anti-overturning stability.
[0049] S204. If the distance between the zero-moment point and the boundary of the effective support polygon is less than a preset distance threshold, the initial driving signal is determined through variable weight convex optimization programming.
[0050] Specifically, for example, the initial drive signal is an optimal control time sequence. For instance, the initial drive signal includes chassis drive (e.g., torque commands / acceleration commands / speed commands for each wheel) and rising column drive (e.g., the desired height trajectory, i.e., the optimal reference trajectory of the rising column on the second floor). ), dual-arm drive (e.g., the expected angle trajectory of each joint, i.e., the optimal reference trajectory of the dual arms in the second layer). )wait.
[0051] S205, based on the initial drive signal, performs active momentum game processing to determine the virtual inertial field parameters and virtual compensation torque, and obtains a drive command with active damping based on the virtual inertial field parameters, virtual compensation torque and initial drive signal.
[0052] Specifically, for example, a drive command with active damping includes a virtual inertial force field. Virtual compensation torque Initial drive signals, etc.; among which, the virtual inertial force field These are the parameters of the virtual inertial force field.
[0053] S206, based on the drive command with active damping, determines the sliding mode compensation torque through robust compensation processing, and feeds forward and superimposes the sliding mode compensation torque onto the drive command with active damping to obtain the compensated drive signal.
[0054] Specifically, for example, the robot includes an omnidirectional wheel chassis, a lifting column, and dual arms; the compensated drive signals include the final driving torque / acceleration commands for each wheel of the omnidirectional wheel chassis, the final desired height / driving force command for the lifting column, the final desired angle / torque commands for each joint of the dual arms, and compensation components (e.g., sliding mode compensation torque). (Mapping quantities on each degree of freedom), etc.
[0055] S207 drives the robot to move based on the compensated drive signal.
[0056] Specifically, for example, the robot body is driven to move based on the compensated drive signal.
[0057] In this embodiment, measurement data of the robot is acquired; the measurement data is processed to determine estimated data of the robot; based on the estimated data, the effective support polygon and zero-moment point of the robot are determined through scene adaptive prediction processing; if the distance between the boundary of the zero-moment point and the effective support polygon is less than a preset distance threshold, the initial drive signal is determined through variable weight convex optimization programming; based on the initial drive signal, the virtual inertial field parameters and virtual compensation torque are determined through active momentum game execution processing, and a drive command with active damping is obtained based on the virtual inertial field parameters, virtual compensation torque and initial drive signal; based on the drive command with active damping, the sliding mode compensation torque is determined through robust compensation processing, and the sliding mode compensation torque is fed forward and superimposed on the drive command with active damping to obtain the compensated drive signal; the robot is driven to move based on the compensated drive signal; thus, the efficiency of robot anti-tipping control is improved.
[0058] In one embodiment, the measurement data includes the gravity direction and body attitude rotation matrix in the body coordinate system measured by the robot's inertial measurement unit, and the estimated data includes the robot's local terrain slope angle. The measurement data is processed to determine the robot's estimated data, including: Based on the gravity direction and body attitude rotation matrix measured by the inertial measurement unit in the body coordinate system, the local slope angle of the robot is determined by the terrain local slope angle identification. The local slope angle of the terrain includes pitch slope angle and roll slope angle.
[0059] Specifically, for example, unified modeling of system dynamics includes: (1) Generalized coordinates and coordinate systems.
[0060] For example, defining the system's generalized coordinates As shown in formula (1): Formula (1) in, For the chassis in the inertial frame The horizontal position in the middle, The chassis heading angle, This refers to the height of the bollard. The angles of each joint in both arms. Let n represent the total number of joint angles of both arms, and n represent the generalized coordinate dimension of the system. Body coordinate system. Fixed to the geometric center of the chassis, body coordinate system relative inertial frame The pose is determined by the rotation matrix. describe, This represents the rotation matrix from the body coordinate system B to the inertial system W.
[0061] (2) Gravity term correction including terrain slope.
[0062] For example, let the local slope angle of the terrain be... (Looking up and down) and (Roll), then the gravitational vector in the inertial frame of reference... After rotation, it is represented in the body coordinate system as follows: As shown in formula (2): Formula (2) in, ; Indicates the local elevation slope angle of the terrain. Indicates the local roll slope angle of the terrain. This represents the gravitational vector in the inertial frame W. Indicates the magnitude of gravitational acceleration. This represents the gravity vector in the body coordinate system B. This represents the rotation matrix from the inertial frame W to the body coordinate system B. This represents the rotation matrix from the body coordinate system B to the inertial system W.
[0063] The system potential energy is correspondingly corrected to As shown in formula (3): Formula (3) in, and The first The mass and centroid position vector of each component This represents the gravity vector in the body coordinate system B. As can be seen from formula (3), the slope is determined by... Directly altering the generalized gravity term renders the stability margin applicable on flat ground no longer valid on slopes, which is why it is necessary to explicitly introduce slope identification.
[0064] (3) Kinematic corrections for omnidirectional wheels with slippage.
[0065] For example, consider wheel-to-ground longitudinal slip ratio and lateral slip angle , No. The actual speed of each wheel Compared with theoretical speed The relationship is shown in formula (4): Formula (4) Among them, the slip deviation term is , i represents the wheel number, for example i=1,2,3,4; This represents the actual speed of the i-th wheel. This represents the theoretical velocity calculated from ideal kinematics or the ideal Jacobian matrix. This represents the longitudinal slip ratio of the i-th wheel. This represents the lateral slip angle of the i-th wheel. Let represent the longitudinal velocity component at the contact point of the i-th wheel. This represents the lateral velocity component at the contact point of the i-th wheel.
[0066] Modified Jacobian matrix with slip compensation As shown in formula (5): Formula (5) in, Represents the ideal Jacobian matrix under no-slip conditions. This represents the Jacobian correction caused by slippage; This represents the longitudinal slip ratio vector, including each wheel. ; This represents the lateral slip angle vector, including each wheel. .
[0067] Jacobi correction Based on the coefficient of friction between wheels and the ground Normal load And the Pacejka magic formula is determined, as shown in formula (6): Formula (6) in, This represents the longitudinal tire force of the i-th wheel. Multiplying it by the tire radius will give the corresponding driving torque. This represents the longitudinal slip ratio of the i-th wheel. This represents the coefficient of friction between the i-th wheel and the ground. This represents the normal load on the i-th wheel; These are tire characteristic parameters. By replacing the ideal Jacobian matrix with the modified Jacobian matrix, the chassis acceleration command output by the planning layer can be converted into the actual achievable wheel force, avoiding instability caused by the actual acceleration deviating from the expected value due to slippage.
[0068] (4) Unify the Lagrange dynamics equations.
[0069] For example, the total kinetic energy of the system As shown in formula (7): Formula (7) in, The variable inertia matrix includes the load. and The first The moment of inertia and angular velocity of each wheel Represents a generalized coordinate vector. This represents the generalized velocity vector.
[0070] Considering external impact force (Actions acting on the robot's end effector or body) and wheel drive torque The Lagrange equation is shown in formula (8): Formula (8) in, This represents the variable inertia matrix including the load. Represents the generalized acceleration vector. For the Coriolis / centrifugal force matrix, Represents Coriolis force / centrifugal force. For generalized gravity, Assign a matrix to drive the process. Let Jacobian matrix be the point of application of the external force. For damping / friction, This refers to the external impact force / torque acting on the end effector or body of the robotic arm. This represents the generalized force after the external impact is mapped to generalized coordinates. Formula (8) is the unified controlled object model shared by the subsequent five layers (layer 0, layer 1, layer 2, layer 3 and layer 4): slope Enter, load through Enter, slide through and Enter, impact This allows for the unified representation of multi-source disturbances.
[0071] For example, Layer 0, online multi-source disturbance identification (sensory identification processing), includes: (1) Topographic slope identification.
[0072] For example, the direction of gravity measured by an IMU (Inertial Measurement Unit). With body posture Estimate the local slope as shown in formula (9): Formula (9) in, This represents the estimated slope of a local terrain feature. This represents the estimated local roll slope of the terrain. Let represent the rotation matrix from the body coordinate system B to the inertial frame W. This represents the estimated direction of gravity or gravity vector in the body coordinate system measured by the IMU. Indicates the magnitude of the gravity vector; This indicates that the slope angle is solved by inversely from the horizontal component of the gravity direction. This identification relies only on the airborne IMU and attitude estimation, without the need for additional terrain sensors, and the output is directly used for Equations (2), (12), and the second layer weight selection.
[0073] (2) Load parameter identification.
[0074] For example, based on generalized momentum Construct a momentum observer as shown in Equation (10), and estimate the change in inertia caused by the load mass using Equation (10). and centroid shift caused by load : Formula (10) in, It represents the generalized momentum of the system. This represents the estimate of generalized momentum. The derivative representing the momentum estimate. The term represents the generalized driving force or driving torque, C represents the Coriolis / centrifugal force matrix, and G represents the generalized gravity term. This is the observer gain matrix. The inertia matrix is also shown when a load change is detected. Instant updates This ensures that the model used in subsequent layers is consistent with the current load state.
[0075] (3) Scene pattern classifier.
[0076] For example, based on feature groups The current operating conditions are divided into five scenario labels: flat operating conditions, slope operating conditions, load operating conditions, slip operating conditions, and impact operating conditions. It also supports multiple tag overlays (e.g., simultaneously under ramp and load conditions); among which, Indicates a flat working condition. Indicates slope working conditions. Indicates the load condition. Indicates the slip condition. This indicates an impact condition. Scene labels are scheduling signals that run through five layers: the first layer selects the shape of the attraction domain through scene labels, the second layer selects the weight matrix through scene labels, and the third layer determines whether to cancel or yield through scene labels.
[0077] For example, the direction of gravity in the body coordinate system measured by an inertial measurement unit. and body posture rotation matrix By identifying the local slope angle of the terrain, the robot's local slope angle is determined. The local slope angle includes the pitch slope angle. and roll slope angle .
[0078] For example, online identification of multi-source disturbances: by using IMU, joint torque sensor and wheel encoder, the terrain slope angle, load parameters (load mass and centroid offset), wheel-ground longitudinal slip ratio and lateral slip angle are estimated in real time, scene labels are generated and the inertia matrix is updated; corresponding to formula (9) and formula (10).
[0079] It should be noted that the online identification of multi-source disturbances and generation of scene labels includes: online identification of terrain slope angle, load mass and center of gravity offset, wheel-ground longitudinal slip ratio and lateral slip angle through IMU, joint torque sensor and wheel encoder; generation of scene labels that can be superimposed with multiple labels, and real-time updating of the system's variable inertia matrix including load; terrain slope is solved by the coupling relationship between IMU gravity vector and body attitude rotation matrix; load parameters are estimated by generalized momentum observer; wheel-ground mechanics are characterized by Pacejka magic formula and used to correct the omnidirectional wheel kinematic Jacobian matrix.
[0080] In one embodiment, the measurement data includes the robot's actual generalized momentum and observed generalized momentum, and the estimation data includes the robot's load mass change and center of mass shift. The measurement data is processed to determine the robot's estimated data, including: Based on the residual between the actual generalized momentum and the observed generalized momentum, the load mass change and centroid shift are determined by identifying the load parameters.
[0081] Specifically, for example, based on generalized momentum Construct a momentum observer as shown in Equation (10), and estimate the change in inertia caused by the load mass using Equation (10). and centroid shift caused by load : Formula (10) in, It represents the system's generalized momentum (actual generalized momentum). This represents the estimated value of generalized momentum (observed generalized momentum). The derivative of the generalized momentum estimate (the derivative of the observed generalized momentum). The term represents the generalized driving force or driving torque, C represents the Coriolis / centrifugal force matrix, and G represents the generalized gravity term. This is the observer gain matrix. The inertia matrix is also shown when a load change is detected. Instant updates This ensures that the model used in subsequent layers is consistent with the current load state.
[0082] For example, based on actual generalized momentum and observation of generalized momentum The residuals between them are identified through load parameters to determine the load quality variation. and centroid offset .
[0083] In one embodiment, the measurement data includes the robot's wheel encoder data, and the estimation data includes the robot's wheel-to-ground slip parameters; The measurement data is processed to determine the robot's estimated data, including: Based on wheel encoder data, wheel-ground slip parameters are identified and determined through wheel-ground slip parameter identification. The wheel-ground slip parameters include the longitudinal slip rate and lateral slip angle. The wheel-ground slip parameters are used to construct a modified Jacobian matrix that includes slip compensation.
[0084] Specifically, for example, considering the wheel-to-ground longitudinal slip ratio and lateral slip angle , No. The actual speed of each wheel Compared with theoretical speed The relationship is shown in formula (4): Formula (4) Among them, the slip deviation term is , i represents the wheel number, for example i=1,2,3,4; This represents the actual speed of the i-th wheel. This represents the theoretical velocity calculated from ideal kinematics or the ideal Jacobian matrix. This represents the longitudinal slip ratio of the i-th wheel. This represents the lateral slip angle of the i-th wheel. Let represent the longitudinal velocity component at the contact point of the i-th wheel. This represents the lateral velocity component at the contact point of the i-th wheel.
[0085] Modified Jacobian matrix with slip compensation As shown in formula (5): Formula (5) in, Represents the ideal Jacobian matrix under no-slip conditions. This represents the Jacobian correction caused by slippage; This represents the longitudinal slip ratio vector, including each wheel. ; This represents the lateral slip angle vector, including each wheel. .
[0086] For example, wheel encoder data includes the first The actual speed of each wheel Compared with theoretical speed Based on wheel encoder data, wheel-ground slip parameters are identified and determined. These parameters include the wheel-ground longitudinal slip ratio. and lateral slip angle Wheel-to-ground slip parameters are used to construct the modified Jacobian matrix, which includes slip compensation. .
[0087] In one embodiment, the estimated data includes at least one scenario label, which includes at least one of flat conditions, slope conditions, load conditions, slip conditions, and impact conditions.
[0088] Specifically, for example, based on feature groups The current operating conditions are divided into five scenario labels: flat operating conditions, slope operating conditions, load operating conditions, slip operating conditions, and impact operating conditions. It also supports multiple tag overlays (e.g., simultaneously under ramp and load conditions); among which, Indicates a flat working condition. Indicates slope working conditions. Indicates the load condition. Indicates the slip condition. This indicates an impact condition. Scene labels are scheduling signals that run through five layers: the first layer selects the shape of the attraction domain through scene labels, the second layer selects the weight matrix through scene labels, and the third layer determines whether to cancel or yield through scene labels.
[0089] In one embodiment, based on estimated data, the effective support polygon of the robot is determined through scene adaptive prediction processing, including: By projecting the ground contact points of each wheel of the robot onto a plane perpendicular to the direction of the equivalent gravity, multiple effective support points are obtained. Based on multiple effective support points, construct the robot's effective support polygon.
[0090] Specifically, for example, in the first layer, the dynamic attraction domain prediction on the SE(3) manifold (scene adaptive prediction processing) includes: (1) Supporting polygon and terrain projection.
[0091] For example, the coordinates of each wheel's grounding point in this system are: ; on the slope Below, the effective supporting polygon is the projection of each grounding point onto a plane perpendicular to the direction of the equivalent gravity, as shown in formula (11): Formula (11) in, This represents the coordinates of the i-th wheel contact point in the body coordinate system; This indicates the equivalent gravity direction in the body coordinate system. Indicates the effective support points after projection. Indicates by each The resulting effective supporting polygon.
[0092] Effectively supports the half-width of polygons and half-length The slope decreases adaptively, as shown in formula (12): Formula (12) in, Indicates the effective support half-width. Indicates effective support half-length. Indicates the local elevation slope angle of the terrain. Indicates the local roll slope angle of the terrain. These are the nominal support half-width and nominal support half-length on the horizontal plane, respectively. The height is the system's center of gravity. It is evident that the higher the center of gravity and the steeper the slope, the more severe the contraction of the effective support area. This directly explains the high risk of the slope condition when the bollard is raised.
[0093] (2) Extended ZMP stability criterion.
[0094] For example, on a non-horizontal surface, the definition of ZMP (zero moment point) is extended as shown in equation (13): Formula (13) in, Indicates the point of zero torque. Indicates the mass of the i-th component. This indicates the position of the centroid of the i-th component. This represents the acceleration of the center of mass of the i-th component. Represents the gravity vector. This represents the vertical acceleration component of the i-th component. Indicates the equivalent gravitational direction component. Let j be the j-th external torque.
[0095] The stability condition is that ZMP lies inside the effective supporting polygon, as shown in formula (14): Formula (14) in, Indicates the point of zero torque. Indicates by The resulting effective supporting polygon.
[0096] It should be noted that the difference from the traditional ZMP criterion is that the denominator of formula (13) uses the component along the equivalent gravity direction. ,and The projection results of formulas (11) and (12) are given, and therefore still hold true on slopes.
[0097] (3) Lyapunov attraction domain.
[0098] For example, defining explicit inclusion of scene factors Extended Lyapunov candidate functions As shown in formula (15): Formula (15) in, Indicates the system status. This represents the variable inertia matrix including the load. This represents state velocity or generalized velocity. Indicates the expectation of ZMP, This represents the positive definite weight matrix of the ZMP bias. Represents the observed external force value. This represents the positive definite weight matrix of the observed external force intensity. The current stable angle, The minimum allowable stable angle threshold, and It is a positive definite weight matrix. Kinetic energy was also measured. ZMP deviation Stability margin and external force observation intensity Four risks, etc.
[0099] right Differentiate and apply the contraction theory; if it exists... Satisfying formula (16): Formula (16) in, This represents the derivative of the Lyapunov function. This represents the shrinkage rate parameter. Represents the residual of external force observation. Indicates a real external force. Indicates an estimate of external forces; To observe the residuals, If it is a positive constant, then the system state trajectory is in In the norm sense, the exponential convergence to the safe manifold, and the corresponding safe feasible region is denoted as . When ZMP is at the boundary of the effective supporting polygon The distance is less than the threshold ,Right now When this happens, a global replanning is triggered, transferring control to the second layer. The key here is prediction: the criterion should trigger an alert before the ZMP actually goes out of bounds, rather than reacting passively after it goes out of bounds.
[0100] For example, the grounding points of each wheel of the robot Projecting onto a plane perpendicular to the direction of equivalent gravity yields multiple effective support points. Based on multiple effective support points Constructing an effective support polygon for the robot .
[0101] For example, dynamic attraction domain prediction: construct an extended ZMP stability criterion with scene factors and a Lyapunov candidate function, and determine whether the system is in a stable attraction domain based on the contraction theory. If it deviates, reprogramming is triggered; corresponding to formulas (13) to (16).
[0102] It should be noted that the dynamic attraction domain criterion with scene factors on the SE(3) manifold includes: projecting the effective support polygon along the equivalent gravity direction to obtain the effective support half-width and half-length that shrink adaptively with the slope; on this basis, extending the ZMP criterion to non-horizontal support surfaces and constructing a Lyapunov candidate function that explicitly includes scene factors (simultaneously incorporating the kinetic energy term, ZMP deviation term, stability margin term, and external force observation term); applying the contraction theory to give the exponential convergence condition, thereby determining whether the system is in a stable attraction domain, and triggering global replanning when ZMP approaches the boundary of the support polygon.
[0103] In one embodiment, the initial driving signal is determined through variable-weight convex optimization programming, including: Obtain available decision-making time; If the available decision time is greater than or equal to the preset time threshold, the optimal control time series is determined through variable weight convex optimization programming, and the optimal control time series is used as the initial driving signal.
[0104] Specifically, for example, the second layer, the variable-weight hp adaptive pseudospectral convex optimization programming (variable-weight convex optimization programming processing), includes: (1) Formulation of the scenario adaptive optimal control problem.
[0105] For example, in the finite time domain Within this framework, solve the following optimal control problem (OCP), as shown in equation (17): Formula (17) Where u represents the control input, This represents the scenario-specific optimal control cost. Indicates the terminal cost. Indicates the operating cost. Represents the state trajectory. Indicates the control trajectory. Indicates the initial time. Indicates the terminal time.
[0106] Terminal cost Operating costs All depend on scene tags As shown in formulas (18) and (19): Formula (18) in, Indicates the terminal status. Indicates a reference state. This represents the terminal weight matrix related to the scene.
[0107] Formula (19) in, Indicates the current state. Indicates a reference state. This represents the scene-related state weight matrix, where u represents the control input. This represents the scene-dependent control weight matrix. Indicates the point of zero torque. Indicates by The effective supporting polygon formed This is a ZMP out-of-bounds penalty item. The tipping penalty weights vary depending on the scene. For example, the scene-adaptive weight matrix configuration is shown in Table 1, with different scene labels corresponding to different weight matrix combinations.
[0108] Table 1: Scene Adaptive Weight Matrix Configuration
[0109] (2) Differential flatness and convex transformation.
[0110] For example, omnidirectional chassis systems have differential flatness, and flat output takes There exists a mapping, as shown in equations (20) and (21): Formula (20) Formula (21) in, This indicates a flat output, meaning the chassis is horizontal and the heading angle is [not specified]. : Represents the derivatives of the flat output. This represents a scene-related state mapping. This represents a scene-dependent control mapping. Using this mapping, nonlinear dynamic constraints are transformed into linear / convex constraints in a flat space, thus making the originally non-convex optimization problem solvable in real time.
[0111] (3) HP adaptive LGR pseudospectral discretization.
[0112] For example, in the normalized time domain Above, using the Legendre-Gauss-Radau (LGR) orthogonal collocation points, the collocation point set is as follows: ,in .state With control quantity The approximation is made using Lagrange interpolation, as shown in equation (22): Formula (22) in, Indicates normalized time. Indicates the number of points allocated, Indicates the first The state at each point. This indicates control at the k-th coordinate point. This represents the basis function for Lagrange interpolation.
[0113] The dynamic differential constraints are discretized into algebraic equations at the collocation points, as shown in equation (23): Formula (23) in, Indicates the first The state at each point. This represents the value of the derivative of the Lagrange basis function at the collocation point. Represents the scene-dependent dynamics function. This represents the state and control at the j-th coordinate point; Indicates the length of the time domain. Represents the distribution point index, .
[0114] HP adaptive strategy: in interval Internal estimation residuals .like Then increase the number of points within that interval ( Refinement, i.e., increasing the polynomial order or the number of collocations, or subdividing the interval ( Refinement (i.e., subdividing the time interval) until convergence. The hp adaptive strategy concentrates computational resources on the interval of dramatic changes in trajectory curvature, ensuring discretization accuracy without increasing the overall number of points; among which, This indicates the residual tolerance.
[0115] It should be noted that, The adaptive LGR pseudospectral method estimates interval residuals. Dynamically adjust the number of points Or interval subdivision until satisfied This is to ensure the accuracy of discretization.
[0116] Finally, the OCP is transformed into an NLP (Nonlinear Programming) problem, as shown in Equation (24): Formula (24) in, This represents the cost after discretization. This represents discrete states and control. Constraints include equations (14) and (23), as well as box constraints for state and control variables. and ;in, and Indicates the upper and lower limits of the state; and This represents the upper and lower control limits. It is solved in real-time using Sequential Quadratic Programming (SQP) or the Interior Point Method (IPOPT), with a solution period of 5ms-20ms, outputting the optimal control time series. .
[0117] For example, if the available decision time is greater than or equal to a preset time threshold of 5ms, then the optimal control time series is determined through variable-weight convex optimization programming. And the optimal control time series As the initial driving signal.
[0118] For example, the variable-weight hp adaptive pseudospectral method for convex optimization planning: Based on scene labels, the weight matrix of the cost function of the optimal control problem is adaptively adjusted; the nonlinear problem is transformed into a convex problem using the differential flatness of the omnidirectional chassis; and then... The adaptive LGR pseudospectral method is discretized into an NLP problem to solve the optimal control time series; corresponding to formulas (17) to (24).
[0119] It should be noted that the scene-adaptive variable weight hp adaptive LGR pseudospectral method convex optimization programming includes: adaptively adjusting the state weight matrix, control weight matrix, and overturning penalty coefficient of the optimal control problem according to the scene label; transforming nonlinear dynamic constraints into convex constraints in a flat space through the differential flatness of the omnidirectional chassis; using Legendre-Gauss-Radau orthogonal collocation discretization, and driving p refinement (increasing the number of collocation points) or h refinement (interval subdivision) with interval residual estimation, and finally transforming it into an NLP problem that can be solved in real time within a period of 5 ms-20 ms by SQP or interior point method.
[0120] In one embodiment, if the available decision time is less than a time threshold, an emergency soft landing process is performed by calling a preset limit boundary soft landing library to obtain an emergency command for the minimum damage trajectory, and the emergency command for the minimum damage trajectory is used as the initial drive signal.
[0121] Specifically, for example, the target switching strategy under extreme conditions, when there is insufficient available decision time (less than 5ms of available decision time) under impact conditions, abandons complete replanning and instead calls the preset extreme boundary soft landing library to switch the control target from maintaining pose to minimum damage trajectory (minimum damage trajectory emergency command), and instructs ATMD to actively retreat in the impact direction, coordinating with the chassis to move in the same direction, thus transforming rigid impact into flexible retreat.
[0122] For example, if the scenario label is impact condition and the available decision time is less than 5ms, then complete replanning is abandoned, the preset limit boundary soft landing library is called, the optimization target is switched from maintaining pose to minimum damage trajectory, and the active tuning mass damper controls the two arms to retract in the direction of impact, and moves in the same direction as the chassis.
[0123] In one embodiment, based on the initial driving signal, processing is performed through active momentum game to determine the virtual inertial force field parameters and virtual compensation torque, including: Determine the robot's current actual overturning moment; Determine the power spectrum of the current actual overturning moment and extract the dominant frequency from the power spectrum; Based on the dominant frequency and optimal control time series, the virtual inertial force field parameters and virtual compensation torque generated by the robot's dual arms and lifting column motion are determined.
[0124] Specifically, for example, layer 3, Active Momentum Game Execution (ATMD mechanism, i.e., Active Momentum Game Execution Processing), includes: (1) Real-time decomposition of the overturning moment main frequency.
[0125] For example, calculating the current actual overturning moment. As shown in formula (25): Formula (25) in, Represents the centroid position vector. Represents the gravity vector. It is an inertial force. The total mass of the system is calculated using a sliding window FFT (Fast Fourier Transform). The power spectrum is shown in formula (26): Formula (26) in, This represents the overturning moment in the frequency domain. Indicates Fourier transform, Indicates the length of the sliding window, and the transform window takes... Extract dominant frequencies This serves as the operating frequency for subsequent phase compensation.
[0126] (2) Active damping control law for double arms / lifting columns.
[0127] For example, the frequency generated by each joint in both arms is Phase lag The periodic oscillation command (i.e., 180°) is shown in formula (27): Formula (27) in, Indicates the desired joint angle of both arms. Indicates the amplitude of the swing. Indicates the dominant frequency. This indicates a phase lag of 180°. This represents the optimal reference trajectory for the two arms in the second layer.
[0128] The rising and lowering columns move synchronously, as shown in formula (28): Formula (28) in, Indicates the desired height of the bollard. Indicates the swing amplitude of the rising column. Indicates nominal height, This represents the optimal reference trajectory for the rising column on the second floor. and The amplitude of the swing. and The optimal reference trajectory for the second layer is used; that is, the active damping command is a cyclic fine-tuning superimposed on the optimal trajectory, without destroying the global optimality of the second layer.
[0129] It should be noted that the commands for both arm joints... To dominate frequency And phase lag The periodic oscillations superimposed on the optimal reference trajectory of the second layer command for raising bollards For Cosine superposition at nominal height With the optimal reference trajectory of the second layer ; corresponding to formulas (27) and (28).
[0130] Virtual inertial force field generated by the movement of the arms and the lifting column As shown in formula (29): Formula (29) in, This represents the acceleration of the center of mass of both arms. This indicates the vertical acceleration of the rising column. and These are the equivalent masses of the two arms and the equivalent mass of the rising column, respectively. A unit vector in the vertical direction, representing a virtual inertial force field. These are the parameters of the virtual inertial force field. The resulting compensating torque (virtual compensating torque) As shown in formula (30): Formula (30) in, Indicates the lever arm of both arms. This indicates the force generated by both arms. Indicates the lever arm of the lifting column. This indicates the force generated by the rising bollard.
[0131] The equilibrium condition for momentum game is shown in equation (31): Formula (31) in, This indicates the compensation torque (virtual compensation torque). This represents the actual overturning moment. Represents residual moment and residual coefficient. satisfy That is, by actively generating a reverse force field, the overturning moment is actively offset to an acceptable residual range; compared with traditional passive damping, the third layer transforms the double arms and lifting columns from sources of risk into stabilizing actuators.
[0132] For example, determining the robot's current actual overturning moment. Determine the current actual overturning moment. power spectrum and from the power spectrum Extract dominant frequency Based on dominant frequency and optimal control time series Determine the virtual inertial force field generated by the movement of the robot's arms and lifting column. and virtual compensation torque Among them, the optimal control time series This includes chassis drive (e.g., torque commands / acceleration commands / speed commands for each wheel drive) and column drive (e.g., the desired height trajectory, i.e., the optimal reference trajectory for the column on the second floor). ), dual-arm drive (e.g., the expected angle trajectory of each joint, i.e., the optimal reference trajectory of the dual arms in the second layer). )wait.
[0133] For example, the momentum game execution of the active tuned mass damper is as follows: the real-time overturning moment is decomposed by FFT spectrum, the main frequency is extracted, and the double arms and lifting columns are controlled to generate a virtual inertial force field in opposite phase, actively offsetting the overturning moment and satisfying the momentum game equilibrium condition; corresponding to formulas (25) to (31).
[0134] It should be noted that the momentum game execution of the Active Tuned Mass Damper (ATMD) includes: performing sliding window FFT spectral decomposition on the real-time overturning moment and extracting the dominant frequency; causing each joint of the two arms and the lifting column to generate periodic swinging and lifting commands at the dominant frequency with a phase difference of 180°, thereby generating a virtual inertial force field and a reverse compensation torque to actively offset the overturning moment to an acceptable residual range; this mechanism actively converts destructive energy into stable energy, rather than passively resisting it.
[0135] In one embodiment, based on a drive command with active damping, the sliding mode compensation torque is determined through robust compensation processing, including: In response to the active damping drive command, a sliding surface is obtained, which is used to characterize the difference between the robot's actual generalized velocity and a reference generalized velocity; Based on the sliding surface, the external impact force of the robot is estimated by using a super-helical sliding mode observer; Based on the estimation of external impact force, the preset Jacobian matrix of the external force application point and the sliding surface, the sliding compensation torque is determined.
[0136] Specifically, for example, in layer 4, robust compensation for the superspiral sliding mode perturbation observer (robust compensation processing) includes: For example, for unmodeled dynamics and sudden external shocks. Design a super-twisting sliding mode observer. Sliding mode surface. Defined as the speed tracking error, as shown in formula (32): Formula (32) in, Indicates the actual generalized speed. This refers to the generalized velocity.
[0137] The observer dynamics are shown in equation (33): Formula (33) in, This indicates an estimate of external shocks. This represents the derivative of the external shock estimate. Indicates the sliding surface. Represents a symbolic function. This is the observer gain.
[0138] Finite-time convergence proof: Take the Lyapunov function The auxiliary variable is taken as It can be proven that a positive definite matrix exists. Make That is, the observation error within a finite time It converges to zero, and the convergence time satisfies ;in, Indicates the convergence gain. This represents the initial Lyapunov value. Finite-time convergence is a necessary condition for dealing with transient shocks, compared to conventional observers that can only guarantee asymptotic convergence.
[0139] Sliding mode compensation torque As shown in formula (34): Formula (34) in, This indicates an estimate of external shocks. Indicates the sliding surface. Represents a symbolic function. Here is the sliding mode gain matrix. Let Jacobian matrix be the point of application of the external force. The sliding mode compensation torque is... As a feedforward compensation superimposed on the driving signal (including the driving command of active damping) of the third layer, it ensures that the impact force is effectively suppressed within 5ms.
[0140] For example, based on sliding surfaces The external impact force of the robot is estimated by using a superhelical sliding mode observer. Based on external impact force estimation Preset Jacobian matrix of the point of application of external force and sliding surface Determine the sliding mode compensation torque .
[0141] For example, robust compensation of superhelical sliding mode (robust compensation of superhelical sliding mode disturbance observer): design a superhelical sliding mode disturbance observer, reconstruct the external impact force within a finite time, feed forward the compensation torque composed of the observation value to the drive command with active damping, obtain the compensated drive signal, and realize the suppression of non-cooperative disturbance; corresponding to formulas (32) to (34).
[0142] It should be noted that the robust compensation of the super-twisting sliding mode disturbance observer includes: using the velocity tracking error as the sliding surface, designing a super-twisting sliding mode disturbance observer to reconstruct the unmodeled dynamics and sudden external impact forces, and providing a Lyapunov proof of finite-time convergence; feeding forward the compensation torque composed of the observation values to the drive command with active damping to obtain the compensated drive signal, thereby suppressing non-cooperative disturbances.
[0143] In one embodiment, for example, the five-layer control architecture coordination table is shown in Table 2.
[0144] Table 2: Five-Layer Control Architecture Coordination Table
[0145] It should be noted that the closed-loop process includes: layer 0 perception, layer 1 judgment of whether the boundary is exceeded, if the boundary is exceeded, then layer 2 replanning, layer 3 and layer 4 real-time fusion execution, and posture feedback back to layer 0 to form a closed loop.
[0146] In one embodiment, for example, an NLP solver alternative: the NLP problem obtained after the second-level discretization can be solved using either Sequential Quadratic Programming (SQP) or Interior Point Method (IPOPT), and the two can be substituted for each other.
[0147] In one embodiment, for example, a mesh refinement method can be used instead: when the interval residual exceeds a threshold, the number of matching points within that interval can be increased. (Refine), or you can choose to subdivide the interval ( (Refinement), the two refinement paths can be used alone or in combination.
[0148] In one embodiment, for example, an alternative approach to handling shock conditions is adopted: a full replanning is used when there is sufficient time; and a pre-defined limit boundary soft landing library is used when there is insufficient available time (e.g., less than 5 ms). These two approaches constitute two implementation paths under the same control objective.
[0149] In one embodiment, for example, the actuator combination can be used instead: the third layer of virtual inertial force field can be generated by the swing of the two arms and the lifting of the lifting column (e.g., formula (29)), or the corresponding component can be generated by only one of the actuators when the actuator is restricted.
[0150] In one embodiment, for example, a computing platform alternative: the modules in the system (e.g., an anti-tipping control system for an omnidirectional mobile robot) can be implemented in parallel using embedded GPUs (Graphics Processing Units), or FPGAs (Field-Programmable Gate Arrays), or a heterogeneous combination of both.
[0151] In one embodiment, for example, the anti-tipping control system of an omnidirectional mobile robot includes: a perception and identification module for online identification of terrain slope, load parameters, wheel-to-ground slip ratio, etc., and generating scene labels; a safety domain prediction module for calculating the stable attraction domain containing scene factors and outputting a replanning trigger signal; a variable weight planning module for solving the scene-adaptive optimal control sequence; a momentum game execution module for controlling the dual arms and the lifting column to generate an anti-phase virtual inertial force field; a robust compensation module for reconstructing and feedforward compensating for external impact forces; and a high-speed real-time communication bus connecting the various modules.
[0152] For example, in the anti-tipping control system of an omnidirectional mobile robot, each module is implemented in parallel operation using an embedded GPU / FPGA heterogeneous computing platform, with a total control cycle of no more than 20ms. Among them, the cycle of the perception and identification module is 10ms-20ms, the cycle of the safety domain prediction module is 5ms-10ms, the cycle of the variable weight planning module is 5ms-20ms, the cycle of the momentum game execution module is 1ms-5ms, and the cycle of the robust compensation module is less than 1ms.
[0153] Applying the embodiments disclosed herein has at least the following beneficial effects: (1) Full operating condition coverage: The unified framework covers five typical instability scenarios: flat ground, slope, under load, slip, and impact, without the need to switch control laws. Compared with the existing schemes that require separate calibration of controllers for each type of operating condition, this significantly reduces the risk of control discontinuity when switching between engineering calibration values and operating conditions.
[0154] (2) Strict stability guarantee: Based on the attraction domain criterion of contraction theory and the finite-time convergence proof of the supercoiled observer, it provides a mathematically absolute safety boundary, rather than relying on empirical threshold tuning.
[0155] (3) Balance between optimality and real-time performance: The adaptive pseudospectral method, while ensuring global optimality, achieves online solution in the range of 5 ms to 20 ms by using scene-adaptive weight dimensionality reduction.
[0156] (4) Active Energy Domain Game: The original ATMD mechanism transforms destructive energy into stable energy instead of passively resisting it, thus significantly reducing energy consumption.
[0157] (5) Strong robustness: The super-helical sliding mode observer is invariant to unmodeled dynamics and external shocks, and its anti-interference ability is several times higher than that of traditional PID.
[0158] To better understand the methods provided in the embodiments of this disclosure, the solutions of the embodiments of this disclosure will be further explained below with reference to specific application scenarios.
[0159] In one embodiment, the method provided by this disclosure belongs to the field of robot motion control and intelligent decision-making technology. The method provided by this disclosure is applied to the anti-tipping control of a humanoid upper body omnidirectional mobile robot with a lifting column and two arms. It is particularly suitable for complex environments with all working conditions, including unstructured terrain, dynamic loads, nonlinear wheel-ground slippage, and sudden external impacts.
[0160] For example, the method provided in this disclosure is applied to the anti-tipping control of a composite mobile operating robot with a humanoid upper body (lifting column and two arms) and a chassis with an X-shaped layout of four omnidirectional wheels.
[0161] In one embodiment, the method provided in this disclosure is applied to emergency braking on flat ground and double-arm extended load, so that the ZMP remains within the support polygon during emergency braking on flat ground.
[0162] Operating conditions: The robot moves forward at 1.5 m / s, the lifting column rises to its highest point (1.2 m), the two arms extend forward to carry a 2 kg load, and an emergency stop command is received.
[0163] The execution process includes: (1) Layer 0 identification: , Identify load kg, centroid offset The scene is labeled as flat and overloaded.
[0164] (2) Calculation of the first layer: m, the ZMP boundary shrinks, the attraction domain shrinks by 15% compared to the unloaded state, triggering replanning.
[0165] (3) Second layer: Take the weights corresponding to the flat and load superposition conditions (centroid weight increase), solve NLP, and the chassis deceleration in the output optimal sequence is m / s² (non-limiting value) (m / s²), the lifting column descends 0.15 m, and the arms swing backward 15° to compensate for the forward tilting moment.
[0166] (4) Layer 3: FFT detects residual 1.2 Hz vibration, and ATMD fine-tunes the two arms with 1.2 Hz phase inversion.
[0167] (5) Layer 4: No significant impact, output is 0. The robot eventually stops smoothly, and ZMP remains within the supporting polygon.
[0168] In one embodiment, the method provided in this disclosure is applied to rapid acceleration and lateral drift on a 15° slope, enabling a safe start-up at an incline even when the effective support width on the 15° slope is only 0.086 m.
[0169] Operating conditions: The robot is stationary on a 15° slope, tilted to the side in the roll direction, and executes a command to accelerate rapidly along the slope.
[0170] The execution process includes: (1) Layer 0 identification: , The effective support width due to gravity projection is calculated according to formula (12) as follows: m, the boundary is extremely contracted.
[0171] (2) First layer: minimum allowable stability angle The attraction field decreases sharply, enters a critical state, and the second layer is triggered urgently.
[0172] (3) Layer 2: The scene label is a combination of slope and slip, with a weighted overturning penalty. Set to the maximum value (heaviest overturning penalty), control the weight. Given a height (limiting torque saturation), the peak acceleration of the optimal acceleration curve is only 0.5 m / s², and the torque is differentially distributed among the four wheels to resist rollover.
[0173] (4) 3rd floor: The lifting column actively descends to the lowest position (0.5 m), and the two arms extend towards the top of the slope to shift the center of mass.
[0174] (5) Layer 4: The sliding mode observer detects a small slippage of the wheel and outputs a compensating torque to limit slippage. Finally, the robot completes the diagonal start at a safe speed.
[0175] In one embodiment, the method provided in this disclosure is applied to high-speed omnidirectional traversal and sudden obstacle collisions; achieving the following: when subjected to a 50 N impact pulse, the observer reconstructs an external force of approximately 48 N within 3 ms, and the robot only moves backward by 0.1 m without overturning.
[0176] Operating condition: The robot is moving laterally to the right at a speed of 1.2 m / s. The robotic arm accidentally collides with an obstacle that suddenly appears on the left, generating an impact pulse. N.
[0177] The execution process includes: (1) Layer 0: The IMU detects a sudden deceleration, the load observer detects a sudden change in external force, and the scene label is switched to slip and impact superposition.
[0178] (2) First layer: External force term in formula (15) The suction field suddenly contracted to its limit, issuing an emergency deceleration command.
[0179] (3) Level 2: Due to time constraints (less than 5 ms), complete replanning is abandoned, and the preset limit boundary soft landing library is directly called. The new target becomes the minimum damage trajectory instead of maintaining pose.
[0180] (4) The third layer: ATMD no longer counteracts the overturning moment, but instead actively pulls the two arms back in the direction of the impact, and moves in the same direction as the chassis, turning the rigid impact into a flexible retreat.
[0181] (5) Layer 4: The superspiral observer reconstructs the image within 3 ms. N, compensation torque The drive chassis wheels generate acceleration in the same direction to dissipate the force. Ultimately, the robot moved backward 0.1m without overturning.
[0182] In a specific application scenario, such as a five-layer progressive anti-rollover control scenario, see [link to relevant documentation]. Figure 2 It demonstrates the complete control process from online identification of multi-source disturbances, scene label generation, attraction domain prediction, variable weight pseudospectral method planning, ATMD momentum game execution to super-spiral sliding mode compensation, including attraction domain over-limit judgment branch, limit boundary soft landing branch under impact conditions, and attitude feedback closed loop.
[0183] For example, such as Figure 2 As shown: Layer 0: Online identification of multi-source disturbances.
[0184] By fusing multiple sensors (IMU / joint torque sensor / wheel encoder), local slope can be identified in real time. and Formula (9)), load parameters (load mass and centroid offset, Formula (10)), wheel-to-ground longitudinal slip ratio (Formulas (4)-(6)) etc., finally generating scene labels S (e.g., flat working conditions, slope working conditions, load working conditions, slip working conditions and impact working conditions, supporting multiple label overlay), and updating the inertia matrix M(q) to This is for use by subsequent layers.
[0185] Layer 1: SE(3) dynamic attraction domain prediction.
[0186] The effective supporting polygon is projected along the direction of gravity (Formula (11)), and the half-width of the effective supporting polygon is... (Formula (12)); Calculate the extended ZMP (Formula (13)) and the extended Lyapunov candidate function. (Formula (15)).
[0187] Node 1: Does ZMP approximate the boundary of the effective supporting polygon (the boundary of the supporting polygon)? If ZMP approximates the boundary of the effective supporting polygon (e.g., the boundary between ZMP and the effective supporting polygon), then... The distance is less than the threshold If the ZMP does not approximate the boundary of the effective supporting polygon, it is located within the attraction domain and returns to the 3rd layer.
[0188] Judgment Node 2: Is the available decision time less than 5ms? If the available decision time is not less than 5ms, then there is enough time, and the system enters the standard planning stage of Level 2. If the available decision time is less than 5ms, then the system enters the extreme condition stage and proceeds to the extreme boundary soft landing stage of Level 2.
[0189] Layer 2: Scene-adaptive variable-weight convex optimization planning (variable-weight convex optimization planning processing).
[0190] Select the weight matrix W(S) (state weight matrix) according to the scene label. and control weight matrix Table 1); Convex transformation is performed using differential flatness (Equation (20) and Equation (21)); The hp adaptive LGR pseudospectral method is discretized into NLP (Equation (22) - Equation (24)); The optimal control sequence (optimal control time series) is solved by SQP / IPOPT.
[0191] Layer 2: Extreme Boundary Soft Landing Pipeline.
[0192] Call the extreme boundary soft landing library; switch to minimum damage trajectory: both arms retreat in the same direction as the impact and the chassis moves in the same direction.
[0193] Layer 3: ATMD Active Momentum Game Execution.
[0194] The current actual overturning moment is decomposed by FFT spectrum (Equation (25) and Equation (26)) to extract the dominant frequency. The arms swing in opposite phase to the lifting column (Equations (27) and (28)), generating a virtual inertial force field and a virtual compensation torque (Equations (29) and (30)).
[0195] Layer 4: Robust compensation for super-spiral sliding mode (robust compensation processing).
[0196] The super-helical sliding mode observer reconstructs the external impact force in a finite time (Equation (32) and Equation (33)); the sliding mode compensation torque is fed forward and superimposed on the drive signal (including the drive command with active damping).
[0197] Attitude feedback closed loop: return to layer 0; the entire control chain forms a closed loop, and the actual attitude, velocity, acceleration and other states after compensation at layer 4 are fed back to layer 0, multi-source disturbance identification is performed again, scene labels and inertia matrix are updated, and the next control cycle is entered; wherein, the control cycle is less than or equal to 20ms.
[0198] This disclosure also provides a control device, the structural schematic diagram of which is shown below. Figure 3As shown, the control device 40 includes an acquisition module 401, a perception and identification processing module 402, a scene adaptive prediction processing module 403, a variable weight convex optimization planning processing module 404, an active momentum game execution processing module 405, a robust compensation processing module 406, and a driving module 407.
[0199] The acquisition module 401 is used to acquire the robot's measurement data; The perception and identification processing module 402 is used to process the measurement data and determine the robot's estimated data. The estimated data includes at least one of the following: the robot's local terrain slope angle, the robot's load mass change, the robot's center of gravity offset, the robot's wheel-ground slip parameters, and scene labels. The scene adaptive prediction processing module 403 is used to determine the robot's effective support polygon and zero moment point based on the estimated data through scene adaptive prediction processing. The variable weight convex optimization planning processing module 404 is used to determine the initial driving signal through variable weight convex optimization planning processing if the distance between the zero moment point and the boundary of the effective support polygon is less than a preset distance threshold. The active momentum game execution processing module 405 is used to determine the virtual inertial field parameters and virtual compensation torque based on the initial driving signal through active momentum game execution processing, and to obtain the driving command with active damping based on the virtual inertial field parameters, virtual compensation torque and initial driving signal. The robust compensation processing module 406 is used to determine the sliding mode compensation torque based on the drive command with active damping through robust compensation processing, and feed forward and superimpose the sliding mode compensation torque onto the drive command with active damping to obtain the compensated drive signal. The drive module 407 is used to drive the robot to move based on the compensated drive signal.
[0200] In one embodiment, the measurement data includes the gravity direction and body attitude rotation matrix in the body coordinate system measured by the robot's inertial measurement unit, and the estimated data includes the robot's local terrain slope angle. When processing measurement data to determine the robot's estimated data, the perception and identification processing module 402 is specifically used for: Based on the gravity direction and body attitude rotation matrix measured by the inertial measurement unit in the body coordinate system, the local slope angle of the robot is determined by the terrain local slope angle identification. The local slope angle of the terrain includes pitch slope angle and roll slope angle.
[0201] In one embodiment, the measurement data includes the robot's actual generalized momentum and observed generalized momentum, and the estimation data includes the robot's load mass change and center of mass shift. When processing measurement data to determine the robot's estimated data, the perception and identification processing module 402 is specifically used for: Based on the residual between the actual generalized momentum and the observed generalized momentum, the load mass change and centroid shift are determined by identifying the load parameters.
[0202] In one embodiment, the measurement data includes the robot's wheel encoder data, and the estimation data includes the robot's wheel-to-ground slip parameters; When processing measurement data to determine the robot's estimated data, the perception and identification processing module 402 is specifically used for: Based on wheel encoder data, wheel-ground slip parameters are identified and determined through wheel-ground slip parameter identification. The wheel-ground slip parameters include the longitudinal slip rate and lateral slip angle. The wheel-ground slip parameters are used to construct a modified Jacobian matrix that includes slip compensation.
[0203] In one embodiment, the estimated data includes at least one scenario label, which includes at least one of flat conditions, slope conditions, load conditions, slip conditions, and impact conditions.
[0204] In one embodiment, when the scene adaptive prediction processing module 403 determines the effective support polygon of the robot based on the estimated data through scene adaptive prediction processing, it is specifically used for: By projecting the ground contact points of each wheel of the robot onto a plane perpendicular to the direction of the equivalent gravity, multiple effective support points are obtained. Based on multiple effective support points, construct the robot's effective support polygon.
[0205] In one embodiment, when determining the initial driving signal through variable-weight convex optimization programming, the variable-weight convex optimization programming module 404 is specifically used for: Obtain available decision-making time; If the available decision time is greater than or equal to the preset time threshold, the optimal control time series is determined through variable weight convex optimization programming, and the optimal control time series is used as the initial driving signal.
[0206] In one embodiment, the variable-weight convex optimization programming processing module 404 is further configured to: If the available decision time is less than the time threshold, an emergency soft landing process is performed by calling the preset limit boundary soft landing library to obtain the minimum damage trajectory emergency command, and the minimum damage trajectory emergency command is used as the initial drive signal.
[0207] In one embodiment, when the active momentum game execution processing module 405 determines the virtual inertial force field parameters and virtual compensation torque based on the initial driving signal through active momentum game execution processing, it is specifically used for: Determine the robot's current actual overturning moment; Determine the power spectrum of the current actual overturning moment and extract the dominant frequency from the power spectrum; Based on the dominant frequency and optimal control time series, the virtual inertial force field parameters and virtual compensation torque generated by the robot's dual arms and lifting column motion are determined.
[0208] In one embodiment, when the robust compensation processing module 406 determines the sliding mode compensation torque based on a drive command with active damping through robust compensation processing, it is specifically used for: In response to the active damping drive command, a sliding surface is obtained, which is used to characterize the difference between the robot's actual generalized velocity and a reference generalized velocity; Based on the sliding surface, the external impact force of the robot is estimated by using a super-helical sliding mode observer; Based on the estimation of external impact force, the preset Jacobian matrix of the external force application point and the sliding surface, the sliding compensation torque is determined.
[0209] Applying the embodiments disclosed herein has at least the following beneficial effects: The process involves acquiring measurement data from the robot, processing the data to determine estimated data, and then using scene adaptive prediction to determine the robot's effective support polygon and zero-moment point. If the distance between the zero-moment point and the boundary of the effective support polygon is less than a preset distance threshold, a variable-weight convex optimization programming process is used to determine the initial drive signal. Based on the initial drive signal, a virtual inertial field parameter and virtual compensation torque are determined through active momentum game execution. A drive command with active damping is then obtained based on the virtual inertial field parameter, virtual compensation torque, and the initial drive signal. Based on the drive command with active damping, a sliding mode compensation torque is determined through robust compensation processing, and this sliding mode compensation torque is fed forward and superimposed onto the drive command with active damping to obtain a compensated drive signal. The robot is then driven based on the compensated drive signal. This improves the efficiency of the robot's anti-tipping control.
[0210] This disclosure also provides an electronic device, the structural schematic diagram of which is shown below. Figure 4 As shown, Figure 4 The illustrated electronic device 4000 includes a processor 4001 and a memory 4003. The processor 4001 and the memory 4003 are connected, for example, via a bus 4002. Optionally, the electronic device 4000 may further include a transceiver 4004, which can be used for data interaction between the electronic device and other electronic devices, such as sending and / or receiving data. It should be noted that in practical applications, the transceiver 4004 is not limited to one type, and the structure of the electronic device 4000 does not constitute a limitation on the embodiments of this disclosure.
[0211] Processor 4001 may be a CPU (Central Processing Unit), a general-purpose processor, a DSP (Digital Signal Processor), an ASIC (Application Specific Integrated Circuit), an FPGA (Field Programmable Gate Array), or other programmable logic devices, transistor logic devices, hardware components, or any combination thereof. It may implement or execute the various exemplary logic blocks, modules, and circuits described in conjunction with this disclosure. Processor 4001 may also be a combination that implements computational functions, such as including one or more microprocessor combinations, a combination of a DSP and a microprocessor, etc.
[0212] Bus 4002 may include a pathway for transmitting information between the aforementioned components. Bus 4002 may be a PCI (Peripheral Component Interconnect) bus or an EISA (Extended Industry Standard Architecture) bus, etc. Bus 4002 can be divided into address bus, data bus, control bus, etc. For ease of representation, Figure 4 Figure 4 The bus is represented by a single thick line, but this does not mean that there is only one bus or one type of bus.
[0213] The memory 4003 may be ROM (Read Only Memory) or other types of static storage devices capable of storing static information and instructions, RAM (Random Access Memory) or other types of dynamic storage devices capable of storing information and instructions, or EEPROM (Electrically Erasable Programmable Read Only Memory), CD-ROM (Compact Disc Read Only Memory) or other optical disc storage, optical disc storage (including compressed optical discs, laser discs, optical discs, digital universal optical discs, Blu-ray discs, etc.), magnetic disk storage media, other magnetic storage devices, or any other medium capable of carrying or storing computer programs and capable of being read by a computer, without limitation herein.
[0214] The memory 4003 is used to store computer programs that execute embodiments of the present disclosure, and is controlled by the processor 4001 to execute them. The processor 4001 is used to execute the computer programs stored in the memory 4003 to implement the steps shown in the foregoing method embodiments.
[0215] Electronic devices include, but are not limited to, robots.
[0216] Applying the embodiments disclosed herein has at least the following beneficial effects: The process involves acquiring measurement data from the robot, processing the data to determine estimated data, and then using scene adaptive prediction to determine the robot's effective support polygon and zero-moment point. If the distance between the zero-moment point and the boundary of the effective support polygon is less than a preset distance threshold, a variable-weight convex optimization programming process is used to determine the initial drive signal. Based on the initial drive signal, a virtual inertial field parameter and virtual compensation torque are determined through active momentum game execution. A drive command with active damping is then obtained based on the virtual inertial field parameter, virtual compensation torque, and the initial drive signal. Based on the drive command with active damping, a sliding mode compensation torque is determined through robust compensation processing, and this sliding mode compensation torque is fed forward and superimposed onto the drive command with active damping to obtain a compensated drive signal. The robot is then driven based on the compensated drive signal. This improves the efficiency of the robot's anti-tipping control.
[0217] This disclosure provides a computer-readable storage medium storing a computer program, which, when executed by a processor, can implement the steps and corresponding content of the aforementioned method embodiments.
[0218] This disclosure also provides a computer program product, including a computer program that, when executed by a processor, can implement the steps and corresponding content of the aforementioned method embodiments.
[0219] It should be understood that although arrows indicate various operation steps in the flowcharts of the embodiments of this disclosure, the order in which these steps are implemented is not limited to the order indicated by the arrows. Unless explicitly stated herein, in some implementation scenarios of the embodiments of this disclosure, the implementation steps in each flowchart can be executed in other orders as required. Furthermore, some or all of the steps in each flowchart may include multiple sub-steps or multiple stages based on the actual implementation scenario. Some or all of these sub-steps or stages can be executed at the same time, and each sub-step or stage can also be executed at different times. In scenarios where execution times differ, the execution order of these sub-steps or stages can be flexibly configured as required, and the embodiments of this disclosure do not limit this.
[0220] The above description is only an optional implementation method for some implementation scenarios of this disclosure. It should be noted that for those skilled in the art, other similar implementation methods based on the technical concept of this disclosure without departing from the technical concept of this disclosure also fall within the protection scope of the embodiments of this disclosure.
Claims
1. A control method, characterized in that, include: Acquire measurement data from the robot; The measurement data is processed to determine the estimated data of the robot. The estimated data includes at least one of the following: the local slope angle of the terrain of the robot, the change in the load mass of the robot, the center of gravity offset of the robot, the wheel slip parameters of the robot, and the scene label. Based on the estimated data, the effective support polygon and zero moment point of the robot are determined through scene adaptive prediction processing. If the distance between the zero-moment point and the boundary of the effective support polygon is less than a preset distance threshold, the initial driving signal is determined through variable weight convex optimization programming. Based on the initial drive signal, the virtual inertial field parameters and virtual compensation torque are determined through active momentum game processing. Based on the virtual inertial field parameters, the virtual compensation torque, and the initial drive signal, a drive command with active damping is obtained. Based on the drive command with active damping, the sliding mode compensation torque is determined through robust compensation processing, and the sliding mode compensation torque is fed forward and superimposed onto the drive command with active damping to obtain the compensated drive signal. The robot is driven to move based on the compensated drive signal; The process of determining the initial driving signal through variable-weight convex optimization programming includes: Obtain available decision-making time; If the available decision time is greater than or equal to a preset time threshold, then the optimal control time sequence is determined through variable weight convex optimization programming, and the optimal control time sequence is used as the initial driving signal. The process of determining virtual inertial field parameters and virtual compensation torque based on the initial driving signal through active momentum game processing includes: Determine the current actual overturning moment of the robot; Determine the power spectrum of the current actual overturning moment, and extract the dominant frequency from the power spectrum; Based on the dominant frequency and the optimal control time series, the virtual inertial force field parameters and virtual compensation torque generated by the movement of the robot's two arms and lifting column are determined.
2. The method according to claim 1, characterized in that, The measurement data includes the gravity direction and body attitude rotation matrix in the body coordinate system measured by the robot's inertial measurement unit, and the estimated data includes the robot's local terrain slope angle. The process of processing the measurement data to determine the estimated data for the robot includes: Based on the gravity direction in the body coordinate system measured by the inertial measurement unit and the body attitude rotation matrix, the local slope angle of the terrain of the robot is determined by the identification of the local slope angle of the terrain. The local slope angle of the terrain includes the pitch slope angle and the roll slope angle.
3. The method according to claim 1, characterized in that, The measurement data includes the robot's actual generalized momentum and observed generalized momentum, and the estimation data includes the robot's load mass change and center of mass shift. The process of processing the measurement data to determine the estimated data for the robot includes: Based on the residual between the actual generalized momentum and the observed generalized momentum, the load mass change and the centroid shift are determined by load parameter identification.
4. The method according to claim 1, characterized in that, The measurement data includes the robot's wheel encoder data, and the estimated data includes the robot's wheel slippage parameters; The process of processing the measurement data to determine the estimated data for the robot includes: Based on wheel encoder data, the wheel-ground slip parameters are identified and determined through wheel-ground slip parameter identification. The wheel-ground slip parameters include the wheel-ground longitudinal slip rate and the lateral slip angle. The wheel-ground slip parameters are used to construct a modified Jacobian matrix including slip compensation.
5. The method according to claim 1, characterized in that, The estimated data includes at least one scenario label, which includes at least one of flat working conditions, slope working conditions, load working conditions, slip working conditions and impact working conditions.
6. The method according to claim 1, characterized in that, Based on the estimated data, the effective support polygon of the robot is determined through scene adaptive prediction processing, including: Projecting the ground contact points of each wheel of the robot onto a plane perpendicular to the direction of equivalent gravity yields multiple effective support points; Based on the multiple effective support points, construct the effective support polygon of the robot.
7. The method according to claim 1, characterized in that, Also includes: If the available decision time is less than the time threshold, an emergency soft landing process is performed by calling the preset limit boundary soft landing library to obtain the minimum damage trajectory emergency command, and the minimum damage trajectory emergency command is used as the initial drive signal.
8. The method according to claim 1, characterized in that, The determination of the sliding mode compensation torque based on the drive command with active damping, through robust compensation processing, includes: In response to the active damping drive command, a sliding surface is obtained, which is used to characterize the difference between the robot's actual generalized velocity and a reference generalized velocity; Based on the sliding surface, the external impact force of the robot is estimated using a super-helical sliding mode observer; Based on the external impact force estimation, the preset Jacobian matrix of the external force application point, and the sliding surface, the sliding compensation torque is determined.
9. A control device, characterized in that, include: The acquisition module is used to acquire the robot's measurement data; The perception and identification processing module is used to process the measurement data and determine the estimated data of the robot. The estimated data includes at least one of the following: the local slope angle of the terrain of the robot, the change in the load mass of the robot, the center of gravity offset of the robot, the wheel slip parameters of the robot, and the scene label. The scene adaptive prediction processing module is used to determine the effective support polygon and zero moment point of the robot based on the estimated data through scene adaptive prediction processing. The variable weight convex optimization planning processing module is used to determine the initial driving signal through variable weight convex optimization planning processing if the distance between the zero moment point and the boundary of the effective support polygon is less than a preset distance threshold. The active momentum game execution processing module is used to determine the virtual inertial field parameters and virtual compensation torque based on the initial drive signal through active momentum game execution processing, and to obtain a drive command with active damping based on the virtual inertial field parameters, the virtual compensation torque and the initial drive signal. The robust compensation processing module is used to determine the sliding mode compensation torque based on the drive command with active damping through robust compensation processing, and feed forward and superimpose the sliding mode compensation torque onto the drive command with active damping to obtain the compensated drive signal. A drive module is used to drive the robot to move based on the compensated drive signal; The variable-weight convex optimization planning processing module, when determining the initial driving signal through variable-weight convex optimization planning processing, is specifically used for: Obtain available decision-making time; If the available decision time is greater than or equal to a preset time threshold, then the optimal control time sequence is determined through variable weight convex optimization programming, and the optimal control time sequence is used as the initial driving signal. The active momentum game execution processing module, based on the initial driving signal, determines the virtual inertial force field parameters and virtual compensation torque through active momentum game execution processing, specifically for: Determine the current actual overturning moment of the robot; Determine the power spectrum of the current actual overturning moment, and extract the dominant frequency from the power spectrum; Based on the dominant frequency and the optimal control time series, the virtual inertial force field parameters and virtual compensation torque generated by the movement of the robot's two arms and lifting column are determined.
10. An electronic device comprising a memory, a processor, and a computer program stored in the memory, characterized in that, The processor executes the computer program to implement the steps of the method according to any one of claims 1-8.
11. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the steps of the method according to any one of claims 1-8.
12. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by a processor, it implements the steps of the method according to any one of claims 1-8.
