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

Abstract

The invention discloses a wheel-foot humanoid robot operation control method and system based on maneuverability optimization, and relates to the technical field of robot intelligent control. According to a mechanical arm tail end trajectory tracking task and mechanical arm joint motion constraints, a weight matrix is calculated in a self-adaptive mode through state vectors of a base and a mechanical arm joint; a robot dynamic model is constructed, and the tail end contact force of the mechanical arm is estimated based on a disturbance observer; the direction of the control force is determined according to an estimation result, and then force maneuverability enhancement is carried out; a speed instruction of an end effector is obtained, the expected base speed and the expected mechanical arm joint speed are obtained through a weight matrix, a hierarchical control frame of a model prediction controller and a whole-body controller is constructed based on an integrated dynamic model, the optimal wheel-leg interaction force spinor and the optimal joint torque are obtained through solving, and the optimal wheel-leg interaction force spinor and the optimal joint torque are obtained. Therefore, the whole-body motion control of the robot is driven. Through organic combination of disturbance compensation and force maneuverability 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 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: In a first aspect, the present invention provides a method for controlling the operation of a wheeled humanoid robot based on maneuverability optimization, comprising: The robot acquires sensor data and obtains state vectors for the base and end effector through a state estimator. Based on the end effector trajectory tracking task and the joint motion constraints of the robot arm, the weight matrix is ​​adaptively calculated using the state vectors of the base and the joints of the robot arm to achieve coordinated motion allocation between the base and the joints of the robot arm. A robot dynamics model is constructed considering the interaction forces between the robot and the environment. The contact force at the end of the robotic arm is estimated based on a perturbation observer using generalized momentum. The direction of the manipulation force is determined based on the estimated contact force at the end of the robotic arm, and force maneuverability enhancement is carried out based on the direction of the manipulation force. The speed command of the end effector is obtained, and the desired base speed and robot joint speed are obtained by the coordinated motion distribution between the base and the robot arm through the weight matrix. Then, based on the integrated dynamics model, a hierarchical control framework of model predictive controller and whole body controller is constructed, and the optimal wheel-leg interaction force spinor and optimal joint torque are obtained to drive the whole body motion control of the robot.

[0007] A further technical solution aims to minimize the actual base speed and actual robotic arm joint speed relative to the desired base speed and desired robotic arm joint speed, respectively, and constructs the optimization problem as follows:

[0008] in, Represents the velocity matrix. This indicates the base speed and the speed of the robotic arm joints. This represents a symmetric positive definite weight matrix. This represents the desired joint speed of the robotic arm. Represents the Jacobian matrix. This represents the desired base speed and the desired robotic arm joint speed.

[0009] A further technical solution is that the solution to the optimization problem is:

[0010] 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.

[0011] Further technical solutions, the disturbance observer of the generalized momentum is based on the comparison between the expected trajectory result and the actual result of the robot dynamics model, and the robot contact force is estimated through the observed state change, and the robot contact force calculation formula is:

[0012] Among them, represents the robot contact force, represents the driving joint selection matrix, represents the Jacobian matrix, represents the disturbance force.

[0013] Further technical solutions, the robot dynamics model is reduced and linearized to obtain an integrated dynamics model.

[0014] Further technical solutions, the optimal wheel-leg interaction force wrench is solved based on a model predictive controller, and the optimization problem of the model predictive controller is represented as:

[0015] Among them, represents the system state of the first step, represents the system state of the first step, represents the system state of the first step, represents the system state of the first step, represents the system state of the first step, represents the system state of the first step, represents the control input of the first step, represents the control input of the first step, represents the control input of the first step, represents the control input of the first step, and represent diagonal positive semi-definite weight matrices, is a penalty term, represent coefficient matrices.

[0016] Further technical solutions, the whole body controller further optimizes the optimal wheel-leg interaction force wrench to obtain the optimal joint torque, and the quadratic optimization problem is represented as:

[0017] Among them, represents the cost function, represents the optimization variable, represents the task corresponding weight matrix, represents the number of tasks, represents the inertia matrix, represents the robot generalized acceleration, ​​denotes a Coriolis 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 a force and joint torque constraint matrix.

[0018] In a second aspect, the present application provides a wheeled-legged robot operation control system based on manipulability optimization, comprising: a motion distribution module configured to: obtain sensor data of the robot, and obtain a base and a robot arm end state vector through a state estimator; and adaptively calculate a weight matrix according to a robot arm end trajectory tracking task and a robot arm joint motion constraint, so as to realize coordinated motion distribution of the base and the robot arm joint. a manipulability enhancement module configured to: construct a robot dynamics model by considering the interaction force between the robot and the environment, estimate the robot arm end contact force based on a generalized momentum disturbance observer, determine a manipulability force direction according to the estimation result of the robot arm end contact force, and develop force manipulability enhancement based on the manipulability force direction. 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 the 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, so as to obtain an optimal wheel-leg interaction force wrench and an optimal joint torque, and drive the whole-body motion control of the robot.

[0019] 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 wheeled-legged robot operation control method based on manipulability optimization according to the first aspect.

[0020] In a fourth aspect, the present application provides a computer 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 implement the steps of the wheeled-legged robot operation control method based on manipulability optimization according to the first aspect.

[0021] The above one or more technical solutions have the following beneficial effects: The application provides a novel robust operation control method based on disturbance compensation and adaptive manipulability optimization, a generalized momentum-based observer is constructed to estimate the force of the arm end effector in real time, including environmental contact force and operation task driving force, which are regarded as disturbance force of the end and are included in the system modeling, dynamic compensation of external disturbance is realized, and control accuracy and stability are improved. At the same time, according to 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, so that the same end output effect is obtained with smaller joint torque in the target direction, the force output capacity of the robot in the target direction is enhanced, and the operation efficiency is improved and the energy consumption is reduced.

[0022] The application combines disturbance compensation and force manipulability optimization, not only enhances the motion stability and operation efficiency of the wheel-legged humanoid robot in a complex environment, but also realizes the robust operation ability of the robot in diversified operation tasks. BRIEF DESCRIPTION OF DRAWINGS

[0023] The drawings accompanying the specification of this application form a part thereof, serve to provide further understanding of the application, and together with the description, explain the application. The specific embodiments of the application and its description are used to explain the application without imposing undue limitations on the application.

[0024] Figure 1 is a flowchart of the wheel-legged humanoid robot operation control method based on manipulability optimization of the embodiment of the application; Figure 2 is a schematic diagram of the coordinate system of the wheel-legged humanoid robot. DETAILED DESCRIPTION

[0025] It should be noted that the following detailed description is exemplary and is intended to provide further explanation of the application. Unless otherwise indicated, 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 application pertains.

[0026] It should be noted that the terms used herein are only for the purpose of describing specific embodiments and are not intended to limit the exemplary embodiments according to the application. As used herein, the singular form is intended to include the plural form unless the context clearly indicates otherwise, and it should also be understood that when the terms "comprise" and / or "include" are used in the specification, there is a presence of the features, steps, operations, devices, components and / or combinations thereof.

[0027] The embodiments in the application and the features in the embodiments can be combined with each other without conflict.

[0028] Embodiment one As Figure 1As shown, the embodiment discloses a wheel-legged robot operation control method based on manipulability optimization, which comprises the following steps: S1: obtaining sensor data of the robot, 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; 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.

[0029] S2: considering the interaction force between the robot and the environment to construct a robot dynamics model, and estimating the contact force at the end of the robot arm based on a generalized momentum disturbance observer; determining the manipulation force direction according to the estimation result of the contact force at the end of the robot arm, and developing force manipulability enhancement based on the manipulation force direction; S3: obtaining the velocity command of the end effector, 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, constructing a hierarchical control framework of the model predictive controller and the whole body controller, and solving to obtain the optimal wheel-leg interaction force wrench and the optimal joint torque to drive the whole body motion control of the robot.

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

[0031] (I) Modeling (1) Define the coordinate system In the embodiment, as shown in the figure, an inertial coordinate system { Figure 2}, a base coordinate system (Base coordinate system) { } and an end effector coordinate system { } are defined, and the generalized coordinate matrix , }, the velocity matrix ( ) and the driving torque matrix ( ) are represented as:

[0032] Among them, represents the translation of the torso, represents the rotation of the torso, ​​represents the joint number of the robot, represents the linear velocity of the trunk, represents the angular velocity of the trunk.

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

[0034] In addition, the application defines a control coordinate system , the origin of which is located at the midpoint of the line connecting the two wheels, the axis points to the direction in which the robot advances. The rotational relationship between the control coordinate system and the inertial coordinate system is represented by a rotation matrix , wherein , represents 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 coordinate system implies the nonholonomic constraint characteristics of the wheeled biped robot, facilitating the extension of the motion analysis from the sagittal plane to the three-dimensional space. It should be noted that the symbol of the coordinate system is not marked in the upper left corner in the following description, which is by default represented in the world coordinate system.

[0035] (2) Kinematic model.

[0036] When the wheel-legged humanoid robot is moving, the base motion is mainly driven by the double-wheel mechanism of the lower limbs, while the upper limbs of the double arms focus on the task. Since the robot belongs to a floating base robot, the base can be modeled as a virtual six-degree-of-freedom joint to expand the workspace of the arms. After calculating the desired trajectory of the base based on the task requirements, the corresponding lower limb motion planning can be generated. The upper limbs of the double arms are both 3-DOF arms, so only the generalized position of the end in the world coordinate system is modeled, and the forward kinematics is as follows: (1) wherein, represents the generalized position of the arm in the world coordinate system, represents the forward kinematics function, which is determined by the joint vectors of the base and the arm, represents the virtual joint number of the base, represents the joint number of the two arms, represents the joint vector of the base, represents the joint vector of the arm; represents the kinematics function of the arm end position in the base coordinate system; denotes a transformation matrix from the base coordinate system to the inertial coordinate system; denotes the serial number of two manipulators, respectively.

[0037] 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: (2) wherein, denotes the generalized velocity integrated with the two manipulator ends; denotes a Jacobian matrix, ; denotes the joint velocity of the base and the manipulator. Since is not full rank, there are multiple solutions in the inverse kinematics solution, under the premise of meeting the end velocity constraint, the actual joint velocity is made as close as possible to the expected velocity, and the importance of different joints is considered, and therefore an optimization problem is constructed as follows: (3) wherein, denotes a velocity matrix, denotes the base velocity and the joint velocity of the manipulator, denotes a symmetric positive definite weight matrix, denotes the expected joint velocity of the manipulator, denotes a Jacobian matrix, denotes the expected base velocity and the expected joint velocity of the manipulator. The solution of the optimization problem according to formula (3) is: (4) wherein, denotes the weight pseudo-inverse matrix of , denotes the generalized velocity integrated with the two manipulator ends, denotes an identity matrix, denotes the joint velocity in the null space, denotes a weight matrix.

[0038] (3) Dynamics model The motion of the wheel-legged humanoid robot is driven by the interaction force between the robot and the environment. On the one hand, the force generated by the lower limbs in contact with the ground is the basis for the movement and balance of the robot; on the other hand, the upper limbs will also be subjected to the force exerted by the environment when performing operation tasks. The unified dynamics model is as follows: (5) wherein, denotes the generalized acceleration of the robot, , and denote the inertia matrix, the Coriolis force matrix and the 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.

[0039] 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: (6) Treating the contact force of the robotic arm as an external disturbance force, it can be represented by a disturbance vector as follows: (7) To reduce high-frequency noise interference and smooth the estimation results, the discrete-time perturbation force is obtained after filtering with a first-order low-pass filter. for: (8) in, Represents the Laplace variable. This represents the cutoff frequency of the low-pass filter. Based on the properties of generalized momentum in the dynamic equations, It can be calculated using the following formula: (9) in, Representing generalized momentum. Choosing an intermediate variable. After transformation, we obtain: (10) Since the actual controller is a discrete-time system, signal sampling is performed with a fixed time step. Therefore, the system needs to be described in discrete time, rather than as a continuous-time differential equation. Thus, formula (10) is transformed into discrete time as follows: (11) in, Indicates the low-pass filter coefficients. ; represents the generalized momentum in discrete time, represents the coefficient related to the filter cutoff frequency, ; represents the Z-transform operator. After the filtering calculation of formula (11), the end contact force is calculated as: (12) where, represents the contact force of the robot arm, represents the driving joint selection matrix, represents the Jacobian matrix, represents the disturbance force.

[0040] It is a widely used and effective method to construct an optimal control problem (OCP) based on a model to achieve the motion control of a robot. Considering the high dimension and nonlinearity of the whole-body dynamics model, it will lead to long calculation time and difficulty in solving the optimization problem in predictive control, thereby putting strict requirements on the real-time performance and computing resources of the on-board controller. In order to ensure real-time performance, it is necessary to use a simplified model to reduce the size of the optimization problem. Therefore, the present application constructs an integrated dynamics model suitable for predictive control, which maintains the key information of dynamic characteristics while ensuring computational efficiency, thereby effectively improving the real-time performance and robustness of the control system.

[0041] For simplicity, the present application does not distinguish between left and right wheels in the following motion analysis. The wheel dynamics equation in the control coordinate system is: (13) (14) where, and represent the weight and inertia tensor of the wheel, represents the acceleration of the wheel in the control coordinate system, represents the support force provided by the ground in the control coordinate system, represents the transformation matrix from the inertial coordinate system to the control coordinate system, represents the interaction force between the wheel and the leg, represents the gravity vector, represents the vector from the wheel center to the wheel-ground contact point, represents the wheel radius, represents the wheel angular velocity, represents the interaction torque between the wheel and the leg. Assuming that there is no relative sliding between the wheel and the ground, the following constraints are satisfied: (15) Joint the above formulas to eliminate the ground contact force And obtain the coordinate system The wheel dynamics equation along the x-axis is: (16) Here, matrix The number in the parentheses at the top right indicates a specific number in the matrix. Okay. Due to the robot's mechanical structure, the distance between the two wheels along the y-axis in the control coordinate system remains constant. This can be derived through forward kinematics, where the vector... , as well as The relationship between them in the acceleration layer can be expressed by the following equation: (17) in, This represents the yaw rate excluding the torso. Since the robot's pitch and roll angular velocities during motion are small and negligible, the above formula can be approximated as: (18) in, This indicates the distance between the torso and the wheel along the x-axis.

[0042] Since the robot's mass is mainly concentrated in the torso and wheels, and the limbs are relatively light, this invention removes the constraints of joint space and ignores the influence of joint velocity on the center of mass momentum and the influence of limb link movement on the system's inertial tensor when simplifying the whole-body dynamics model. Assuming that the centers of mass of all links except the wheels are concentrated at the torso's center of mass, this invention models the robot's torso as a floating base. The single rigid body dynamics model of the torso in the world coordinate system is as follows: (19) (20) in, Indicates the mass of the torso. The inertial tensor representing the torso. Indicates the left and right wheel legs, for example and These represent the vectors pointing from the center of mass of the torso to the points of application of the interaction forces between the left and right wheels and legs; This represents the interaction force between the left and right wheels and the legs in the world coordinate system. This represents the interaction torque between the left and right wheels and the leg in the world coordinate system. This indicates that the end effector force of the robotic arm, estimated based on generalized momentum, is added to the model as a known quantity. Let represent the vector pointing from the end effector of the robotic arm to the base centroid. Assuming small roll and pitch velocities, and negligible off-diagonal terms of the inertia tensor, the above equation can be approximated as: (21) Due to the small pitch and roll angles, the inertia tensor in the body frame can be obtained by the following equation: (22) where, represents the fixed inertia tensor in the trunk coordinate frame.

[0043] 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 to be equal in size and opposite in 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 leg. Combined with the above formula, we can get: (23) Definition , and represent the distance between the left and right wheels and the trunk along the x-axis direction in the control coordinate frame.

[0044] The state variable is selected as , and is taken as the control input, the dynamics equation that satisfies the non-holonomic constraint and the internal force / torque transmission is as follows: (24) where, (25) (26) (27) where, and involve row permutation operations, that is: , and .

[0045] (II) Motion planning 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: (28) wherein, 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.

[0046] (1) Adaptive motion distribution 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: (29) wherein, represents the weight coefficient. Considering the stability of the robot, the attitude of the base is not included in the motion distribution. A stable and unchanged 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 it exceeds the motion range of the robot arm, the motion needs to be distributed to the base. The weight coefficient is calculated by the following formula: (30) (31) 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: (32) 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.

[0047] (2) Force manipulability enhancement task 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).

[0048] 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: (33) The partial derivative of the joint of the robot arm can be represented as: (34) Thus, the joint speed command of the robot arm is: (35) The velocity command of the null-space internal force manipulability task is: (36) Substitute it into equation (4) to obtain the desired base velocity and robot joint velocity for force manipulability enhancement of the robot contact disturbance force.

[0049] (Three) motion control (1) Model predictive controller MPC The present application designs a discrete-time finite horizon model predictive controller, which generates the desired force wrench between the wheels and the torso 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 looped, and only the optimal control input at the current time is applied each iteration, and it is recalculated at the next time.

[0050] The discrete state space form of the dynamics equation (24) can be expressed as: (37) Wherein, , is the coefficient matrix; represents the system state at the step, represents the control input at the step. Assuming that the prediction step is , in order to make the state of the robot track the trajectory within the prediction step as much as possible, the MPC optimization problem is constructed to solve the optimal wheel-leg interaction force wrench at the current time as follows: (38) Wherein, represents the system state at the step, represents the system state at the step, represents the reference system state at the time, represents the control input at the step, represents the control input of the system at the time, , and represents a diagonal positive semi - definite weight matrix, The term is a penalty term that penalizes sudden changes in the control input between consecutive time steps to prevent the robot from being unstable. To prevent the hub motor current from overloading and the wheels from slipping, a safety constraint is used to limit the wheel reaction force within a conical boundary: (39) where, , , represent the components of the wheel - leg interaction force along the x, y, and z axes, represents the drive coefficient, the magnitude of which is related to the performance of the wheel motor. The above - mentioned constraint equation is approximately linearized using the friction cone as follows: (40) In addition, due to the non - holonomic constraint, the wheels cannot actively provide lateral driving force and driving torques about the x - axis and z - axis in the control coordinate system. Therefore, this constraint condition needs to be incorporated into the framework of the optimization problem, expressed as: (41) 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. ​​​​​​​​​​​​​​​​corresponding weight matrix, denotes the number of tasks, denotes the contact Jacobian matrix, denotes the final desired wheel-leg interaction force wrench, including wheel-leg interaction force and torque, denotes the wheel-leg interaction force and torque optimized by the model predictive controller, denotes the non-holonomic constraint matrix, denotes the force and joint torque constraint matrix. Each task is described in the form of such equality constraint. To satisfy the dynamics constraint and coordinate the balance between MPC and WBC, the present invention introduces a slack variable for relaxing the constraint to increase the solvability of the problem. The optimization variable is defined as: , next, each task will be described in detail.

[0052] (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.

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

[0054] wherein, denotes the Jacobian matrix corresponding to the sternal translation task, denotes the expected acceleration of the sternum, , and denote the displacement, velocity and acceleration of the sternal reference, , denote the feedback gain matrix. The expected sternal acceleration is generated by the PD control rate from the reference trajectory containing displacement, velocity and acceleration, and the current motion state of the sternal reference is obtained by the Kalman filter.

[0055] 2) Sternal rotation, the sternal rotation task in the acceleration layer is defined as:

[0056] wherein, denotes the Jacobian matrix corresponding to the sternal rotation task, denotes the expected angular acceleration of the sternum, and denote the quaternion corresponding to the reference rotation direction and the quaternion corresponding to the current rotation direction, denotes the reference rotation angular velocity. The error of the rotation direction uses the minus box operator Definition, it calculates the difference between the two quaternions and converts it to a three-dimensional rotation.

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

[0058]

[0059] wherein, represents the Jacobian matrix corresponding to the left leg swing task, represents the Jacobian matrix corresponding to the right leg swing task, represents the expected acceleration of the relative distance between the wheel and the torso, represents the reference value of the relative distance between the wheel and the torso.

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

[0061] wherein, and respectively represent the Jacobian matrices corresponding to the wheel and torso positions in the world coordinate system, and the difference between the two and the conversion to the control coordinate system can obtain the Jacobian matrix of the leg swing task.

[0062] (2) Energy efficiency and relaxation optimization: in order to optimize the joint torque output, the energy efficiency function is introduced. Compared with the leg joints, the wheel joints are assigned a larger weight coefficient, so as to reduce the significant oscillation in the system. In addition, minimizing the relaxation variable can maintain the prediction performance of the system, while ensuring that the constraints are strictly followed in the optimization process. This task can be described as: .

[0063] Embodiment two The embodiment discloses a wheel-legged robot operation control system based on manipulability optimization, comprising: a motion distribution module configured to: acquire sensor data of the robot, obtain state vectors of the base and the end of the robot arm through a state estimator; according to the robot arm end trajectory tracking task and the robot arm joint motion constraint, adaptively calculate a weight matrix by using the state vectors of the base and the robot arm joints, and realize coordinated motion distribution of the base and the robot arm joints; a manipulability enhancement module configured to: 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; determine the manipulability 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 manipulability direction; The cooperative control module is configured to: acquire a speed instruction of the 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, subsequently, 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 whole-body motion control of the robot.

[0064] Embodiment three The purpose of this 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 implements the steps of the method of embodiment one when executing the program.

[0065] Embodiment four The purpose of this 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 perform the steps of the method of embodiment one.

[0066] The steps involved in the devices of embodiments three and four above correspond to the method of embodiment one, and the specific embodiments can be seen in the relevant description of embodiment one. The term "computer-readable storage medium" should be understood to include a single medium or multiple media of one or more instruction sets; it should also be understood to include any medium capable of storing, encoding, or carrying instruction sets for execution by a processor and causing the processor to perform any of the methods of the present application.

[0067] Those skilled in the art should understand that the modules or steps of the present application described above can be implemented by a general computer device, and alternatively, they can be implemented by program code executable by a computing device, so that they can be stored in a storage device for execution by a computing device, or they can be made into individual integrated circuit modules, or a plurality of modules or steps among them can be made into a single integrated circuit module. The present application is not limited to any specific combination of hardware and software.

[0068] The above description is only the preferred embodiments of the present application and is not intended to limit the present application. For those skilled in the art, the present application can have various modifications and changes. Any modification, equivalent replacement, improvement, etc. made within the spirit and principles of the present application shall be included in the protection scope of the present application.

[0069] The above description of the specific embodiments of the present application in conjunction with the accompanying drawings is not a limitation on the protection scope of the present application, and those skilled in the art should understand that various modifications or changes made by those skilled in the art on the basis of the technical solutions of the present application without creative labor are still within the protection scope of the present application.

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