CN117021097A - Method, device and system for motion planning of quadruped robot and storage medium - Google Patents
Method, device and system for motion planning of quadruped robot and storage medium Download PDFInfo
- Publication number
- CN117021097A CN117021097A CN202311056139.3A CN202311056139A CN117021097A CN 117021097 A CN117021097 A CN 117021097A CN 202311056139 A CN202311056139 A CN 202311056139A CN 117021097 A CN117021097 A CN 117021097A
- Authority
- CN
- China
- Prior art keywords
- motion
- robot
- trunk
- state
- initial
- 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
Links
- 230000033001 locomotion Effects 0.000 title claims abstract description 251
- 238000000034 method Methods 0.000 title claims abstract description 86
- 238000005457 optimization Methods 0.000 claims abstract description 19
- 239000011159 matrix material Substances 0.000 claims abstract description 11
- 230000006870 function Effects 0.000 claims description 11
- 230000001133 acceleration Effects 0.000 claims description 9
- 238000004590 computer program Methods 0.000 claims description 9
- 238000010276 construction Methods 0.000 claims description 3
- 230000005021 gait Effects 0.000 abstract description 14
- 210000005010 torso Anatomy 0.000 description 20
- 210000002414 leg Anatomy 0.000 description 14
- 230000009191 jumping Effects 0.000 description 4
- 238000004891 communication Methods 0.000 description 3
- 210000001503 joint Anatomy 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 244000309466 calf Species 0.000 description 1
- 238000013500 data storage Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000000452 restraining effect Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000002459 sustained effect Effects 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 210000001364 upper extremity Anatomy 0.000 description 1
- 210000000689 upper leg Anatomy 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
The application provides a method, a device, a system and a storage medium for motion planning of a quadruped robot, which are used for respectively obtaining initial Newton Euler equations corresponding to the front half trunk and the rear half trunk of the quadruped robot with a spine structure according to parameters of the quadruped robot with the spine structure, combining the acting forces of a stress point, a foot end and a spine in the motion process of the robot, and further constructing a motion state equation of the robot according to the initial Newton Euler equations and an inertia matrix; the motion state equation is used for pushing each part of trunk state of the last discrete time by adopting each part of trunk state of the previous discrete time in the motion process of the robot; and constructing an optimization problem according to a motion state equation and a preset motion trail of the robot, and obtaining an optimized motion speed and optimized motion time in a motion planning process of the quadruped robot. The dynamic model of the embodiment of the specification is more concise, and breaks through the fact that the quadruped robot in the prior art cannot realize higher-speed movement gait planning.
Description
Technical Field
The application relates to the technical field of foot robots, in particular to a method, a device, a system and a storage medium for motion planning of a quadruped robot.
Background
With the development of artificial intelligence, foot robots are widely used in a variety of fields. The motion planning scheme of the existing foot type robot is mostly based on the fact that the trunk is a single rigid body foot type robot to conduct motion planning research.
When the legs are regarded as light legs, the whole robot can be regarded as a single rigid body, and a dynamic model is established, so that the robot is applied to walking gait and jogging gait with lower speed. However, with the increasing motion requirements of robots, due to the limitations of robots, the four-foot robots of the prior art cannot adapt to higher-speed motions and various motion gaits at all.
Therefore, a new solution for motion planning of a quadruped robot is needed.
Disclosure of Invention
In view of this, the embodiments of the present disclosure provide a method, apparatus, system and storage medium for motion planning of a quadruped robot, which are suitable for the motion planning process of the quadruped robot.
The embodiment of the specification provides the following technical scheme:
the embodiment of the specification provides a four-foot robot motion planning method, which comprises the following steps:
respectively obtaining initial Newton Euler equations corresponding to the front half trunk and the rear half trunk of the four-foot robot with the spine structure according to parameters of the four-foot robot with the spine structure;
according to an initial Newton Euler equation corresponding to the front half trunk and the rear half trunk, combining the acting forces of the stress points, the foot ends and the vertebration in the motion process of the robot, and obtaining inertia matrixes of the mass center positions of the front half trunk and the rear half trunk;
constructing a robot motion state equation according to the initial Newton Euler equation and the inertia matrix; the motion state equation is used for pushing each part of trunk state of the last discrete time by adopting each part of trunk state of the previous discrete time in the motion process of the robot;
and constructing an optimization problem according to a motion state equation and a preset motion trail of the robot, and obtaining an optimized motion speed and optimized motion time in a motion planning process of the quadruped robot.
The embodiment of the specification also provides a four-legged robot motion planning device, the four-legged robot motion planning device includes:
the input module is used for respectively obtaining initial Newton Euler equations corresponding to the front half trunk and the rear half trunk of the four-foot robot with the spine structure according to the parameters of the four-foot robot with the spine structure;
the acquisition module is used for acquiring an inertia matrix of the mass center positions of the front half trunk and the rear half trunk according to an initial Newton Euler equation corresponding to the front half trunk and the rear half trunk and combining the acting forces of the stress points, the foot ends and the vertebration in the motion process of the robot;
the construction module is used for constructing a robot motion state equation according to the initial Newton Euler equation and the inertia matrix; the motion state equation is used for pushing each part of trunk state of the last discrete time by adopting each part of trunk state of the previous discrete time in the motion process of the robot;
the optimization module is used for constructing an optimization problem according to a motion state equation and a preset motion track of the robot to obtain an optimized motion speed and optimized motion time in the motion planning process of the quadruped robot.
Compared with the prior art, the beneficial effects that above-mentioned at least one technical scheme that this description embodiment adopted can reach include at least:
according to the embodiment of the specification, the dynamic model is built for the front and rear parts of the trunk corresponding to the active/passive vertebra in the controllable vertebra, so that a motion state equation in a motion process is obtained, and the high-speed motion of the quadruped robot under different motion types is planned.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings that are needed in the embodiments will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present application, and that other drawings can be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a diagram of a four-legged robot force analysis of an active/passive spine in accordance with the present application;
FIG. 2 is a flow chart of a method of motion planning for a four-legged robot in accordance with the present application;
fig. 3 is a schematic diagram of a motion planning process for a quadruped robot of the present application.
Detailed Description
Embodiments of the present application will be described in detail below with reference to the accompanying drawings.
Other advantages and effects of the present application will become apparent to those skilled in the art from the following disclosure, which describes the embodiments of the present application with reference to specific examples. It will be apparent that the described embodiments are only some, but not all, embodiments of the application. The application may be practiced or carried out in other embodiments that depart from the specific details, and the details of the present description may be modified or varied from the spirit and scope of the present application. It should be noted that the following embodiments and features in the embodiments may be combined with each other without conflict. All other embodiments, which can be made by those skilled in the art based on the embodiments of the application without making any inventive effort, are intended to be within the scope of the application.
It is noted that various aspects of the embodiments are described below within the scope of the following claims. It should be apparent that the aspects described herein may be embodied in a wide variety of forms and that any specific structure and/or function described herein is merely illustrative. Based on the present disclosure, one skilled in the art will appreciate that one aspect described herein may be implemented independently of any other aspect, and that two or more of these aspects may be combined in various ways. For example, apparatus may be implemented and/or methods practiced using any number and aspects set forth herein. In addition, such apparatus may be implemented and/or such methods practiced using other structure and/or functionality in addition to one or more of the aspects set forth herein.
It should also be noted that the illustrations provided in the following embodiments merely illustrate the basic concept of the present application by way of illustration, and only the components related to the present application are shown in the drawings and are not drawn according to the number, shape and size of the components in actual implementation, and the form, number and proportion of the components in actual implementation may be arbitrarily changed, and the layout of the components may be more complicated.
In addition, in the following description, specific details are provided in order to provide a thorough understanding of the examples. However, it will be understood by those skilled in the art that the present application may be practiced without these specific details.
The four-foot robot in the prior art is based on a single rigid body, when the legs are regarded as light legs, the whole robot can be regarded as a single rigid body, and a dynamic model is established, so that the four-foot robot is mainly applied to walking gait and diagonal jogging gait with lower speed. With the improvement of performance requirements such as robot speed, the motion of the four-legged robot in the prior art can not be realized at a higher speed.
If the foot robot based on a single rigid body moves at a higher speed, such as a small running state based on a diagonal angle, the foot robot needs to be realized by increasing the stride and increasing the stride frequency. However, increasing the stride, i.e., increasing the size of the leg structure, results in the leg joint motor providing a greater torque at equal desired foot end forces; the step frequency is increased, namely the movement period of a single leg is reduced, and the reciprocating movement frequency is increased, so that the leg joint motor is required to work at a higher rotating speed. Therefore, limited by the motor performance, there is an upper speed bound for small diagonal runs, which is not suitable for higher exercise speeds.
Even though most of the legged robots including the spine are based on structural design, the dynamic model applicable to a single rigid body cannot realize higher-speed movement due to the complex corresponding spatial degrees of freedom.
Based on this, the embodiment of the present specification proposes a motion planning scheme for a quadruped robot: in combination with the analysis of the stress of the four-legged robot in fig. 1, the application establishes a dynamic model based on the front and rear parts of the trunk, establishes a motion state equation of the robot, and continuously optimizes the motion process of the robot by adopting the states of the trunk of each part of the former discrete time to break the states of the trunk of each part of the latter discrete time, so that the four-legged robot containing the vertebra can optimize and give a feasible motion planning scheme of the robot according to the appointed initial state, the time required by the motion process, the motion gait, the end state and the like in a given motion task.
According to the embodiment of the specification, the dynamic model is built for the front and rear parts of the trunk corresponding to the active/passive vertebra in the controllable vertebra, so that a motion state equation in a motion process is obtained, and the high-speed motion planning of the four-foot robot with the vertebra structure is realized. The kinetic model of the embodiment of the present specification is more concise.
The following describes the technical scheme provided by each embodiment of the present application with reference to the accompanying drawings.
As shown in fig. 2, the present disclosure provides a flow chart of a motion planning method for a quadruped robot. As shown in fig. 2, the method includes steps S210 to S240. Step S210 is to obtain initial newton euler equations corresponding to the front half trunk and the rear half trunk of the quadruped robot with the spine structure according to parameters of the quadruped robot with the spine structure. Step S220, according to an initial Newton Euler equation corresponding to the front half trunk and the rear half trunk, combining the acting forces of the stress points, the foot ends and the vertebration in the motion process of the robot, and obtaining inertia matrixes of mass center positions of the front half trunk and the rear half trunk. Step S230, constructing a robot motion state equation according to the initial Newton Euler equation and the inertia matrix; the motion state equation is used for pushing out the trunk states of the parts of the last discrete time by adopting the trunk states of the previous discrete time in the motion process of the robot. And step 240, constructing an optimization problem according to a motion state equation and a preset motion track of the robot, and obtaining an optimized motion speed and optimized motion time in the motion planning process of the quadruped robot. In this embodiment of the present disclosure, step S210 and step S220 may be performed simultaneously or sequentially.
And S210, respectively obtaining initial Newton Euler equations corresponding to the front half trunk and the rear half trunk of the four-foot robot with the spine structure according to parameters of the four-foot robot with the spine structure.
The parameters of the four-foot robot with the spine structure comprise physical parameters and motion parameters. The physical parameters include: the mass, the moment of inertia, the physical size, the ground friction coefficient and the gravitational acceleration constant of the front half trunk and the rear half trunk of the four-foot robot. The motion parameters include: the initial position and speed of the front trunk centroid, the final position and speed, the total motion time and the time discretization count N.
Specifically, according to parameters of the four-foot robot with the spine structure, initial Newton Euler equations corresponding to the front half trunk and the rear half trunk of the four-foot robot with the spine structure are obtained respectively.
Initial newton euler equation:
where i=1, 2 is the number of the front and rear trunk, j=1, 2,3,4 is the number of the four feet of the robot. q ci And theta ci The centroid coordinates and attitude angles of the trunk. Delta j In order to achieve the foot end grounding state, 1 is taken when the foot end touches the ground, otherwise 0 is taken. r is (r) j And r waist The position of the stress point relative to the centroid, namely the front centroid and the rear centroid point to the foot end and the vertebra respectively. f (f) j Is applied by external force on foot end, f waist And T is waist The mutual acting force and the acting moment of the vertebra position are equal in size and opposite in direction for the front trunk and the rear trunk.
Step S220, according to an initial Newton Euler equation corresponding to the front half trunk and the rear half trunk, combining the acting forces of the stress points, the foot ends and the vertebration in the motion process of the robot, and obtaining inertia matrixes of mass center positions of the front half trunk and the rear half trunk.
In connection with the above embodiment, it is deduced from equation (1):
the left expansion of equation (2) is:
equation (2) can thus be written as:
therefore, according to the initial Newton Euler equation corresponding to the front half trunk and the rear half trunk, the acting forces of the stress points, the foot ends and the vertebration in the motion process of the robot are combined, such as r j And r waist The position of the stress point relative to the centroid, namely the front centroid and the rear centroid point to the foot end and the vertebra respectively. f (f) j Is applied by external force on foot end, f waist And T is waist The mutual acting force and the acting moment of the vertebra position are equal in size and opposite in direction for the front trunk and the rear trunk. And obtaining inertia matrixes of the mass center positions of the front half trunk and the rear half trunk, as shown in the equation (4) and the equation (5).
Step S230, constructing a robot motion state equation according to the initial Newton Euler equation and the inertia matrix; the motion state equation is used for pushing out the trunk states of the parts of the last discrete time by adopting the trunk states of the previous discrete time in the motion process of the robot.
In combination with the above embodiment, q ci 、θ ci And the first-order differential is a state quantity, namely, based on the front half trunk and the rear half trunk, i=1, 2 is brought into equation (3) and equation (5) to obtain four equations, and a state equation with the following structure is constructed:
wherein,
e is a third-order unit arrayg is a gravitational acceleration term.
u＝[f 1 ，f 2 ，f 3 ，f 4 ，f waist ，T waist ]，B comprises mass, inertia, r j And r waist C is the Coriolis term.
In addition to the state quantity x and the control quantity u described above, r in B j And r waist The coordinates of the foot end and the vertebra are required to be constructed as parameter r. In the process of planning the movement of the robot, the range of 0 to t is set end K groups [ x, u, r ] in time] T As an argument (i.e., torso state), motion planning is implemented. In a given exercise task, e.g. for a period of time 0-t end Dividing into k groups, and using the motion state equation to infer the state of each part of the trunk at the last discrete time by using the state of each part of the trunk at the last discrete time during the motion process of the robot, such as state quantity x at time k+1 k+1 Equal in size to x based on the state equation k Solving the obtainedThereby optimizing the robot motion process.
And step 240, constructing an optimization problem according to a motion state equation and a preset motion track of the robot, and obtaining an optimized motion speed and optimized motion time in the motion planning process of the quadruped robot.
In combination with the above embodiment, the movement track is preset by the robot, such as the movement track given by the robotThe initial position and speed of the center of mass of the front trunk and the back trunk, the final position and speed, the total movement time and the discretization count N in time are set. And obtaining the optimized motion speed and the optimized motion time in the motion planning process of the four-foot robot by using the motion state equation. For example, the given motion trail sets the initial position of the center of mass of the front trunk and the rear trunk at the point A, the speed is 0, the end position is D, the speeds are 0, and the total motion time is 10s. The front 2s moves to the point B in an acceleration way within 10s, the rear 4s moves from the point B to the point C at a uniform speed, and the rear 4s moves to the point D in a deceleration way. The counting time points of the discretization are respectively 2s,6s and 10s. Thus based on the respective partial torso state [ x, u, r ] according to the motion state equation] T The optimization as an independent variable obtains the optimized motion speed and the optimized motion time in the robot motion planning process. For example, in comparison to a given motion profile, the present embodiment has completed acceleration at 2s discrete time, resulting in an optimized motion speed, e.g., a speed in a uniform profile has been achieved, and the optimized motion time for a deceleration profile is correspondingly reduced. Compared with the low-speed movement of the quadruped robot in the prior art, the application realizes the higher-speed movement of the quadruped robot with the spine, thereby optimizing and obtaining the high-speed operation and the feasible movement track by constructing a simpler movement state equation. Wherein the optimized movement speed and the optimized movement time in the movement planning process of the quadruped robot are obtained.
In some embodiments, setting an objective function according to an initial torso state corresponding to the motion state equation and an objective torso state corresponding to a preset motion trajectory, so that the initial torso state and the objective torso state obtained by the motion state equation are converged; wherein the objective function takes a weighted square representation of the error of the initial torso-state and the target torso-state. The trunk state comprises a spine centroid, an attitude angle, a linear speed, an angular speed, a force born by the foot end in three directions, a spine position and a foot end position.
By combining the above embodiments, the motion state equation of the robot is constructed and completed to obtain the quantity to be optimized, such as r in the state quantity x and the control quantity u and B j And r waist Also, the amount to be optimized is the coordinate structure of the foot end and the vertebraIs constructed as parameter r. Will be 0 to t end K groups [ x, u, r ] in time] T As an argument, the initial torso state in the motion state equation is obtained. According to the preset motion track, based on the state quantity of the quadruped robot at the starting point and the ending point, the expected track of the position and the gesture is given through linear interpolation, such as 0-t end Desired path of arguments [ x ] in time exp ,u exp ,r exp ] T Namely, presetting a target trunk state corresponding to the motion trail.
Some embodiments are such that an initial torso state corresponding to a state equation converges with a target torso state (i.e., the torso state representing the dynamics corresponding to the state equation approaches infinity from the desired torso state), an objective function such as min f (objective function) is set by the initial torso state and the target torso state, by giving 0-t end The optimization is continued over time such that the actual torso state eventually reaches the target torso state (i.e., the desired torso state).
Wherein setting the objective function based on the initial torso-state and the target torso-state includes setting a weighted square sum of errors between the independent variable of the initial torso-state and the desired independent variable (i.e., target torso-state) trajectory as the objective function, but the setting of the specific objective function may be obtained by adding other contents according to the actual situation.
In some embodiments, the four-legged robot motion planning method further includes obtaining constraints that optimize a motion state equation during the motion of the robot.
By combining the embodiment, the constraint condition of the motion state equation in the process of optimizing the robot motion planning is based on the motion state equation and the preset motion trail. Wherein the constraints include equality constraints and inequality constraints. The torso rigid connection constraint condition and the state quantity constraint condition of the tail end moment are set based on the motion state equation.
Specifically, based on the motion state equation, the equation constraints in the optimization process include: 1. initial state quantity constraints, e.g. state quantity x at time 0 and x at time 0 exp Equal, i.e. x=x at time 0 exp . 2. Dynamic model constraints for time k+1State quantity x of (2) k+1 Equal in size to x based on the state equation k Solving the obtained(i.e. state quantity at end time)The state quantities include the centroid, attitude angle, speed, and acceleration of the torso). 3. The foot end is contacted with the position constraint, for the ground contact leg, the foot end height is equal to the ground height, and for the horizontal ground, the foot end height is 0. 4. The foot end sliding constraint, for the ground contact leg, the foot end coordinate remains unchanged. 5. The trunk is rigidly connected and restrained. And the vertebra coordinates obtained by solving based on the front centroid and the rear centroid are kept consistent.
Inequality constraints include: 1. foot end workspace constraints, the foot end remains within its workspace at a given range of joint angles and component sizes of the four-foot robot sections. 2. The foot end force constraint is that for the forward force of the foot end, the force is always non-negative and is smaller than the maximum ground forward force which can be born by the foot robot; for the normal force of the foot end, the magnitude of the normal force does not exceed the maximum static friction given the ground friction coefficient.
In some embodiments, the parameters of the quadruped robot include physical parameters and kinetic parameters.
The physical parameters given in the motion planning process of the four-foot robot in combination with the embodiment comprise: the mass, the moment of inertia, the physical size, the ground friction coefficient and the gravitational acceleration constant of the front half trunk and the rear half trunk of the four-foot robot. Motion parameters, including: the initial position and speed of the front trunk centroid, the final position and speed, the total motion time and the time discretization count N. The embodiment of the present disclosure also needs to provide the desired trajectory based on the preset motion trajectory in the motion planning process, including: desired trajectory of argument over N steps [ x ] exp ,u exp ,r exp ] T And corresponding to the touchdown state on N steps.
In some embodiments, the four-legged robot motion planning method includes: and determining the upper bound and the lower bound of the corresponding independent variables of the initial trunk state.
Specifically, according to different motion types, such as jump gait and high gait motion, in the motion planning process of the quadruped robot, an upper bound and a lower bound can be set for corresponding independent variables of an initial trunk state in the motion planning process of the quadruped robot, so that the optimal motion speed and the optimal motion time in the motion planning process of the quadruped robot can be obtained according to a motion state equation and a preset motion track of the robot.
For example, taking the straight-line motion of the flat ground in jumping gait as an example, 0 to t end K groups [ x, u, r ] in time] T As an argument corresponding to the initial torso state. In the state quantity x set in the motion optimization process, only pitching motion is needed when the jumping gait moves linearly, so that the motion of other two degrees of freedom of the vertebra can be restrained, namely, the upper and lower boundaries of the attitude angles of the centroid on the x axis and the z axis are all 0; for attitude angle on y-axis, θ y,min ，θ y,max The method comprises the steps of carrying out a first treatment on the surface of the For position, q x Based on the initial position and the final position, the limit is properly relaxed as an upper and a lower bound, q y The upper and lower bounds are both 0, q z Then defined by the desired lowest centroid height and highest jump height; similarly, for linear velocity and angular velocity, the upper and lower bounds are given for the required part of velocity, and the rest is taken as 0; in the control quantity u, the maximum force which can be born in three directions of the foot end is taken as an upper boundary and a lower boundary; in the parameter r, the upper and lower boundaries of the foot end position and the spine position are given based on the state quantity x.
In some embodiments, a plurality of discrete time corresponding to the set motion positions of the robot are set in the preset motion trail of the robot; the motion planning method of the quadruped robot further comprises the following steps: a motion planning trajectory is obtained based on the robot motion planning, wherein the motion planning trajectory encompasses a higher motion speed and comprises a set motion position.
In combination with the above embodiment, in the motion planning process of the four-foot robot with a spine structure in the embodiment of the present disclosure, a plurality of discrete time corresponding to the set motion positions of the robot are set in a preset motion track, if the given motion track sets the initial positions of the centroids of the front trunk and the rear trunk at the point a, the speed is 0, the end positions are D, the speeds are also 0, and the total motion time is 10s. The front 2s moves to the point B in an acceleration way within 10s, the rear 4s moves from the point B to the point C at a uniform speed, and the rear 4s moves to the point D in a deceleration way.
In combination with the above embodiment, the motion planning trajectory is obtained based on the robot motion planning, i.e. the robot achieves a feasible motion trajectory planning at a higher speed in the total time. The low-speed motion in the prior art cannot complete the motion task according to a preset motion track. In the embodiment of the specification, a plurality of discrete time corresponding to the set motion positions of the robot are set in a preset motion track, the robot needs to accelerate to move to a point B within 2s, if the speed of the robot within 2s is required to reach 15m/s, the speed cannot be realized for the prior art, and in the embodiment of the specification, the motion speed of the robot is improved through continuous optimization, so that the robot can finish tasks according to the preset motion track. Finally, the robot can realize feasible motion track planning according to a preset motion track at a higher motion speed, and the robot can reach a set motion position in a shorter time.
Compared with the prior art that the low-speed movement of the robot cannot realize the preset movement track at all, the four-foot robot with the spine structure can complete various movement tasks at higher speed by establishing a simpler dynamic model, such as single-step planning, large-distance movement process planning and the like.
The motion planning process of fig. 3, taking the example of straight-line motion of a jump gait, describes the embodiments of the present specification. Mass m of the front and rear trunk 1 、m 2 Moment of inertia I 1 、I 2 The front and rear trunk are assumed to be rectangular with L multiplied by W multiplied by H, and the thighs and the calves are L; taking mu as a ground friction coefficient and g as a gravitational acceleration constant without considering actual collision of the vertebra part;
selecting the vicinity of the origin as a motion starting point, and the height of the mass center as height 0 I.e. q c1,0 ＝[L/2,0,height 0 ] T ，q c2,0 ＝[-L/2,0,height 0 ] T The method comprises the steps of carrying out a first treatment on the surface of the Assuming that the movement path is X, q c1,end ＝[X+L/2,0,height 0 ] T ，q c2,end ＝[X-L/2,0,height 0 ] T The gesture, linear velocity and angular velocity of the mass center at the starting point and the ending point are all taken as 0, the movement time is taken as T, and the movement time is divided into N steps;
based on the state quantity of the starting point and the end point, the expected track of the position and the gesture is given through linear interpolation, and the expected track of the linear speed is taken as constant v exp The expected track of the angular velocity is taken to be 0, namely the expected vertebra part does not move as much as possible; the track of the coordinates of the foot end and the spine in the parameter r is given based on the state quantity track at each moment and the positions of the initial foot end and the spine relative to the mass center; the desired trajectory in the control quantity u is given as 0, i.e. it is desired to minimize the forces; the foot-end ground-contact state is according to the jumping gait, in one period, four phases of front leg ground contact, soaring, rear leg ground contact and soaring are all N, assuming that the four phases occupy the same time p Meanwhile, a four-leg supporting stage with a certain time is given in a starting stage and a stopping stage, and the total step number in the whole process is N;
by the independent variables [ x, u, r] T And the desired argument [ x ] exp ,u exp ,r exp ] T The error over N steps is an objective function, given the following equality constraint: 1. initial state quantity constraints. State quantity x at time 0 and x at time 0 exp Equal; 2. and (5) restraining a dynamic model. For state quantity x at time k+1 k+1 Equal in size to x based on the state equation k Solving the obtained3. Foot end touchdown position constraints. Considering the ground as a horizontal ground, the foot end height is 0; 4. foot end slip constraint. For the ground-contacting leg, the foot end coordinates thereof remain unchanged; 5. the trunk is rigidly connected and restrained. And the vertebra coordinates obtained by solving based on the front centroid and the rear centroid are kept consistent.
The inequality constraint is as follows: 1. foot end workspace constraints. Assuming that three joints of each leg can achieve movement in the range of 0-pi, the foot end only needs to ensure that the distance relative to the shoulder (hip) is no more than 2l; 2. foot end force constraint. For the forward force of the foot end, it is always non-Negative and less than the maximum ground forward force that can be sustained by the foot robot; for normal force of foot end, given the ground friction coefficient, its magnitude is not more than μf i,z 。
In the state quantity x, only pitching motion is needed when the jumping gait moves linearly, so that the movement of the other two degrees of freedom of the vertebra can be restrained, namely, the upper and lower boundaries of the attitude angles of the centroid on the x axis and the z axis are all 0; for attitude angle on y-axis, θ y,nin ，θ y,max The method comprises the steps of carrying out a first treatment on the surface of the For position, q x Based on the initial position and the final position, the limit is properly relaxed as an upper and a lower bound, q y The upper and lower bounds are both 0, q z Then defined by the desired lowest centroid height and highest jump height; similarly, for linear velocity and angular velocity, the upper and lower bounds are given for the required part of velocity, and the rest is taken as 0; in the control quantity u, the maximum force which can be born in three directions of the foot end is taken as an upper boundary and a lower boundary; in the parameter r, the upper and lower boundaries of the foot end position and the spine position are given based on the state quantity x.
And selecting a proper solver for solving the nonlinear optimization problem. One commonly used nonlinear solver is called Casadi. And solving the constructed optimization problem to obtain a feasible optimal motion track which meets the dynamic model and is closest to the expected track.
By combining the embodiment to the motion planning of the controllable active/passive spine quadruped robot, adopting a light leg assumption, simplifying the spine structure-containing quadruped robot into front and rear torsos, and establishing a dynamics model on two centroids. Therefore, the method realizes higher-speed movement tasks and shortens movement time, and also realizes the optimization of various movement tasks, such as single-step planning and large-distance movement process planning.
The embodiment of the present specification provides a four-legged robot motion planning device, four-legged robot motion planning device includes:
the input module is used for respectively obtaining initial Newton Euler equations corresponding to the front half trunk and the rear half trunk of the four-foot robot with the spine structure according to the parameters of the four-foot robot with the spine structure;
the acquisition module is used for acquiring an inertia matrix of the mass center positions of the front half trunk and the rear half trunk according to an initial Newton Euler equation corresponding to the front half trunk and the rear half trunk and combining the acting forces of the stress points, the foot ends and the vertebration in the motion process of the robot;
the construction module is used for constructing a robot motion state equation according to the initial Newton Euler equation and the inertia matrix; the motion state equation is used for pushing each part of trunk state of the last discrete time by adopting each part of trunk state of the previous discrete time in the motion process of the robot;
the optimization module is used for constructing an optimization problem according to a motion state equation and a preset motion track of the robot and obtaining an optimized motion speed and optimized motion time in the motion planning process of the quadruped robot.
The four-legged robot motion planning system provided in the embodiments of the present specification includes: a processor, a memory and a computer program; wherein the method comprises the steps of
And a memory for storing the computer program, which may also be a flash memory (flash). Such as application programs, functional modules, etc. implementing the methods described above.
And the processor is used for executing the computer program stored in the memory to realize each step executed by the equipment in the method. Reference may be made in particular to the description of the embodiments of the method described above.
In the alternative, the memory may be separate or integrated with the processor.
When the memory is a device separate from the processor, the apparatus may further include:
and the bus is used for connecting the memory and the processor.
The present application also provides a readable storage medium having stored therein a computer program for implementing the methods provided by the various embodiments described above when executed by a processor.
The readable storage medium may be a computer storage medium or a communication medium. Communication media includes any medium that facilitates transfer of a computer program from one place to another. Computer storage media can be any available media that can be accessed by a general purpose or special purpose computer. For example, a readable storage medium is coupled to the processor such that the processor can read information from, and write information to, the readable storage medium. In the alternative, the readable storage medium may be integral to the processor. The processor and the readable storage medium may reside in an application specific integrated circuit (Application Specific Integrated Circuits, ASIC for short). In addition, the ASIC may reside in a user device. The processor and the readable storage medium may reside as discrete components in a communication device. The readable storage medium may be read-only memory (ROM), random-access memory (RAM), CD-ROMs, magnetic tape, floppy disk, optical data storage device, etc.
It is noted that the terms "first," "second," "third," "fourth," and the like in the description and claims of the application and in the foregoing figures, if any, are used for distinguishing between similar objects and not necessarily for describing a particular sequential or chronological order. It is to be understood that the data so used may be interchanged where appropriate such that the embodiments of the application described herein may be implemented in sequences other than those illustrated or otherwise described herein.
The same and similar parts of the embodiments in this specification are all mutually referred to, and each embodiment focuses on the differences from the other embodiments. In particular, for the product embodiments described later, since they correspond to the methods, the description is relatively simple, and reference is made to the description of parts of the system embodiments.
The foregoing is merely illustrative of the present application, and the present application is not limited thereto, and any changes or substitutions easily contemplated by those skilled in the art within the scope of the present application should be included in the present application. Therefore, the protection scope of the application is subject to the protection scope of the claims.
Claims (10)
1. The motion planning method for the quadruped robot is characterized by comprising the following steps of:
respectively obtaining initial Newton Euler equations corresponding to the front half trunk and the rear half trunk of the four-foot robot with the spine structure according to parameters of the four-foot robot with the spine structure;
according to an initial Newton Euler equation corresponding to the front half trunk and the rear half trunk, combining the acting forces of the stress points, the foot ends and the vertebration in the motion process of the robot, and obtaining inertia matrixes of the mass center positions of the front half trunk and the rear half trunk;
constructing a robot motion state equation according to the initial Newton Euler equation and the inertia matrix; the motion state equation is used for pushing each part of trunk state of the last discrete time by adopting each part of trunk state of the previous discrete time in the motion process of the robot;
and constructing an optimization problem according to a motion state equation and a preset motion trail of the robot, and obtaining an optimized motion speed and optimized motion time in a motion planning process of the quadruped robot.
2. The four-legged robot motion planning method according to claim 1, further comprising:
setting an objective function according to an initial trunk state corresponding to the motion state equation and an objective trunk state corresponding to a preset motion track, so that the initial trunk state obtained by the motion state equation is converged with the objective trunk state;
wherein the objective function is represented by a weighted square sum of the errors of the initial torso-state and the target torso-state.
3. The four-legged robot motion planning method according to claim 1, further comprising:
and obtaining constraint conditions of a motion state equation in the motion process of the optimized robot.
4. A method of motion planning for a quadruped robot according to claim 3, wherein the constraints include: the trunk is rigidly connected with constraint conditions and state quantity constraint conditions at the tail end moment; wherein the state quantities include the centroid, attitude angle, speed and acceleration of the torso.
5. The method of motion planning for a quadruped robot of claim 1 wherein the parameters of the quadruped robot include physical parameters and motion parameters.
6. The four-legged robot motion planning method according to claim 2, wherein the four-legged robot motion planning method further comprises:
and determining the upper bound and the lower bound of the independent variable corresponding to the initial trunk state.
7. The motion planning method of a quadruped robot according to claim 1, wherein a plurality of discrete time corresponding set motion positions of the robot are set in a preset motion track of the robot;
the motion planning method of the quadruped robot further comprises the following steps:
a motion planning trajectory is obtained based on the robot motion planning, wherein the motion planning trajectory encompasses a higher motion speed and comprises a set motion position.
8. A quadruped robot motion planning apparatus, comprising:
the input module is used for respectively obtaining initial Newton Euler equations corresponding to the front half trunk and the rear half trunk of the four-foot robot with the spine structure according to the parameters of the four-foot robot;
the acquisition module is used for acquiring an inertia matrix of the mass center positions of the front half trunk and the rear half trunk according to an initial Newton Euler equation corresponding to the front half trunk and the rear half trunk and combining the acting forces of the stress points, the foot ends and the vertebration in the motion process of the robot;
the construction module is used for constructing a robot motion state equation according to the initial Newton Euler equation and the inertia matrix; the motion state equation is used for pushing each part of trunk state of the last discrete time by adopting each part of trunk state of the previous discrete time in the motion process of the robot;
the optimization module is used for constructing an optimization problem according to a motion state equation and a preset motion track of the robot to obtain an optimized motion speed and optimized motion time in the motion planning process of the quadruped robot.
9. A four-legged robot motion planning system comprising a memory, a processor and a computer program, the computer program being stored in the memory, the processor running the computer program to perform the four-legged robot motion planning method of any one of claims 1-7.
10. A readable storage medium, characterized in that the readable storage medium has stored therein a computer program for implementing the four-legged robot motion planning method according to any one of claims 1-7 when executed by a processor.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311056139.3A CN117021097A (en) | 2023-08-21 | 2023-08-21 | Method, device and system for motion planning of quadruped robot and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311056139.3A CN117021097A (en) | 2023-08-21 | 2023-08-21 | Method, device and system for motion planning of quadruped robot and storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117021097A true CN117021097A (en) | 2023-11-10 |
Family
ID=88635208
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311056139.3A Pending CN117021097A (en) | 2023-08-21 | 2023-08-21 | Method, device and system for motion planning of quadruped robot and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117021097A (en) |
-
2023
- 2023-08-21 CN CN202311056139.3A patent/CN117021097A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Ayusawa et al. | Motion retargeting for humanoid robots based on simultaneous morphing parameter identification and motion optimization | |
| CN102375416B (en) | Human type robot kicking action information processing method based on rapid search tree | |
| CN113524177B (en) | Control method of foot type robot | |
| Kim et al. | Computationally-robust and efficient prioritized whole-body controller with contact constraints | |
| Shkolnik et al. | Inverse kinematics for a point-foot quadruped robot with dynamic redundancy resolution | |
| CN112051797A (en) | Method, device, equipment and medium for controlling motion of foot type robot | |
| CN115128960B (en) | Method and system for controlling motion of biped robot based on deep reinforcement learning | |
| CN112987769A (en) | Active leg adjusting method for stable transition of quadruped robot in variable-rigidity terrain | |
| Khusainov et al. | Bipedal robot locomotion modelling with virtual height inverted pendulum and preview control approaches in Simulink environment | |
| Liu et al. | An articulated closed kinematic chain planar robotic leg for high-speed locomotion | |
| Zhang et al. | Research on bionic jumping and soft landing of single leg system in quadruped robot | |
| CN117021097A (en) | Method, device and system for motion planning of quadruped robot and storage medium | |
| CN113568422A (en) | Quadruped robot control method based on model prediction control optimization reinforcement learning | |
| CN114986526A (en) | Robot motion control method, device, robot and storage medium | |
| CN115994288B (en) | Method and device for solving inverse kinematics of biped robot | |
| Koch et al. | Discrete mechanics and optimal control of walking gaits | |
| Du et al. | A compact form dynamics controller for a high-DOF tetrapod-on-wheel robot with one manipulator via null space based convex optimization and compatible impedance controllers | |
| CN115840455A (en) | Motion control method and device for foot type robot and foot type robot | |
| Miripour Fard et al. | On the manipulability of swing foot and stability of human locomotion | |
| Hu et al. | Hybrid kinematic and dynamic simulation of running machines | |
| Turetta et al. | Distributed control architecture for self-reconfigurable manipulators | |
| Yang et al. | 3D solid robot animation design based on ADAMS | |
| US11787045B2 (en) | Robot with an inverse kinematics (IK)-based controller for retargeting input motions | |
| CN105438305A (en) | Six-limb insect motion mode determination method, bionic six-limb insect robot and use method thereof | |
| Xia et al. | Application of ADAMS User-Written Subroutine to Simulation of Multi-gait for Spherical 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 | ||
| CB03 | Change of inventor or designer information |
Inventor after: Lin Zhaopeng Inventor after: Zhou Shiyu Inventor after: Pan Zheng Inventor after: Liu Shaoxun Inventor after: Li Boyuan Inventor after: Ying Fei Inventor before: Lin Zhaopeng |
|
| CB03 | Change of inventor or designer information |