Posture optimization adjustment method for robot with floating type driving wheels
By dynamically adjusting the floating angle of the floating drive wheel using fuzzy PID and PSO algorithms, the problems of the floating wheel being suspended and the robot's posture being unstable on rough terrain are solved, thus achieving optimization and stability of the robot's posture.
Patent Information
- Application Number
- CN202411155744.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-22
- Publication Date
- 2024-11-12
- Estimated Expiration
- 2044-08-22
AI Technical Summary
In existing technologies, robots with floating drive wheels may tip over when traveling on rough terrain due to the floating wheels becoming suspended or the attitude angle being too large, making it impossible to achieve dynamic adjustment to maintain stability.
By using fuzzy PID control algorithm and particle swarm optimization algorithm (PSO), the floating angle of the floating drive wheel is dynamically adjusted to ensure that all wheels maintain real contact with the ground, and a robot posture optimization model is established to optimize the pitch angle and roll angle to maintain stability.
It ensures that all floating wheels of the robot maintain a ground contact state with adequate support on rough roads, and dynamically optimizes the pitch and roll angles to ensure the stability and levelness of the robot during its movement.
Smart Images

Figure CN118938979A_ABST
Abstract
Description
Technical Field
[0001] The invention belongs to the technical field of motion robot posture control and relates to a robot posture optimization adjustment method with floating drive wheels. Background Art
[0002] At present, there are many types of robot body drive forms, such as crawler, Mecanum wheel, steering wheel, differential wheel, Ekman steering wheel and other circular motion drive modes. Considering that the robot needs to pass through rough roads, it needs to have certain obstacle passability and body stability, so the robot needs to have a suspension structure, such as the spring-damping suspension and multi-link suspension commonly used in cars. However, the above suspension structures are mostly passive suspensions, and the body posture changes with the change of terrain, and it is impossible to actively adjust the suspension travel to change the body posture.
[0003] The simplest application of a robot with floating drive wheels is to adjust the height of the robot chassis to pass through roads with different degrees of roughness. However, if the floating angle of the floating drive wheel is fixed during driving, there may be an unstable state where three wheels touch the ground and one wheel is off the ground when driving on a rough road. Even after the four wheels touch the ground, the robot is in a stable state, but the robot's posture angle may be too tilted at this time, for example, the pitch angle and roll angle are too large. When the task function system carried by the robot performs work under the current robot posture, there may be a large amplitude of movement, causing the center of gravity to shift, causing the body to overturn. In the prior art, the robot is adjusted by either simply adjusting all wheels to touch the ground, but the change in the robot's posture angle during walking is not considered; or the robot's posture angle is only adjusted to meet the standard, but there is still a situation where the wheels are suspended.
[0004] Therefore, based on the above-mentioned deficiencies of the existing floating adjustment of robots, the present invention discloses a method for optimizing the posture of a robot with floating drive wheels. Summary of the invention
[0005] The object 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 walking process of the robot so that all the floating drive wheels of the robot are in a ground-contacting state with sufficient supporting force, and on this basis dynamically adjust the floating angle to optimize the pitch angle and roll angle of the robot 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 comprises four sets of floating drive wheels and a rotary arm driving the floating drive wheels to float, comprising the following steps:
[0008] Step 1: Calculate the support force of each floating drive wheel according to the torque of the floating drive wheel steering gear, 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 each floating driving wheel and the ground, proceed to step 3; if there is virtual contact between any floating driving wheel and the ground, establish a fuzzy PID control algorithm to iteratively optimize and adjust the floating angle of the floating driving wheel until all floating driving 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 roll angle and floating angle based on the current position coordinates of the floating drive wheel;
[0011] Step 4: Taking the robot's pitch angle and roll angle as the minimum optimization goal and the wheel center coordinates of the floating drive wheel satisfying the mapping function model as the constraint condition, calculate the optimal solution of 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 steering gear 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, establishing a mean square error objective function of the support force of the floating drive wheel, and optimizing 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 steering gear controller based on the support force error membership;
[0021] Step A2.3, calculating the expected value of the parameter of the floating driving wheel steering gear controller according to the parameter membership degree of the floating driving wheel steering gear 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 with the center of the robot, and obtain the wheel center coordinate value of the floating drive wheel in the robot coordinate system by solving the kinematic model, and obtain the center coordinate value of the swing arm;
[0025] Step 3.2, establishing a wheel center line according to the relationship between the wheel center coordinate values, establishing a swing arm center line according to 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: Establish 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 line, the swing arm center line, and the swing arm fitting line.
[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 connection line on the same side based on the wheel center coordinate values of the two floating drive wheels located on the same side of the robot, establishing a swing arm center connection line on the same side based on the swing arm center coordinate values of the two swing arms located on the same side of the robot, and establishing a swing arm fitting line based on the wheel center coordinate values of the floating drive wheel and the swing arm center coordinate values of the corresponding swing arm;
[0029] Step B2, establishing a mapping function model between the pitch angle and the floating angle based on the same-side wheel center connection line, the same-side swing arm center connection line, and the swing arm fitting line;
[0030] Step B3, calculating the same-side terrain angle between the same-side wheel center line and the horizontal line, and calculating the pitch angle based on a mapping function model between the pitch angle and the floating 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 floating angle are as follows:
[0032] Step C1, establishing a wheel center connecting line on the opposite side based on the wheel center coordinate values of the two floating drive wheels located on the opposite sides of the robot, establishing a center connecting line of the opposite side swing arms based on the swing arm center coordinate values of the two swing arms located on the opposite sides of the robot, and establishing a swing arm fitting line based on the wheel center coordinate values of the floating drive wheel and the swing arm center coordinate values of the corresponding swing arms;
[0033] Step C2, establishing a mapping function model between the roll angle and the floating angle based on the opposite wheel center connection line, the opposite swing arm center connection line, and the swing arm fitting line;
[0034] Step C3, calculating the opposite side terrain angle between the opposite side wheel center line and the horizontal line, and calculating 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, taking the minimum pitch angle and roll angle as the optimization goal, and taking the wheel center coordinate values of the three floating drive wheels satisfying the wheel center connection line as the constraint condition, iterative solution is performed to obtain the optimal solution of the floating angle of the three floating drive wheels;
[0038] Step 4.3, using the optimal solution of the floating angle of the three floating drive wheels and the wheel center connection line to update the coordinate value of the center of the robot in the robot coordinate system;
[0039] Step 4.4, using the updated coordinate value of the center of the robot in the robot coordinate system and the wheel center line, 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] Where: K τ Indicates the torque coefficient of the servo; It represents 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 utilizes a fuzzy PID control algorithm to ensure that all floating drive wheels of the robot always maintain real contact with the ground with a supporting force that meets the standard on any rugged road surface, and thereby establishes a kinematic model of the robot, and further derives a mapping function model between the floating angle of the floating drive wheel and the pitch angle and roll angle of the robot; 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 It is a schematic diagram of the steps of the present invention;
[0049] Figure 2 It is a schematic diagram of the mapping relationship between the pitch angle and the floating angle;
[0050] Figure 3 It is a schematic diagram of the mapping relationship between the roll angle and the floating angle;
[0051] Figure 4 This is a schematic diagram of the robot's structure;
[0052] Figure 5 is a schematic diagram of a wheel center connection line lin1 between a first floating drive wheel and a 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 descriptions are all illustrative and are 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 meanings as those 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 exemplary embodiments according to the present invention. As used herein, unless the present invention explicitly states otherwise, singular forms are also intended to include plural forms. In addition, it should be understood that when the terms "comprising" and / or "including" are used in this specification, it indicates 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, rather than indicating or implying that the referred device or element must have a specific orientation, be constructed and operated in a specific orientation. Therefore, it cannot be understood as a limitation on the present invention.
[0057] Terminology explanation section: The terms "install", "connect", "connect", "fixed" 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 integrated connection; it can be a mechanical connection or an electrical connection; it can be a direct connection or an indirect connection through an intermediate medium; it can be 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] Embodiment 1:
[0059] A method for optimizing the posture of a robot with floating drive wheels in this embodiment is as follows: Figure 4 As shown, the robot includes four groups of floating drive wheels and a swing arm that drives the floating drive wheels to float. The robot includes a robot body. The four corners of the robot body are provided with floating drive wheel steering gears. The output shaft of the floating drive wheel steering gear is connected to the wheel center of the floating drive wheel through the swing arm. The position and posture of the floating drive wheel are adjusted by the rotation of the floating drive wheel steering gear. A posture sensor is provided on the robot body. The pitch angle and roll angle of the robot body can be detected in real time through the posture sensor, and the detected pitch angle and roll angle data are sent to an external computer through the posture sensor. The external computer performs posture optimization adjustment on the pitch angle and roll angle 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 steering gear, 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 each floating driving wheel and the ground, proceed to step 3; if there is virtual contact between any floating driving wheel and the ground, establish a fuzzy PID control algorithm to iteratively optimize and adjust the floating angle of the floating driving wheel until all floating driving 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 roll angle and floating angle based on the current position coordinates of the floating drive wheel;
[0064] Step 4: Taking the robot's pitch angle and roll angle as the minimum optimization goal and the wheel center coordinates of the floating drive wheel satisfying the mapping function model as the constraint condition, calculate the optimal solution of 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] Where: K τ Indicates the torque coefficient of the servo; It represents 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 comprises 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 steering gear 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, establishing a mean square error objective function of the support force of the floating drive wheel, and optimizing 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 steering gear controller based on the support force error membership;
[0080] Step A2.3, calculating the expected value of the parameter of the floating driving wheel steering gear controller according to the parameter membership degree of the floating driving wheel steering gear 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 with the center of the robot, and obtain the wheel center coordinate value of the floating drive wheel in the robot coordinate system by solving the kinematic model, and obtain the center coordinate value of the swing arm;
[0084] Step 3.2, establishing a wheel center line according to the relationship between the wheel center coordinate values, establishing a swing arm center line according to 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: Establish 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 line, the swing arm center line, and the swing arm fitting line.
[0086] The specific steps of establishing the mapping function model between the pitch angle and the floating angle are as follows:
[0087] Step B1, establishing a wheel center connection line on the same side based on the wheel center coordinate values of the two floating drive wheels located on the same side of the robot, establishing a swing arm center connection line on the same side based on the swing arm center coordinate values of the two swing arms located on the same side of the robot, and establishing a swing arm fitting line based on the wheel center coordinate values of the floating drive wheel and the swing arm center coordinate values of the corresponding swing arm;
[0088] Step B2, establishing a mapping function model between the pitch angle and the floating angle based on the same-side wheel center connection line, the same-side swing arm center connection line, and the swing arm fitting line;
[0089] Step B3, calculating the same-side terrain angle between the same-side wheel center line and the horizontal line, and calculating the pitch angle based on a mapping function model between the pitch angle and the floating 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 wheel center connecting line on the opposite side based on the wheel center coordinate values of the two floating drive wheels located on the opposite sides of the robot, establishing a center connecting line of the opposite side swing arms based on the swing arm center coordinate values of the two swing arms located on the opposite sides of the robot, and establishing a swing arm fitting line based on the wheel center coordinate values of the floating drive wheel and the swing arm center coordinate values of the corresponding swing arms;
[0092] Step C2, establishing a mapping function model between the roll angle and the floating angle based on the opposite wheel center connection line, the opposite swing arm center connection line, and the swing arm fitting line;
[0093] Step C3, calculating the opposite side terrain angle between the opposite side wheel center line and the horizontal line, and calculating 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, taking the minimum pitch angle and roll angle as the optimization goal, and taking the wheel center coordinate values of the three floating drive wheels satisfying the wheel center connection line as the constraint condition, iterative solution is performed to obtain the optimal solution of the floating angle of the three floating drive wheels;
[0097] Step 4.3, using the optimal solution of the floating angle of the three floating drive wheels and the wheel center connection line to update the coordinate value of the center of the robot in the robot coordinate system;
[0098] Step 4.4, using the updated coordinate value of the center of the robot in the robot coordinate system and the wheel center line, calculate the floating angle of the fourth floating drive wheel.
[0099] Embodiment 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, such as Figure 1 As shown, specifically:
[0101] Read the feedback current value of the servo of the four floating drive wheels Calculate the current servo torque T t n :
[0102]
[0103] Where: K τ Indicates the torque coefficient of the servo; It represents 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 on the floating drive wheel and the steering gear 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 driving wheel at time t. The preceding operator 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 equivalent force arm of the supporting force 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 l2 represents the distance from the center of the second floating drive wheel to the center of the corresponding swing arm, i.e., 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] To judge whether there is virtual contact between 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 supporting forces of all floating driving wheels do not satisfy the above judgment formula, it indicates that all floating driving wheels do not have virtual contact; if the supporting force of any floating driving wheel satisfies the above judgment formula, the floating driving 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 driving wheel until all floating driving 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 driving 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 taken 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 the error derivative The membership degrees are:
[0121] e t The membership degree is: μ1(e t ),μ2(e t );
[0122] The membership degree of 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 prior art and will not be described in detail here.
[0125] Then, through fuzzification, we know that K p , K i , K d The membership expression of 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 of all floating drive wheels; σ represents the mean square error objective function of the support force; σ set It indicates the setting threshold of the mean square deviation of the supporting force; n indicates the number of floating driving 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 controller.p , K i , K d , thereby outputting it to the floating angle PID controller of the robot's floating driving wheel to adjust the floating angle of the floating driving wheel with virtual contact until the supporting force of the floating driving wheel with virtual contact is adjusted to be equal to the target supporting force.
[0140] After all floating drive wheels are adjusted to the non-virtual contact state, the robot posture can be optimized, that is, the pitch angle and roll angle of the robot can be adjusted as follows:
[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 driving wheel at time t; O zero Indicates the origin coordinates of the family coordinate system; represents the three-axis coordinates of the wheel center of the nth floating drive wheel at time t in the robot coordinate system; R 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 of the robot coordinate system to the world coordinate system; It represents the floating angle of the nth floating driving wheel at time t.
[0144] Taking the robot including 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 After that, the mapping function model is established. The wheel centers on the same side of the robot are respectively used. Here, the first floating drive wheel and the second floating drive wheel are on the same side, the third floating drive wheel and the fourth floating drive wheel are on the same side, the first floating drive wheel and the third floating drive wheel are on the opposite side, and the second floating drive wheel and the fourth floating drive wheel are on the opposite side.
[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, the expression of the wheel center 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 driving wheel at time t in the robot coordinate system; represents the three-axis coordinates of the wheel center of the second floating driving wheel at time t in the robot coordinate system; Indicates the three-axis coordinates of the wheel center of the third floating driving wheel at time t in the robot coordinate system; represents the three-axis coordinates of the wheel center of the fourth floating driving wheel at time t in the robot coordinate system; (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 with wheels 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 connecting 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 driving 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 floating drive wheel and the third floating drive wheel:
[0157] Based on the floating angle of the robot feet:
[0158] like Figure 3 As shown, extend p3p1, extend The two extended lines intersect at p k Point, using the principle of similar triangles: N stands for 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 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 changes, the pitch angle and roll angle of the robot change accordingly. At this time, the robot coordinate system W rob With the world coordinate system W world There are two angle flips in two directions, namely the pitch angle and the roll angle, so we need to get 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: The 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 the smallest.
[0167] Restrictions: That is, the y-coordinate and the z-coordinate of the wheel center coordinate of the floating drive wheel satisfy the linear 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 other parts of this embodiment are the same as those of Embodiment 1, and thus will not be described in detail.
[0173] The above are only preferred embodiments of the present invention and are not intended to 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 protection scope 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 steering gear, 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 each floating driving wheel and the ground, proceed to step 3; if there is virtual contact between any floating driving wheel and the ground, establish a fuzzy PID control algorithm to iteratively optimize and adjust the floating angle of the floating driving wheel until all floating driving 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 roll angle and floating angle based on the current position coordinates of the floating drive wheel; Step 4: Taking the robot's pitch angle and roll angle as the minimum optimization goal and the wheel center coordinates of the floating drive wheel satisfying the mapping function model as the constraint condition, calculate the optimal solution of 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.
2. A method for optimizing the posture of a robot with floating drive wheels according to claim 1, characterized in that: In step 2, establishing a fuzzy PID control algorithm comprises 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 steering gear 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 of the support force of the floating drive wheel, and optimizing 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.
3. A method for optimizing the posture of a robot with floating drive wheels according to claim 2, 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 steering gear controller based on the support force error membership; Step A2.3, calculating the expected value of the parameter of the floating driving wheel steering gear controller according to the parameter membership degree of the floating driving wheel steering gear 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.
4. A method for optimizing the posture of a robot with floating drive wheels according to any one of claims 1 to 3, characterized in that: The step 3 comprises: Step 3.1, establish a robot coordinate system with the center of the robot, and obtain the wheel center coordinate value of the floating drive wheel in the robot coordinate system by solving the kinematic model, and obtain the center coordinate value of the swing arm; Step 3.2, establishing a wheel center line according to the relationship between the wheel center coordinate values, establishing a swing arm center line according to 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: Establish 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 line, the swing arm center line, and the swing arm fitting line.
5. The method for optimizing the posture of a robot with floating drive wheels according to claim 4, characterized in that: The specific steps of establishing the mapping function model between the pitch angle and the floating angle are as follows: Step B1, establishing a wheel center connection line on the same side based on the wheel center coordinate values of the two floating drive wheels located on the same side of the robot, establishing a swing arm center connection line on the same side based on the swing arm center coordinate values of the two swing arms located on the same side of the robot, and establishing a swing arm fitting line based on the wheel center coordinate values of the floating drive wheel and the swing arm center coordinate values of the corresponding swing arm; Step B2, establishing a mapping function model between the pitch angle and the floating angle based on the same-side wheel center connection line, the same-side swing arm center connection line, and the swing arm fitting line; Step B3, calculating the same-side terrain angle between the same-side wheel center line and the horizontal line, and calculating the pitch angle based on a mapping function model between the pitch angle and the floating angle and the relationship between the same-side terrain angle.
6. The method for optimizing the posture of a robot with floating drive wheels according to claim 4, characterized in that: The specific steps of establishing the mapping function model between the roll angle and the float angle are as follows: Step C1, establishing a wheel center connecting line on the opposite side based on the wheel center coordinate values of the two floating drive wheels located on the opposite sides of the robot, establishing a center connecting line of the opposite side swing arms based on the swing arm center coordinate values of the two swing arms located on the opposite sides of the robot, and establishing a swing arm fitting line based on the wheel center coordinate values of the floating drive wheel and the swing arm center coordinate values of the corresponding swing arms; Step C2, establishing a mapping function model between the roll angle and the floating angle based on the opposite wheel center connection line, the opposite swing arm center connection line, and the swing arm fitting line; Step C3, calculating the opposite side terrain angle between the opposite side wheel center line and the horizontal line, and calculating 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.
7. The method for optimizing the posture of a robot with floating drive wheels according to claim 4, characterized in that: In step 4: 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, taking the minimum pitch angle and roll angle as the optimization goal, and taking the wheel center coordinate values of the three floating drive wheels satisfying the wheel center connection line as the constraint condition, iterative solution is performed to obtain the optimal solution of the floating angle of the three floating drive wheels; Step 4.3, using the optimal solution of the floating angle of the three floating drive wheels and the wheel center connection line to update the coordinate value of the center of the robot in the robot coordinate system; Step 4.4, using the updated coordinate value of the center of the robot in the robot coordinate system and the wheel center line, calculate the floating angle of the fourth floating drive wheel.
8. A method for optimizing the posture of a robot with floating drive wheels according to any one of claims 1 to 3, 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.
9. A method for optimizing the posture of a robot with floating drive wheels according to claim 8, characterized in that: The calculation formula of the steering gear torque is as follows: Where: K τ Indicates the torque coefficient of the servo; It represents the feedback current value of the servo of the nth floating drive wheel at time t.
Citation Information
Patent Citations
Balanced and stable control method for quadruped robot based on gait adjustment in three-dimensional space
CN107807655A
Self-adaption method for pavement posture of quadruped robot
CN115951696A
Embedded magnetic sensing for soft actuators
US20220379493A1
