A method for optimizing the posture of a robot with floating drive wheels

Abstract

The present invention discloses a posture optimization and adjustment method for a robot with floating drive wheels. The present invention utilizes a fuzzy PID control algorithm to ensure that all floating drive wheels of the robot always maintain solid contact with the ground with a supporting force that meets the standard on any rough road surface. Based on this, a kinematic model of the robot is established, and a mapping function model between the floating angle of the floating drive wheel and the pitch angle and roll angle of the robot is further derived. Finally, based on a PSO algorithm, dynamic optimization of the pitch angle and roll angle of the robot is achieved by establishing its calculation object, optimization target, and constraint conditions, thereby ensuring the horizontality and stability of the vehicle body during driving.

Description

Technical Field

[0001] The invention belongs to the technical field of motion robot posture control and relates to a posture optimization adjustment method for a robot with floating drive wheels. Background Art

[0002] Currently, robots employ a wide variety of propulsion systems, including tracked, Mecanum, steering, differential, and Ekman steering wheels, all designed for circular motion. To navigate rough terrain, robots must possess a certain level of obstacle avoidance and stability. Therefore, they require a suspension structure, such as the spring-damper and multi-link suspension commonly used in automobiles. However, these suspension structures are mostly passive, meaning the vehicle's posture changes with terrain, and they lack the ability to actively adjust the suspension travel to alter its posture.

[0003] The simplest application of a robot with floating drive wheels is adjusting the height of the robot's chassis to navigate roads of varying roughness. However, if the floating angle of the floating drive wheels is fixed during driving, the robot may find itself in an unstable state with three wheels touching the ground and one wheel off the ground when navigating rough terrain. Even if the robot is stable with four wheels touching the ground, its attitude angle may be excessively tilted, for example, with excessive pitch and roll angles. When the robot's onboard mission function system performs its work in the current robot posture, it may experience excessive movement, shifting its center of gravity and causing the robot to tip over. Existing robot adjustments either simply adjust all wheels to touch the ground, without considering the changes in attitude angle during driving; or simply adjust the robot's attitude angle to meet the standard, but still leave wheels hanging in the air.

[0004] Therefore, based on the above-mentioned deficiencies in the existing floating adjustment of robots, the present invention discloses a posture optimization adjustment method for a robot with floating drive wheels. Summary of the Invention

[0005] The purpose of the present invention is to provide a method for optimizing and adjusting the posture of a robot with floating drive wheels, which can dynamically adjust the floating angle of the floating drive wheels during the robot's walking process so that all the floating drive wheels of the robot are in a ground-contacting state with sufficient support force. On this basis, the floating angle is dynamically adjusted to optimize the robot's pitch angle and roll angle to ensure the stability of the robot's walking process.

[0006] The present invention is achieved through the following technical solutions:

[0007] A method for optimizing the posture of a robot with floating drive wheels, wherein the robot includes four sets of floating drive wheels and a rotary arm that drives the floating drive wheels to float, comprises the following steps:

[0008] Step 1: Calculate the support force of each floating drive wheel according to the torque of the floating drive wheel servo, and determine whether each floating drive wheel is in virtual contact with the ground according to the magnitude of the support force;

[0009] Step 2: If there is no virtual contact between any of the floating drive wheels and the ground, proceed to Step 3; If there is virtual contact between any of the floating drive wheels and the ground, establish a fuzzy PID control algorithm to iteratively optimize and adjust the floating angle of the floating drive wheel until all of the floating drive wheels have no virtual contact with the ground, then proceed to Step 3;

[0010] Step 3: Establish a mapping function model between the robot's pitch angle and floating angle, and between the robot's roll angle and floating angle based on the current position coordinates of the floating drive wheel;

[0011] Step 4: Taking the minimization of the robot's pitch angle and roll angle as the optimization goal and the constraint that the wheel center coordinates of the floating drive wheels satisfy the mapping function model, calculate the optimal solution for the floating angle of each floating drive wheel;

[0012] Step 5: The floating drive wheel servo completes the posture optimization adjustment of the robot according to the optimal solution of the floating angle of the floating drive wheel.

[0013] In order to better implement the present invention, further, in step 2, establishing a fuzzy PID control algorithm includes the following steps:

[0014] Step A1: obtaining a target support force of a floating drive wheel with virtual contact, wherein the target support force is a minimum support force of a floating drive wheel without virtual contact;

[0015] Step A2: obtaining a parameter increment value of a floating drive wheel servo controller based on a fuzzy PID control algorithm;

[0016] Step A3: establishing a fuzzy PID control algorithm based on the parameter increment value of the floating drive wheel steering gear controller;

[0017] Step A4: Establish a mean square error objective function of the support force of the floating drive wheel, and optimize the parameters of the floating drive wheel servo controller through closed-loop iteration of the fuzzy PID control algorithm until the support force of the floating drive wheel with virtual contact is adjusted to be equal to the target support force.

[0018] In order to better implement the present invention, further, the step A2 specifically includes:

[0019] Step A2.1, calculating the support force error membership of the floating drive wheel based on the target support force and the actual value of the support force of the floating drive wheel;

[0020] Step A2.2, calculating the parameter membership of the floating drive wheel servo controller based on the support force error membership;

[0021] Step A2.3, calculating the expected parameter value of the floating drive wheel servo controller according to the parameter membership of the floating drive wheel servo controller;

[0022] Step A2.4: Calculate the parameter increment value of the floating drive wheel servo controller according to the expected parameter value of the floating drive wheel servo controller.

[0023] In order to better implement the present invention, further, the step 3 includes:

[0024] Step 3.1. Establish a robot coordinate system based on the center of the robot. Use the kinematic model to calculate the wheel center coordinates of the floating drive wheel in the robot coordinate system and the center coordinates of the swing arm.

[0025] Step 3.2, establishing a wheel center connection line based on the relationship between the wheel center coordinate values, establishing a swing arm center connection line based on the relationship between the swing arm center coordinate values, and establishing a swing arm fitting line based on the relationship between the wheel center coordinate values ​​and the swing arm center coordinate values;

[0026] Step 3.3: Based on the wheel center connection line, the swing arm center connection line, and the swing arm fitting line, a mapping function model between the pitch angle and the floating angle, and a mapping function model between the roll angle and the floating angle are established.

[0027] In order to better implement the present invention, further, the specific steps of establishing the mapping function model between the pitch angle and the floating angle are as follows:

[0028] Step B1: establishing a wheel center line on the same side based on the wheel center coordinates of the two floating drive wheels on the same side of the robot, establishing a swing arm center line on the same side based on the swing arm center coordinates of the two swing arms on the same side of the robot, and establishing a swing arm fitting line based on the wheel center coordinates of the floating drive wheels and the swing arm center coordinates of the corresponding swing arms;

[0029] Step B2: establishing a mapping function model between the pitch angle and the float angle based on the ipsilateral wheel center connecting line, the ipsilateral swing arm center connecting line, and the swing arm fitting line;

[0030] Step B3: Calculate the same-side terrain angle between the same-side wheel center line and the horizontal line, and calculate the pitch angle based on the mapping function model between the pitch angle and the float angle and the relationship between the same-side terrain angle.

[0031] In order to better implement the present invention, further, the specific steps of establishing the mapping function model between the roll angle and the float angle are as follows:

[0032] Step C1: establishing a connecting line of opposite wheel centers based on the wheel center coordinates of the two floating drive wheels on opposite sides of the robot, establishing a connecting line of opposite swing arm centers based on the swing arm center coordinates of the two swing arms on opposite sides of the robot, and establishing a swing arm fitting line based on the wheel center coordinates of the floating drive wheels and the swing arm center coordinates of the corresponding swing arms;

[0033] Step C2: establishing a mapping function model between the roll angle and the float angle based on the opposite wheel center line, the opposite swing arm center line, and the swing arm fitting line;

[0034] Step C3: Calculate the opposite side terrain angle between the opposite side wheel center line and the horizontal line, and calculate the roll angle based on the mapping function model between the roll angle and the floating angle and the relationship between the opposite side terrain angle.

[0035] In order to better implement the present invention, further, in step 4:

[0036] Step 4.1. Select any three floating drive wheels and introduce a mapping function model between the pitch angle and the floating angle, and a mapping function model between the roll angle and the floating angle based on the PSO algorithm.

[0037] Step 4.2: With the minimum pitch angle and roll angle as the optimization goal, perform iterative calculations with the constraint that the wheel center coordinates of the three floating drive wheels satisfy the wheel center connection line to obtain the optimal solution for the floating angles of the three floating drive wheels;

[0038] Step 4.3: Update the coordinate value of the center of the robot in the robot coordinate system using the optimal solution of the floating angle of the three floating drive wheels and the wheel center connection line;

[0039] Step 4.4: Use the updated coordinate value of the robot's center in the robot coordinate system and the line connecting the wheel centers to calculate the floating angle of the fourth floating drive wheel.

[0040] In order to better implement the present invention, further, in step 1, the formula for calculating the supporting force of the floating drive wheel is as follows:

[0041]

[0042] in: represents the support force on the nth floating driving wheel at time t; represents the servo torque of the nth floating drive wheel at time t; It represents the supporting force equivalent arm of the nth floating driving wheel at time t.

[0043] In order to better implement the present invention, further, the calculation formula of the steering gear torque is as follows:

[0044]

[0045] Among them: K τ Indicates the servo torque coefficient; Indicates the feedback current value of the servo of the nth floating drive wheel at time t.

[0046] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0047] The present invention uses a fuzzy PID control algorithm to ensure that all floating drive wheels of the robot always maintain solid contact with the ground with sufficient support force on any rugged road surface. Based on this, a kinematic model of the robot is established, and a mapping function model between the floating angle of the floating drive wheel and the pitch angle and roll angle of the robot is further derived. Finally, based on the PSO algorithm, the dynamic optimization of the pitch angle and roll angle of the robot is achieved by establishing its calculation object, optimization target, and constraint conditions, thereby ensuring the horizontality and stability of the vehicle body during driving. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 Schematic diagram of the steps of the present invention;

[0049] Figure 2 Schematic diagram of the mapping relationship between pitch angle and floating angle;

[0050] Figure 3 Schematic diagram of the mapping relationship between roll angle and float angle;

[0051] Figure 4 This is a schematic diagram of the robot's structure;

[0052] Figure 5 Schematic diagram of the wheel center connection line lin1 between the first floating drive wheel and the second floating drive wheel;

[0053] Figure 6 Schematic diagram of the wheel center connection line lin2 between the third floating drive wheel and the fourth floating drive wheel. DETAILED DESCRIPTION

[0054] The following detailed description is illustrative and is intended to provide further explanation of the present invention. Unless otherwise specified, all technical and scientific terms used in the present invention have the same meaning as commonly understood by those skilled in the art to which the present invention belongs.

[0055] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present invention. As used herein, unless otherwise expressly specified in the present invention, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprise" and / or "include" are used in this specification, they indicate the presence of features, steps, operations, devices, components and / or combinations thereof.

[0056] For the convenience of description, if the words "up", "down", "left" and "right" appear in the present invention, they only indicate that they are consistent with the up, down, left and right directions of the drawings themselves, and do not limit the structure. They are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or component referred to must have a specific orientation, be constructed and operate in a specific orientation. Therefore, they should not be understood as limiting the present invention.

[0057] Explanation of terms: The terms "install", "connect", "connect", "fix" and the like in the present invention should be understood in a broad sense. For example, it can be a fixed connection, a detachable connection, or an integral whole; it can be a mechanical connection or an electrical connection, a direct connection, or an indirect connection through an intermediate medium, an internal connection between two elements, or an interaction relationship between two elements. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to the specific circumstances.

[0058] Example 1:

[0059] A method for optimizing and adjusting the posture of a robot with floating drive wheels in this embodiment is as follows: Figure 4 As shown, the robot includes four sets of floating drive wheels and a rotating arm that drives the floating drive wheels to float. The robot also includes a robot body, with floating drive wheel servos installed at the four corners of the robot body. The output shafts of the floating drive wheel servos are connected to the wheel centers of the floating drive wheels via the rotating arms. The position of the floating drive wheels is adjusted by the rotation of the floating drive wheel servos. The robot body is equipped with a posture sensor that can detect the pitch and roll angles of the robot body in real time. The posture sensor transmits the detected pitch and roll angle data to an external computer, which performs posture optimization adjustment of the pitch and roll angles according to the following method.

[0060] like Figure 1 As shown, the robot posture optimization adjustment method includes the following steps:

[0061] Step 1: Calculate the support force of each floating drive wheel according to the torque of the floating drive wheel servo, and determine whether each floating drive wheel is in virtual contact with the ground according to the magnitude of the support force;

[0062] Step 2: If there is no virtual contact between any of the floating drive wheels and the ground, proceed to Step 3; If there is virtual contact between any of the floating drive wheels and the ground, establish a fuzzy PID control algorithm to iteratively optimize and adjust the floating angle of the floating drive wheel until all of the floating drive wheels have no virtual contact with the ground, then proceed to Step 3;

[0063] Step 3: Establish a mapping function model between the robot's pitch angle and floating angle, and between the robot's roll angle and floating angle based on the current position coordinates of the floating drive wheel;

[0064] Step 4: Taking the minimization of the robot's pitch angle and roll angle as the optimization goal and the constraint that the wheel center coordinates of the floating drive wheels satisfy the mapping function model, calculate the optimal solution for the floating angle of each floating drive wheel;

[0065] Step 5: The floating drive wheel servo completes the posture optimization adjustment of the robot according to the optimal solution of the floating angle of the floating drive wheel.

[0066] In step 1, the formula for calculating the supporting force of the floating drive wheel is as follows:

[0067]

[0068] in: represents the support force on the nth floating driving wheel at time t; represents the servo torque of the nth floating drive wheel at time t; It represents the supporting force equivalent arm of the nth floating driving wheel at time t.

[0069] The calculation formula of the steering gear torque is as follows:

[0070]

[0071] Among them: K τ Indicates the servo torque coefficient; Indicates the feedback current value of the servo of the nth floating drive wheel at time t.

[0072] In step 2, establishing a fuzzy PID control algorithm includes the following steps:

[0073] Step A1: obtaining a target support force of a floating drive wheel with virtual contact, wherein the target support force is a minimum support force of a floating drive wheel without virtual contact;

[0074] Step A2: obtaining a parameter increment value of a floating drive wheel servo controller based on a fuzzy PID control algorithm;

[0075] Step A3: establishing a fuzzy PID control algorithm based on the parameter increment value of the floating drive wheel steering gear controller;

[0076] Step A4: Establish a mean square error objective function of the support force of the floating drive wheel, and optimize the parameters of the floating drive wheel servo controller through closed-loop iteration of the fuzzy PID control algorithm until the support force of the floating drive wheel with virtual contact is adjusted to be equal to the target support force.

[0077] The step A2 specifically includes:

[0078] Step A2.1, calculating the support force error membership of the floating drive wheel based on the target support force and the actual value of the support force of the floating drive wheel;

[0079] Step A2.2, calculating the parameter membership of the floating drive wheel servo controller based on the support force error membership;

[0080] Step A2.3, calculating the expected parameter value of the floating drive wheel servo controller according to the parameter membership of the floating drive wheel servo controller;

[0081] Step A2.4: Calculate the parameter increment value of the floating drive wheel servo controller according to the expected parameter value of the floating drive wheel servo controller.

[0082] The step 3 comprises:

[0083] Step 3.1. Establish a robot coordinate system based on the center of the robot. Use the kinematic model to calculate the wheel center coordinates of the floating drive wheel in the robot coordinate system and the center coordinates of the swing arm.

[0084] Step 3.2, establishing a wheel center connection line based on the relationship between the wheel center coordinate values, establishing a swing arm center connection line based on the relationship between the swing arm center coordinate values, and establishing a swing arm fitting line based on the relationship between the wheel center coordinate values ​​and the swing arm center coordinate values;

[0085] Step 3.3: Based on the wheel center connection line, the swing arm center connection line, and the swing arm fitting line, a mapping function model between the pitch angle and the floating angle, and a mapping function model between the roll angle and the floating angle are established.

[0086] The specific steps of establishing the mapping function model between the pitch angle and the float angle are as follows:

[0087] Step B1: establishing a wheel center line on the same side based on the wheel center coordinates of the two floating drive wheels on the same side of the robot, establishing a swing arm center line on the same side based on the swing arm center coordinates of the two swing arms on the same side of the robot, and establishing a swing arm fitting line based on the wheel center coordinates of the floating drive wheels and the swing arm center coordinates of the corresponding swing arms;

[0088] Step B2: establishing a mapping function model between the pitch angle and the float angle based on the ipsilateral wheel center connecting line, the ipsilateral swing arm center connecting line, and the swing arm fitting line;

[0089] Step B3: Calculate the same-side terrain angle between the same-side wheel center line and the horizontal line, and calculate the pitch angle based on the mapping function model between the pitch angle and the float angle and the relationship between the same-side terrain angle.

[0090] The specific steps of establishing the mapping function model between the roll angle and the float angle are as follows:

[0091] Step C1: establishing a connecting line of opposite wheel centers based on the wheel center coordinates of the two floating drive wheels on opposite sides of the robot, establishing a connecting line of opposite swing arm centers based on the swing arm center coordinates of the two swing arms on opposite sides of the robot, and establishing a swing arm fitting line based on the wheel center coordinates of the floating drive wheels and the swing arm center coordinates of the corresponding swing arms;

[0092] Step C2: establishing a mapping function model between the roll angle and the float angle based on the opposite wheel center line, the opposite swing arm center line, and the swing arm fitting line;

[0093] Step C3: Calculate the opposite side terrain angle between the opposite side wheel center line and the horizontal line, and calculate the roll angle based on the mapping function model between the roll angle and the floating angle and the relationship between the opposite side terrain angle.

[0094] In step 4:

[0095] Step 4.1. Select any three floating drive wheels and introduce a mapping function model between the pitch angle and the floating angle, and a mapping function model between the roll angle and the floating angle based on the PSO algorithm.

[0096] Step 4.2: With the minimum pitch angle and roll angle as the optimization goal, perform iterative calculations with the constraint that the wheel center coordinates of the three floating drive wheels satisfy the wheel center connection line to obtain the optimal solution for the floating angles of the three floating drive wheels;

[0097] Step 4.3: Update the coordinate value of the center of the robot in the robot coordinate system using the optimal solution of the floating angle of the three floating drive wheels and the wheel center connection line;

[0098] Step 4.4: Use the updated coordinate value of the robot's center in the robot coordinate system and the line connecting the wheel centers to calculate the floating angle of the fourth floating drive wheel.

[0099] Example 2:

[0100] A method for optimizing and adjusting the posture of a robot with a floating drive wheel is improved on the basis of Example 1, as shown in FIG. Figure 1 As shown, specifically:

[0101] Read the feedback current values ​​of the four floating drive wheel servos Calculate the current servo torque T t n :

[0102]

[0103] Among them: K τ Indicates the servo torque coefficient; Indicates the feedback current value of the servo of the nth floating drive wheel at time t.

[0104] As shown in the figure, a simplified model of the transformation relationship between the support force of the floating drive wheel and the servo torque is established to calculate the support force F of the four floating drive wheels. t n :

[0105]

[0106] in: represents the support force on the nth floating driving wheel at time t; represents the servo torque of the nth floating drive wheel at time t; represents the supporting force equivalent arm of the nth floating driving wheel at time t; l n Indicates the distance from the wheel center of the nth floating drive wheel to the center of the corresponding swing arm; represents the pitch angle of the robot at time t; It represents the floating angle of the nth floating drive wheel at time t. The operator in front of it is "+" or "-" depending on the positional relationship between the floating angle and the pitch angle.

[0107] As shown in the figure, taking the first floating drive wheel and the second floating drive wheel on the same side of the robot as an example, the wheel center of the first floating drive wheel is The wheel center of the second floating drive wheel is The center of the swing arm that drives the first floating drive wheel is p1, and the center of the swing arm that drives the second floating drive wheel is p2. Then:

[0108]

[0109] in: represents the support force on the first floating driving wheel at time t; represents the support force on the second floating driving wheel at time t; represents the servo torque of the first floating drive wheel at time t; represents the servo torque of the second floating drive wheel at time t; represents the supporting force equivalent arm of the first floating driving wheel at time t; represents the supporting force equivalent arm of the second floating drive wheel at time t; l1 represents the distance from the wheel center of the first floating drive wheel to the center of the corresponding swing arm, that is, the wheel center of the first floating drive wheel The distance from the center of the first floating drive wheel to the center of the swing arm p1; l2 represents the distance from the center of the second floating drive wheel to the center of the corresponding swing arm, that is, the center of the second floating drive wheel The distance to the center of the swing arm p2 that drives the second floating drive wheel; represents the floating angle of the first floating driving wheel at time t; Represents the floating angle of the second floating drive wheel at time t.

[0110] Determine whether there is virtual contact between the floating drive wheels. The judgment formula is: min(F t n )≤F th , where F th The target support force set for judging virtual and real contact.

[0111] If the support forces of all floating drive wheels do not satisfy the above judgment formula, it indicates that none of the floating drive wheels have virtual contact. If the support force of any floating drive wheel satisfies the above judgment formula, the floating drive wheel with virtual contact is found, and a fuzzy PID control algorithm is established to iteratively optimize and adjust the floating angle of the floating drive wheel until all floating drive wheels have no virtual contact with the ground.

[0112] The iterative optimization adjustment of the floating angle by the fuzzy PID control algorithm is as follows:

[0113] Find the floating drive wheel with virtual contact and mark its support force as Obtain the target support force of the virtual contact floating drive wheel The formula for obtaining the target support force of the virtual contact floating drive wheel is:

[0114] That is, the minimum supporting force received by the remaining floating driving wheels except the floating driving wheel in virtual contact is used as the target supporting force of the floating driving wheels in virtual contact.

[0115] Based on the fuzzy PID control algorithm, the controller K of the floating drive wheel is obtained. p , K i , K d Parameter increment value. Specifically:

[0116] (1) Select the domain and membership function to obtain the support force error membership and error derivative membership.

[0117] The domain is set to x∈[-h,h], where h is a positive real number. The membership function includes trapezoidal membership function, triangular membership function, parabolic membership function, etc., which can be selected according to the actual situation. Here, the triangular membership function is used as an example for description. The triangular membership function μ(x) is expressed as:

[0118]

[0119] Then, the support force error of the floating drive wheel is expressed as e t =F t i_target -F t i_out , where F t i_out is the actual value of the support force on the floating drive wheel, and the membership value range is [0, 1].

[0120] Finally, using the interval mapping relationship, we can know the support force error e t and error derivative The membership degrees are:

[0121] e t The membership degree is: μ1(e t ),μ2(e t );

[0122] The membership degree is:

[0123] (2) Establishing PID control algorithm about K p , K i , K d Fuzzy rules, get K p , K i , K d The relationship between the membership degree and the support force error membership degree.

[0124] Among them, K p , K i , K d The fuzzy rules use conventional fuzzy PID inference tables as fuzzy rules, which is an existing technology and will not be described in detail here.

[0125] Then, through fuzzification, we can know that K p , K i , K d The membership expression is:

[0126]

[0127] Finally, according to the fuzzy classification category, Kp , K i , K d The membership degree of is:

[0128]

[0129] Wherein, n is an integer.

[0130] (3) Obtain the controller K of the floating drive wheel p , K i , K d Parameter increment value.

[0131] First, according to the above domain range x∈[-h,h], obtain K p , K i , K d The expected values ​​of the parameters are:

[0132]

[0133] Then, according to the interval mapping relationship, obtain K p , K i , K d Parameter increment ΔK p ,ΔK i ,ΔK d , thus the fuzzy PID control algorithm expression is:

[0134]

[0135] Among them, K' p =K p +ΔK p , K' d =K d +ΔK d , K' i =K i +ΔK i .

[0136] The floating driving wheel force is set to meet the support force mean square error objective function:

[0137]

[0138] Among them: F tk represents the average value of the support force on all floating drive wheels; σ represents the support force mean square error objective function; σ set It indicates the support force mean square deviation setting threshold; n indicates the number of floating drive wheels.

[0139] Based on the set support force mean square error objective function, the PID control algorithm closed-loop iteratively optimizes the parameter K of the floating drive wheel servo controllerp , K i , K d , and then output it to the floating angle PID controller of the robot's floating drive wheel to adjust the floating angle of the floating drive wheel with virtual contact until the supporting force of the floating drive wheel with virtual contact is adjusted to be equal to the target supporting force.

[0140] After all the floating drive wheels are adjusted to a non-virtual contact state, you can continue to optimize the robot's posture, that is, adjust the robot's pitch angle and roll angle, specifically:

[0141] Establish the robot coordinate system W at the center of the robot rob , and use this robot coordinate system as the world coordinate system W world The kinematic model of the robot is established and the floating angle of the floating drive wheel is calculated to be θ t n The wheel center is in the world coordinate system W world The coordinate values ​​in are:

[0142]

[0143] in: represents the wheel center of the nth floating drive wheel at time t; O zero Indicates the origin coordinates of the family coordinate system; R represents the three-axis coordinates of the wheel center of the nth floating drive wheel at time t in the robot coordinate system; t Represents the rotation matrix from the robot coordinate system to the world coordinate system; T t1 Represents the first direction translation matrix of the robot coordinate system transformed to the world coordinate system; T t2 The second direction translation matrix representing the transformation from the robot coordinate system to the world coordinate system; It represents the floating angle of the nth floating drive wheel at time t.

[0144] Taking the robot with four floating drive wheels as an example, we have:

[0145]

[0146] According to the floating angle of the floating drive wheel Get the wheel center coordinates of the four floating drive wheels Then, a mapping function model is established. The wheel centers on the same side of the robot are used. Here, the first and second floating drive wheels are on the same side, the third and fourth floating drive wheels are on the same side, the first and third floating drive wheels are on opposite sides, and the second and fourth floating drive wheels are on opposite sides.

[0147] like Figure 5 As shown, the wheel center connection line lin1 is obtained by using the first floating drive wheel and the second floating drive wheel; Figure 6 As shown in the figure, the expression of the wheel center connection line lin2 obtained by using the third floating drive wheel and the fourth floating drive wheel is:

[0148]

[0149] in: Indicates the three-axis coordinates of the wheel center of the first floating drive wheel in the robot coordinate system at time t; Indicates the three-axis coordinates of the wheel center of the second floating drive wheel in the robot coordinate system at time t; Indicates the three-axis coordinates of the wheel center of the third floating drive wheel in the robot coordinate system at time t; represents the three-axis coordinates of the wheel center of the fourth floating drive wheel in the robot coordinate system at time t; (A l , B l , C l ) represents the three-axis direction vector coefficient of lin1; (A r , B r , C r ) represents the three-axis direction vector coefficients of lin2.

[0150] like Figure 2 As shown, establish the pitch angle of the robot Floating angle of wheel The mapping function model is to realize the pitch angle of the robot by changing the floating angle of the first floating drive wheel and the second floating drive wheel. Adjustment:

[0151] like Figure 2 As shown, based on the triangle sine and cosine theorem, we get: M stands for The line segment between P2 and P3.

[0152] but: have to:

[0153] but: have to:

[0154] but: have to: γ is the terrain angle on the same side, obtained by the straight line lin1 and the horizontal line.

[0155] Wherein: p1 represents the cantilever center of the first floating drive wheel; p2 represents the cantilever center of the second floating drive wheel; Indicates the wheel center of the first floating drive wheel; represents the wheel center of the second floating drive wheel; L represents the length of the line segment between the cantilever center p1 of the first floating drive wheel and the cantilever center p2 of the second floating drive wheel; l1 represents the cantilever length of the first floating drive wheel in the front view direction; l2 represents the cantilever length of the second floating drive wheel in the front view direction;

[0156] like Figure 3 As shown, establish the robot's roll angle ψ t With floating angle The mapping function model is used to adjust the robot's roll angle by changing the floating angles of the first and third floating drive wheels:

[0157] Based on the floating angle of the robot's legs:

[0158] like Figure 3 As shown, extend p3p1, extend The two extended lines intersect at p k Point, using the principle of similar triangles we can get: N represents p k The length of the line segment between p1 and p2, represents the projection of the cantilever l1 in the top-view direction, represents the projection of the cantilever l3 in the top-view direction; w represents the length of the line segment between p1 and p3 in the top-view direction.

[0159] According to trigonometric functions, we can get:

[0160] Finally, we can get: t =β t -λ, λ is the opposite side terrain angle, obtained from the straight lines lin1 and lin2.

[0161] After obtaining the mapping function relationship between the robot's pitch angle, roll angle and floating angle, when the floating angle When the robot's pitch angle and roll angle change, the robot's coordinate system W rob With the world coordinate system W world There are two angle flips in the direction, namely the pitch angle and the roll angle, so it is necessary to obtain the angle in the world coordinate system W world The transformation matrix of the wheel center of the floating drive wheel is the coordinate value of the wheel center in the world coordinate system, that is:

[0162]

[0163] in: Indicates the coordinate value of the wheel center in the world coordinate system; T Tt n represents the transformation matrix; Δψ t Indicates the change in roll angle; Indicates the change in pitch angle.

[0164] Then, the optimal solution of the floating angle is calculated based on the PSO algorithm. Here, the first floating drive wheel, the second floating drive wheel, and the third floating drive wheel are optimized:

[0165] Optimization object: floating angle of the first floating drive wheel Floating angle of the second floating drive wheel Floating angle of the third floating drive wheel For calculation object.

[0166] Optimization goal: That is, the robot's pitch angle and roll angle are minimum.

[0167] Restrictions: That is, the y-coordinate and the z-coordinate of the wheel center coordinate of the floating drive wheel satisfy the straight line equations lin1 and lin2.

[0168] The floating angle of the first floating drive wheel is obtained by iterative calculation using the PSO algorithm to minimize the pitch angle and roll angle of the robot. Floating angle of the second floating drive wheel Floating angle of the third floating drive wheel The optimal solution of .

[0169] Floating angle of the fourth floating drive wheel The specific solution process is as follows:

[0170] use Use lin1 and lin2 to find the robot center O zero In the world coordinate system W world The floating angle of the fourth floating drive wheel is solved by using the coordinate value of the robot center and lin1 and lin2 inversely.

[0171] Will get The input is sent to the robot control system, and the robot control system controls the floating drive wheel servo to adjust the floating angle of each floating drive wheel to the optimal solution, thereby completing the posture optimization of the robot.

[0172] The rest of this embodiment is the same as that of embodiment 1, so it will not be described again.

[0173] The above are only preferred embodiments of the present invention and do not limit the present invention in any form. Any simple modifications or equivalent changes made to the above embodiments based on the technical essence of the present invention fall within the scope of protection of the present invention.

Claims

1. A method for optimizing the posture of a robot with floating drive wheels, wherein the robot comprises four sets of floating drive wheels and a rotary arm for driving the floating drive wheels to float, characterized in that: The following steps are involved: Step 1: Calculate the support force of each floating drive wheel according to the torque of the floating drive wheel servo, and determine whether each floating drive wheel is in virtual contact with the ground according to the magnitude of the support force; Step 2: If there is no virtual contact between any of the floating drive wheels and the ground, proceed to Step 3; If there is virtual contact between any of the floating drive wheels and the ground, establish a fuzzy PID control algorithm to iteratively optimize and adjust the floating angle of the floating drive wheel until all of the floating drive wheels have no virtual contact with the ground, then proceed to Step 3; Step 3: Establish a mapping function model between the robot's pitch angle and floating angle, and between the robot's roll angle and floating angle based on the current position coordinates of the floating drive wheel; Step 4: Taking the minimization of the robot's pitch angle and roll angle as the optimization goal and the constraint that the wheel center coordinates of the floating drive wheels satisfy the mapping function model, calculate the optimal solution for the floating angle of each floating drive wheel; Step 5: The floating drive wheel servo completes the posture optimization adjustment of the robot according to the optimal solution of the floating angle of the floating drive wheel; In step 2, establishing a fuzzy PID control algorithm includes the following steps: Step A1: obtaining a target support force of a floating drive wheel with virtual contact, wherein the target support force is a minimum support force of a floating drive wheel without virtual contact; Step A2: obtaining a parameter increment value of a floating drive wheel servo controller based on a fuzzy PID control algorithm; Step A3: establishing a fuzzy PID control algorithm based on the parameter increment value of the floating drive wheel steering gear controller; Step A4: establishing a mean square error objective function for the support force of the floating drive wheel, and optimizing the parameters of the floating drive wheel servo controller through closed-loop iteration of a fuzzy PID control algorithm until the support force of the floating drive wheel with virtual contact is adjusted to be equal to the target support force; The step 3 comprises: Step 3.

1. Establish a robot coordinate system based on the center of the robot. Use the kinematic model to calculate the wheel center coordinates of the floating drive wheel in the robot coordinate system and the center coordinates of the swing arm. Step 3.2, establishing a wheel center connection line based on the relationship between the wheel center coordinate values, establishing a swing arm center connection line based on the relationship between the swing arm center coordinate values, and establishing a swing arm fitting line based on the relationship between the wheel center coordinate values ​​and the swing arm center coordinate values; Step 3.3, establishing a mapping function model between the pitch angle and the floating angle, and a mapping function model between the roll angle and the floating angle based on the wheel center connection line, the swing arm center connection line, and the swing arm fitting line; The step 4 includes: Step 4.

1. Select any three floating drive wheels and introduce a mapping function model between the pitch angle and the floating angle, and a mapping function model between the roll angle and the floating angle based on the PSO algorithm. Step 4.2: With the minimum pitch angle and roll angle as the optimization goal, perform iterative calculations with the constraint that the wheel center coordinates of the three floating drive wheels satisfy the wheel center connection line to obtain the optimal solution for the floating angles of the three floating drive wheels; Step 4.3: Update the coordinate value of the center of the robot in the robot coordinate system using the optimal solution of the floating angle of the three floating drive wheels and the wheel center connection line; Step 4.4: Use the updated coordinate value of the robot's center in the robot coordinate system and the line connecting the wheel centers to calculate the floating angle of the fourth floating drive wheel.

2. The method for optimizing the posture of a robot with floating drive wheels according to claim 1, characterized in that: The step A2 specifically includes: Step A2.1, calculating the support force error membership of the floating drive wheel based on the target support force and the actual value of the support force of the floating drive wheel; Step A2.2, calculating the parameter membership of the floating drive wheel servo controller based on the support force error membership; Step A2.3, calculating the expected parameter value of the floating drive wheel servo controller according to the parameter membership of the floating drive wheel servo controller; Step A2.4: Calculate the parameter increment value of the floating drive wheel servo controller according to the expected parameter value of the floating drive wheel servo controller.

3. The method for optimizing the posture of a robot with floating drive wheels according to claim 1, wherein: The specific steps of establishing the mapping function model between the pitch angle and the float angle are as follows: Step B1: establishing a wheel center line on the same side based on the wheel center coordinates of the two floating drive wheels on the same side of the robot, establishing a swing arm center line on the same side based on the swing arm center coordinates of the two swing arms on the same side of the robot, and establishing a swing arm fitting line based on the wheel center coordinates of the floating drive wheels and the swing arm center coordinates of the corresponding swing arms; Step B2: establishing a mapping function model between the pitch angle and the float angle based on the ipsilateral wheel center connecting line, the ipsilateral swing arm center connecting line, and the swing arm fitting line; Step B3: Calculate the same-side terrain angle between the same-side wheel center line and the horizontal line, and calculate the pitch angle based on the mapping function model between the pitch angle and the float angle and the relationship between the same-side terrain angle.

4. The method for optimizing the posture of a robot with floating drive wheels according to claim 1, wherein: The specific steps of establishing the mapping function model between the roll angle and the float angle are as follows: Step C1: establishing a connecting line of opposite wheel centers based on the wheel center coordinates of the two floating drive wheels on opposite sides of the robot, establishing a connecting line of opposite swing arm centers based on the swing arm center coordinates of the two swing arms on opposite sides of the robot, and establishing a swing arm fitting line based on the wheel center coordinates of the floating drive wheels and the swing arm center coordinates of the corresponding swing arms; Step C2: establishing a mapping function model between the roll angle and the float angle based on the opposite wheel center line, the opposite swing arm center line, and the swing arm fitting line; Step C3: Calculate the opposite side terrain angle between the opposite side wheel center line and the horizontal line, and calculate the roll angle based on the mapping function model between the roll angle and the floating angle and the relationship between the opposite side terrain angle.

5. A method for optimizing the posture of a robot with floating drive wheels according to any one of claims 1 to 2, characterized in that: In step 1, the formula for calculating the supporting force of the floating drive wheel is as follows: ； in: represents the support force on the nth floating driving wheel at time t; represents the servo torque of the nth floating drive wheel at time t; It represents the supporting force equivalent arm of the nth floating driving wheel at time t.

6. The method for optimizing the posture of a robot with floating drive wheels according to claim 5, characterized in that: The calculation formula of the steering gear torque is as follows: ； in: Indicates the servo torque coefficient; Indicates the feedback current value of the servo of the nth floating drive wheel at time t.

Images