Six-degree-of-freedom wheel-foot robot control method, system and device and storage medium

Abstract

The invention discloses a six-degree-of-freedom wheel-foot robot control method, system and device and a storage medium, and relates to the technical field of robotics.The method comprises the steps that whole-body dynamics modeling is conducted on a wheel-foot robot, and a target motion equation of the wheel-foot robot is obtained; calculating a current task state of the wheel-foot robot, a Jacobian matrix and a ground normal vector at a wheel-foot contact point according to the sensor data; determining a target expected acceleration according to the current task state and the reference task state; constructing an optimization problem by using the Jacobian matrix, the ground normal vector and the target expected acceleration; solving the optimization problem under the constraint of the target motion equation to obtain a driving torque; and controlling the wheel-foot robot to move according to the driving torque. The optimization problem constructed by the method comprises the ground normal vector at the wheel-foot contact point, leg dynamics is considered when the wheel-foot robot is controlled, the determined driving torque can more stably drive the wheel-foot robot to pass through the uneven terrain, and the robustness and the passing capacity are improved.

Description

Technical Field

[0001] The present application relates to the field of robotics technology, and in particular to a control method, system, device and storage medium for a six-degree-of-freedom wheeled robot. Background Art

[0002] Wheeled bipedal robots have attracted increasing attention in the fields of exploration and inspection. However, most current studies ignore leg dynamics to simplify computation, which limits the full motion potential of wheeled robots. At the same time, wheeled robots face challenges when traversing uneven terrain. Summary of the invention

[0003] The main purpose of the embodiments of the present application is to propose a control method, system, device and storage medium for a six-degree-of-freedom wheeled robot to improve the real-time performance and adaptability of the wheeled robot to uneven terrain.

[0004] To achieve the above object, an embodiment of the present application provides a control method for a six-degree-of-freedom wheeled robot, the method comprising the following steps:

[0005] Performing whole-body dynamics modeling on the wheeled robot to obtain a target motion equation of the wheeled robot;

[0006] Calculate the current task state, Jacobian matrix and ground normal vector at the wheel-foot contact point of the wheel-foot robot according to the sensor data;

[0007] Determining a target expected acceleration according to the current task state and the reference task state;

[0008] Constructing an optimization problem using the Jacobian matrix, the ground normal vector, and the target expected acceleration;

[0009] Solving the optimization problem under the constraint of the target motion equation to obtain the driving torque;

[0010] The movement of the wheeled robot is controlled according to the driving torque.

[0011] In some embodiments, performing whole-body dynamics modeling on the wheeled robot to obtain the target motion equation of the wheeled robot includes the following steps:

[0012] Cutting off a passive joint in the kinematic chain of the wheeled-legged robot, thereby generating a spanning tree of a closed-loop system;

[0013] defining a first generalized coordinate of the spanning tree and the driving torque, thereby generating a first motion equation of the spanning tree;

[0014] The expression of the first motion equation is:

[0015]

[0016] Where q represents the first generalized coordinate; H∈R 16×16 represents the generalized inertia matrix, C∈R 16 represents the generalized bias force, and the generalized inertia matrix includes Coriolis force, centripetal force and gravity terms; u∈R 16 and denote the generalized velocity set and generalized acceleration set respectively; S∈R 16×6 is the selection matrix, τ a ∈R 6 represents the driving torque of the driving joint; τ gc ∈R 16 represents the ground contact force applied in the joint space;

[0017] Adding a constraint force that restricts the movement of the wheel-foot of the wheel-foot robot to the first motion equation to obtain a second motion equation;

[0018] Eliminate the constraint force in the second motion equation according to the virtual power principle to obtain a third motion equation;

[0019] Calculating the ground contact force, and substituting the ground contact force into the third motion equation to obtain the target motion equation;

[0020] The expression of the target motion equation includes:

[0021]

[0022] Among them, H y =G T HG∈R 12×12 , C y =G T C∈R 12 ; represents acceleration, y represents the second generalized coordinate, Constrained to 0;

[0023] The expression of the ground contact force is:

[0024]

[0025] in, is the contact Jacobian matrix; F C ∈R 4 is the rolling restraint force, C F ∈R 2 ×4 represents the friction curve related to speed; C J IC and C F The calculation of is related to the ground normal vector.

[0026] In some embodiments, the step of calculating the current task state, the Jacobian matrix, and the ground normal vector at the wheel-foot contact point of the wheel-foot robot according to the sensor data comprises the following steps:

[0027] Inputting the sensor data into an extended Kalman filter to determine the second generalized coordinates and velocity;

[0028] Determine the current task state, the Jacobian matrix, and the contact position of the wheel-foot of the wheel-foot robot according to the second generalized coordinates and the speed;

[0029] Determine a global point cloud set based on the sensor data;

[0030] Determine the normal vectors in the global point cloud set using principal component analysis to obtain a global ground normal vector map;

[0031] The normal vector of the contact position is determined in the global ground normal vector map as the ground normal vector.

[0032] In some embodiments, determining the target expected acceleration according to the current task state and the reference task state comprises the following steps:

[0033] Determine a first expected acceleration of the posture task of the wheeled robot according to the current task state and the reference task state using a proportional-differential controller;

[0034] Determine a second expected acceleration of the balancing task of the wheeled robot according to the current task state and the reference task state using a linear quadratic regulator;

[0035] The first expected acceleration and the second acceleration are reordered according to task priority to obtain the target expected acceleration.

[0036] In some embodiments, the determining of the first expected acceleration of the posture task of the wheeled robot according to the current task state and the reference task state by using a proportional-differential controller comprises the following steps:

[0037] Define the posture task status;

[0038] The expression of the posture task state is:

[0039] Λ=[φ h α β γ] T ∈R 5 ;

[0040] Wherein, Λ represents the posture task state; φ represents the separation angle between the wheel-foot robot; h represents the height of the wheel-foot robot; α, β, and γ represent the roll angle, pitch angle, and yaw angle of the head of the wheel-foot robot, respectively;

[0041] Determining the attitude task according to the proportional-derivative controller and the attitude task state;

[0042] The expression of the posture task is:

[0043]

[0044] in, des a n represents the posture task; n is the index in Λ; ref Λ n K represents the nth reference task state; pn and K dn Represent the proportional gain and differential gain respectively;

[0045] defining the first expected acceleration according to the posture task;

[0046] The expression of the first expected acceleration is:

[0047] des a=[ des a1… des a5] T ∈R 5 ;

[0048] in, des a represents the first expected acceleration.

[0049] In some embodiments, the determining the second expected acceleration of the balancing task of the wheeled robot according to the current task state and the reference task state by using a linear quadratic regulator comprises the following steps:

[0050] Modeling the wheeled robot as a single rigid body based on a linear quadratic regulator;

[0051] orthogonally projecting the center of mass of the rigid body onto the sagittal plane;

[0052] The center-of-mass momentum of the rigid body is defined in the sagittal plane; the center-of-mass momentum includes the center-of-mass linear momentum, the center-of-mass angular momentum and the contact force;

[0053] determining a derivative of the center of mass momentum based on the gravity force twist and the ground contact force twist;

[0054] Determining the constraint conditions satisfied when the wheeled-legged robot is in a balanced state;

[0055] The expected relative acceleration of the center of mass of the rigid body is determined as the second expected acceleration according to the derivative of the center of mass momentum and the constraint condition.

[0056] In some embodiments, solving the optimization problem under the constraint of the target motion equation to obtain the driving torque comprises the following steps:

[0057] Defining optimization variables of the optimization problem; wherein the optimization variables include generalized acceleration, rolling restraint force and the driving torque;

[0058] Converting the target motion equation into a linear equality constraint equation;

[0059] Mapping the generalized acceleration to a task space using the Jacobian matrix and then converting it into a least squares problem;

[0060] Determine a plurality of quadratic programming problems with priorities according to the linear constraint equation and the least squares problem;

[0061] Solving each of the quadratic programming problems according to priority from high to low; wherein, in each solution, the solution result of the previous priority is used for the equality constraint of the quadratic programming problem of the next priority;

[0062] The driving torque of the optimization variable in the quadratic programming problem of a last priority is determined.

[0063] To achieve the above-mentioned purpose, another aspect of the embodiment of the present application provides a six-degree-of-freedom wheeled robot control device, the device comprising:

[0064] A modeling unit, used for performing whole-body dynamics modeling on the wheeled robot to obtain a target motion equation of the wheeled robot;

[0065] An estimation unit, used for calculating the current task state, Jacobian matrix and ground normal vector at the wheel-foot contact point of the wheel-foot robot according to the sensor data;

[0066] A task control unit, used to determine a target expected acceleration according to the current task state and a reference task state;

[0067] An optimization problem construction unit, used to construct an optimization problem using the Jacobian matrix, the ground normal vector and the target expected acceleration;

[0068] An optimization problem solving unit, used for solving the optimization problem under the constraint of the target motion equation to obtain a driving torque;

[0069] A control unit is used to control the movement of the wheeled robot according to the driving torque.

[0070] To achieve the above objective, another aspect of an embodiment of the present application provides a system, the system comprising a memory and a processor, the memory storing a computer program, and the processor implementing the above method when executing the computer program.

[0071] To achieve the above objective, another aspect of an embodiment of the present application provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and the computer program implements the above method when executed by a processor.

[0072] The embodiments of the present application include at least the following beneficial effects:

[0073] The present application can perform whole-body dynamics modeling on a wheeled robot to obtain the target motion equation of the wheeled robot; calculate the current task state, Jacobian matrix, and ground normal vector at the wheel-foot contact point of the wheeled robot based on sensor data; determine the target expected acceleration based on the current task state and the reference task state; construct an optimization problem using the Jacobian matrix, ground normal vector, and target expected acceleration; solve the optimization problem under the constraints of the target motion equation to obtain the driving torque; and control the movement of the wheeled robot based on the driving torque. The optimization problem constructed in the present application includes the ground normal vector at the wheel-foot contact point, that is, the leg dynamics are taken into account when controlling the wheeled robot, and then the driving torque obtained is determined to be able to drive the wheeled robot through uneven terrain more smoothly, thereby improving the robustness and passability of the wheeled robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0074] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings required for use in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0075] Figure 1 An example diagram of a six-degree-of-freedom two-wheeled leg robot provided in an embodiment of the present application;

[0076] Figure 2 A schematic flow chart of a control method for a six-degree-of-freedom wheeled robot provided in an embodiment of the present application;

[0077] Figure 3 An example diagram of dynamic modeling of a wheeled robot provided in an embodiment of the present application;

[0078] Figure 4 A flow chart of the whole body control framework provided for an embodiment of the present application;

[0079] Figure 5A schematic diagram of a local control framework provided in an embodiment of the present application;

[0080] Figure 6 This is an x-axis impact comparison experiment diagram provided in the embodiment of the present application;

[0081] Figure 7 Performance comparison diagram of WBC and CMC provided in the embodiments of the present application;

[0082] Figure 8 An experimental diagram for adapting to different ground heights provided in the embodiment of the present application;

[0083] Fig. 9 A performance comparison diagram of different ground height experiments provided in the embodiments of the present application;

[0084] Fig.10 A slope U-turn test diagram provided in an embodiment of the present application;

[0085] Fig.11 A performance comparison diagram of a slope U-turn test provided in an embodiment of the present application;

[0086] Fig.12 A schematic diagram of the structure of a six-degree-of-freedom wheeled robot control device provided in an embodiment of the present application;

[0087] Fig.13 A schematic diagram of the hardware structure of a system provided in an embodiment of the present application. DETAILED DESCRIPTION

[0088] In order to make the purpose, technical solutions and advantages of the present application clearer, the present application is further described in detail below in conjunction with the accompanying drawings and examples. It should be understood that the specific embodiments described herein are only used to explain the present application and are not intended to limit the present application. When the following description refers to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the embodiments of the present application. They are only examples of devices and methods consistent with some aspects of the embodiments of the present application as detailed in the attached claims.

[0089] It is understood that the terms "first", "second", etc. used in this application can be used to describe various concepts in this article, but unless otherwise specified, these concepts are not limited by these terms. These terms are only used to distinguish one concept from another concept. For example, without departing from the scope of the embodiment of the present application, the first information may also be referred to as the second information, and similarly, the second information may also be referred to as the first information. Depending on the context, the words "if" and "if" as used herein can be interpreted as "at the time of" or "when" or "in response to determination".

[0090] The terms "at least one", "multiple", "each", "any", etc. used in this application, at least one includes one, two or more, multiple includes two or more, each refers to each of the corresponding multiple, and any refers to any one of the multiple.

[0091] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art to which this application belongs. The terms used herein are only for the purpose of describing the embodiments of this application and are not intended to limit this application.

[0092] Before describing the embodiments of the present application in detail, some related technologies involved in the embodiments of the present application are first described as follows:

[0093] The two-wheeled legged robot is a mobile robot that combines many advantages of traditional mobile robots. It retains the high movement speed and efficiency of wheeled robots, while its leg structure can buffer impact when traversing uneven terrain or encountering external interference. In addition, it can dynamically adjust its center of mass to adapt to different load conditions.

[0094] In recent years, many two-wheeled and legged robots with various configurations have been designed. Related technology 1 developed the Handle robot, which demonstrated excellent mobility and capabilities. However, the specific implementation method of Handle has not been disclosed. Ascento developed by related technology 2 is driven by four motors and has the advantages of compact design and low cost. However, its mobility and exploration capabilities are also limited by its configuration. Ollie designed by related technology 3 is a two-wheeled and legged robot with planar parallel mechanisms on two legs. This structure has higher rigidity and stability, but requires a larger volume, which limits the robot's workspace and to a certain extent limits Ollie's exploration capabilities in extreme environments. Taking into account the cost and mobility performance, the present application provides a six-degree-of-freedom two-wheeled and legged robot Diablo with a two-legged serial mechanism, as shown in the example figure. Figure 1 shown.

[0095] Figure 1 The DIABLO is a wheeled biped robot with six degrees of freedom, composed entirely of direct drive joints. The motion control system of DIABLO is managed by a microcontroller with an integrated high-performance IMU (BMI088) and a microcomputer. The encoders built into the motors provide the robot's joint angles and angular velocities. The Livox Mid-360 LiDAR installed on the robot's head scans the environment and collects point cloud data in real time.

[0096] The two-wheeled foot robot is a typical example of an underactuated robot. In research, the two-wheeled foot robot is often modeled as a wheeled inverted pendulum (WIP) model or its variants. Then control strategies are developed based on these simplified models. The six-degree-of-freedom robot SR600 designed by Related Art 4 is modeled as a WIP, and a PID controller is used to achieve balance control and height control. Similarly, Related Art 5 adopts the same modeling method and uses an LQR controller for balance control. Related Art 6 proposes a wheeled spring-loaded inverted pendulum model and plans jumping actions based on it. Related Art 7 et al. designed an eight-degree-of-freedom two-wheeled foot robot and proposed a wheeled rigid body dynamics model to provide additional degrees of freedom. They used an MPC-based method to control the robot's posture and leg splitting. The related work of this application developed a second-order WIP model and performed a separate dynamic analysis of the robot's rigid body. In addition, a comprehensive motion controller was proposed to control the robot to complete various tasks. The above method can achieve specific motion control of the robot and show satisfactory performance on flat ground. However, the simplified model cannot fully represent all the dynamic characteristics of the two-wheeled foot robot. Therefore, it will face challenges in tasks that require leg buffering, such as traversing uneven terrain. Related technology 8 designed a six-degree-of-freedom two-wheeled leg robot (WBR) SKATER and proposed a hierarchical control framework. In addition, two control strategies were adopted to adjust the robot's head roll angle, allowing the robot to achieve high-speed turns and adapt to different terrain heights. Experiments demonstrated its mobility and terrain adaptability. However, this method requires a height difference between the two legs to be effective. Therefore, it may encounter challenges when traversing terrain with extremely small height differences (such as slopes).

[0097] Whole body control (WBC) is a model-based controller that maps joint space to task space, taking advantage of the inherent redundancy of the robot. This method maximizes the use of the robot's joint degrees of freedom and effectively coordinates its actions to complete various tasks. Initially, WBC was mainly used to control humanoid robots. In recent years, it has been successfully applied to two-wheeled robots. Related technology 9 proposed a WBC scheme for related technology 5, and experiments demonstrated the robot's robustness in the face of external interference and its adaptability to different ground heights. Related technology 5 deals with the constraints of the robot's contact with the ground by introducing contour parameters. The contour parameters depend on the ground normal vector. However, related technology 5 does not provide a method for estimating the ground normal vector. The present application proposes a method for estimating the ground normal vector. When ground information is obtained in advance, the present application can adopt appropriate planning and control strategies. Compared with the passive adaptation of the SKATER robot, the present application enables the robot to actively adapt to the terrain.

[0098] This application uses point cloud data captured by LiDAR and a method for estimating point cloud surface normals to estimate ground normals. Currently, point cloud surface normal estimation methods can be roughly divided into traditional geometry-based methods and learning-based methods. Geometry-based methods, such as principal component analysis (PCA) and moving least squares (MLS), rely heavily on the choice of neighborhood size and are highly dependent on user experience. Learning-based methods, such as PCPNet, DeepFit, AdaFit, and TRFit, significantly improve the prediction of surface normals, especially in challenging areas such as edges and corners. However, they require a lot of computing resources and have slow processing speeds. In order to ensure real-time and stable applicability, this application uses an improved adaptive optimal neighborhood PCA method for terrain estimation.

[0099] This application includes the following technical solutions:

[0100] The complete three-dimensional dynamic model of the closed-loop wheeled biped robot was derived. This technical feature provides a solid theoretical basis for a deeper understanding of the robot's motion characteristics, and helps to more accurately control the robot's movements and give full play to its performance.

[0101] An online terrain estimation algorithm based on LiDAR is proposed. The algorithm can perceive the terrain environment of the robot in real time, provide key information for the robot's actions in complex terrain, and enable it to better adapt to different terrain conditions.

[0102] A whole body control (WBC) framework with integrated terrain estimation is developed based on a wheeled biped robot. This framework closely combines terrain estimation with robot control, enabling the robot to adjust its movements in time according to terrain changes, thus improving its stability and adaptability in different terrains.

[0103] In summary, wheeled bipedal robots are gaining increasing attention in the field of exploration and inspection. However, most current studies ignore leg dynamics to simplify computation, which limits the full motion potential of robots. At the same time, robots face challenges when traversing uneven terrain.

[0104] To solve the above problems, this application provides a complete dynamic model for a six-degree-of-freedom wheeled bipedal robot and designs a full-body control framework with terrain estimation function. The model combines the closed-loop dynamics of the robot and the ground contact model based on the estimated ground normal vector. The team proposed a terrain estimation algorithm based on FAST-LIO and improved principal component analysis. In terms of the task controller, PD control law and LQR are used for posture control and balance control based on center of mass dynamics, respectively. In addition, a hierarchical optimization method is used to solve the whole-body control problem. The experiment verifies the real-time performance of the terrain estimation algorithm and demonstrates the robustness and ability of the robot in traversing uneven terrain.

[0105] Next, the six-degree-of-freedom wheeled-legged robot control method provided in this application is explained.

[0106] Reference Figure 2 The embodiment of the present application provides a control method for a six-degree-of-freedom wheeled robot. The method may include but is not limited to S200 to S250, which are as follows:

[0107] S200: Performing whole-body dynamics modeling on the wheeled robot to obtain a target motion equation of the wheeled robot.

[0108] Further, S200 may include:

[0109] A passive joint is cut off in the kinematic chain of the wheeled robot, thereby generating a spanning tree of the closed-loop system; it should be noted that in this embodiment, cutting off a passive joint in the kinematic chain does not mean physically cutting off the wheeled robot, but virtually cutting off a passive joint in the kinematic chain for the convenience of calculation;

[0110] defining a first generalized coordinate of the spanning tree and the driving torque, thereby generating a first motion equation of the spanning tree;

[0111] The expression of the first motion equation is:

[0112]

[0113] Where q represents the first generalized coordinate; H∈R 16×16 represents the generalized inertia matrix, C∈R 16 represents the generalized bias force, and the generalized inertia matrix includes Coriolis force, centripetal force and gravity terms; u∈R 16 and denote the generalized velocity set and generalized acceleration set respectively; S∈R 16×6 is the selection matrix, τ a ∈R 6 represents the driving torque of the driving joint; τ gc ∈R 16 represents the ground contact force applied in the joint space;

[0114] Adding a constraint force that restricts the movement of the wheel-foot of the wheel-foot robot to the first motion equation to obtain a second motion equation;

[0115] Eliminate the constraint force in the second motion equation according to the virtual power principle to obtain a third motion equation;

[0116] Calculating the ground contact force, and substituting the ground contact force into the third motion equation to obtain the target motion equation;

[0117] The expression of the target motion equation includes:

[0118]

[0119] Among them, H y =G T HG∈R 12×12 , C y =G T C∈R 12 ; represents acceleration, y represents the second generalized coordinate, Constrained to 0;

[0120] The expression of the ground contact force is:

[0121]

[0122] in, is the contact Jacobian matrix; F C ∈R 4 is the rolling restraint force, C F ∈R 2 ×4 represents the friction curve related to speed; C J IC and C F The calculation of is related to the ground normal vector.

[0123] Specifically, Figure 3 As shown, this embodiment defines an inertial coordinate system I, a floating base coordinate system B, and a contact coordinate system C (wherein the contact coordinate system C of the left wheel is l , is the contact coordinate system C of the right wheel r ). In the application examples, red, green and blue arrows represent the x-axis, y-axis and z-axis of the coordinate system, respectively. In addition, bold lowercase letters are used to represent vectors, and bold uppercase letters are used to represent matrices in the application examples.

[0124] Figure 3 It includes the robot coordinate system, generalized coordinates, driving torque and leg structure. Figure 3 (a)q {left,right} and τ {left,right} represent the generalized coordinates and driving torques on both sides of the robot, respectively. I n l and I n r is the ground normal vector at the point where the wheel contacts the ground. Figure 3 (b) B {left,right} represents the rigid bodies on both sides of the robot. The red lines represent the simplified version of the leg linkage. The swing angle θ is the angle between the simplified leg and the vertical direction.

[0125] Specifically, this embodiment can perform whole-body dynamics modeling. A common strategy for formulating the motion equations of a closed-loop system is to first virtually cut off a passive joint in the kinematic chain to generate a spanning tree for the system. Then, closed-loop constraints are applied to the motion equations of the spanning tree.

[0126] Specifically, Figure 3 As shown in (a), this embodiment defines the generalized coordinates q and driving torque τ of the spanning tree a as follows:

[0127]

[0128] Among them, q b ∈R 3 ×SO(3) represents the coordinates of the undriven base, q j ∈R 10 represents the joint coordinates, I r IB 1 and R IB are the translation and rotation of the robot base.

[0129] The equation of motion for the spanning tree (the first equation of motion) can be expressed as follows:

[0130]

[0131] Among them, H∈R 16×16 represents the generalized inertia matrix, C∈R 16 Represents the generalized bias force, which takes into account the Coriolis force, centripetal force and gravity terms. u∈R 16 and They represent the generalized velocity set and generalized acceleration set respectively. 16×6 is the selection matrix, τ a ∈R 6 Represents the torque driving the joint. τ gc ∈R 16 Represents the ground contact force applied in joint space.

[0132] like Figure 3 As shown in (b), each leg of the robot has a parallelogram mechanism consisting of four rigid bodies, which imposes clear motion constraints on the robot system:

[0133]

[0134] Let y∈R 3 ×SO(3)×R 6 represents the independent position variable vector of the closed-loop system, which uniquely defines q. This embodiment defines y as follows:

[0135] y=[ I r IB R IB q1 q5 q4 q6 q 10 q9] T #(3.4)

[0136] And provide the loop closure function as follows:

[0137] q=γ(y)#(3.5)

[0138] Differentiating the above formula yields:

[0139]

[0140] in, I r IB represents the position vector of point B relative to the origin of coordinate system I, and is represented in coordinate system I. The above notation usage also applies to the position, velocity and Jacobian matrix in this article. In addition, superscripts represent the components of vectors in specific directions, which are required in some embodiments (e.g. express I r IB component in the x-direction).

[0141] In this embodiment, τ c Defined as the constraint force that limits the movement of the legs. By introducing this force into formula (3.2), the motion equation of the closed-loop system (the second motion equation) is:

[0142]

[0143] According to Jourdain's virtual power principle, τ c It has the following properties:

[0144] G T τ c =0#(3.8)

[0145] Therefore, by multiplying G on the left of formula (3.7) T To eliminate τ c , this embodiment obtains another form of the closed-loop system motion equation (the third motion equation):

[0146]

[0147] Among them, H y =G T HG∈R 12×12 , C y =G T C∈R 12 , and this embodiment obtains a new set of generalized coordinates y, speed u y∈R 12 and acceleration

[0148] This embodiment uses the following method to calculate the ground contact force τ gc In order to prevent the relative movement between the contact point and the ground, the accelerations in the x and z directions in the contact coordinate system are Constrain to 0, and set τ gc It is expressed as:

[0149]

[0150] in is the contact Jacobian matrix. F C ∈R 4 is the rolling restraint force, C F ∈R 2 ×4 Represents the friction curve as a function of speed. C J IC and C F Calculation of the ground normal vector I n is related. Combining formulas (3.9) and (3.10), this embodiment obtains the final motion equation (target motion equation):

[0151]

[0152] S210: Calculate the current task state, Jacobian matrix and ground normal vector at the wheel-foot contact point of the wheel-foot robot according to the sensor data.

[0153] Further, S210 may include:

[0154] Inputting the sensor data into an extended Kalman filter to determine the second generalized coordinates and velocity;

[0155] Determine the current task state, the Jacobian matrix, and the contact position of the wheel-foot of the wheel-foot robot according to the second generalized coordinates and the speed;

[0156] Determine a global point cloud set based on the sensor data;

[0157] Determine the normal vectors in the global point cloud set using principal component analysis to obtain a global ground normal vector map;

[0158] The normal vector of the contact position is determined in the global ground normal vector map as the ground normal vector.

[0159] Reference Figure 4The whole-body control (WBC) framework of this embodiment is divided into three algorithm modules: (a) Estimation module: This module processes the sensor data and calculates the robot's task state Λ task And the Jacobian matrix J. In addition, this embodiment estimates the ground normal vectors at the contact points of the two legs I n. (b) Task control module: User input reference task status ref Λ task and Λ task , use the mission controller to control the desired acceleration des a task , and then pass it to the optimization module. (c) Optimization module: This module receives J, I n and des a task They are used to formulate the optimization problem and solve for the drive torque τ a , which is then used as input to the robot actuators.

[0160] The technical solutions of the estimation module include:

[0161] like Figure 4 As shown in (a), the sensor data is input into the state estimation module to obtain the generalized coordinates y and velocity u y and point cloud collection In this paper, this embodiment uses an extended Kalman filter (EKF) to estimate y and u y , and use FAST-LIO to obtain Then, y and u y Input to the kinematics module to calculate J, Λ task and the contact position p of the two wheels cl 、p cr Using a modified principal component analysis (PCA), the normal estimator computes normal vectors and stores them in a global map Afterwards, the normal vector search module uses p cl 、p cr exist Search for the ground normal vector in I n.

[0162] FAST-LIO processes the raw point cloud data and IMU data to compute a global point cloud frame, which is then merged into Next, this embodiment uses principal component analysis (PCA) to estimate The normal vector on . This method performs well in providing accurate estimates for smooth surfaces and is remarkably efficient. However, its results are very sensitive to the chosen neighborhood size. An inappropriate choice in this regard may introduce instabilities in the estimation process.

[0163] To solve this problem, this embodiment adopts an improved principal component analysis method, which determines the optimal neighborhood size k of each vertex by minimizing an entropy function.

[0164]

[0165] where λ k1 ,λ k2 ,λ k3 are the three eigenvalues ​​of the covariance matrix when the neighborhood size is k. This approach balances accuracy, speed, and robustness without requiring much manual selection of the neighborhood size.

[0166] After determining the size of the neighborhood, this embodiment calculates the covariance matrix of all vertices in the neighborhood of the current point cloud vertex, and uses the eigenvector corresponding to the minimum eigenvalue of the matrix as the normal vector of the current vertex.

[0167] The estimated normal vector set is stored in the global ground normal vector map In this embodiment, a separate thread is used for position search, which continuously synchronizes the update of the map and the estimated contact positions p on both sides. cl 、p cr Used to search for the ground normal vector in I n.

[0168] S220: Determine a target expected acceleration according to the current task state and the reference task state.

[0169] Further, S220 may include:

[0170] Determine a first expected acceleration of the posture task of the wheeled robot according to the current task state and the reference task state using a proportional-differential controller;

[0171] Determine a second expected acceleration of the balancing task of the wheeled robot according to the current task state and the reference task state using a linear quadratic regulator;

[0172] The first expected acceleration and the second acceleration are reordered according to task priority to obtain the target expected acceleration.

[0173] Reference Figure 4 (b), the user provides input ref Λ task , the estimation module provides input Λ to the task controller taskIn this embodiment, the robot's motion task is divided into a balance task and a posture task. A LQR (Linear Quadratic Regulator) controller is used for the balance task, and a PD (Proportional-Differential) controller is used for the posture task to map the robot's motion to acceleration levels: the expected acceleration of the balance task is des a CoM and the expected acceleration of the attitude task des a. This embodiment reorders the tasks according to their priorities to obtain des a task .

[0174] Furthermore, the step of determining the first expected acceleration of the posture task of the wheeled robot according to the current task state and the reference task state by using a proportional-differential controller comprises the following steps:

[0175] Define the posture task status;

[0176] The expression of the posture task state is:

[0177] Λ=[φ h α β γ] T ∈R 5 ;

[0178] Wherein, Λ represents the posture task state; φ represents the separation angle between the wheel-foot robot; h represents the height of the wheel-foot robot; α, β, and γ represent the roll angle, pitch angle, and yaw angle of the head of the wheel-foot robot, respectively;

[0179] Determining the attitude task according to the proportional-derivative controller and the attitude task state;

[0180] The expression of the posture task is:

[0181]

[0182] in, des a n represents the posture task; n is the index in Λ; ref Λ n K represents the nth reference task state; pn and K dn Represent the proportional gain and differential gain respectively;

[0183] defining the first expected acceleration according to the posture task;

[0184] The expression of the first expected acceleration is:

[0185] des a=[ des a1 … des a5]T ∈R 5 ;

[0186] in, des a represents the first expected acceleration.

[0187] Specifically, the posture task includes a separation angle task, a height task, and a head orientation task.

[0188] The separation angle task is a special task. The robot of this embodiment has six identical actuators at the hips, knees and wheels. Therefore, it can squat, jump and split like a roller skater. This embodiment defines the separation angle φ as the difference between the two side swing angles.

[0189] φ=θ left -θ right #(4.3)

[0190] However, similar to roller skaters, when the robot's wheels are in contact with the ground, in order to perform a turn, a relative distance needs to be maintained between the two wheels. Therefore, during the robot's movement, this embodiment imposes a constraint, which also limits its separation angle.

[0191] Reference Figure 5 , Figure 5 is the local control frame. The sagittal and horizontal planes of the robot. C' is the orthographic projection of the center of mass (CoM) on the sagittal plane. C'r is the contact point of the right wheel when the separation angle is zero. This embodiment defines a local control frame N. The origin of the frame N is located at the contact point C l and C r In frame N, the x-axis is aligned with the direction of the robot head, the z-axis is aligned with the normal vector of the horizontal plane, and the y-axis is aligned with the support line when the separation angle is zero.

[0192] On the horizontal plane, d w It can be expressed as:

[0193]

[0194] This embodiment also estimates the height h of the robot in frame N:

[0195]

[0196] And this embodiment uses the Euler angles calculated in the ZYX order to determine the direction of the robot head. γ, β, α based on the ZYX order. γ, β, α represent the yaw angle, pitch angle, and roll angle, respectively. Then this embodiment defines the attitude task state as:

[0197] Λ=[φ h α β γ] T ∈R 5 #(4.6)

[0198] The control objectives of the posture tasks are decoupled, and this embodiment uses the PD control law to control them. n is the index in Λ, and the posture task with index n can be expressed as:

[0199]

[0200] Where K pn and K dn is the gain. Then this example defines the expected acceleration of the attitude task:

[0201] des a=[ des a1 … des a5] T ∈R 5 #(4.8)

[0202] Furthermore, the step of determining the second expected acceleration of the balancing task of the wheeled robot according to the current task state and the reference task state by using a linear quadratic regulator comprises the following steps:

[0203] Modeling the wheeled robot as a single rigid body based on a linear quadratic regulator;

[0204] orthogonally projecting the center of mass of the rigid body onto the sagittal plane;

[0205] The center-of-mass momentum of the rigid body is defined in the sagittal plane; the center-of-mass momentum includes the center-of-mass linear momentum, the center-of-mass angular momentum and the contact force;

[0206] determining a derivative of the center of mass momentum based on the gravity force twist and the ground contact force twist;

[0207] Determining the constraint conditions satisfied when the wheeled-legged robot is in a balanced state;

[0208] The expected relative acceleration of the center of mass of the rigid body is determined as the second expected acceleration according to the derivative of the center of mass momentum and the constraint condition.

[0209] Specifically, unlike other tasks, the control objectives of balance and forward motion are coupled. Therefore, this embodiment adopts LQR (Linear Quadratic Regulator) as the balance control strategy.

[0210] LQR is a model-based control strategy. By simplifying the robot into a single rigid body and orthogonally projecting its center of mass (CoM) to point C' on the sagittal plane, this embodiment establishes a center of mass dynamics model, such as Figure 5 shown.

[0211] In this embodiment, k CoM ∈R3 Defined as the center-of-mass momentum of the robot, it includes the linear momentum of the center of mass p CoM ∈R 2 , center of mass angular momentum N CoM ∈R and the contact force F NC ∈R 2 , all of which were in the sagittal plane.

[0212]

[0213] According to Newton-Euler theorem

[0214]

[0215] Where W g represents the gravitational force screw, W gc is the ground contact force screw, s CoM ,ω CoM and r CoM They represent the absolute position, angular velocity and relative position of the center of mass relative to the frame origin N. In the task space, this embodiment has:

[0216]

[0217] When the robot is in equilibrium, the following constraints must be met:

[0218] 1. The acceleration in the Z-axis direction is zero.

[0219] 2. The angular acceleration is zero.

[0220] This embodiment obtains the following state space representation from formula (4.10).

[0221]

[0222] By solving the Ricatti equation, this embodiment obtains the gain matrix K, and then, this embodiment obtains the expected relative acceleration of the center of mass.

[0223]

[0224] In addition, this embodiment defines the expected acceleration of the balancing task:

[0225]

[0226] S230: Construct an optimization problem using the Jacobian matrix, the ground normal vector, and the target expected acceleration.

[0227] S240: Solving the optimization problem under the constraint of the target motion equation to obtain the driving torque.

[0228] Furthermore, S240 may include:

[0229] Defining optimization variables of the optimization problem; wherein the optimization variables include generalized acceleration, rolling restraint force and the driving torque;

[0230] Converting the target motion equation into a linear equality constraint equation;

[0231] Mapping the generalized acceleration to a task space using the Jacobian matrix and then converting it into a least squares problem;

[0232] Determine a plurality of quadratic programming problems with priorities according to the linear constraint equation and the least squares problem;

[0233] Solving each of the quadratic programming problems according to priority from high to low; wherein, in each solution, the solution result of the previous priority is used for the equality constraint of the quadratic programming problem of the next priority;

[0234] The driving torque of the optimization variable in the quadratic programming problem of a last priority is determined.

[0235] Specifically, a whole body motion controller (WBC) performs tasks by coordinating the motion of all joints. This embodiment uses an optimization method to solve the WBC problem and define optimization variables.

[0236]

[0237] In the process of solving the whole body motion controller (WBC) optimization problem, this embodiment must follow the whole body dynamics. Therefore, this embodiment expresses the final motion equation (3.11) as the following linear equality constraint.

[0238]

[0239] Since this embodiment uses the generalized acceleration As part of the optimization variables, the present embodiment therefore uses a task controller to describe all tasks at the acceleration level. The present embodiment prioritizes des a and des a CoM Rearrange to get des a task ∈R 6 : balance, height, pitch angle, roll angle, leg separation angle and yaw angle. For each task with priority i, this embodiment uses the Jacobian matrix J i Map generalized acceleration to task space.

[0240]

[0241] Formula (4.17) can be written as a least squares problem:

[0242]

[0243] Then, this embodiment stacks all six tasks into matrix a according to the priority order task and vector b task middle.

[0244]

[0245] This embodiment uses a series of constrained quadratic programming (QP) to solve this problem. Figure 4 As shown in (c), in order to ensure that tasks are executed according to their priorities, this embodiment adopts a hierarchical optimization method. The QP problem with priorities can be written as:

[0246]

[0247] Among them, A eq,i , b eq,i represents an equality constraint. ineq , b ineq Represents an inequality constraint. In each optimization iteration, the result of the previous higher priority is used as the equality constraint of the subsequent lower priority.

[0248] In this embodiment, in order to ensure that the robot can perform the experiment correctly, this embodiment uses the posture task controller to calculate the expected acceleration instead of . Then, this embodiment incorporates the sum from formula (4.19) and the dynamic constraint (4.16) into the initial set of equality constraints. The inequality constraint is used to constrain the upper and lower limits of the drive torque.

[0249] After the last iteration, the driving torque τ as part of the optimization variable a will be input into the robot's actuators.

[0250] S250: Controlling the movement of the wheeled robot according to the driving torque.

[0251] The optimization problem constructed in this application includes the ground normal vector at the wheel-foot contact point, that is, the leg dynamics are taken into account when controlling the wheel-foot robot, and then it is determined that the obtained driving torque can drive the wheel-foot robot more smoothly through uneven terrain, thereby improving the robustness and passability of the wheel-foot robot.

[0252] Next, the solution of the embodiment of the present application will be described in detail and explained in conjunction with specific application examples:

[0253] This embodiment is a simulation verification, and the specific technical solution includes:

[0254] 1. Simulation platform:

[0255] This embodiment builds a simulation environment and verifies the control framework using the open source mobile robot simulation software Webots. The controller is developed using MATLAB, and the robot dynamics calculation is performed using the Spatial v2 dynamics library. In addition, this embodiment uses the laser radar, accelerometer, gyroscope, and inertial measurement unit (IMU) provided in Webots to obtain sensor data.

[0256] 2. Experiment:

[0257] This embodiment designs an experiment to test the robot's robustness and ability to traverse uneven terrain.

[0258] 2.1X-axis impact comparison experiment:

[0259] Figure 6 This is an x-axis impact comparison experiment diagram. In this embodiment, an x-axis impact comparison experiment is designed to verify the robot's ability to maintain balance, and the comprehensive motion controller (CMC) proposed in previous work is used as a comparison baseline.

[0260] Figure 7 Comparison of the performance of WBC and CMC. Figure 7 The upper, middle and lower figures are the x-coordinate, height h and center of mass deviation distance of the robot in the experiment. changes.

[0261] like Figure 6 As shown, a 12 kg ball (close to 50% of the robot's mass) is tied with a 1 meter long rope, pulled to a 90 degree position, and then released, causing an impact on the robot in the x-axis direction.

[0262] like Figure 7 As shown, during this process, the robot controlled by the baseline (the integrated motion controller CMC in the previous work) was pushed back by about 0.96 meters, the height change was 0.8 centimeters, the center of mass deviation reached 0.106 meters, and the robot recovered in 6.45 seconds. In contrast, the robot controlled by the WBC controller was pushed back by about 0.7 meters, the height change was 0.43 centimeters, the center of mass deviation reached 0.104 meters, and the robot recovered in 5 seconds. Compared with the baseline, WBC reduced the repulse distance by 27% and the height change by 46%. In addition, compared with the baseline, WBC restored stability in only 78% of the time required by the baseline. The comparison results show that the WBC controller of this embodiment significantly improves robustness.

[0263] 2.2 Adapt to different ground height experiments:

[0264] Figure 8 To adapt to the experimental diagram at different ground heights. Fig. 9 The upper and lower figures show the changes in the robot's height h and roll angle α during the experiment.

[0265] like Figure 8 As shown, in this experiment, the robot traverses two trapezoidal surfaces, each 0.17 m high and with an inclination of 15°, at a speed of 2 m / s.

[0266] like Fig. 9 As shown in the figure, during the whole process, the robot adjusted the contraction and extension of its left and right legs to maintain its posture and prevent it from falling. The height change of the robot was kept within 2 cm, and its roll angle was kept within 2°. This experiment demonstrated the robot's ability to traverse and adapt to uneven terrain.

[0267] 2.3 Slope U-turn experiment:

[0268] Fig.10 The above picture shows the slope U-turn experiment. Fig.10 The figure below visualizes the terrain estimate at time t4. Fig.11 The upper and lower figures are the estimated tilt angles Deviation distance from the robot's center of mass ψ is the actual tilt angle.

[0269] like Fig.10 As shown, the robot climbs a 15° slope at a speed of 1.5 m / s and makes a U-turn before returning to flat ground. At a specific moment t4 during the robot's descent, this embodiment visualizes the terrain estimation algorithm. The real-time point cloud data detected by the lidar is displayed in rainbow colors. This embodiment uses a grid map to present the global normal vector map The small yellow line represents the estimated normal vector of each grid. The three-color coordinate axes in the image represent the current position and orientation of the robot, while the purple arrow represents the position and orientation of the current estimated normal vector.

[0270] In order to enable the robot to detect the change of the ground normal vector earlier, this embodiment estimates the normal vector 0.9 meters ahead of the current position. In addition, in order to reduce the impact of sudden terrain changes on the robot, a low-pass filter is applied to smooth the perceived normal vector.

[0271] like Fig.11 As shown, this embodiment calculates the inclination angle between the estimated ground normal vector and the vertical direction To make the normal vector estimation result more intuitive. The terrain contour is well aligned with the terrain contour, and the average estimated slope is 12.3°. The terrain estimation of this embodiment demonstrates real-time performance and accuracy.

[0272] This embodiment also compares WBC controllers with and without terrain estimation. When entering a slope, the center of mass deviation of the robot without a normal vector is 0.08 meters. Similarly, when leaving a slope, the center of mass deviation without a normal vector reaches 0.051 meters. In contrast, with a normal vector, the center of mass deviation of the robot when entering a slope is 0.068 meters and when leaving a slope is 0.045 meters. Compared with a robot without a ground normal vector, the center of mass deviation is reduced by 15% when entering a slope and by 12% when leaving a slope. These results show that when the robot leaves the current terrain, the center of mass deviation is smaller, showing better terrain adaptability.

[0273] In summary, this embodiment designs a whole body motion controller (WBC) framework for a closed-loop wheeled bipedal robot, derives a complete dynamic model, and proposes an online terrain estimation method to estimate the ground normal vector. In addition, this embodiment uses a task controller to control the task and performs hierarchical optimization to solve the WBC problem. In the x-axis impact experiment, the controller of this embodiment reduced the repulsion distance by 27% compared with the previous controller, showing a stronger anti-interference ability. In the experiment of adapting to different ground heights, this embodiment demonstrated the robot's ability to adapt to uneven terrain. In the slope U-turn maneuvering experiment, the terrain estimation system of this embodiment showed real-time performance and high precision. In addition, the integration of normal vectors improves the robot's motion performance, and the center of mass deviation is reduced by more than 10% when entering and leaving the slope.

[0274] Reference Fig.12 The embodiment of the present application also provides a six-degree-of-freedom wheeled robot control device, which can implement the above-mentioned six-degree-of-freedom wheeled robot control method, and the device includes:

[0275] A modeling unit, used for performing whole-body dynamics modeling on the wheeled robot to obtain a target motion equation of the wheeled robot;

[0276] An estimation unit, used for calculating the current task state, Jacobian matrix and ground normal vector at the wheel-foot contact point of the wheel-foot robot according to the sensor data;

[0277] A task control unit, used to determine a target expected acceleration according to the current task state and a reference task state;

[0278] An optimization problem construction unit, used to construct an optimization problem using the Jacobian matrix, the ground normal vector and the target expected acceleration;

[0279] An optimization problem solving unit, used for solving the optimization problem under the constraint of the target motion equation to obtain a driving torque;

[0280] A control unit is used to control the movement of the wheeled robot according to the driving torque.

[0281] It can be understood that the contents of the above method embodiments are all applicable to the present device embodiments, the functions specifically implemented by the present device embodiments are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those achieved by the above method embodiments.

[0282] The embodiment of the present application also provides a system, which includes a memory and a processor, wherein the memory stores a computer program, and the processor implements the above-mentioned six-degree-of-freedom wheeled robot control method when executing the computer program. The system can be any intelligent terminal including a tablet computer, a vehicle-mounted computer, etc.

[0283] It can be understood that the contents of the above method embodiments are all applicable to the present system embodiments, the functions specifically implemented by the present system embodiments are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those achieved by the above method embodiments.

[0284] See also Fig.13 , Fig.13 The hardware structure of a system of another embodiment is illustrated, and the system includes:

[0285] The processor 1301 may be implemented by a general-purpose CPU (Central Processing Unit), a microprocessor, an application-specific integrated circuit (ASIC), or one or more integrated circuits, and is used to execute relevant programs to implement the technical solutions provided in the embodiments of the present application;

[0286] The memory 1302 can be implemented in the form of a read-only memory (ROM), a static storage device, a dynamic storage device, or a random access memory (RAM). The memory 1302 can store an operating system and other application programs. When the technical solution provided in the embodiment of this specification is implemented by software or firmware, the relevant program code is stored in the memory 1302, and the processor 1301 calls and executes the six-degree-of-freedom wheeled robot control method of the embodiment of this application;

[0287] Input / output interface 1303, used to implement information input and output;

[0288] Communication interface 1304, used to realize communication interaction between the system and other devices, which can be realized through wired mode (such as USB, network cable, etc.) or wireless mode (such as mobile network, WIFI, Bluetooth, etc.);

[0289] Bus 1305, which transmits information between various components of the present system (e.g., processor 1301, memory 1302, input / output interface 1303, and communication interface 1304);

[0290] The processor 1301 , the memory 1302 , the input / output interface 1303 and the communication interface 1304 are connected to each other in communication within the system via the bus 1305 .

[0291] An embodiment of the present application also provides a computer-readable storage medium, which stores a computer program. When the computer program is executed by a processor, the above-mentioned six-degree-of-freedom wheeled-legged robot control method is implemented.

[0292] It can be understood that the contents of the above method embodiments are all applicable to the present storage medium embodiments, the functions specifically implemented by the present storage medium embodiments are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those achieved by the above method embodiments.

[0293] The memory, as a non-transient computer-readable storage medium, can be used to store non-transient software programs and non-transient computer executable programs. In addition, the memory may include a high-speed random access memory, and may also include a non-transient memory, such as at least one disk storage device, a flash memory device, or other non-transient solid-state storage device. In some embodiments, the memory may optionally include a memory remotely disposed relative to the processor, and these remote memories may be connected to the processor via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0294] The embodiments described in the embodiments of the present application are intended to more clearly illustrate the technical solutions of the embodiments of the present application and do not constitute a limitation on the technical solutions provided in the embodiments of the present application. Those skilled in the art will appreciate that with the evolution of technology and the emergence of new application scenarios, the technical solutions provided in the embodiments of the present application are also applicable to similar technical problems.

[0295] Those skilled in the art will appreciate that the technical solutions shown in the figures do not constitute a limitation on the embodiments of the present application, and may include more or fewer steps than shown in the figures, or a combination of certain steps, or different steps.

[0296] The device embodiments described above are merely illustrative, and the units described as separate components may or may not be physically separated, that is, they may be located in one place or distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0297] Those skilled in the art will appreciate that all or some of the steps in the methods disclosed above, and the functional modules / units in the systems and devices may be implemented as software, firmware, hardware, or a suitable combination thereof.

[0298] The terms "first", "second", "third", "fourth", etc. (if any) in the specification of the present application and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchangeable where appropriate, so that the embodiments of the present application described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any of their variations are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device comprising a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0299] It should be understood that in the present application, "at least one (item)" means one or more, and "plurality" means two or more. "And / or" is used to describe the association relationship of associated objects, indicating that three relationships may exist. For example, "A and / or B" can mean: only A exists, only B exists, and A and B exist at the same time, where A and B can be singular or plural. The character " / " generally indicates that the objects associated before and after are in an "or" relationship. "At least one of the following" or similar expressions refers to any combination of these items, including any combination of single or plural items. For example, at least one of a, b or c can mean: a, b, c, "a and b", "a and c", "b and c", or "a and b and c", where a, b, c can be single or multiple.

[0300] In the several embodiments provided in the present application, it should be understood that the disclosed devices and methods can be implemented in other ways. For example, the device embodiments described above are only schematic. For example, the division of the above units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms.

[0301] The units described above as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0302] In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0303] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application, or the part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including multiple instructions to enable a computer device (which can be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods of various embodiments of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (Read-Only Memory, referred to as ROM), random access memory (Random Access Memory, referred to as RAM), disk or optical disk and other media that can store programs.

[0304] The preferred embodiments of the present invention are described above with reference to the accompanying drawings, but the scope of the rights of the present invention is not limited thereto. Any modification, equivalent substitution and improvement made by a person skilled in the art without departing from the scope and essence of the present invention should be within the scope of the rights of the present invention.

Claims

1. A control method for a six-degree-of-freedom wheeled robot, characterized in that: The method comprises the following steps: Performing whole-body dynamics modeling on the wheeled robot to obtain a target motion equation of the wheeled robot; Calculate the current task state, Jacobian matrix and ground normal vector at the wheel-foot contact point of the wheel-foot robot according to the sensor data; Determine a target expected acceleration according to the current task state and the reference task state; Constructing an optimization problem using the Jacobian matrix, the ground normal vector, and the target expected acceleration; Solving the optimization problem under the constraint of the target motion equation to obtain the driving torque; The movement of the wheeled robot is controlled according to the driving torque.

2. The six-degree-of-freedom wheeled robot control method according to claim 1, characterized in that: The whole-body dynamics modeling of the wheeled robot is performed to obtain the target motion equation of the wheeled robot, including the following steps: Cutting off a passive joint in the kinematic chain of the wheeled robot, thereby generating a spanning tree of a closed-loop system; defining a first generalized coordinate of the spanning tree and the driving torque, thereby generating a first motion equation of the spanning tree; The expression of the first motion equation is: Where q represents the first generalized coordinate; H∈j 16×16 represents the generalized inertia matrix, C∈j 16 represents the generalized bias force, and the generalized inertia matrix includes Coriolis force, centripetal force and gravity terms; u∈R 16 and denote the generalized velocity set and generalized acceleration set respectively; S∈R 16×6 is the selection matrix, τ a ∈R 6 represents the driving torque of the driving joint; τ gc ∈R 16 represents the ground contact force applied in the joint space; Adding a constraint force that restricts the movement of the wheel-foot of the wheel-foot robot to the first motion equation to obtain a second motion equation; Eliminate the constraint force in the second motion equation according to the virtual power principle to obtain a third motion equation; Calculating the ground contact force, and substituting the ground contact force into the third motion equation to obtain the target motion equation; The expression of the target motion equation includes: Among them, H y =G T HG∈R 12×12 , C y =G T C∈R 12 ; represents acceleration, y represents the second generalized coordinate, Constrained to 0; The expression of the ground contact force is: in, is the contact Jacobian matrix; F C ∈R 4 is the rolling restraint force, C F ∈R 2×4 represents the friction curve related to speed; C J IC and C F The calculation of is related to the ground normal vector.

3. The six-degree-of-freedom wheeled robot control method according to claim 2, characterized in that: The step of calculating the current task state, the Jacobian matrix and the ground normal vector at the wheel-foot contact point of the wheel-foot robot according to the sensor data comprises the following steps: Inputting the sensor data into an extended Kalman filter to determine the second generalized coordinates and velocity; Determine the current task state, the Jacobian matrix, and the contact position of the wheel-foot of the wheel-foot robot according to the second generalized coordinates and the speed; Determine a global point cloud set based on the sensor data; Determine the normal vectors in the global point cloud set using principal component analysis to obtain a global ground normal vector map; The normal vector of the contact position is determined in the global ground normal vector map as the ground normal vector.

4. The six-degree-of-freedom wheeled robot control method according to claim 1, characterized in that: Determining the target expected acceleration according to the current task state and the reference task state comprises the following steps: Determine a first expected acceleration of the posture task of the wheeled robot according to the current task state and the reference task state using a proportional-differential controller; Determine a second expected acceleration of the balancing task of the wheeled robot according to the current task state and the reference task state using a linear quadratic regulator; The first expected acceleration and the second acceleration are reordered according to task priority to obtain the target expected acceleration.

5. The six-degree-of-freedom wheeled robot control method according to claim 4, characterized in that: The method of using a proportional-differential controller to determine a first expected acceleration of the posture task of the wheeled robot according to the current task state and the reference task state comprises the following steps: Define the posture task status; The expression of the posture task state is: Λ=[φ h a b c] T ∈R 5 ; Wherein, Λ represents the posture task state; φ represents the separation angle between the wheel-foot robot; h represents the height of the wheel-foot robot; α, β, and γ represent the roll angle, pitch angle, and yaw angle of the head of the wheel-foot robot, respectively; Determining the attitude task according to the proportional-derivative controller and the attitude task state; The expression of the posture task is: in, des a n represents the posture task; n is the index in Λ; ref Λ n K represents the nth reference task state; pn and K dn Represent the proportional gain and differential gain respectively; defining the first expected acceleration according to the posture task; The expression of the first expected acceleration is: <h2 style=";text-align:left;direction:ltr"> des <h2 style=";text-align:left;direction:ltr"> a=[<h2 style=";text-align:left;direction:ltr"> des <h2 style=";text-align:left;direction:ltr"> a1…<h2 style=";text-align:left;direction:ltr"> des <h2 style=";text-align:left;direction:ltr"> a5]<h2 style=";text-align:left;direction:ltr"> T <h2 style=";text-align:left;direction:ltr"> ∈R<h2 style=";text-align:left;direction:ltr"> 5 <h2 style=";text-align:left;direction:ltr"> ; in, des a represents the first expected acceleration.

6. The six-degree-of-freedom wheeled-legged robot control method according to claim 4, characterized in that: The method of using a linear quadratic regulator to determine a second expected acceleration of the balancing task of the wheeled robot according to the current task state and the reference task state comprises the following steps: Modeling the wheeled robot as a single rigid body based on a linear quadratic regulator; orthogonally projecting the center of mass of the rigid body onto the sagittal plane; The center-of-mass momentum of the rigid body is defined in the sagittal plane; the center-of-mass momentum includes the center-of-mass linear momentum, the center-of-mass angular momentum and the contact force; determining a derivative of the center of mass momentum based on the gravity force twist and the ground contact force twist; Determining the constraint conditions satisfied when the wheeled-legged robot is in a balanced state; The expected relative acceleration of the center of mass of the rigid body is determined as the second expected acceleration according to the derivative of the center of mass momentum and the constraint condition.

7. The control method of a six-degree-of-freedom wheeled robot according to any one of claims 1 to 6, characterized in that: Solving the optimization problem under the constraint of the target motion equation to obtain the driving torque includes the following steps: Defining optimization variables of the optimization problem; wherein the optimization variables include generalized acceleration, rolling restraint force and the driving torque; Converting the target motion equation into a linear equality constraint equation; Mapping the generalized acceleration to a task space using the Jacobian matrix and then converting it into a least squares problem; Determine a plurality of quadratic programming problems with priorities according to the linear constraint equation and the least squares problem; Solving each of the quadratic programming problems according to priority from high to low; wherein, in each solution, the solution result of the previous priority is used for the equality constraint of the quadratic programming problem of the next priority; The driving torque of the optimization variable in the quadratic programming problem of a last priority is determined.

8. A six-degree-of-freedom wheel-foot robot control device, characterized in that: The device comprises: A modeling unit, used for performing whole-body dynamics modeling on the wheeled robot to obtain a target motion equation of the wheeled robot; An estimation unit, used for calculating the current task state, Jacobian matrix and ground normal vector at the wheel-foot contact point of the wheel-foot robot according to the sensor data; A task control unit, used to determine a target expected acceleration according to the current task state and a reference task state; An optimization problem construction unit, used to construct an optimization problem using the Jacobian matrix, the ground normal vector and the target expected acceleration; An optimization problem solving unit, used for solving the optimization problem under the constraint of the target motion equation to obtain a driving torque; A control unit is used to control the movement of the wheeled robot according to the driving torque.

9. A system, characterized in that: The system comprises a memory and a processor, wherein the memory stores a computer program, and the processor implements the method according to any one of claims 1 to 7 when executing the computer program.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the method according to any one of claims 1 to 7 is implemented.

Images