CN116661293B - Movement control method and system for wheel-leg type vehicle jumping obstacle - Google Patents
Movement control method and system for wheel-leg type vehicle jumping obstacle Download PDFInfo
- Publication number
- CN116661293B CN116661293B CN202310935159.1A CN202310935159A CN116661293B CN 116661293 B CN116661293 B CN 116661293B CN 202310935159 A CN202310935159 A CN 202310935159A CN 116661293 B CN116661293 B CN 116661293B
- Authority
- CN
- China
- Prior art keywords
- wheel
- centroid
- vertical
- joint
- stage
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 42
- 230000009191 jumping Effects 0.000 title claims abstract description 24
- 230000001133 acceleration Effects 0.000 claims abstract description 28
- 210000002414 leg Anatomy 0.000 claims description 69
- 210000004394 hip joint Anatomy 0.000 claims description 33
- 238000006073 displacement reaction Methods 0.000 claims description 31
- 239000013598 vector Substances 0.000 claims description 20
- 210000001503 joint Anatomy 0.000 claims description 16
- 238000013016 damping Methods 0.000 claims description 14
- 210000000629 knee joint Anatomy 0.000 claims description 13
- 150000001875 compounds Chemical class 0.000 claims description 7
- 210000000689 upper leg Anatomy 0.000 claims description 5
- 239000011159 matrix material Substances 0.000 claims description 4
- 244000309466 calf Species 0.000 claims 2
- 238000010586 diagram Methods 0.000 description 8
- 230000005484 gravity Effects 0.000 description 3
- 230000009194 climbing Effects 0.000 description 2
- 239000002131 composite material Substances 0.000 description 2
- 230000008447 perception Effects 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 230000003139 buffering effect Effects 0.000 description 1
- 230000008602 contraction Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000005021 gait Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000009916 joint effect Effects 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
- B62D—MOTOR VEHICLES; TRAILERS
- B62D57/00—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track
- B62D57/02—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members
- B62D57/028—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members having wheels and mechanical legs
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B11/00—Automatic controllers
- G05B11/01—Automatic controllers electric
- G05B11/36—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential
- G05B11/42—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential for obtaining a characteristic which is both proportional and time-dependent, e.g. P. I., P. I. D.
Landscapes
- Engineering & Computer Science (AREA)
- Chemical & Material Sciences (AREA)
- Combustion & Propulsion (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a movement control method and a movement control system for jumping obstacle of a wheel-leg type vehicle, and relates to the technical field of vehicle movement control, wherein the method comprises the following steps: according to the information of the obstacle in the movement direction of the wheel-leg type vehicle, determining the longitudinal and vertical speeds of the mass center at the moment of departure from the ground; determining the vertical track of the mass center of the wheel-leg type vehicle in the take-off, emptying and landing stages and the vertical track of the wheel end in the emptying stage based on the longitudinal and vertical speeds of the mass center of the wheel-leg type vehicle at the moment of leaving the ground; determining the expected joint angle of the leg according to the vertical track of the mass center in the take-off, emptying and landing stages, the vertical track of the wheel end in the emptying stage and the relation between the mass center posture and the joint angle of the leg; outputting a control moment signal of the joint according to the actual joint angle, the actual joint angular velocity, the expected joint angle, the expected joint angular velocity and the expected angular acceleration; a control torque signal for the wheel is determined based on the desired wheel speed. The invention improves the obstacle surmounting capability and the high dynamic movement capability of the wheel leg type vehicle.
Description
Technical Field
The invention relates to the technical field of vehicle motion control, in particular to a motion control method and a motion control system for jumping obstacle of a wheel-leg type vehicle.
Background
The wheel leg type vehicle has multi-mode composite driving capability, can be applied to complex terrain environments, meets the requirements of diversified task working conditions, and is important to complete obstacle crossing actions by leg joint actions under unstructured road conditions. However, there is currently little research in the field of wheeled-leg vehicles, and there is much more research in the area of obstacle surmounting of wheeled-leg vehicles. In the existing obstacle surmounting method for the wheel-leg type vehicle, the obstacle surmounting action is mostly completed around the gait or the leg, the performance of the obstacle surmounting action is limited by structural parameters such as leg length, wheelbase, upper and lower limits of joint angles and the like, and the wheel-leg type vehicle is difficult to pass when facing higher obstacles or farther ditches. The climbing movement is difficult to control and adapt to various working conditions, foot falling positions of each step need to be planned again when the climbing movement faces obstacles with different heights, so that the obstacle crossing method is difficult to generalize, a large number of planning needs to be carried out under different working conditions, and the high dynamic movement capacity of the wheel-leg type vehicle is reduced.
Disclosure of Invention
The invention aims to provide a movement control method and a movement control system for a wheel-leg type vehicle jumping obstacle, which improve the high dynamic movement capacity of the wheel-leg type vehicle jumping obstacle.
In order to achieve the above object, the present invention provides the following solutions:
a method of motion control of a wheel-legged vehicle jump obstacle, comprising:
determining the longitudinal speed and the vertical speed of the mass center of the wheel-leg type vehicle at the moment of leaving the ground according to the height and the length of an obstacle in the movement direction of the wheel-leg type vehicle;
determining a vertical centroid track of a take-off stage, a vertical centroid track of a vacation stage, a vertical wheel end track of the vacation stage and a vertical centroid track of a landing stage of the wheel-leg type vehicle based on the longitudinal speed and the vertical speed of the centroid at the moment of the wheel-leg type vehicle leaving the ground;
determining the relation between the centroid posture and the leg joint angle according to the geometrical vector relation of the legs of the wheel-leg type vehicle and the inverse kinematics of the legs;
determining the expected joint angle of the leg according to the vertical centroid track of the take-off stage, the vertical centroid track of the vacation stage, the vertical wheel end track of the vacation stage, the vertical centroid track of the landing stage and the relation between the centroid posture and the leg joint angle;
outputting control moment signals of the joint according to the actual joint angle, the actual joint angular velocity, the expected joint angle, the expected joint angular velocity and the expected angular acceleration fed back by the joint encoder; the desired joint angular velocity and the desired angular acceleration are differentially determined from the desired joint angle;
and determining expected wheel speeds of the take-off stage, the vacation stage and the landing stage according to the longitudinal speed of the mass center at the initial moment of the take-off stage, and determining a control moment signal of the wheel according to the expected wheel speeds.
The invention also discloses a motion control system of the wheel leg type vehicle jumping obstacle, which comprises:
the ground-leaving moment mass center speed determining module is used for determining the longitudinal speed and the vertical speed of the ground-leaving moment mass center of the wheel-leg type vehicle according to the height and the length of an obstacle in the movement direction of the wheel-leg type vehicle;
the vertical centroid track determining module is used for determining the vertical centroid track of the wheel-leg vehicle in the take-off stage, the vertical centroid track of the vacation stage, the vertical centroid track of the wheel end in the vacation stage and the vertical centroid track of the landing stage based on the longitudinal speed and the vertical speed of the centroid of the wheel-leg vehicle at the moment of leaving the ground;
the relation determining module is used for determining the relation between the centroid gesture and the leg joint angle according to the geometrical vector relation of the leg of the wheel-leg type vehicle and the inverse kinematics of the leg;
the expected joint angle determining module is used for determining an expected joint angle of the leg according to the vertical centroid track of the take-off stage, the vertical centroid track of the vacation stage, the vertical wheel end track of the vacation stage, the vertical centroid track of the landing stage and the relation between the centroid posture and the leg joint angle;
the control moment signal determining module of the joint is used for outputting control moment signals of the joint according to the actual joint angle, the actual joint angular velocity, the expected joint angle, the expected joint angular velocity and the expected angular acceleration fed back by the joint encoder; the desired joint angular velocity and the desired angular acceleration are differentially determined from the desired joint angle;
and the control moment signal determining module is used for determining expected wheel rotating speeds of the take-off stage, the vacation stage and the landing stage according to the longitudinal speed of the mass center at the initial moment of the take-off stage and determining the control moment signal of the wheel according to the expected wheel rotating speeds.
According to the specific embodiment provided by the invention, the invention discloses the following technical effects:
according to the invention, the time and the speed of the ground leaving moment are determined according to the obstacle information and the information of the vehicle, the centroid track and the wheel end track are planned, the wheels provide higher longitudinal speed by utilizing the composite driving of the wheels and the legs, the leg joints provide vertical jumping power, the long-distance jumping is realized, and the obstacle crossing performance of the wheel-leg type vehicle is improved. In addition, when facing different obstacles, the method can still plan a proper jump track faster to realize obstacle crossing.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions of the prior art, the drawings that are needed in the embodiments will be briefly described below, it being obvious that the drawings in the following description are only some embodiments of the present invention, and that other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
Fig. 1 is a schematic flow chart of a motion control method for jumping obstacle of a wheel-leg vehicle according to an embodiment of the present invention;
fig. 2 is a schematic diagram of a control principle of a movement control method of a wheel-leg type vehicle jumping obstacle according to an embodiment of the present invention;
FIG. 3 is a schematic diagram of a movement process of a wheel-legged vehicle jump obstacle according to an embodiment of the present invention;
FIG. 4 is a schematic diagram of a virtual spring damping model constructed according to a wheel-leg vehicle according to an embodiment of the present invention;
FIG. 5 is a graph showing the relationship between the center of mass and the leg joint angle provided by an embodiment of the present invention;
FIG. 6 is a schematic diagram of a single-leg kinematic sagittal plane analysis provided by an embodiment of the present invention;
fig. 7 is a schematic diagram of a control principle of an inverse dynamic PD controller according to an embodiment of the present invention;
fig. 8 is a schematic diagram of a motion control system for a jumping obstacle of a wheel-leg vehicle according to an embodiment of the present invention;
fig. 9 is a schematic diagram of a motion control system for a wheel-leg vehicle jumping obstacle according to an embodiment of the present invention.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
The invention aims to provide a movement control method and a movement control system for a wheel-leg type vehicle jumping obstacle, which improve the high dynamic movement capacity of the wheel-leg type vehicle jumping obstacle.
In order that the above-recited objects, features and advantages of the present invention will become more readily apparent, a more particular description of the invention will be rendered by reference to the appended drawings and appended detailed description.
Example 1
As shown in fig. 1 and 2, the present embodiment provides a motion control method of a wheel-leg vehicle jumping obstacle, the method including the following steps.
Step 101: and determining the longitudinal speed and the vertical speed of the mass center of the wheel-leg type vehicle at the moment of leaving the ground according to the height and the length of the obstacle in the movement direction of the wheel-leg type vehicle.
The centroid is the centroid of the wheel-legged vehicle.
Step 102: and determining the vertical track of the mass center at the take-off stage, the vertical track of the mass center at the vacation stage, the vertical track of the wheel end at the vacation stage and the vertical track of the mass center at the grounding stage of the wheel-leg type vehicle based on the longitudinal speed and the vertical speed of the mass center at the ground leaving moment of the wheel-leg type vehicle.
Step 103: and determining the relation between the centroid posture and the leg joint angle according to the geometrical vector relation of the leg of the wheel-leg type vehicle and the inverse kinematics of the leg.
Step 104: and determining the expected joint angle of the leg according to the vertical track of the mass center of the take-off stage, the vertical track of the mass center of the vacation stage, the vertical track of the wheel end of the vacation stage, the vertical track of the mass center of the landing stage and the relation between the mass center gesture and the leg joint angle.
Step 105: outputting control moment signals of the joint according to the actual joint angle, the actual joint angular velocity, the expected joint angle, the expected joint angular velocity and the expected angular acceleration fed back by the joint encoder; the desired joint angular velocity and the desired angular acceleration are differentially determined from the desired joint angle.
Step 106: and determining expected wheel speeds of the take-off stage, the vacation stage and the landing stage according to the longitudinal speed of the mass center at the initial moment of the take-off stage, and determining a control moment signal of the wheel according to the expected wheel speeds.
The legs of the wheel-leg type vehicle comprise 4 legs, each leg has the same structure, each leg comprises a hip joint, a thigh, a knee joint and a shank which are sequentially connected, one end of each shank is connected with the knee joint, and the other end of each shank is connected with a wheel; the legs of the wheeled legged vehicle move synchronously with 4 legs during the take-off phase, the vacation phase, and the landing phase.
As shown in fig. 3, the entire process of jumping obstacles of the wheeled leg type vehicle is divided into a take-off phase, a vacation phase, and a landing phase, where (a) in fig. 3 represents the take-off phase, (b) represents the vacation phase, and (c) represents the landing phase. Determining the moment of departure (ground leaving moment) t based on the information of the obstacle with the current moment being 0 1 I.e. the end of the take-off phase. Setting starting time of starting jump stage according to joint actuating capabilityDelta represents the duration of the entire take-off phase, delta being an adjustable parameter.
The step 101 specifically includes:
the obstacle height H and the obstacle length S are acquired by a perception sensor.
(1)
And (3) calculating the longitudinal speed and the vertical speed of the mass center of the ground leaving moment of the wheel-leg type vehicle according to the formula (1).
Wherein t is s Represents the flight time of the wheel-legged vehicle, H represents the obstacle height, S represents the obstacle length, λ represents the preset height margin,is an adjustable parameter, ++>Represents the minimum distance of the wheel end to the hip joint in the vertical direction, +.>Represents the maximum distance from the wheel end to the hip joint in the vertical direction, wherein gamma is constant and is slightly more than half of the height of the car body, and +.>Longitudinal speed representing the centroid of the moment of departure, +.>Represents the vertical velocity of the centroid at the moment of departure, g represents the gravitational acceleration.
The longitudinal direction is the front-rear direction (horizontal direction) of the vehicle movement, i.e. the X-axis direction in fig. 5, and the vertical direction is the height direction of the vehicle movement, i.e. the Z-axis direction in fig. 5, the freedom of the wheel-legged vehicle in the entire jumping movement being only referred to in the X-Z sagittal plane.
In step 102, determining a vertical trajectory of a center of mass of the wheel-leg vehicle in a take-off stage specifically includes:
taking the centroid state at the initial moment of the jump stage and the expected centroid state at the ground leaving moment as constraint conditions, and planning the centroid vertical track of the jump stage by adopting a five-time polynomial method; the centroid state and the desired centroid state each include a centroid position, a vertical velocity and a vertical acceleration.
The vertical trajectory of the centroid of the take-off phase is expressed as:
(2)
wherein h is CoM (t) represents the vertical displacement (height) of the centroid at time t,representing the centroid position at the initial moment of the jump phase, t 0 Indicating the initial time of the take-off stage, a 0 Representing a first coefficient, a 1 Representing a second coefficient, a 2 Represents a third coefficient, a 3 Represents the fourth coefficient, a 4 Representing the fifth coefficient, a 5 Representing a sixth coefficient;
the motion constraint condition of the take-off stage is expressed as:
(3)
wherein h is CoM (t 0 ) Representing t 0 The centroid of the moment of time is vertically displaced,representing t 0 The centroid speed of the moment in time,representing t 0 Centroid acceleration at moment, h CoM (t 1 ) Representing t 1 Moment in time, vertical displacement of centroid->Representing t 1 Centroid speed of moment>Representing t 1 Moment centroid acceleration, g represents gravitational acceleration, t 0 Indicating the initial time of the take-off stage, t 1 Indicates the time of departure from the ground,/->Represents the centroid position at the moment of departure, +.>Representing the centroid speed at the moment of departure from the ground.
Substituting the formula (3) into the formula (2), and solving the undetermined coefficient to obtain the vertical displacement track of the mass center in the take-off stage.
The wheel leg type vehicle only receives the gravity action in the emptying stage, can be regarded as ideal oblique throwing movement, and determines the vertical track of the mass center in the emptying stage, and specifically comprises the following steps:
and determining the vertical trajectory of the mass center in the emptying stage based on the vertical speed of the mass center at the moment of leaving the ground and an ideal oblique throwing kinematic model.
The vertical trajectory of the centroid of the flight phase is expressed as:
(4)
wherein h is CoM (t) represents the vertical displacement of the centroid at time t,the centroid speed at the moment of departure is shown, and g is the gravitational acceleration.
In addition to planning the trajectory of the centroid, the vacation phase is taken as a phase for realizing obstacle surmounting, and the wheel end trajectory is properly planned. Because the wheel leg type vehicle is free from other external force and external moment except gravity in the air, the longitudinal movement of the wheel end is synchronous with the hip joint and the mass center of the vehicle body, the vertical movement of the wheel end is further planned to ensure that the wheel end can surmount a certain height, and the vertical track of the wheel end in the emptying stage is determined, and the method specifically comprises the following steps:
and determining the wheel end vertical track in the emptying stage by adopting a compound cycloid planning method based on the preset height allowance. The compound cycloid planning method is a compound cycloid planning method which modifies common compound cycloids according to a zero-impact principle, and achieves the purpose of reducing wheel end ground contact impact.
The vertical track of the wheel end in the vacation stage is expressed as:
(5)
wherein,represents the vertical displacement of the wheel end at the moment t, lambda represents the preset height allowance, t s Representing the flight time of the wheeled leg vehicle.
The purpose of carrying out centroid track to the ground stage is to reduce wheel leg formula vehicle whereabouts and strikeed, realizes the bradyseism from the motion planning aspect. In the invention, the flexible thought of the virtual spring damping model is adopted, the centroid track of the grounding stage is planned, and the virtual spring damping model is shown in fig. 4.
k r C, for the virtual spring rate in the virtual spring damping model r For virtual damping in the virtual spring damping model, the two physical quantities are adjustable parameters, m T The mass of a wheeled leg vehicle is a known quantity.
Determining a centroid vertical trajectory of a landing stage, specifically comprising:
and determining a barycenter dynamics equation of the wheel-leg vehicle based on the virtual spring damping model of the wheel-leg vehicle, and determining a barycenter vertical track of the landing stage according to the barycenter dynamics equation.
The mass dynamics equation of the wheel-legged vehicle can be simplified to equation (6).
(6)
Wherein,represents centroid speed at time t +.>The centroid acceleration at time t is indicated,representing the variation value of the center of mass height of the wheel-legged vehicle compared to the moment of landing, < > in the virtual spring damping model>Can characterize the expansion and contraction of the virtual spring, +.>Indicating the height of the stationary state of the wheel-legged vehicle.
The motion constraint condition that the mass center needs to meet is (7), and the mass center speed is required to be continuous at the moment of landing so as to realize buffering and damping.
(7)
h CoM (0) Represents the vertical height of the centroid at the time of landing,representing the vertical velocity of the centroid at the time of landing.
Substituting equation (7) into equation (6) yields a centroid vertical trajectory for the touchdown phase, expressed as:
(8)
wherein h is CoM (t 0 ) Representing t 0 Moment of vertical displacement of centroid, k r C, for the virtual spring rate in the virtual spring damping model r For virtual damping in the virtual spring damping model, m T For the mass of the wheeled-legged vehicle, C 1 As a first intermediate parameter, C 2 As a second intermediate parameter, the first intermediate parameter,，/>，/>indicating when the ground is lifted offCentroid speed of inscription, ++>Indicating the centroid position at the moment of departure from the ground.
The grounding stage time is the adjusting time of the virtual spring damping system and is determined by the whole mass, the virtual rigidity and the virtual damping of the wheel leg platform.
And combining the vertical trajectories of the mass centers of the take-off stage, the vacation stage and the grounding stage to obtain the vertical trajectory of the mass center in the whole jumping process.
As shown in FIG. 5, O B Is the centroid position of the wheel-leg vehicle, O W Is the origin position of the world coordinate system. The movement of the jumping obstacle according to the present invention is accomplished by simultaneous actuation of four legs, and thus, one of the legs is taken as an example for analysis, and the other three legs are identical thereto, and the vector relationship can be described by the formula (9).
(9)
In the method, in the process of the invention,three linear displacements, x, representing centroid poses CoM Linear displacement, y, representing the x-direction of the centroid pose CoM Linear displacement, z, representing the x-direction of the centroid pose CoM Representing the linear displacement of the centroid pose in the x-direction. Three linear displacements are determined by structural parameters of the wheel-leg vehicle, namely the position vector of the hip joint relative to the center of mass in a coordinate system taking the center of mass of the vehicle body as an origin +.>Is determined, is a known quantity. O (O) H Is the hip joint position, G H Is the wheel end (wheel) position. In the method of the invention, the position of the wheel end is set except the vertical direction, namely the position vector of the projection of the wheel end on the ground relative to the centroid projection is +.>Are known. R is R B Is a rotation matrix formed by centroid attitude angles, and the expression is shown as a formula (10).
(10)
Wherein phi, theta and omega respectively represent the roll angle, pitch angle and yaw angle of the centroid relative to the world coordinate system. Because only the vertical position is emphasized, it is set, x CoM =0，y CoM The vectors can be found by taking the right side of the equal sign in equation (9) as a known or variable, i.e. =0, Φ=0, θ=0, ω=0。
FIG. 6 is a diagram of a single-leg kinematic sagittal plane analysis, as shown in FIG. 6, vectorIn leg kinematics the position coordinates of the wheel end with respect to the hip joint are described, i.e. +.>。
Wherein,representing a linear displacement of the hip joint in the x-direction, < >>Representing a linear displacement of the hip joint in the y-direction,representing a linear displacement of the hip joint in the z-direction. The single-leg inverse kinematics relation can be obtained through the triangle geometry relation and the cosine theorem, and because the wheel-leg type vehicle adopts the elbow-knee structure, a group of solutions in the inverse kinematics relation are omitted according to the arrangement form of the legs, and the unique solution is obtained as shown in the formula (11). That is, the vertical trajectory of the mass center in the take-off stage and the landing stage can be calculated as the desired joint angle by the formulas (9) - (11).
When the wheel-legged vehicle motion is in the take-off phase and the landing phase, the desired joint angle of the hip joint and the desired joint angle of the knee joint are expressed as:
(11)
wherein q H Represents the desired joint angle, q, of the hip joint K Representing a desired joint angle of the knee joint, L H Indicating thigh length, L l Representing the length of the lower leg,representing the x-axis coordinates of the wheel end, +.>Representing the z-axis coordinates of the wheel end.
Since the wheel end trajectory has been planned in step 102 for the vacation phase to achieve obstacle surmounting behaviour, equation (5), the desired joint angle of the hip joint and the desired joint angle of the knee joint are expressed as:
(12)
wherein,representing the vertical displacement of the wheel end, the centroid vertical trajectory planned in step 102 can be resolved into the desired joint angle by equations (9) - (12).
Step 105 specifically includes:
and outputting control moment signals of the joint according to the actual joint angle, the actual joint angular velocity, the expected joint angle, the expected joint angular velocity and the expected angular acceleration fed back by the joint encoder by adopting the inverse dynamics PD controller.
The control principle of the inverse dynamic PD controller (joint controller) is shown in fig. 7. Wherein,，/>the actual joint angle and the actual joint angular velocity fed back by the joint encoder are available physical quantities respectively. />Is a combination of coriolis force, centrifugal force and gravity force>Middle->Namely +.>，/>Namely +.>，/>Is an inertial matrix, the two matrices can be defined by +.>，/>Obtained through a certain operation. />All are joint controller gain coefficients, and appropriate values can be set. />Is the desired joint angle, obtained from step 104, < >>Respectively the desired angular velocity of the jointThe degree, desired angular acceleration, may be obtained by differentiating the desired joint angle in step 104.
The joint controller actual control torque signal (control torque signal of the joint) is expressed as:
；
wherein,a control moment signal representing the joint.
Wherein, in step 106, the wheel rotation speed is desiredThe calculation formula of (2) is expressed as:
；
wherein r is the radius of the wheel,is the longitudinal speed of the mass center of the ground leaving moment of the wheel-leg type vehicle.
The actual control torque signal (control torque signal of the wheel) generated by the wheel controller is expressed as:
；
wherein,，/>all are wheel controller gain coefficients, +.>，/>Respectively areThe actual rotational speed and the actual angle of the wheel, which are fed back by the encoder, < >>Is the desired angle of the wheel>By->Integration is obtained.
The movement control method of the wheel-leg vehicle jump obstacle further includes: and (3) inputting the control moment signals of the joints in the step (105) to a joint motor, and inputting the control moment signals of the wheels in the step (106) to a wheel motor, so as to complete the whole jumping obstacle movement.
Example 2
As shown in fig. 8, the present embodiment provides a motion control system of a wheel-leg vehicle jumping obstacle, the system comprising:
the ground-leaving moment centroid speed determining module 201 is configured to determine a longitudinal speed and a vertical speed of a ground-leaving moment centroid of the wheel-leg vehicle according to a height and a length of an obstacle in a movement direction of the wheel-leg vehicle.
The vertical centroid and vertical wheel end trajectory determining module 202 is configured to determine a vertical centroid trajectory at a take-off stage, a vertical centroid trajectory at a vacation stage, a vertical wheel end trajectory at a vacation stage, and a vertical centroid trajectory at a landing stage of the wheeled-leg vehicle based on a longitudinal speed and a vertical speed of a centroid at a moment when the wheeled-leg vehicle is off the ground.
The relationship determining module 203 of the centroid gesture and the leg joint angle is configured to determine a relationship of the centroid gesture and the leg joint angle according to the geometric vector relationship of the leg of the wheel-leg vehicle and the inverse kinematics of the leg.
The expected joint angle determining module 204 is configured to determine an expected joint angle of the leg according to the vertical trajectory of the centroid of the jump stage, the vertical trajectory of the centroid of the vacation stage, the vertical trajectory of the wheel end of the vacation stage, the vertical trajectory of the centroid of the landing stage, and the relationship between the centroid posture and the leg joint angle.
The control moment signal determining module 205 of the joint is configured to output a control moment signal of the joint according to the actual joint angle, the actual joint angular velocity, and the desired joint angle, the desired joint angular velocity, and the desired angular acceleration fed back by the joint encoder; the desired joint angular velocity and the desired angular acceleration are differentially determined from the desired joint angle.
The control moment signal determining module 206 of the wheel is configured to determine a desired wheel rotational speed of the take-off phase, the vacation phase and the landing phase according to a longitudinal speed of a centroid at an initial moment of the take-off phase, and determine a control moment signal of the wheel according to the desired wheel rotational speed.
Example 3
As shown in fig. 9, the present embodiment provides a motion control system of a wheel-leg vehicle jumping obstacle, the system comprising:
the environment perception information extraction and behavior decision module is used for acquiring the obstacle information, determining the take-off time and providing upper layer information for the subsequent planning module.
And the centroid and wheel end track planning module is used for planning a centroid vertical track of a wheel leg type vehicle in a take-off stage based on a pentagram polynomial method, planning a centroid vertical track of a vacation stage based on a kinematic model, planning a centroid vertical track of a landing stage based on a spring damping model and planning a wheel end vertical track of the vacation stage based on a modified compound cycloid equation.
And the gesture resolving module is used for resolving the expected centroid vertical track into the expected joint angle by combining the wheel end vertical track through a kinematic vector method and a leg inverse kinematic relation.
The joint and wheel control module is used for obtaining corresponding actual control moment according to the expected joint angle, the expected joint angular velocity, the expected angular acceleration, the wheel rotating speed, the wheel angle, the fed-back joint angle, the joint angular velocity, the wheel rotating speed and the wheel angle.
And the feedback module is used for inputting the actual angles and angular velocities of the hip joint and the knee joint to the joint controller and inputting the actual wheel rotating speed and the actual wheel angle to the wheel controller.
In the present specification, each embodiment is described in a progressive manner, and each embodiment is mainly described in a different point from other embodiments, and identical and similar parts between the embodiments are all enough to refer to each other. For the system disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant points refer to the description of the method section.
The principles and embodiments of the present invention have been described herein with reference to specific examples, the description of which is intended only to assist in understanding the methods of the present invention and the core ideas thereof; also, it is within the scope of the present invention to be modified by those of ordinary skill in the art in light of the present teachings. In view of the foregoing, this description should not be construed as limiting the invention.
Claims (5)
1. A method of controlling movement of a wheel-legged vehicle jump obstacle, comprising:
determining the longitudinal speed and the vertical speed of the mass center of the wheel-leg type vehicle at the moment of leaving the ground according to the height and the length of an obstacle in the movement direction of the wheel-leg type vehicle;
determining a vertical centroid track of a take-off stage, a vertical centroid track of a vacation stage, a vertical wheel end track of the vacation stage and a vertical centroid track of a landing stage of the wheel-leg type vehicle based on the longitudinal speed and the vertical speed of the centroid at the moment of the wheel-leg type vehicle leaving the ground;
determining the relation between the centroid posture and the leg joint angle according to the geometrical vector relation of the legs of the wheel-leg type vehicle and the inverse kinematics of the legs;
determining the expected joint angle of the leg according to the vertical centroid track of the take-off stage, the vertical centroid track of the vacation stage, the vertical wheel end track of the vacation stage, the vertical centroid track of the landing stage and the relation between the centroid posture and the leg joint angle;
outputting control moment signals of the joint according to the actual joint angle, the actual joint angular velocity, the expected joint angle, the expected joint angular velocity and the expected angular acceleration fed back by the joint encoder; the desired joint angular velocity and the desired angular acceleration are differentially determined from the desired joint angle;
determining expected wheel speeds of the take-off stage, the vacation stage and the landing stage according to the longitudinal speed of the mass center at the initial moment of the take-off stage, and determining a control moment signal of the wheel according to the expected wheel speeds;
the method for determining the vertical track of the mass center of the wheel leg type vehicle in the take-off stage specifically comprises the following steps:
taking the centroid state at the initial moment of the jump stage and the expected centroid state at the ground leaving moment as constraint conditions, and planning the centroid vertical track of the jump stage by adopting a five-time polynomial method; the centroid state and the desired centroid state each include a centroid position, a vertical velocity, and a vertical acceleration;
determining a vertical trajectory of a centroid of the vacation phase, specifically comprising:
determining a centroid vertical track in a vacation stage based on the vertical speed of the centroid at the moment of departure and an ideal oblique throwing kinematic model;
determining the vertical track of the wheel end in the vacation stage, which specifically comprises the following steps:
determining a wheel end vertical track in a emptying stage by adopting a compound cycloid planning method based on a preset height allowance;
determining a centroid vertical trajectory of a landing stage, specifically comprising:
determining a barycenter dynamics equation of the wheel-leg vehicle based on the virtual spring damping model of the wheel-leg vehicle, and determining a barycenter vertical track of the landing stage according to the barycenter dynamics equation;
the vector relationship of one leg is expressed as:
wherein O is B Is the centroid position of the wheel-leg vehicle, O W Is the origin position of the world coordinate system, G W Is a wheel endThe position of the device is determined by the position,three linear displacements, x, representing centroid poses CoM Linear displacement, y, representing the x-direction of the centroid pose CoM Linear displacement, z, representing the x-direction of the centroid pose CoM Linear displacement in x-direction representing centroid pose, +.>Representing the position vector of the hip joint relative to the centroid in a coordinate system with the body centroid as origin, +.>A position vector representing the projection of the wheel end on the ground relative to the centroid projection, R B Is a rotation matrix formed by centroid attitude angles;
a vector representing position coordinates describing the wheel end relative to the hip joint:
wherein,representing a linear displacement of the hip joint in the x-direction, < >>Representing a linear displacement of the hip joint in the y-direction, < >>Representing a linear displacement of the hip joint in the z-direction;
when the wheel-legged vehicle motion is in the take-off phase and the landing phase, the desired joint angle of the hip joint and the desired joint angle of the knee joint are expressed as:
wherein q H Represents the desired joint angle, q, of the hip joint K Representing a desired joint angle of the knee joint, L H Indicating thigh length, L l Representing the calf length;
when the wheel-legged vehicle motion is in the vacation phase, the desired joint angle of the hip joint and the desired joint angle of the knee joint are expressed as:
wherein,representing the vertical displacement of the wheel end.
2. The method for controlling the movement of a wheel-legged vehicle jump obstacle according to claim 1, wherein the determining of the longitudinal speed and the vertical speed of the center of mass of the wheel-legged vehicle at the moment of departure from the ground according to the height and the length of the obstacle in the movement direction of the wheel-legged vehicle comprises:
according to the formulaCalculating the longitudinal speed and the vertical speed of the mass center of the wheel-leg type vehicle at the moment of leaving the ground;
wherein t is s Represents the flight time of the wheel-legged vehicle, H represents the obstacle height, S represents the obstacle length, λ represents the preset height margin, represents the minimum distance of the wheel end to the hip joint in the vertical direction, +.>Represents the maximum distance from the wheel end to the hip joint in the vertical direction, gamma is constant, +.>Longitudinal speed representing the centroid of the moment of departure, +.>Represents the vertical velocity of the centroid at the moment of departure, g represents the gravitational acceleration.
3. The method for controlling the movement of a wheel-legged vehicle jumping obstacle according to claim 1, wherein the control moment signal of the joint is outputted according to the actual joint angle, the actual joint angular velocity, and the desired joint angle, the desired joint angular velocity, and the desired angular acceleration fed back by the joint encoder, and specifically comprises:
and outputting control moment signals of the joint according to the actual joint angle, the actual joint angular velocity, the expected joint angle, the expected joint angular velocity and the expected angular acceleration fed back by the joint encoder by adopting the inverse dynamics PD controller.
4. The method of claim 1, wherein the legs of the wheeled-legged vehicle comprise 4 legs, each leg being identical in structure, each leg comprising a hip joint, a thigh, a knee joint, and a shank connected in sequence; the legs of the wheeled legged vehicle move synchronously with 4 legs during the take-off phase, the vacation phase, and the landing phase.
5. A motion control system for a wheel-legged vehicle jump obstacle, comprising:
the ground-leaving moment mass center speed determining module is used for determining the longitudinal speed and the vertical speed of the ground-leaving moment mass center of the wheel-leg type vehicle according to the height and the length of an obstacle in the movement direction of the wheel-leg type vehicle;
the vertical centroid track determining module is used for determining the vertical centroid track of the wheel-leg vehicle in the take-off stage, the vertical centroid track of the vacation stage, the vertical centroid track of the wheel end in the vacation stage and the vertical centroid track of the landing stage based on the longitudinal speed and the vertical speed of the centroid of the wheel-leg vehicle at the moment of leaving the ground;
the relation determining module is used for determining the relation between the centroid gesture and the leg joint angle according to the geometrical vector relation of the leg of the wheel-leg type vehicle and the inverse kinematics of the leg;
the expected joint angle determining module is used for determining an expected joint angle of the leg according to the vertical centroid track of the take-off stage, the vertical centroid track of the vacation stage, the vertical wheel end track of the vacation stage, the vertical centroid track of the landing stage and the relation between the centroid posture and the leg joint angle;
the control moment signal determining module of the joint is used for outputting control moment signals of the joint according to the actual joint angle, the actual joint angular velocity, the expected joint angle, the expected joint angular velocity and the expected angular acceleration fed back by the joint encoder; the desired joint angular velocity and the desired angular acceleration are differentially determined from the desired joint angle;
the control moment signal determining module of the wheel is used for determining expected wheel rotating speeds of the take-off stage, the vacation stage and the landing stage according to the longitudinal speed of the mass center at the initial moment of the take-off stage, and determining a control moment signal of the wheel according to the expected wheel rotating speeds;
the method for determining the vertical track of the mass center of the wheel leg type vehicle in the take-off stage specifically comprises the following steps:
taking the centroid state at the initial moment of the jump stage and the expected centroid state at the ground leaving moment as constraint conditions, and planning the centroid vertical track of the jump stage by adopting a five-time polynomial method; the centroid state and the desired centroid state each include a centroid position, a vertical velocity, and a vertical acceleration;
determining a vertical trajectory of a centroid of the vacation phase, specifically comprising:
determining a centroid vertical track in a vacation stage based on the vertical speed of the centroid at the moment of departure and an ideal oblique throwing kinematic model;
determining the vertical track of the wheel end in the vacation stage, which specifically comprises the following steps:
determining a wheel end vertical track in a emptying stage by adopting a compound cycloid planning method based on a preset height allowance;
determining a centroid vertical trajectory of a landing stage, specifically comprising:
determining a barycenter dynamics equation of the wheel-leg vehicle based on the virtual spring damping model of the wheel-leg vehicle, and determining a barycenter vertical track of the landing stage according to the barycenter dynamics equation;
the vector relationship of one leg is expressed as:
wherein O is B Is the centroid position of the wheel-leg vehicle, O W Is the origin position of the world coordinate system, G W Is the position of the wheel end,three linear displacements, x, representing centroid poses CoM Linear displacement, y, representing the x-direction of the centroid pose CoM Linear displacement, z, representing the x-direction of the centroid pose CoM Linear displacement in x-direction representing centroid pose, +.>Representing the position vector of the hip joint relative to the centroid in a coordinate system with the body centroid as origin, +.>A position vector representing the projection of the wheel end on the ground relative to the centroid projection, R B Is a rotation matrix formed by centroid attitude angles;
a vector representing position coordinates describing the wheel end relative to the hip joint:
wherein,representing a linear displacement of the hip joint in the x-direction, < >>Representing a linear displacement of the hip joint in the y-direction, < >>Representing a linear displacement of the hip joint in the z-direction;
when the wheel-legged vehicle motion is in the take-off phase and the landing phase, the desired joint angle of the hip joint and the desired joint angle of the knee joint are expressed as:
wherein q H Represents the desired joint angle, q, of the hip joint K Representing a desired joint angle of the knee joint, L H Indicating thigh length, L l Representing the calf length;
when the wheel-legged vehicle motion is in the vacation phase, the desired joint angle of the hip joint and the desired joint angle of the knee joint are expressed as:
wherein,representing the vertical displacement of the wheel end.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310935159.1A CN116661293B (en) | 2023-07-28 | 2023-07-28 | Movement control method and system for wheel-leg type vehicle jumping obstacle |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310935159.1A CN116661293B (en) | 2023-07-28 | 2023-07-28 | Movement control method and system for wheel-leg type vehicle jumping obstacle |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116661293A CN116661293A (en) | 2023-08-29 |
| CN116661293B true CN116661293B (en) | 2023-11-07 |
Family
ID=87712137
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310935159.1A Active CN116661293B (en) | 2023-07-28 | 2023-07-28 | Movement control method and system for wheel-leg type vehicle jumping obstacle |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116661293B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118092144B (en) * | 2024-04-18 | 2024-06-21 | 北京理工大学 | Wheel leg robot motion control method and system |
| CN118067133B (en) * | 2024-04-18 | 2024-06-18 | 北京理工大学 | Jumping track generation method and device applied to single-wheel-leg robot |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112207825A (en) * | 2020-09-28 | 2021-01-12 | 杭州云深处科技有限公司 | Control method and device for bionic jumping action of quadruped robot, electronic equipment and computer readable medium |
| CN114578836A (en) * | 2022-02-11 | 2022-06-03 | 中国北方车辆研究所 | Jumping control method for double-wheel foot robot |
| CN115356920A (en) * | 2022-08-08 | 2022-11-18 | 深圳市优必选科技股份有限公司 | Single-leg jumping motion control method and device, readable storage medium and robot |
| CN115480594A (en) * | 2021-05-31 | 2022-12-16 | 腾讯科技（深圳）有限公司 | Jump control method, apparatus, device, and medium |
-
2023
- 2023-07-28 CN CN202310935159.1A patent/CN116661293B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112207825A (en) * | 2020-09-28 | 2021-01-12 | 杭州云深处科技有限公司 | Control method and device for bionic jumping action of quadruped robot, electronic equipment and computer readable medium |
| CN115480594A (en) * | 2021-05-31 | 2022-12-16 | 腾讯科技（深圳）有限公司 | Jump control method, apparatus, device, and medium |
| CN114578836A (en) * | 2022-02-11 | 2022-06-03 | 中国北方车辆研究所 | Jumping control method for double-wheel foot robot |
| CN115356920A (en) * | 2022-08-08 | 2022-11-18 | 深圳市优必选科技股份有限公司 | Single-leg jumping motion control method and device, readable storage medium and robot |
Non-Patent Citations (2)
| Title |
|---|
| 一种弹跳机器人的越障性能仿真分析;柴辉 等;机械制造;第47卷(第541期);第16-17页 * |
| 仿青蛙机器人跳跃之力学分析与轨迹优化;任毅豪 等;力学季刊;第38卷(第02期);第289-295页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116661293A (en) | 2023-08-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN116661293B (en) | Movement control method and system for wheel-leg type vehicle jumping obstacle | |
| US7319919B2 (en) | Control device and footstep determination device for legged mobile robot | |
| JP3679105B2 (en) | Gait generator for legged mobile robot | |
| JP3672100B2 (en) | Gait generator for legged mobile robot | |
| JP5483997B2 (en) | Control device for legged mobile robot | |
| JP5219956B2 (en) | Control device for moving body | |
| US8825213B2 (en) | Gait generating device for legged mobile robot and operational target generating device for robot | |
| JP4155993B2 (en) | Control device for legged mobile robot | |
| JP4246638B2 (en) | Control device for legged mobile robot | |
| JP4199236B2 (en) | Gait generator for legged mobile robot | |
| CN101068663B (en) | Legged mobile robot and control method | |
| KR101140810B1 (en) | Gait generator for mobile robot | |
| KR101160161B1 (en) | Gait producing device for moving robot | |
| CN114578836B (en) | Jumping control method for double-wheel-foot robot | |
| JP5404543B2 (en) | Control device for legged mobile robot | |
| JP5232120B2 (en) | Control device for moving body | |
| CN114995476B (en) | Gait generation and attitude control method for walking robot on rugged terrain on sloping field | |
| JP5404544B2 (en) | Control device for legged mobile robot | |
| Ren et al. | Study on Mechanism Design and Posture Control of a Wheel-Legged Vehicle | |
| Xie et al. | Motion Planning and Control of Wheel-legged Vehicle Compound Jumping | |
| CN114200836A (en) | Two-step planning method for track of quadruped wheel-leg robot |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |