Wheel-legged robot operation control method and system based on maneuverability optimization

Abstract

The application discloses a wheel-legged robot operation control method and system based on manipulability optimization, relates to the technical field of robot intelligent control, and comprises the following steps: adaptively calculating a weight matrix by using the state vectors of a base and a joint of a mechanical arm according to a mechanical arm end trajectory tracking task and a mechanical arm joint motion constraint; constructing a robot dynamics model, estimating a mechanical arm end contact force based on a disturbance observer, determining a manipulable force direction according to the estimation result, and then developing force manipulability enhancement; obtaining a speed instruction of an end effector, obtaining an expected base speed and a mechanical arm joint speed through the weight matrix, constructing a hierarchical control framework of a model predictive controller and a whole body controller based on an integrated dynamics model, and solving to obtain an optimal wheel-leg interaction force screw and an optimal joint torque, so as to drive the whole body motion control of the robot. Through the organic combination of disturbance compensation and force manipulability optimization, the motion stability and operation efficiency of the robot are enhanced.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot intelligent control, and in particular to a wheel-legged robot operation control method and system based on manipulability optimization. BACKGROUND

[0002] The statements in this section merely provide background information related to the present disclosure and do not necessarily constitute the prior art.

[0003] As a new generation of mobile operation robot, the wheel-legged robot combines the advantages of wheeled and legged mechanisms. Compared with traditional wheeled robots, it not only realizes high-speed, high-efficiency and low-energy consumption movement on flat ground, but also obtains the ability to cross obstacles, maintain balance and stability in complex unstructured environments through legged mechanisms. At the same time, compared with pure legged robots, the wheel-legged robot performs better in terms of movement efficiency and energy consumption control. In addition, the introduction of humanoid structure makes it have stronger environmental adaptability and operational flexibility, and can complete tasks such as pushing, holding and carrying closely related to human life and production scenes. Therefore, the wheel-legged robot has broad application prospects in the fields of service, industry and special operation.

[0004] However, the existing wheel-legged robot still has obvious deficiencies in motion and operation control. First, in the process of interacting with the environment, the external disturbance force on the manipulator is difficult to describe through an accurate model, resulting in insufficient disturbance compensation and easy deviation of the motion trajectory and decline of the operation stability. Second, as a typical under-actuated system, the wheel-legged robot has strong coupling and high nonlinearity, and its modeling is usually too simplified to accurately depict complex dynamic behavior, especially when moving and operating in parallel. In addition, the robot has obvious differences in force demand in different operation tasks (such as pushing forward and holding upward), while the existing control methods mainly focus on end trajectory tracking, lack of optimization of operation direction force manipulability, often leading to increased joint energy consumption, insufficient control robustness, and even insufficient execution force or unstable posture. In summary, the existing technology is difficult to simultaneously consider disturbance compensation and operation force direction optimization, which restricts the efficient and stable operation of the wheel-legged robot in complex environments. SUMMARY

[0005] To overcome the deficiencies of the prior art, the present application provides a wheel-legged robot operation control method and system based on manipulability optimization, which combines disturbance compensation and force manipulability optimization to enhance the motion stability and operation efficiency of the wheel-legged robot in complex environments.

[0006] To achieve the above purpose, one or more embodiments of the present application provide the following technical solutions:

[0007] In a first aspect, the present application provides a wheel-legged robot operation control method based on manipulability optimization, comprising:

[0008] Obtaining sensor data of the robot, and obtaining state vectors of the base and the end of the robot arm through a state estimator; according to the trajectory tracking task of the end of the robot arm and the joint motion constraint of the robot arm, a weight matrix is adaptively calculated by using the state vectors of the base and the joints of the robot arm, so as to realize the coordinated motion distribution of the base and the joints of the robot arm;

[0009] Considering the interaction force between the robot and the environment, a robot dynamics model is constructed, and the contact force at the end of the robot arm is estimated based on a generalized momentum disturbance observer; the direction of the manipulation force is determined according to the estimation result of the contact force at the end of the robot arm, and the force manipulability is enhanced based on the direction of the manipulation force;

[0010] Obtaining the velocity command of the end effector, and realizing the coordinated motion distribution of the base and the robot arm through the weight matrix to obtain the expected base velocity and the joint velocity of the robot arm, and then based on the integrated dynamics model, a hierarchical control framework of the model predictive controller and the whole body controller is constructed, and the optimal wheel-leg interaction force wrench and the optimal joint torque are solved to drive the whole body motion control of the robot.

[0011] Further technical solutions, taking the minimization of the actual base velocity and the actual joint velocity of the robot arm as the target, an optimization problem is constructed as follows:

[0012]

[0013] Wherein, represents the velocity matrix, represents the base velocity and the joint velocity of the robot arm, represents the symmetric positive definite weight matrix, represents the expected joint velocity of the robot arm, represents the Jacobian matrix, represents the expected base velocity and the expected joint velocity of the robot arm.

[0014] Further technical solutions, the solution of the optimization problem is:

[0015]

[0016] Wherein, represents the weight pseudo-inverse matrix of , represents the generalized velocity of the integrated two robot arm ends, represents the unit matrix, represents the joint velocity in the null space, represents the weight matrix.

[0017] A further technical solution involves the generalized momentum perturbation observer comparing the expected trajectory results with the actual results from the robot dynamics model. It estimates the robotic arm contact force by observing state changes. The formula for calculating the robotic arm contact force is as follows:

[0018]

[0019] in, Indicates the contact force of the robotic arm. This represents the drive joint selection matrix. Represents the Jacobian matrix. It indicates a disturbance force.

[0020] A further technical solution involves reducing the order and linearizing the robot's dynamics model to obtain an integrated dynamics model.

[0021] A further technical solution involves obtaining the optimal wheel-leg interaction force spinor based on a model predictive controller. The optimization problem of the model predictive controller is expressed as:

[0022]

[0023] in, Indicates the first The system status of the step, Indicates the first The system status of the step, express The system status is constantly being referenced. Indicates the first Step control input, express Control inputs of the timing system , and This represents a diagonal positive semidefinite weight matrix. As a penalty item, , This represents the coefficient matrix.

[0024] A further technical solution involves optimizing the rotation of the optimal wheel-leg interaction force using the whole-body controller, thereby obtaining the optimal joint torque. The quadratic optimization problem is expressed as follows:

[0025]

[0026] in, Represents the cost function, Represents the optimization variable. Indicates task The corresponding weight matrix, represents the number of tasks, represents the inertia matrix, represents the robot generalized acceleration, represents the Coriolis force matrix, represents the gravity matrix, represents the driven joint selection matrix, represents the driven torque matrix, represents the contact Jacobian matrix, represents the final desired wheel-leg interaction force wrench, represents the wheel-leg interaction force wrench optimized by the model predictive controller, represents the slack variable, represents the nonholonomic constraint matrix, represents the force and joint torque constraint matrix.

[0027] In a second aspect, the present application provides a wheel-legged humanoid robot operation control system based on manipulability optimization, comprising:

[0028] a motion distribution module configured to: acquire sensor data of the robot, obtain a base and a mechanical arm end state vector through a state estimator; and adaptively calculate a weight matrix according to a mechanical arm end trajectory tracking task and a mechanical arm joint motion constraint, so as to realize coordinated motion distribution of the base and the mechanical arm joint.

[0029] a manipulability enhancement module configured to: construct a robot dynamics model by considering the interaction force between the robot and the environment, estimate the mechanical arm end contact force based on a generalized momentum disturbance observer; and determine a manipulability force direction according to the estimation result of the mechanical arm end contact force, and develop force manipulability enhancement based on the manipulability force direction.

[0030] a collaborative control module configured to: acquire a speed instruction of an end effector, realize coordinated motion distribution of the base and the mechanical arm through a weight matrix to obtain a desired base speed and a mechanical arm joint speed, and then construct a hierarchical control framework of a model predictive controller and a whole-body controller based on an integrated dynamics model, so as to obtain an optimal wheel-leg interaction force wrench and an optimal joint torque, thereby driving the whole-body motion control of the robot.

[0031] In a third aspect, the present application provides a computer readable storage medium having a computer program stored thereon, the program being executed by a processor to implement the steps of the wheel-legged humanoid robot operation control method based on manipulability optimization according to the first aspect.

[0032] In a fourth aspect, the present application provides a computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the manipulability-optimization-based control method for a hexapod robot according to the first aspect when executing the program.

[0033] The above one or more technical solutions have the following beneficial effects:

[0034] The present application provides a novel robust control method based on disturbance compensation and adaptive manipulability optimization, which estimates the force of the end effector in real time by constructing an observer based on generalized momentum, including environmental contact force and task-driven force, which are regarded as disturbances of the end effector and are included in the system modeling, to realize dynamic compensation of external disturbances and improve control accuracy and stability. At the same time, for the operation requirements of different operation tasks, on the basis of ensuring the main task of end position tracking, an adaptive manipulability optimization method is proposed to obtain the same end output effect with smaller joint torque in the target direction, enhance the force output capability of the robot in the target direction, and thus improve the operation efficiency and reduce energy consumption.

[0035] The present application combines disturbance compensation and force manipulability optimization, which not only enhances the motion stability and operation efficiency of the hexapod robot in complex environments, but also realizes the robust operation ability of the robot in diversified operation tasks. BRIEF DESCRIPTION OF DRAWINGS

[0036] The accompanying drawings, which form a part of this application, are included to provide a further understanding of the application, and are incorporated in and constitute a part of this application. The embodiments of these drawings illustrate the present application and, together with their description, serve to explain the application without imposing undue limitation thereto.

[0037] Figure 1 is a flowchart of the manipulability-optimization-based control method for a hexapod robot according to an embodiment of the present application;

[0038] Figure 2 is a schematic diagram of the coordinate system of a hexapod robot according to an embodiment of the present application. DETAILED DESCRIPTION

[0039] It should be noted that the following detailed description is exemplary in nature and is intended to provide further description of the present application. Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which the present application belongs.

[0040] It is to be understood that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of example embodiments in accordance with the present application. As used herein, the singular forms "a", "an" and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise. It will be further understood that the terms "comprises" and / or "comprising," when used in this specification, specify the presence of stated features, steps, operations, elements, components, and / or groups thereof, but do not preclude the presence or addition of one or more other features, steps, operations, elements, components, and / or groups thereof.

[0041] The embodiments in the present application and the features in the embodiments can be combined with each other in the case of no conflict.

[0042] Embodiment One

[0043] As shown in the figure, the embodiment discloses a wheel-legged robot operation control method based on manipulability optimization, which comprises the following steps: Figure 1

[0044] S1: Obtain the sensor data of the robot, and obtain the state vectors of the base and the end of the robot arm through a state estimator; according to the trajectory tracking task of the end of the robot arm and the joint motion constraint of the robot arm, a weight matrix is adaptively calculated by using the state vectors of the base and the joints of the robot arm, so as to realize the coordinated motion distribution of the base and the joints of the robot arm; the state vector of the base includes the attitude and velocity of the base, the state vector of the end of the robot arm includes the position and velocity of the end of the robot arm, and the position and velocity of the wheel tread are also obtained through the state estimator, and the state vector of the joint of the robot arm is directly obtained by reading the joint encoder.

[0045] S2: Construct a robot dynamics model considering the interaction force between the robot and the environment, estimate the contact force at the end of the robot arm based on a generalized momentum disturbance observer, and determine the manipulative force direction according to the estimation result of the contact force at the end of the robot arm, and carry out force manipulability enhancement based on the manipulative force direction;

[0046] S3: Obtain the velocity instruction of the end effector, realize the coordinated motion distribution of the base and the robot arm through the weight matrix to obtain the expected base velocity and the joint velocity of the robot arm, and then construct a hierarchical control framework of a model predictive controller and a whole-body controller based on an integrated dynamics model, solve to obtain the optimal wheel-leg interaction force wrench and the optimal joint torque, so as to drive the whole-body motion control of the robot.

[0047] The wheel-legged robot operation control is divided into three parts, including modeling, motion planning and motion control, which will be described in detail below.

[0048] (I) Modeling

[0049] (1) Define the coordinate system

[0050] In the embodiment, as shown in the figure, Figure 2 ​The inertial frame is defined as shown, the base frame, and the end-effector frame, , The generalized coordinate matrix , the velocity matrix , and the driving torque matrix are represented as:

[0051]

[0052] where denotes the translation of the torso, denotes the rotation of the torso, denotes the number of joints of the robot, denotes the linear velocity of the torso, denotes the angular velocity of the torso.

[0053] The sensor data includes the Euler angles, angular velocity, linear acceleration of the imu feedback, and the joint angles and joint velocities of the joint encoder feedback.

[0054] In addition, the present application defines a control frame with its origin at the midpoint of the line connecting the two wheels, and its axis pointing in the direction of the robot's forward movement. The rotational relationship between the control frame and the inertial frame is represented by the rotation matrix , where , denotes the yaw angle of the robot (base). The rotation matrix represents the mapping of a vector in the coordinate system to the coordinate system . The definition of the control frame implies the nonholonomic constraint characteristics of the wheeled biped robot, facilitating the extension of motion analysis from the sagittal plane to three-dimensional space. It should be noted that in the following text, the symbol without the upper left corner indicating the coordinate system is expressed in the world coordinate system by default.

[0055] (2) Kinematic model.

[0056] ​​​When the wheel-legged robot is moving, its base is mainly driven by the double-wheel mechanism of the lower limbs, while the upper limbs focus on the task. Since the robot is a floating base robot, the base can be modeled as a virtual six-degree-of-freedom joint to expand the workspace of the manipulator. After calculating the desired trajectory of the base based on the task requirements, the corresponding motion planning of the lower limbs can be generated. The upper limbs are both three-degree-of-freedom manipulators, so only the generalized position of the end in the world coordinate system is modeled, and the forward kinematics is as follows:

[0057] (1)

[0058] wherein, represents the generalized position of the manipulator in the world coordinate system, represents the forward kinematics function, which is jointly determined by the joint vectors of the base and the manipulator , represents the virtual joint number of the base, represents the joint number of the two manipulators, represents the joint vector of the base, represents the joint vector of the manipulator; represents the kinematics function of the end position of the manipulator in the base coordinate system; represents the transformation matrix from the base coordinate system to the inertial coordinate system; represents the serial number of the two manipulators, respectively.

[0059] Based on the kinematics formula (1), the relationship between the end velocity of the manipulator and the joint velocity is obtained by differentiation as follows:

[0060] (2)

[0061] wherein, represents the generalized velocity of the two manipulator ends; represents the Jacobian matrix, ; represents the joint velocity of the base and the manipulator. Since is not full rank, there are multiple solutions in the inverse kinematics solution, and under the premise of meeting the end velocity constraint, the actual joint velocity is made as close as possible to the desired velocity, and the importance of different joints is considered, so the optimization problem is constructed as follows:

[0062] (3)

[0063] wherein, represents the velocity matrix, represents the base velocity and the manipulator joint velocity, represents the symmetric positive definite weight matrix, This represents the desired joint speed of the robotic arm. Represents the Jacobian matrix. Let represent the desired base speed and the desired robot arm joint speed. The solution to this optimization problem is obtained according to formula (3):

[0064] (4)

[0065] in, express The weight pseudo-inverse matrix, This represents the generalized velocity of the integrated ends of two robotic arms. Represents the identity matrix. Represents the joint velocity in null space. This represents the weight matrix.

[0066] (3) Dynamic model

[0067] The movement of wheel-legged humanoid robots is driven by the interaction forces between them and their environment. On one hand, the force generated by the contact between the lower limbs and the ground is the basis for the robot's movement and balance; on the other hand, the upper limbs are also subjected to forces from the environment when performing tasks. The unified dynamic model is as follows:

[0068] (5)

[0069] in, Represents the generalized acceleration of a robot. , and These represent the inertia matrix, Coriolis force matrix, and gravity matrix, respectively. This represents the drive joint selection matrix. Indicates the contact Jacobian matrix. and These represent the spinosity of the interaction force between the robotic arm and the environment, and the interaction force between the wheels and legs, respectively. Used to drive the movement of the entire robot system, it is in a state of inevitable contact with the environment, and Interactive forces are generated only when working or encountering obstacles, and can be regarded as a kind of external disturbance force, which needs to be estimated in real time.

[0070] This invention designs a generalized momentum perturbation observer to estimate the end-effector contact force (arm end-effector contact force). The core idea is that, given the torque applied at the joint, the expected trajectory result based on the dynamic model can be compared with the actual result. Any discrepancy is attributed to external perturbation. By observing the state changes of the system, the magnitude of the external force is obtained. First, the base and the arm, as well as the coupling between them, are extracted from the dynamic model (Equation (5)) to obtain:

[0071] (6)

[0072] The contact force of the manipulator is taken as the external disturbance force, which can be expressed as a disturbance vector:

[0073] (7)

[0074] In order to reduce the interference of high-frequency noise and smooth the estimation results, the disturbance force in discrete time is obtained after filtering by a first-order low-pass filter:

[0075] (8)

[0076] wherein, represents the Laplace variable, represents the cut-off frequency of the low-pass filter. Based on the properties of the generalized momentum in the dynamic equation, it can be calculated by the following formula:

[0077] (9)

[0078] wherein, represents the generalized momentum. The intermediate variable is selected, and after transformation, we get:

[0079] (10)

[0080] Since the actual controller belongs to a discrete-time system, the signal sampling is performed at a fixed time step , so the system needs to be described in discrete time, rather than a continuous-time differential equation, therefore, formula (10) is converted to discrete time as:

[0081] (11)

[0082] wherein, represents the low-pass filter coefficient, ; represents the generalized momentum in discrete time, represents the coefficient related to the cut-off frequency of the filter, ;

[0083] represents the Z transform operator. After the filtering calculation of formula (11), the end contact force is calculated as:

[0084] (12)

[0085] wherein, represents the contact force of the manipulator, ​This represents the drive joint selection matrix. Represents the Jacobian matrix. It indicates a disturbance force.

[0086] Model-based optimization (OCP) problem-solving for robot motion control is a widely used and effective method. However, considering the high dimensionality and nonlinearity of the whole-body dynamics model, the optimization problem becomes computationally time-consuming and difficult to solve in predictive control, thus imposing stringent requirements on the real-time performance and computational resources of the onboard controller. To ensure real-time performance, it is necessary to use a simplified model to reduce the size of the optimization problem. Therefore, this invention constructs an integrated dynamics model suitable for predictive control. Through model order reduction and linearization, key information about the dynamic characteristics is maintained while ensuring computational efficiency, thereby effectively improving the real-time performance and robustness of the control system.

[0087] For simplicity, this invention does not distinguish between left and right wheels in the following motion analysis. The wheel dynamics equations in the control coordinate system are:

[0088] (13)

[0089] (14)

[0090] in, and This represents the weight and inertia tensor of the wheel. This represents the acceleration of the wheel in the control coordinate system. This represents the supporting force provided by the ground in the control coordinate system. This represents the transformation matrix from the inertial coordinate system to the control coordinate system. This represents the interaction force between the wheel and the leg. Represents the gravity vector. This represents the vector pointing from the center of the wheel to the point of contact between the wheel and the ground. Indicates the wheel radius. Represents the angular velocity of the wheel. This represents the torque generated by the interaction between the wheel and the leg. Assume there is no relative slippage between the wheel and the ground, satisfying the following constraints:

[0091] (15)

[0092] Combining the above formulas, eliminate ground contact force. And obtain the coordinate system The wheel dynamics equation along the x-axis is:

[0093] (16)

[0094] Here, matrix The numbers in the upper right corner represent the specific row of the matrix. The distance between the two wheels in the y-axis direction in the control coordinate system is constant, as determined by the mechanical structure of the robot. Through forward kinematics, the vector and The relationship between the acceleration layers of

[0095] (17)

[0096] where represents the yaw angular velocity excluding the torso. Since the pitch and roll angular velocities of the robot during movement are small and can be ignored, the above formula can be approximated as:

[0097] (18)

[0098] where represents the distance between the torso and the wheels in the x-axis direction.

[0099] Since the mass of the robot is mainly concentrated in the torso and the wheels, and the limbs are relatively light, when simplifying the whole body dynamics model, the present application removes the joint space constraints, ignores the effects of joint velocity on the center of mass momentum and the effects of limb link movement on the system inertia tensor. Assuming that the center of mass of all links except the wheels is concentrated at the center of mass of the torso, the present application models the robot torso as a floating base. In the world coordinate system, the single rigid body dynamics model of the torso is as follows:

[0100] (19)

[0101] (20)

[0102] where represents the mass of the torso, represents the inertia tensor of the torso, represents the left and right wheel legs, for example and represent vectors from the center of mass of the torso to the interaction points of the left and right wheel legs, respectively; represents the interaction force between the left and right wheels and the legs in the world coordinate system, represents the interaction torque between the left and right wheels and the legs in the world coordinate system, represents the disturbance force at the end of the robot arm estimated based on generalized momentum, which is added as a known quantity in the model construction; represents a vector from the end of the robot arm to the center of mass of the base. Assuming that the roll and pitch velocities are small and the off-diagonal terms of the inertia tensor can be ignored, the above formula can be approximated as: ​

[0103] (21)

[0104] Due to the small pitch and roll angles, the inertia tensor in the body frame can be obtained by the following equation:

[0105] (22)

[0106] where, represents the fixed inertia tensor in the body frame.

[0107] After derivation, the dynamics of the wheel and the trunk have been characterized. It can be found that the motion states of the wheel and the trunk are determined by the interaction force moment and the interaction force appears as equal size and opposite sign. According to the above analysis, the relative position of the trunk and the wheel in the forward direction is taken as the control variable instead of the wheel position, which not only realizes the dimension reduction of the state vector to shorten the calculation time of MPC, but also effectively realizes the collaborative control of the wheel and the leg. Combined with the above formula, we can get:

[0108] (23)

[0109] Definition , and represent the distance between the left and right wheels and the trunk along the x-axis direction in the control coordinate system.

[0110] The state variable is selected as ,

[0111] Take as the control input, and the dynamics equation that satisfies the non-holonomic constraint and the internal force / torque transmission is as follows:

[0112] (24)

[0113] where, (25)

[0114] (26)

[0115] (27)

[0116] where, and involve row permutation operations, that is: , and Similarly.

[0117] (II) Motion planning

[0118] In mobile manipulator robots with redundant degrees of freedom, manipulability analysis methods are widely used in task performance evaluation and motion planning. As an important indicator of the robot's ability to perform in the task space, manipulability can reflect the motion flexibility and task adaptability under different joint configurations. In existing research, common forms of manipulability mainly include motion manipulability, velocity manipulability and force manipulability, which respectively characterize the performance of the robot from the perspectives of kinematic mapping, velocity transmission efficiency and dynamic characteristics. However, in task scenarios involving complex environmental interaction and external force, relying solely on traditional motion manipulability is insufficient to fully describe the advantages and disadvantages of the robot in force control and environmental interaction. Based on this, the invention introduces and studies force manipulability to better reveal the task execution ability and force distribution characteristics of redundant mobile manipulator robots under external force. The force manipulability function is defined as:

[0119] (28)

[0120] where, represents the force optimization direction of the two robot arms, represents the Jacobian matrix of the robot arm, represents the scaling matrix used to normalize the joint torque.

[0121] (1) Adaptive motion distribution

[0122] In redundant robot motion planning, how to distribute motion between the base and the end effector (the end of the robot arm) is a key problem. As shown in equation (4), to achieve coordinated motion, a weight matrix is introduced, and the motion contribution of different degrees of freedom is adjusted according to the priority and motion constraints. The weight matrix is defined as:

[0123] (29)

[0124] where, indicates the weight coefficient. Considering the stability of the robot, the attitude of the base is not included in the motion distribution. A stable and constant attitude is conducive to the performance of the task. When , it means that the end motion only depends on the base. When , it means that the end motion only depends on the arm. When , it means that the end motion is realized by the base and the arm together. The size of is mainly determined by the workspace and motion range of the robot arm. Since the motion accuracy of the robot arm is much higher than that of the base, within the reachable workspace, the robot arm is used as much as possible to realize the desired motion of the end. When the motion range of the robot arm is exceeded, the motion needs to be distributed to the base. Here, the weight coefficient is calculated by the following formula:

[0125] (30)

[0126] (31)

[0127] wherein, represents the upper limit of the index of arm movement only, and respectively represent the maximum and minimum values of the arm joint speed, which are related to the workspace of the robot arm, and are determined by the following constraints:

[0128] (32)

[0129] wherein, and represent the limits of each joint of the robot arm, and represent the maximum speed and acceleration of the joint driving motor of the robot arm, represents the sampling period.

[0130] (2) Force manipulability enhancement task

[0131] The above describes the motion distribution problem of the robot during operation. When completing the task of reaching the desired end position, the system still has a degree of redundancy, which provides the possibility for the robot to perform additional tasks while completing the main task. In order to enhance the force control performance and stability of the robot when interacting with the environment, a key additional task is force manipulability enhancement. Force manipulability can be understood as the ability of the robot end effector to exert force or resist external force in any direction. When the configuration of the robot approaches a singularity point, its force manipulability in some directions will become extremely poor, and a slight joint torque error can cause a huge force error at the end, or completely unable to generate effective force in some directions. Therefore, the goal of the force manipulability enhancement task is to adjust the configuration of the robot to a more optimal state in terms of force manipulability using the redundant degrees of freedom while ensuring the completion of the main task (end trajectory tracking).

[0132] According to the force manipulability function described in formula (28), the partial derivative of each joint of the robot arm is selected as the joint speed command of the subtask, which means adjusting the robot to a configuration that maximizes . At the same time, the disturbance force at the end of the robot arm has been estimated by the present application, so the force manipulability direction that needs to be optimized by the present application is the direction that resists the disturbance force at the end of the robot arm, and the vector can be obtained by the following formula:

[0133] (33)

[0134] Partial derivative of the joint of the robot arm Can be expressed as:

[0135] (34)

[0136] Thus the joint velocity command of the robot arm is:

[0137] (35)

[0138] The velocity command of the null-space internal force manipulability task is:

[0139] (36)

[0140] Substituting it into formula (4) can obtain the desired base velocity and robot arm joint velocity for force manipulability enhancement of the robot arm contact disturbance force.

[0141] (Three) motion control

[0142] (1) Model predictive controller MPC

[0143] The present application designs a discrete-time finite horizon model predictive controller, which generates the expected force screw between the wheels and the trunk according to the current state. When facing a multi-input multi-output (MIMO) system, MPC can coordinate multiple control variables to ensure the overall coordination and optimal performance of the system. Based on the constructed reduced-order system dynamics model and the current system state, the future system behavior is predicted using a rolling optimization window, and control measures are taken in advance to improve the response speed and accuracy of the system. By directly incorporating constraints into the optimization problem, MPC can effectively handle input and state constraints to achieve the desired trajectory tracking. The optimization process is constantly cycled, and only the optimal control input at the current time is applied each iteration, and it is recalculated at the next time.

[0144] The approximation of angular velocity dynamics and the fusion of gravity terms into the state variable, the discrete state space form of the dynamics equation (24) can be expressed as:

[0145] (37)

[0146] Wherein, , is the coefficient matrix; represents the system state of the step, represents the control input of the step. Assuming that the prediction step is To make the robot's state track the trajectory within the prediction horizon as much as possible, an MPC optimization problem is constructed to solve the optimal wheel-leg interaction wrench at the current time step as follows:

[0147] (38)

[0148] where, represents the system state at the th step, represents the system state at the th step, represents the system state at the th step, represents the system state at the th step, represents the system state at the th step, , and represent diagonal positive semi-definite weight matrices, The term penalizes the sudden change of control input between consecutive time steps to prevent the robot from being unstable. To prevent the wheel motor current overload and wheel slip phenomenon, a safety constraint is used to limit the wheel reaction force in a conical boundary:

[0149] (39)

[0150] where, , , represent the components of the wheel-leg interaction force in x, y, z axes, represents the driving coefficient, which is related to the wheel motor performance. The above constraint equation is linearized by using the friction cone as follows:

[0151] (40)

[0152] In addition, subject to nonholonomic constraints, the wheel cannot actively provide lateral driving force and driving torque around the x and z axes in the control coordinate system. Therefore, this constraint condition needs to be included in the framework of the optimization problem, which is expressed as:

[0153] (41)

[0154] The qpOASES solver is used to solve the optimization problem in real time, where is selected as the optimal input vector for the current control period.

[0155] (2) Weighted multi-task whole-body controller

[0156] The multi-task processing capability of the robot system, especially benefits from the redundancy of leg degrees of freedom. These redundant degrees of freedom can be flexibly allocated to perform specific control objectives. However, if the dynamic characteristics of the legs are ignored, the reliability and accuracy of task execution will be weakened, and the collaborative effect between multiple tasks will be affected. Therefore, it is essential to incorporate leg dynamics into the whole-body control framework to improve system stability, execution accuracy, dynamic performance and response speed. The present invention proposes a weighted multi-task whole-body controller (WBC) which formulates the multi-task tracking problem as a weighted optimization problem, where each task is assigned a specific weight. This method allows the controller to handle all tasks simultaneously, simplifying the optimization process by eliminating sequential optimization and null-space projection. In order to achieve the generalized acceleration that meets the requirements of multiple tasks, while balancing long-term prediction and high-frequency feedback control, the present invention formulates a quadratic optimization problem to obtain the optimal joint torque, that is, the optimal wheel-leg interaction force wrench obtained by the model predictive controller is input into the whole-body controller for further optimization:

[0157] (42)

[0158] wherein, represents the cost function, represents the optimization variable, represents the task corresponding weight matrix, represents the number of tasks, represents the contact Jacobian matrix, represents the final desired wheel-leg interaction force wrench, including wheel-leg interaction force and torque, represents the wheel-leg interaction force and torque optimized by the model predictive controller, represents the nonholonomic constraint matrix, represents the force and joint torque constraint matrix. Each task is described in the form of such an equality constraint. In order to satisfy the dynamic constraints and coordinate the balance between MPC and WBC, the present invention introduces a slack variable to relax the constraints and increase the solvability of the problem. The optimization variable is defined as: , and then each task will be described in detail.

[0159] (1) Space motion tracking task. This task is used to track the required motion trajectory of the human body, including translation and rotation, as well as the target position of the wheels.

[0160] 1) Sternal translation, the sternal translation task in the acceleration layer is defined as:

[0161]

[0162] wherein, Jtrunk, represents the Jacobian matrix corresponding to the trunk translation task, represents the desired trunk acceleration, , and represent the trunk reference displacement, velocity and acceleration, respectively, , represents the feedback gain matrix. The desired trunk acceleration is generated by a PD controller from the reference trajectory containing displacement, velocity and acceleration, and the current trunk motion state is obtained by a state estimator designed by fusing IMU and joint encoder feedback information.

[0163] 2) Trunk rotation, the trunk rotation task in the acceleration layer is defined as:

[0164]

[0165] wherein, Jtrunk, represents the Jacobian matrix corresponding to the trunk rotation task, represents the desired trunk angular acceleration, and represents the reference rotation direction corresponding quaternion and the current rotation direction corresponding quaternion, represents the reference rotation angular velocity. The rotation direction error is defined using the minus bracket operator , which calculates the difference between two quaternions and converts it into a three-dimensional rotation.

[0166] 3) Leg swing, the leg swing task in the acceleration layer is defined as:

[0167]

[0168]

[0169] wherein, Jswing, represents the Jacobian matrix corresponding to the left leg swing task, Jswing, represents the Jacobian matrix corresponding to the right leg swing task, represents the desired acceleration of the relative distance between the wheel and the trunk, represents the reference value of the relative distance between the wheel and the trunk.

[0170] The Jacobian matrix of this task can be obtained by the following equation:

[0171]

[0172] wherein, and represent the Jacobian matrix corresponding to the wheel and trunk position in the world coordinate system, respectively. By subtracting the two and converting to the control coordinate system, the Jacobian matrix of the leg swing task can be obtained.

[0173] (2) Energy efficiency and slack optimization: To optimize the joint torque output, the present invention introduces an energy efficiency function. Compared with the leg joints, the wheel joints are assigned a larger weight coefficient, thereby reducing significant oscillations in the system. In addition, minimizing the slack variables can maintain the prediction performance of the system while ensuring that the constraints are strictly followed during the optimization process. This task can be described as:

[0174] .

[0175] Embodiment Two

[0176] The embodiment discloses a leg-wheel robot operation control system based on manipulability optimization, comprising:

[0177] a motion distribution module configured to: obtain sensor data of the robot, and obtain state vectors of the base and the end of the robot arm through a state estimator; and adaptively calculate a weight matrix based on the state vectors of the base and the robot arm joints, and realize coordinated motion distribution of the base and the robot arm joints according to the robot arm end trajectory tracking task and the robot arm joint motion constraint;

[0178] a manipulability enhancement module configured to: construct a robot dynamics model by considering the interaction force between the robot and the environment, estimate the contact force at the end of the robot arm based on a generalized momentum disturbance observer, determine a manipulable force direction according to the estimation result of the contact force at the end of the robot arm, and carry out force manipulability enhancement based on the manipulable force direction;

[0179] a cooperative control module configured to: obtain a speed instruction of an end effector, realize coordinated motion distribution of the base and the robot arm through a weight matrix to obtain a desired base speed and a robot arm joint speed, and then construct a hierarchical control framework of a model predictive controller and a whole-body controller based on an integrated dynamics model, and solve to obtain an optimal wheel-leg interaction force wrench and an optimal joint torque to drive the whole-body motion control of the robot.

[0180] Embodiment Three

[0181] The purpose of the embodiment is to provide a computing device, comprising a memory, a processor and a computer program stored on the memory and executable on the processor, wherein the processor executes the program to realize the steps of the method of embodiment one.

[0182] Embodiment Four

[0183] The purpose of the embodiment is to provide a computer-readable storage medium, a computer-readable storage medium having a computer program stored thereon, wherein the program is executed by a processor to execute the steps of the method of embodiment one.

[0184] The steps and methods involved in the apparatuses of Embodiments 3 and 4 above correspond to those in Embodiment 1. For specific implementation details, please refer to the relevant description section of Embodiment 1. The term "computer-readable storage medium" should be understood as a single medium or multiple media including one or more instruction sets; it should also be understood as including any medium capable of storing, encoding, or carrying an instruction set for execution by a processor and enabling the processor to perform any of the methods in this invention.

[0185] Those skilled in the art will understand that the modules or steps of the present invention described above can be implemented using general-purpose computer devices. Optionally, they can be implemented using computer-executable program code, thereby allowing them to be stored in a storage device for execution by a computer device, or they can be fabricated as separate integrated circuit modules, or multiple modules or steps can be fabricated as a single integrated circuit module. The present invention is not limited to any particular combination of hardware and software.

[0186] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

[0187] While the specific embodiments of the present invention have been described above in conjunction with the accompanying drawings, this is not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that various modifications or variations that can be made by those skilled in the art without creative effort based on the technical solutions of the present invention are still within the scope of protection of the present invention.

Claims

1. A wheel-legged robot operation control method based on manipulability optimization, characterized by, The method comprises the following steps: obtaining sensor data of the robot, and obtaining state vectors of the base and the end of the robot arm through a state estimator; according to a trajectory tracking task of the end of the robot arm and a motion constraint of the joints of the robot arm, a weight matrix is adaptively calculated by using the state vectors of the base and the joints of the robot arm, so as to realize coordinated motion distribution of the base and the joints of the robot arm; a robot dynamics model is constructed by considering interaction forces between the robot and the environment, and an end-of-arm contact force is estimated based on a generalized momentum disturbance observer; a manipulation force direction is determined according to an estimation result of the end-of-arm contact force, and force manipulability enhancement is carried out based on the manipulation force direction; a velocity command of an end effector is obtained, coordinated motion distribution of the base and the robot arm is realized by using the weight matrix to obtain expected base velocity and joint velocity of the robot arm, then, based on an integrated dynamics model, a hierarchical control framework of a model predictive controller and a whole-body controller is constructed, and optimal wheel-leg interaction force wrench and optimal joint torque are solved to drive whole-body motion control of the robot.

2. The locomitability-optimization-based wheel-legged robot job control method according to claim 1, wherein An optimization problem is constructed by taking minimization of actual base velocity and actual joint velocity of the robot arm as an objective, and the optimization problem is as follows: wherein, denotes a velocity matrix, denotes a base velocity and a robot joint velocity, denotes a symmetric positive definite weight matrix, denotes a desired robot joint velocity, denotes a Jacobian matrix, denotes a desired base velocity and a desired robot joint velocity.

3. The locomotion-optimization-based wheel-legged robot job control method according to claim 2, wherein a solution of the optimization problem is as follows: wherein, denotes the weighted pseudo-inverse matrix of denotes the generalized velocity of the integration of the two robot arms' tips, denotes the identity matrix, denotes the joint velocity in the null space, denotes the weight matrix.

4. The locomotion-optimization-based wheel-legged robot job control method according to claim 1, wherein The generalized momentum disturbance observer estimates the contact force of the robot arm by comparing an expected trajectory result of the robot dynamics model with an actual result, and a calculation formula of the contact force of the robot arm is as follows: wherein, represents a mechanical arm contact force, represents a driving joint selection matrix, represents a Jacobian matrix, represents a disturbance force.

5. The locomotion-optimization-based wheel-legged robot job control method according to claim 1, wherein An integrated dynamics model is obtained by reducing and linearizing the robot dynamics model.

6. The locomotion-optimization-based wheel-legged robot job control method according to claim 1, wherein Optimal wheel-leg interaction force wrench is solved based on a model predictive controller, and an optimization problem of the model predictive controller is as follows: wherein denotes the system state at the step, denotes the system state at the step, denotes the system state at the time instant denotes the control input at the step, denotes the control input of the system at the time instant , and denote diagonal positive semi-definite weight matrices, is a penalty term, , denote coefficient matrices.

7. The manipulability-optimized wheel-legged robot job control method according to claim 1, wherein The whole-body controller further optimizes the optimal wheel-leg interaction force wrench to solve optimal joint torque, and a quadratic optimization problem of the whole-body controller is as follows: wherein, denotes a cost function, denotes an optimization variable, denotes a task a corresponding weight matrix, denotes a number of tasks, denotes an inertia matrix, denotes a robot generalized acceleration, denotes a Coriolis force matrix, denotes a gravity matrix, denotes a driven joint selection matrix, denotes a driven torque matrix, denotes a contact Jacobian matrix, denotes a final desired wheel-leg interaction force wrench, denotes a wheel-leg interaction force wrench optimized by a model predictive controller, denotes a slack variable, denotes a nonholonomic constraint matrix, denotes an effort and joint torque constraint matrix.

8. A wheel-legged robot operation control system based on manipulability optimization, characterized by, The method comprises the following steps: a motion distribution module configured to obtain sensor data of the robot, and obtain state vectors of the base and the end of the robot arm through a state estimator; according to a trajectory tracking task of the end of the robot arm and a motion constraint of the joints of the robot arm, a weight matrix is adaptively calculated by using the state vectors of the base and the joints of the robot arm, so as to realize coordinated motion distribution of the base and the joints of the robot arm; a manipulability enhancement module configured to construct a robot dynamics model by considering interaction forces between the robot and the environment, and estimate an end-of-arm contact force based on a generalized momentum disturbance observer; a manipulation force direction is determined according to an estimation result of the end-of-arm contact force, and force manipulability enhancement is carried out based on the manipulation force direction; a coordinated control module configured to obtain a velocity command of an end effector, realize coordinated motion distribution of the base and the robot arm by using the weight matrix to obtain expected base velocity and joint velocity of the robot arm, then, based on an integrated dynamics model, construct a hierarchical control framework of a model predictive controller and a whole-body controller, and solve optimal wheel-leg interaction force wrench and optimal joint torque to drive whole-body motion control of the robot.

9. A computer readable storage medium having stored thereon a computer program, characterized in that, The program is executed by the processor to realize the steps in the wheel-legged humanoid robot operation control method based on manipulability optimization in any one of claims 1-7.

10. A computer device comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, The processor implements the steps in the locomotion-optimization-based control method for a wheel-legged robot as claimed in any one of claims 1-7 when executing the program.

Images