CN114200836A - Two-step planning method for track of quadruped wheel-leg robot - Google Patents

Two-step planning method for track of quadruped wheel-leg robot Download PDF

Info

Publication number
CN114200836A
CN114200836A CN202111478058.3A CN202111478058A CN114200836A CN 114200836 A CN114200836 A CN 114200836A CN 202111478058 A CN202111478058 A CN 202111478058A CN 114200836 A CN114200836 A CN 114200836A
Authority
CN
China
Prior art keywords
track
wheel
robot
trajectory
speed
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.)
Pending
Application number
CN202111478058.3A
Other languages
Chinese (zh)
Inventor
钟其水
施开波
白金平
杨金
韩胜
李思捷
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Yangtze River Delta Research Institute of UESTC Huzhou
Original Assignee
Yangtze River Delta Research Institute of UESTC Huzhou
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Yangtze River Delta Research Institute of UESTC Huzhou filed Critical Yangtze River Delta Research Institute of UESTC Huzhou
Priority to CN202111478058.3A priority Critical patent/CN114200836A/en
Publication of CN114200836A publication Critical patent/CN114200836A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • 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
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance

Landscapes

  • Engineering & Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)

Abstract

The invention discloses a track two-step planning method of a quadruped wheel-leg robot, which comprises the following steps of 1: receiving an external reference input reference track point and speed; step 2: constructing an objective function and constraint conditions of the wheel; and step 3: solving the wheel track and calculating the ZMP constraint condition; and 4, step 4: constructing an objective function and constraint conditions of the base; and 5: and solving the base track. The method adopts the ZMP model as the main constraint of stability, reduces the excessive dependence on the force sensor of the robot, measures the stability of the robot body by using the stability margin, changes the traditional control parameters for achieving stability by direct solution into solution under the constraint of the stability margin, allows more postures of the robot body caused by solution results, and increases the probability of final solution success.

Description

Two-step planning method for track of quadruped wheel-leg robot
Technical Field
The invention relates to the technical field of robot trajectory planning, in particular to a trajectory two-step planning method for a quadruped wheel-leg robot.
Background
The quadruped robot has very wide application in the military field, the engineering and construction field, the special field, the household life field and other fields, and the quadruped wheel-leg robot has the advantages that the flexibility of the quadruped robot is improved, and the application field of the quadruped robot is expanded. The quadruped robot includes a quadruped robot and a quadruped wheel-leg robot. In the field of robot research, machine vision, SLAM, motion control, trajectory planning, motor drive, etc. are popular fields. Compared with the common quadruped robot, the quadruped wheel-leg robot has the advantages that the wheel part is added at the foot end, one degree of freedom is added to the robot, the advancing part of the wheel for rolling and sliding is added, and new gait and mixed gait are allowed.
The trajectory planning refers to planning the trajectory of the foot end (or the wheel) of the wheel-leg robot, belongs to local and real-time trajectory planning, and mainly solves the problems that the planned reasonable trajectory forms a classic gait, and the actions of crossing obstacles, finishing climbing steps and the like are achieved. In the path planning of the wheel-leg robot, the base of the robot needs to be planned and controlled, and the wheel-leg robot is guaranteed to be always balanced. The correct modeling of the physical and kinematic properties results in a model.
In the field of trajectory planning, generally, reference quantities (such as direction, gait, reference speed and the like) are required to be used as input quantities, online real-time calculation is carried out through a processor of a robot, control quantities of joints are obtained, and then the control quantities are sent to an actuator for execution. The technical scheme researches a track planning block, namely a problem of how to generate the track of the wheel mainly in normal gait and in the presence of obstacles.
In the prior art, the research on the quadruped wheel-leg robot is relatively less. The prior art scheme and the technical scheme are both based on the research of a common quadruped robot for the research of the trajectory planning of the quadruped wheel-leg robot. The specific method is that the wheel is fixed and reduced by one degree of freedom, common gaits (trot, walk and the like) are met, and a drive (wheel rolling) gaits can be added on the basis of the common gaits.
The path planning of the common quadruped robot with non-wheel legs mainly adopts a Model Predictive Control (MPC) method, describes models of a base and legs of the quadruped robot by using a dynamic Model, obtains an equation from a foot end to the base, and then solves the equation. The dynamic modeling mode mainly comprises a Newton-Euler equation and a Lagrange energy method. To achieve more complex and efficient movements, non-linear dynamics and control are used. For the control algorithm of the four feet, the most optimal control (optimal control) is used, namely, the control parameters of the wheel are solved once by jointly solving a plurality of equations and inequalities of equations of the base and the leg.
Model predictive control is further performed on the whole body modeling of the four-legged robot, which is also called Whole Body Control (WBC), although the solution accuracy is higher, the solution is often too huge, the calculation cannot meet the requirement of real-time performance, and even cannot be solved under certain conditions.
Disclosure of Invention
In order to solve the problems in the background art, the invention provides a two-step planning method for the track of a quadruped wheel-leg robot.
In order to achieve the purpose, the invention is realized by the following technical scheme:
a two-step planning method for the track of a quadruped wheel-leg robot comprises the following steps:
step 1: receiving an external reference input reference track point and speed;
step 2: constructing an objective function and constraint conditions of the wheel;
and step 3: solving the wheel track and calculating the ZMP constraint condition;
and 4, step 4: constructing an objective function and constraint conditions of the base;
and 5: and solving the base track.
Further, the step 1 of accepting external reference input reference track points and speed includes:
the track of the whole wheel is modeled into an air part and a ground part, the track of the air part refers to a track of the wheel which is in the air and does not contact the ground due to gait and jumping, the track of the air part meets the requirements of obstacle crossing and the stability requirements, a complex curve needs to be realized, a quintic spline is adopted as a section of track of the air part in the algorithm, modeling is respectively carried out on three directions of x, y and z of the track, 6 polynomial coefficients are arranged in each direction,
Figure BDA0003394310840000021
Figure BDA0003394310840000022
where r (t) is the trajectory equation of the wheel in the air, t is an independent variable with respect to time, αi,*The method is characterized in that a coefficient matrix consisting of 6 coefficients is adopted, and the whole trajectory equation comprises 18 polynomial coefficients;
different splines are adopted for the track modeling of the ground part and the track of the air part, in order to meet the constraint of speeds in different directions, the track of the ground part is required to be as flat and straight as possible, the tracks in the upper and lower z directions and the left and right y directions are strictly constrained, the speed is set to be 0, and for the advancing x direction, a quadratic spline is adopted for modeling, as shown in the following formula:
Figure BDA0003394310840000023
Figure BDA0003394310840000024
for the equation of the speed of the wheel on the ground, t is an independent variable with respect to time, αi,0、αi,1And alphai,2The velocity is in the x direction, so the velocity is parameterized directly, and then the trajectory equation is obtained by integrating over time, as shown in the following equation:
Figure BDA0003394310840000031
wherein the rotation matrix R (t ω)ref) Is a rotation matrix describing the reference frame of the wheel,
Figure BDA0003394310840000032
carry in R (t omega)ref) The trajectory equation of the ground part can be obtained from the equation:
Figure BDA0003394310840000033
and integrating the time period delta ti to obtain an expression of the ground part track:
Figure BDA0003394310840000034
in practical cases, the present invention sets the yaw angle to 0 in order to expect the wheel-legged robot to have the advancing locus in a straight line and to coincide with the direction of the body, and can simplify the above equation to the following equation:
Figure BDA0003394310840000035
further, the step 2 of constructing an objective function of the wheel comprises:
(1) minimizing acceleration
One purpose of the trajectory planning of the wheel-leg robot and the common quadruped robot is to enable the speed change to be small, protect a driving motor of a wheel-leg part, and minimize the acceleration is a common optimization item in the trajectory planning;
Figure BDA0003394310840000041
(2) minimizing the difference between the current solution and the previous solution
In the step, due to the consideration of the continuity of the track, in order to avoid the discontinuity between the splines of the track, which causes the instantaneous high current of the driving motor, the term is added into the design of the objective function;
Figure BDA0003394310840000042
(3) minimizing the difference between the current speed and the reference speed
In the planning of the track of the wheel, the handle input is used as a reference speed, the reference speed can indicate the advancing speed of the current track, and the current track speed and the reference speed are subjected to difference to carry out minimization constraint, wherein the track of the wheel is the track on the ground;
Figure BDA0003394310840000043
(4) minimizing the difference between the current coordinates and the desired coordinates
The desired coordinates are determined primarily by the trajectory of the gait and the actual obstacle situation;
Figure BDA0003394310840000044
(5) foothold planning
In this term the track of the wheels is the track in the air;
Figure BDA0003394310840000045
(6) minimizing the difference between the swing height and the desired height
In order to ensure that the wheel crosses the obstacle, the height of the obstacle plus a certain redundancy is selected as an expected height, the amount of the track in the z direction and the expected height are minimized in an objective function, and the track of the wheel in the item is the track in the air;
Figure BDA0003394310840000046
the quadratic programming problem expression of the wheel can be obtained by the algorithm design idea as follows:
Figure BDA0003394310840000051
further, the constraint conditions in step 2 are divided into equality constraints and inequality constraints.
Further, the equality constraint and the inequality constraint are expressed as follows:
s.t.
Figure BDA0003394310840000052
Figure BDA0003394310840000053
Figure BDA0003394310840000054
further, the ZMP constraints in step 3 include:
let mass of robot member i be miCenter of gravity of ri＝(xi,yi,zi) Acceleration of center of mass of
Figure BDA0003394310840000055
The resultant force F of the gravity and the inertia force applied to the robot is:
Figure BDA0003394310840000056
the moment of the resultant force on the fixed coordinate system is:
Figure BDA0003394310840000057
moving the resultant force F to the ZMP point, where F has a moment of 0 to the x-axis and y-axis, then:
Mx-Fzyzmp＝0
My+Fzxzmp＝0
the coordinate positions of the ZMP points are given by:
Figure BDA0003394310840000061
Figure BDA0003394310840000062
assuming that the height of the center of gravity remains constant during the movement of the robot with wheel legs, the coordinate position is set to (x)z,yz,zz) The ZMP point location is given by:
Figure BDA0003394310840000063
when the robot adopts static gait to do uniform motion, the acceleration in the directions of the x axis and the y axis is 0, and at the moment, the position coordinates of the ZMP point are as follows:
Figure BDA0003394310840000064
compared with the prior art, the invention has the following advantages:
1. the problem scale becomes small and the solving speed is high. The conventional Whole Body Control (WBC) carries out dynamic modeling on the whole robot without hierarchical modeling and distribution solving, the track calculation frequency of each time in the conventional method is about 20-40Hz, and the track two-step planning method can achieve the track calculation frequency of 100 plus 200 Hz.
2. The ZMP model is used as the main constraint of stability, the excessive dependence on a force sensor of the robot is reduced, the stability of the robot body is measured by using the stability margin, the control parameters for achieving the stability by the traditional direct solution are changed into the solution under the constraint of the stability margin, more attitude of the robot body caused by the solution result is allowed, and the probability of the final solution success is increased.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, and it is obvious that the drawings in the following description are only embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to these drawings without creative efforts.
FIG. 1 is a wheel-legged robot coordinate system definition diagram;
FIG. 2 is a diagram of a single-leg model of a wheel-legged robot for kinematic and kinetic modeling;
FIG. 3 is an input reference trajectory map as an input reference for the algorithm;
FIG. 4 is a graph of the obstacle crossing trajectory calculated by the algorithm;
FIG. 5 is a graph of velocity over an obstacle calculated by the algorithm;
fig. 6 is a code flow chart of the present controller, a code implementation and control flow chart of the whole wheel-leg robot.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention will be described in further detail with reference to the accompanying drawings in conjunction with the following detailed description.
A two-step planning method for the track of a quadruped wheel-leg robot comprises the following steps:
1. basic model of kinematics
The four-foot wheel-leg robot has 22 degrees of freedom, each leg has 4 degrees of freedom, and the four-foot wheel-leg robot is respectively positioned at the abduction joint, the hip joint, the knee joint and the rotation degree of freedom of the wheel and also comprises 6 space degrees of freedom of the robot. To facilitate subsequent theoretical analysis, a world coordinate system, a base coordinate system and a hip coordinate system are defined for the robot as shown in fig. 1, and are simply called a wheel-legged robot.
The four legs of the wheel-leg robot researched by the invention have the same structural size, each leg has 3 degrees of freedom in a wheel-fixed and leg-motion mode, and one leg is taken as a research object when a single-leg kinematics and dynamics equation is derived, wherein the right front leg is taken as the research object. In order to simplify the analysis process and the calculation result, the leg structure model is simplified, the size of the wheels is ignored, three parts in the leg structure are regarded as 3 connecting rod mechanisms with uniform mass distribution, the single-leg structure is simplified into the leg connecting rod model shown in fig. 2, then a D-H coordinate system is established on the model according to a D-H analysis method, and the kinematic analysis is carried out by the D-H method.
And positive kinematics, the position of the robot foot position in a reference coordinate system can be obtained through joint and joint angle calculation, and the position is as follows:
Figure BDA0003394310840000071
inverse kinematics, solving for joint drive angle θ mainly from the position of foot end P1,θ2,θ3Let the coordinate of the terminal P in the body reference frame {0} be (P)x,py,pz) The following can be obtained:
Figure BDA0003394310840000072
wherein: + -means that there are two different sets of solutions to the inverse kinematics equation, each representing the direction in which the leg is bent.
2. Principle of algorithm
The planning method of the invention adopts the general idea that a spline function is adopted to fit a track curve to obtain a specific motion equation of the wheel and the base, an equation of speed and acceleration is obtained after derivation, and a quadratic planning problem is respectively constructed on the tracks of the wheel and the base part by designing and constructing a target function and constraint on the dimensions of the track, the speed, the acceleration and the like, so as to solve the problem.
2.1 construction of quadratic programming problem at the wheel end
The planning method of the invention describes the track of the wheel as the smooth connection of a plurality of sections of spline curves. The track of the whole wheel is modeled into an aerial part and a ground part, the aerial part track refers to the track of the wheel which is not contacted with the ground in the air due to gait, jumping and the like, and the aerial part track needs to realize a complex curve in order to meet the requirements of obstacle crossing, stability and the like. The algorithm adopts a quintic spline as a section of track of the aerial part, models the three directions of x, y and z of the track respectively, and each direction has 6 polynomial coefficients.
Figure BDA0003394310840000081
Where r (t) is the trajectory equation of the wheel in the air, t is an independent variable with respect to time, αi,*The whole trajectory equation comprises 18 polynomial coefficients for a coefficient matrix consisting of 6 coefficients.
The trajectory modeling of the ground portion and the trajectory of the aerial portion use different splines and in order to satisfy the constraints of different directional velocities. The invention requires the track of the ground part to be as flat and straight as possible, strictly restricts the tracks of the upper and lower z directions and the left and right y directions, sets the speed to be 0, and adopts a quadratic spline to model the advancing x direction. As shown in formula (4):
Figure BDA0003394310840000082
Figure BDA0003394310840000083
for the equation of the speed of the wheel on the ground, t is an independent variable with respect to time, αi,0、αi,1And alphai,2Is velocity in the x direction. So that the speed is parameterized directly and then by timeThe integral is then used to obtain the trajectory equation, as shown in equation (5):
Figure BDA0003394310840000084
wherein the rotation matrix R (t ω)ref) Is a rotation matrix describing the reference frame of the wheel,
Figure BDA0003394310840000091
carry in R (t omega)ref) The trajectory equation of the ground part can be obtained from the equation:
Figure BDA0003394310840000092
and integrating the time period delta ti to obtain an expression of the ground part track:
Figure BDA0003394310840000093
in practical cases, the invention sets the yaw angle to 0, so as to expect the advancing track of the wheel-legged robot to be a straight line and to coincide with the direction of the body, and can simplify the formula (8) to the formula (9):
Figure BDA0003394310840000094
the planning method is based on the derivation of kinematics and dynamics, the aerial and ground track equations of the wheel are constructed, the wheel track model is completed, and discretization is carried out according to time sampling in actual solution to obtain the prediction model. And designing a target function and constraint conditions such as constraint conditions and dynamic constraint conditions according to the deviation of the state trajectory and the real state variable based on the discretized prediction model, and finally giving the MPC optimization problem of the wheel part.
Design of objective function
1. Minimizing acceleration
One purpose of trajectory planning for wheel-leg robots and common quadruped robots is to make the speed change small, protect the driving motors of the wheel-leg parts, and minimize the acceleration, which is a common optimization item in trajectory planning.
Figure BDA0003394310840000095
2. Minimizing the difference between the current solution and the previous solution
This step is added to the design of the objective function in view of the continuity of the trajectory, in order to avoid discontinuities between the splines of the trajectory, resulting in an instantaneous high current of the drive motor.
Figure BDA0003394310840000101
3. Minimizing the difference between the current speed and the reference speed
In wheel trajectory planning, the present invention uses the handle input as a reference speed that may indicate the forward speed of the current trajectory, and the current trajectory speed is subtracted from the reference speed to minimize the constraint. The track of the wheels in this case is the track on the ground.
Figure BDA0003394310840000102
4. Minimizing the difference between the current coordinates and the desired coordinates
The desired coordinates are determined primarily by the trajectory of the gait and the actual obstacle situation.
Figure BDA0003394310840000103
5. Foothold planning
The track of the wheels in this case is the track in the air.
Figure BDA0003394310840000104
6. Minimizing the difference between the swing height and the desired height
To ensure that an obstacle is crossed, the height of the obstacle plus a certain redundancy is selected as the expected height, and the amount of the track in the z direction and the expected height are minimized in the objective function. The track of the wheels in this case is the track in the air.
Figure BDA0003394310840000105
The quadratic programming problem expression of the wheel can be obtained by the algorithm design idea as follows:
Figure BDA0003394310840000106
the constraint conditions are divided into equality constraints and inequality constraints.
The equality constraint is constrained primarily from both the initial state and spline continuity. In the initial state, the space coordinate, the speed and the acceleration value of the wheel can be obtained through the sensor, and the three values are used as initial values to perform equation constraint on the track equation of the wheel. And (4) spline continuity constraint, namely constraining the joint of two adjacent splines, and ensuring that the tracks, the speeds and the accelerations of the two adjacent splines are equal.
The inequality constraints are constrained primarily from a kinetic perspective. The position of the robot foot end capable of moving in the space is the foot end working space of the robot, the foot end working space of the robot has great influence on the motion control and the posture of the robot, and the motion position of the foot end cannot exceed the range of the working space of the robot. In order to avoid the phenomenon that the track of the wheels exceeds the working space at the foot end of the robot, the projection of the track of the wheels on the horizontal plane is restricted to be in the internal rectangle of the working space at the foot end.
The equality constraint and inequality constraint are shown in equation (17).
Figure BDA0003394310840000111
From this point, MPC optimization problem modeling of the wheel section is complete. And solving by means of a Yalmip toolbox and a CPLEX solver, and obtaining a wheel track planning effect. The algorithm inputs a reference trajectory curve, which is a reference trajectory that glides, steps up, and glides again, as shown in FIG. 3. The reference trajectory is input as part of the input of the algorithm, and the algorithm is solved to obtain a calculated trajectory curve, the trajectory curve and the velocity curve being shown in fig. 4 and 5.
2.2ZMP constraints
The zero moment point is often used to analyze the dynamic stability of the robot, and the walking stability control of the quadruped robot is generally based on the zero moment point principle. When the robot is stable, the zero moment point projection is positioned in a polygonal area defined by the supporting legs. The quadruped robot is mainly subjected to three acting forces of gravity, inertia force and ground supporting force in the moving process, when the resultant force of the three acting forces reaches balance, the robot is in a stable state, and the resultant force point is a zero moment point ZMP. In general, the terrain environment in which the robot moves is unknown, the resultant force point is not easily found, and the zero moment point is determined by the gravity and the inertia force.
Let mass of robot member i be miCenter of gravity of ri＝(xi,yi,zi) Acceleration of center of mass of
Figure BDA0003394310840000112
The resultant force F of the gravity and the inertia force applied to the robot is:
Figure BDA0003394310840000121
the moment of the resultant force on the fixed coordinate system is:
Figure BDA0003394310840000122
moving the resultant force F to the ZMP point, where F has a moment of 0 to the x-axis and y-axis, then:
Figure BDA0003394310840000123
as can be seen from equations (18) to (20), the coordinate positions of the ZMP points are:
Figure BDA0003394310840000124
assuming that the height of the center of gravity remains constant during the movement of the robot with wheel legs, the coordinate position is set to (x)z,yz,zz) The available ZMP points are, according to equation (21):
Figure BDA0003394310840000125
when the robot adopts static gait to do uniform motion, the acceleration in the directions of the x axis and the y axis is 0, and at the moment, the position coordinates of the ZMP point are as follows:
Figure BDA0003394310840000126
from the equation (23), when the robot adopts a static gait, the ZMP coincides with the center of gravity projection point, and since the origin of the body coordinate system of the four-legged wheel-legged robot studied by the present invention is set at a position right below the center of mass of the base, the origin projection points of the ZMP and the body coordinate system coincide.
3. Two-step planning method design for track of four-foot wheel-leg robot
The invention provides a two-step planning method for a four-foot wheel-leg robot track, which comprises the following steps:
step 1: receiving an external reference input reference track point and speed;
step 2: constructing an objective function and constraint conditions of the wheel;
and step 3: solving the wheel track and calculating the ZMP constraint condition;
and 4, step 4: constructing an objective function and constraint conditions of the base;
and 5: and solving the base track.
The order of solving in the planning method of the invention is to solve in the time window t firstfThe track of the wheels of the robot with the inner four-foot wheel legs is obtained, the position coordinates of the four wheels are obtained, a supporting polygon formed by the supporting legs is calculated, a new zero moment point is calculated, ZMP inequality constraint is constructed on the condition that the new zero moment point does not exceed the supporting polygon, and then the optimization problem of the base is constructed.
3.1 input reference quantity
The planning method model of the invention needs the input of the reference quantity. Inputting general reference track points (foot end departure point, highest point and floor point) of the foot end for common trot gait, walk gait and the like; for the case of an obstacle (e.g., a step), the reference trajectory point at the foot end is input. For the obstacle (step), the reference track point is designed and input according to the height and other information of the step, and the obtained reference track is as shown in fig. 3.
3.2 quadratic programming problem for building wheels
Firstly, a spline function of a track to be planned is constructed, the air part is constructed into a quintic spline function, and the ground part is constructed into a quadratic spline function. And constructing a quadratic programming problem, constructing an objective function, constructing an equality constraint condition and constructing an inequality constraint condition. One leg is modeled and the model for the remaining legs is the same.
3.3 solving the quadratic programming problem of the base
And calculating the partial derivative of the quadratic programming problem by using a step-by-step quadratic programming method (SQP), solving the solution of the subproblem and iterating. And solving the convergence to obtain the track of the wheel part. And solving the quadratic programming problem at the wheel end, as shown in figures 4 and 5. The solved trajectory profile matches the input reference trajectory and has less speed in liftoff and touchdown.
3.4 obtaining ZMP constraints
The trajectory of the wheel at the next moment determines the support polygon formed by the wheels contacting the ground. A Zero Moment Point (ZMP) is calculated,
in the whole control flow of the four-leg wheel-leg robot, the planning method solves the problem of trajectory planning.
3.5 quadratic programming problem of building bases
The quadratic programming problem of the base mainly solves the displacement track of the centroid of the base, the objective function and the equality constraint condition are the same as the quadratic programming problem of the wheel, and the ZMP constraint condition is added in the inequality constraint.
3.6 solving the footprint of the susceptor
After the track of the base is solved, the motor angle of the leg of the robot with the wheel legs is solved by inverse kinematics in combination with the track of the wheel, and the control parameters are sent to the actuator.
The code flow of the controller is shown in fig. 6, which is a code implementation and control flow chart of the whole wheel-leg robot. The controller is realized by C + + programming and is divided into an initialization part, a track planning algorithm part and an executor execution part. In order to acquire external input information and control the joint motors of the wheel-legged robot, the initialization part needs to perform initialization setting on the motors and force sensors of the hip joint (hip), the knee joint (knee) and the wheel end (wheel), and perform data zeroing and correction on the sensors such as an azimuth angle sensor, an IMU, an accelerometer and a contact sensor of the body. The controller is provided with physical quantities such as the weight of the robot with wheel legs, the length of a connecting rod of the leg part, the initial angle of a joint, kinematic parameters such as the position, the speed and the acceleration, and control parameters such as PID. After the initialization of the controller is completed, a flag is set and the next stage code is executed. And a track planning algorithm part of the controller reads data of the IMU, the accelerometer and the contact sensor before planning each new track, updates a state model of the robot, constructs and solves a model according to the track planning algorithm, and obtains control quantities such as joint target angles. And transmitting the control quantity to an actuator for execution.
While the present invention has been described in detail and with reference to the accompanying drawings, it is not to be construed as limiting the scope of the invention. Various modifications and changes may be made by those skilled in the art without inventive step within the scope of the present invention as described in the claims.

Claims (6)

1. A two-step planning method for a track of a quadruped wheel-leg robot is characterized by comprising the following steps:
step 1: receiving an external reference input reference track point and speed;
step 2: constructing an objective function and constraint conditions of the wheel;
and step 3: solving the wheel track and calculating the ZMP constraint condition;
and 4, step 4: constructing an objective function and constraint conditions of the base;
and 5: and solving the base track.
2. The two-step planning method for the trajectory of a quadruped wheel-legged robot according to claim 1, characterized in that: the method comprises the following steps of receiving an external reference input reference track point and speed in the step 1, wherein the method comprises the following steps:
the track of the whole wheel is modeled into an air part and a ground part, the track of the air part refers to a track of the wheel which is in the air and does not contact the ground due to gait and jumping, the track of the air part meets the requirements of obstacle crossing and the stability requirements, a complex curve needs to be realized, a quintic spline is adopted as a section of track of the air part in the algorithm, modeling is respectively carried out on three directions of x, y and z of the track, 6 polynomial coefficients are arranged in each direction,
Figure FDA0003394310830000011
Figure FDA0003394310830000012
where r (t) is the trajectory equation of the wheel in the air, t is an independent variable with respect to time, αi,*The method is characterized in that a coefficient matrix consisting of 6 coefficients is adopted, and the whole trajectory equation comprises 18 polynomial coefficients;
different splines are adopted for the track modeling of the ground part and the track of the air part, in order to meet the constraint of speeds in different directions, the track of the ground part is required to be as flat and straight as possible, the tracks in the upper and lower z directions and the left and right y directions are strictly constrained, the speed is set to be 0, and for the advancing x direction, a quadratic spline is adopted for modeling, as shown in the following formula:
Figure FDA0003394310830000013
Figure FDA0003394310830000014
for the equation of the speed of the wheel on the ground, t is an independent variable with respect to time, αi,0、αi,1And alphai,2The velocity is in the x direction, so the velocity is parameterized directly, and then the trajectory equation is obtained by integrating over time, as shown in the following equation:
Figure FDA0003394310830000015
wherein the rotation matrix R (t ω)ref) Is a rotation matrix describing the reference frame of the wheel,
Figure FDA0003394310830000021
carry in R (t omega)ref) The trajectory equation of the ground part can be obtained from the equation:
Figure FDA0003394310830000022
for Δ tiAnd (3) integrating in a time period to obtain an expression of the ground part track:
Figure FDA0003394310830000023
in practical cases, the present invention sets the yaw angle to 0 in order to expect the wheel-legged robot to have the advancing locus in a straight line and to coincide with the direction of the body, and can simplify the above equation to the following equation:
Figure FDA0003394310830000024
3. the two-step planning method for the trajectory of a quadruped wheel-legged robot according to claim 1, characterized in that: constructing an objective function of the wheel in the step 2, wherein the method comprises the following steps:
(1) minimizing acceleration
One purpose of the trajectory planning of the wheel-leg robot and the common quadruped robot is to enable the speed change to be small, protect a driving motor of a wheel-leg part, and minimize the acceleration is a common optimization item in the trajectory planning;
Figure FDA0003394310830000025
(2) minimizing the difference between the current solution and the previous solution
In the step, due to the consideration of the continuity of the track, in order to avoid the discontinuity between the splines of the track, which causes the instantaneous high current of the driving motor, the term is added into the design of the objective function;
Figure FDA0003394310830000031
(3) minimizing the difference between the current speed and the reference speed
In the planning of the track of the wheel, the handle input is used as a reference speed, the reference speed can indicate the advancing speed of the current track, and the current track speed and the reference speed are subjected to difference to carry out minimization constraint, wherein the track of the wheel is the track on the ground;
Figure FDA0003394310830000032
(4) minimizing the difference between the current coordinates and the desired coordinates
The desired coordinates are determined primarily by the trajectory of the gait and the actual obstacle situation;
Figure FDA0003394310830000033
(5) foothold planning
In this term the track of the wheels is the track in the air;
Figure FDA0003394310830000034
(6) minimizing the difference between the swing height and the desired height
In order to ensure that the wheel crosses the obstacle, the height of the obstacle plus a certain redundancy is selected as an expected height, the amount of the track in the z direction and the expected height are minimized in an objective function, and the track of the wheel in the item is the track in the air;
Figure FDA0003394310830000035
the quadratic programming problem expression of the wheel can be obtained by the algorithm design idea as follows:
Figure FDA0003394310830000036
if leg in contact:
Figure FDA0003394310830000037
if leg in air:
Figure FDA0003394310830000038
4. the two-step planning method for the trajectory of a quadruped wheel-legged robot according to claim 1, characterized in that: the constraint conditions in step 2 are divided into equality constraints and inequality constraints.
5. The two-step planning method for the trajectory of a quadruped wheel-legged robot as claimed in claim 4, wherein: the equality constraint and inequality constraint are expressed as follows:
s.t.
r(0)＝rinit,
Figure FDA0003394310830000041
Figure FDA0003394310830000042
Figure FDA0003394310830000043
6. the two-step planning method for the trajectory of a quadruped wheel-legged robot according to claim 1, characterized in that: the ZMP constraints in step 3 include:
let mass of robot member i be miCenter of gravity of ri＝(xi,yi,zi) Acceleration of center of mass of
Figure FDA0003394310830000044
The resultant force F of the gravity and the inertia force applied to the robot is:
Figure FDA0003394310830000045
the moment of the resultant force on the fixed coordinate system is:
Figure FDA0003394310830000046
moving the resultant force F to the ZMP point, where F has a moment of 0 to the x-axis and y-axis, then:
Mx-Fzyzmp＝0
My+Fzxzmp＝0
the coordinate positions of the ZMP points are given by:
Figure FDA0003394310830000051
Figure FDA0003394310830000052
assuming that the height of the center of gravity remains constant during the movement of the robot with wheel legs, the coordinate position is set to (x)z,yz,zz) The ZMP point location is given by:
Figure FDA0003394310830000053
when the robot adopts static gait to do uniform motion, the acceleration in the directions of the x axis and the y axis is 0, and at the moment, the position coordinates of the ZMP point are as follows:
Figure FDA0003394310830000054
CN202111478058.3A 2021-12-06 2021-12-06 Two-step planning method for track of quadruped wheel-leg robot Pending CN114200836A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111478058.3A CN114200836A (en) 2021-12-06 2021-12-06 Two-step planning method for track of quadruped wheel-leg robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111478058.3A CN114200836A (en) 2021-12-06 2021-12-06 Two-step planning method for track of quadruped wheel-leg robot

Publications (1)

Publication Number Publication Date
CN114200836A true CN114200836A (en) 2022-03-18

Family

ID=80650697

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111478058.3A Pending CN114200836A (en) 2021-12-06 2021-12-06 Two-step planning method for track of quadruped wheel-leg robot

Country Status (1)

Country Link
CN (1) CN114200836A (en)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110231050A1 (en) * 2010-03-22 2011-09-22 Goulding John R In-Line Legged Robot Vehicle and Method for Operating
CN111070201A (en) * 2019-08-30 2020-04-28 南京理工大学 Reactive robust control method of quadruped robot based on ZMP theory under load mutation
CN111752150A (en) * 2020-06-12 2020-10-09 北京理工大学 Four-wheel cooperative control method for wheeled-foot robot
CN112757299A (en) * 2020-12-30 2021-05-07 乐聚(深圳)机器人技术有限公司 Method and device for acquiring centroid trajectory, robot and storage medium

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110231050A1 (en) * 2010-03-22 2011-09-22 Goulding John R In-Line Legged Robot Vehicle and Method for Operating
CN111070201A (en) * 2019-08-30 2020-04-28 南京理工大学 Reactive robust control method of quadruped robot based on ZMP theory under load mutation
CN111752150A (en) * 2020-06-12 2020-10-09 北京理工大学 Four-wheel cooperative control method for wheeled-foot robot
CN112757299A (en) * 2020-12-30 2021-05-07 乐聚(深圳)机器人技术有限公司 Method and device for acquiring centroid trajectory, robot and storage medium

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
MARKO BJELONIC, ET AL.: "Rolling in the Deep-Hybrid Locomotion for Wheeled-Legged Robots Using Trajectory Optimization" *
廖绍辉: "基于零力矩点的四足轮腿机器人运动轨迹规划研究" *
李清 等: "四足轮腿式移动机器人的步态分析及轨迹规划" *

Similar Documents

Publication Publication Date Title
JP5284923B2 (en) Control device for legged mobile robot
JP5483997B2 (en) Control device for legged mobile robot
JP5219956B2 (en) Control device for moving body
EP2017042B1 (en) Motion controller and motion control method for legged walking robot, and robot apparatus
CN110405762B (en) Biped robot attitude control method based on spatial second-order inverted pendulum model
CN113485398B (en) Gesture control method for wheeled biped robot
CN111290272B (en) Attitude stationarity adjusting method based on multi-legged robot
JP2001212775A (en) Motion controller for leg-type mobile robot, motion control method, and leg-type mobile robot
CN112643679B (en) Robot motion control method, device, controller and storage medium
JP2004167676A (en) Robot device, motion control device and motion control method of robot device
WO2004030870A1 (en) Robot device and control method of robot device
CN110244714B (en) Sliding mode control-based robot single-leg swinging phase double-closed-loop control method
Yang et al. Dynamic legged manipulation of a ball through multi-contact optimization
JP4483254B2 (en) Robot apparatus and control method of robot apparatus
Zhong et al. Trajectory planning of an intermittent jumping quadruped robot with variable redundant and underactuated joints
JP2001138272A (en) Leg type mobile robot and control method for its motion
Ficht et al. Fast whole-body motion control of humanoid robots with inertia constraints
CN114200836A (en) Two-step planning method for track of quadruped wheel-leg robot
JP3674779B2 (en) Motion control device and motion control method for legged mobile robot, and robot device
Yu et al. Biped robot gait planning based on 3D linear inverted pendulum model
JP2003117858A (en) Method and device for control of robot walk
CN113843799B (en) Quadruped robot posture reset control method, device and storage medium
Lim et al. Emotion-based biped walking
JP5232120B2 (en) Control device for moving body
JP5404543B2 (en) Control device for legged mobile 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