CN113377113A - Foot end trajectory planning method and system of foot type robot and control method and system - Google Patents
Foot end trajectory planning method and system of foot type robot and control method and system Download PDFInfo
- Publication number
- CN113377113A CN113377113A CN202110752263.8A CN202110752263A CN113377113A CN 113377113 A CN113377113 A CN 113377113A CN 202110752263 A CN202110752263 A CN 202110752263A CN 113377113 A CN113377113 A CN 113377113A
- Authority
- CN
- China
- Prior art keywords
- joint angle
- function
- foot
- joint
- trajectory
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 94
- 230000008859 change Effects 0.000 claims abstract description 26
- 238000012886 linear function Methods 0.000 claims abstract description 24
- 230000007704 transition Effects 0.000 claims abstract description 20
- 239000011159 matrix material Substances 0.000 claims description 6
- 230000009466 transformation Effects 0.000 claims description 5
- 230000008569 process Effects 0.000 description 10
- 230000009471 action Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 230000014509 gene expression Effects 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 239000012636 effector Substances 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000005021 gait Effects 0.000 description 2
- 235000001968 nicotinic acid Nutrition 0.000 description 2
- 241001465754 Metazoa Species 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 238000010561 standard procedure Methods 0.000 description 1
- 239000000725 suspension Substances 0.000 description 1
- 230000002194 synthesizing effect Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
Abstract
The invention relates to a foot end trajectory planning method and system of a foot type robot, a control method and a control system, wherein when the trajectory of the foot end of a support stage is planned, a linear function with parabolic transition is adopted to interpolate the trajectory to obtain a trajectory parameter equation of the support stage, a first joint angle function of each joint of a single leg and foot structure in the support stage is calculated according to the trajectory parameter equation, when the trajectory of the foot end of a swing stage is planned, a polynomial interpolation method is adopted to interpolate the change amount of the joint angle to obtain an interpolation function of the change amount of the joint angle, a second joint angle function of each joint of the single leg and foot structure in the swing stage is calculated according to the first joint angle function and the interpolation function of the change amount of the joint angle, the planned trajectory of the foot end is used for controlling the robot, so that the loss of motion elements serving as the joints can be effectively reduced, the motion of the leg and foot structure is smoother.
Description
Technical Field
The invention relates to the technical field of robot control, in particular to a foot end trajectory planning method and system of a foot type robot, and a control method and system.
Background
Wheeled robots and tracked robots have been widely used in life for a long time in the past due to their rapidity, high efficiency and ease of control of movement. However, as the range of human activities is enlarged, more and more human trails appear in the mountainous regions with high and low heights, the plateaus with vertical and horizontal ravines, and even unknown extraterrestrial planets, and ordinary wheeled robots and tracked robots cannot well move on the terrains.
With the development of artificial intelligence technology, the legged robots with complex control structures are promoted to appear in the visual field of people more and more. The advent of legged robots has originated from the bionics research of animals in nature, with great attention and interest in high adaptability to terrain and dexterity of motion. The foot type robot can almost move on various complex terrains and complete movement operation, and is particularly widely applied to aspects of ruin exploration, pipeline inspection, forest search and rescue, frontier patrol, interplanetary exploration and the like.
The foot robot pushes the body to move forward by means of the counterforce of the foot end contacted with the supporting structures such as the ground and the like, and the whole motion process is completed by the regular alternate action of each leg and foot structure. From the perspective of bionics, people develop various motion gaits suitable for robots with different foot numbers by combining the structural characteristics of the designed robot, so that the alternate time sequence of the leg-foot structure of the foot type robot is guaranteed. At present, the research of the foot type robot is mostly focused on gait planning, and the planning method aiming at the foot tip track is not much. However, another core of the motion of the foot-type robot is the planning of the foot end trajectory of the robot, because it is the reaction force of the foot end that provides the motion power for the robot, the planning of the foot end trajectory will directly affect the motion direction and the motion stability of the robot. The essence of foot end trajectory planning is that a continuous closed curve is solved in space by performing mathematical modeling on a single-leg foot structure, and the curve is the trajectory of the foot end of the legged robot.
The existing foot end trajectory planning method does not consider the motion of an actual leg and foot mechanism in space trajectory planning, some trajectories planned by the existing planning method are straight line turns when a robot falls on feet, some trajectories adopt a supporting trajectory as a ground straight line, a swinging trajectory is two straight lines and one parabola, the rotation speed of a joint is not considered at each curve junction node of a formed trajectory graph, the trajectories planned by the existing method can cause that after a trajectory curve is solved into a joint space through inverse kinematics, the speed of the joint change has a large sudden change, the speed sudden change means the sudden change of short-time moment, and the service life of a motion element (such as a steering engine) of the joint can be influenced.
Disclosure of Invention
The invention aims to provide a foot end trajectory planning method and system, a control method and a control system of a foot robot, which plan the foot end trajectory under the premise of considering the actual motion of a leg and foot structure, effectively reduce the loss of motion elements serving as joints and enable the motion of the leg and foot structure to be smoother.
In order to achieve the purpose, the invention provides the following scheme:
in a first aspect, the present invention is directed to a foot end trajectory planning method for a legged robot, which is used to plan a foot end trajectory of a single leg and foot structure in a single cycle, and the planning method includes:
1) planning the foot end track of the supporting stage:
interpolating the track by adopting a linear function with parabolic transition to obtain a track parameter equation of a support stage;
calculating a first joint angle function of each joint of the single leg-foot structure in a supporting stage according to the track parameter equation;
2) planning the foot end track of the swing stage:
interpolating the joint angle variation by adopting a polynomial interpolation method to obtain a joint angle variation interpolation function;
calculating a second joint angle function of each joint of the single leg-foot structure in a swing stage according to the first joint angle function and the joint angle variation interpolation function;
the first joint angle function and the second joint angle function are representations of the foot end locus in joint space within a single period.
The invention also provides a foot end trajectory planning system of the foot type robot, which plans the foot end trajectory of a single leg and foot structure in a single period, and the planning system comprises:
a support trajectory planning module for planning the foot end trajectory in the support phase: interpolating the track by adopting a linear function with parabolic transition to obtain a track parameter equation of a support stage; calculating a first joint angle function of each joint of the single leg-foot structure in a supporting stage according to the track parameter equation;
the swing track planning module is used for planning the foot end track in the swing stage: interpolating the joint angle variation by adopting a polynomial interpolation method to obtain a joint angle variation interpolation function; calculating a second joint angle function of each joint of the single leg-foot structure in a swing stage according to the first joint angle function and the joint angle variation interpolation function; the first joint angle function and the second joint angle function are representations of the foot end locus in joint space within a single period.
In a second aspect, the present invention provides a method for controlling a foot end trajectory of a legged robot, which controls a motion of a single-leg foot structure in a single cycle, the method comprising:
controlling the motion of the foot type robot according to the first joint angle function obtained by the trajectory planning method to finish the motion of the supporting stage;
and controlling the motion of the foot type robot according to the second joint angle function obtained by the trajectory planning method to finish the motion in the swing stage.
The present invention is also directed to a foot end trajectory control system for a legged robot, for controlling the motion of a single leg and foot structure in a single cycle, the control system including:
the support control module is used for controlling the motion of the legged robot according to the first joint angle function obtained by the trajectory planning method to complete the motion in the support stage;
and the swing control module is used for controlling the motion of the foot type robot according to the second joint angle function obtained by the track planning method to finish the motion in a swing stage.
According to the specific embodiment provided by the invention, the invention discloses the following technical effects:
the invention provides a foot end trajectory planning method and system for a foot robot, which are used for planning the foot end trajectory of a single leg and foot structure in a single period, and comprise the steps of planning the foot end trajectory in a supporting stage and planning the foot end trajectory in a swinging stage. When the foot end track of the support stage is planned, a linear function with parabolic transition is adopted to interpolate the track to obtain a track parameter equation of the support stage, a first joint angle function of each joint of a single leg and foot structure in the support stage is calculated according to the track parameter equation, then a constant acceleration or deceleration braking process is provided for the joints in a head-tail parabolic mode, and the stability and continuity of the foot end linear velocity and the joint angular velocity in the motion process are guaranteed. When the foot end track in the swing stage is planned, the joint angle variation is interpolated by adopting a polynomial interpolation method to obtain a joint angle variation interpolation function, and a second joint angle function of each joint of a single leg-foot structure in the swing stage is calculated according to the first joint angle function and the joint angle variation interpolation function, so that the track planning can be performed from the angle of positive kinematics, and the problems of no solution, multiple solutions and the like are avoided. The time curve of each joint of the foot end track planned by the invention is continuous and gentle, the change of the angular velocity of each joint is stable, the loss of motion elements serving as the joints can be effectively reduced, the motion of the leg and foot structure is smoother, the operation can be obviously simplified, and the speed of foot end track planning is improved.
The invention is also used for providing a foot end track control method and a foot end track control system for the foot-type robot, which are used for controlling the motion of a single leg and foot structure in a single period, controlling the motion of the foot-type robot according to a first joint angle function obtained by the track planning method, completing the motion in a supporting stage, controlling the motion of the foot-type robot according to a second joint angle function obtained by the track planning method, completing the motion in a swinging stage, and further controlling the robot by using the first joint angle function and the second joint angle function obtained by the planning method, so that the robot moves according to a planned track, the loss of motion elements serving as joints can be effectively reduced, and the motion of the leg and foot structure is smoother.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings 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 invention, and it is obvious for those skilled in the art to obtain other drawings without creative efforts.
Fig. 1 is a flowchart of a planning method according to embodiment 1 of the present invention;
FIG. 2 is a flowchart of a method for calculating a trajectory parameter equation provided in embodiment 1 of the present invention;
fig. 3 is a flowchart of a method for calculating an interpolation function of joint angle variation according to embodiment 1 of the present invention;
FIG. 4 is a schematic structural diagram of a leg and foot structure including 3 joints according to embodiment 1 of the present invention;
FIG. 5 is a geometric model of a leg and foot structure comprising 3 joints according to example 1 of the present invention;
FIG. 6 is a graph of the coordinate components of the foot end trajectory provided in embodiment 1 of the present invention;
FIG. 7 is a schematic diagram of a spatial curve of a foot end trajectory provided in embodiment 1 of the present invention;
FIG. 8 is a graph of joint angle functions for three joints provided in example 1 of the present invention;
fig. 9 is a system block diagram of a planning system provided in embodiment 2 of the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
The invention aims to provide a foot end trajectory planning method and system, a control method and a control system of a foot robot, which plan the foot end trajectory under the premise of considering the actual motion of a leg and foot structure, effectively reduce the loss of motion elements serving as joints and enable the motion of the leg and foot structure to be smoother.
In order to make the aforementioned objects, features and advantages of the present invention comprehensible, embodiments accompanied with figures are described in further detail below.
Example 1:
the present embodiment is configured to provide a foot end trajectory planning method for a legged robot, where the planning methods for each leg and foot structure included in the legged robot are the same, and the planning methods for each leg and foot structure in each period are the same. The foot end trajectory of a certain leg and foot structure on the foot robot in a single cycle mainly comprises two stages, wherein one stage is a time period in which the foot end is in contact with the ground to generate force and is called a supporting stage (or a supporting phase), and the other stage is a time period in which the foot end swings in the air and is called a swinging stage (or a swinging phase). The motion time of the two phases is the same, and the motion time is recorded as TfAnd the tracks of the two stages are connected end to form a closed curve, and the closed curve is the foot end track of the single leg-foot structure of the foot type robot in a single period. This embodiment provides a flexible foot end trajectory planning method for a foot-type robot, which utilizes three elements to determine the trajectory of the foot end, so the planning method can also be called a three-element trajectory determination method, and the three elements are respectively the coordinates P of the starting point of the support stage0＝(x0,y0,z0) Coordinates P of the end point of the support phase1＝(x1,y1,z1) And a preset joint angle variation amount [ Delta theta ] of the swing stage1，Δθ2，...，Δθi，...Δθn]，ΔθiThe joint angle variation is set for the ith joint, and n is the number of joints of the leg-foot structure.
Specifically, as shown in fig. 1, the planning method for planning the foot end trajectory of the single-leg foot structure in a single cycle includes:
1) planning the foot end track of the supporting stage:
in the advancing process of the foot type robot, only if the foot end track is a straight line, the body can be ensured to move forwards linearly, so that the embodiment plans the foot end track in the supporting stage under a Cartesian coordinate system. In order to ensure the stability and continuity of the linear velocity and the angular velocity of the joint at the foot end in the motion process, the embodiment adopts a linear function with parabolic transition to interpolate three coordinate components of the track respectively, and specifically comprises the following steps:
s1: interpolating the track by adopting a linear function with parabolic transition to obtain a track parameter equation of a support stage;
as shown in fig. 2, S1 may include:
s11: acquiring a first known parameter of a support stage; the first known parameter comprises a starting point coordinate and an end point coordinate of the support stage;
starting point coordinate P of support stage0＝(x0,y0,z0) Coordinate of end point P1＝(x1,y1,z1)。
S12: establishing an initial linear function with a parabolic transition;
the initial linear function established is as follows:
in formula 1, TbA parabolic transition time for a parabolic transition;
af，bf，cf，kf，df，ef，mf，nfare the correlation coefficients of the functions, and t is time.
S13: solving the initial linear function according to the first known parameter and a first limiting condition to obtain a track parameter equation; the first constraint includes a start-end position, a start-end velocity, a trajectory continuation, and a velocity continuation.
In this embodiment, the X, Y, Z coordinate components of the trajectory need to be interpolated, so that X, Y, Z linear functions with parabolic transitions corresponding to the three components are obtained, and the trajectory parameter equation can be obtained by combining the linear functions. The solving method for the linear function with the parabolic transition of the three components is the same, and here, taking the X component as an example, the solving process is described in detail:
the first constraint can be described by the following equation:
in the formula 2, the 1 st formula and the 2 nd formula are initial and final position constraints; the 3 rd and 4 th formulas are track continuity constraints which are the limiting conditions generated for making the linear function with the parabola transition continuous; the 5 th and 6 th formulas are speed continuous constraints which are limit conditions generated for ensuring the speed continuity; the 7 th and 8 th equations are the start and end velocity constraints, which are the constraints for preparing the swing phase.
According to first stepKnowing the parameters and equation 2, a can be foundf，bf，cf，kf，df，ef，mf，nfThe specific values of these correlation coefficients can then be used to obtain a linear function X (t) with a parabolic transition corresponding to the X component. According to the method, a linear function Y (t) with a parabolic transition corresponding to the Y component and a linear function Z (t) with a parabolic transition corresponding to the Z component can be obtained.
Further, the trajectory parameter equation is: [ X (t), Y (t), Z (t) ].
S2: calculating a first joint angle function of each joint of the single leg-foot structure in a supporting stage according to the track parameter equation;
s2 may include: obtaining an inverse kinematics equation of a single leg and foot structure, specifically obtaining a D-H parameter table of the single leg and foot structure, and calculating an adjacent connecting rod transformation matrix according to the D-H parameter table. The positive kinematics analysis and the inverse kinematics analysis can be carried out according to the adjacent connecting rod transformation matrix, the positive kinematics analysis is the angle of each joint of the given robot, the position and the attitude of the tail end of the robot are calculated, the inverse kinematics analysis is the expected attitude of the given tail end, the angle of each joint of the single leg and foot structure is calculated, and the inverse kinematics equation of the single leg and foot structure can be obtained by using a geometric method according to the adjacent connecting rod transformation matrix. After the inverse kinematics equation is obtained, a track parameter equation is used as input, and the inverse kinematics equation is used for calculating a first joint angle function of each joint of the single leg-foot structure in the supporting stage.
The first joint angle function may be expressed as: thetaS(t)＝[θ1(t)，θ2(t)，...，θi(t)，...，θn(t)]。
In the embodiment, the linear function with the parabola is used for interpolation in the support stage, so that the interpolation is smoother than the simple linear interpolation, and the rigid impact cannot be caused.
2) Planning the foot end track of the swing stage:
the foot-leg structure of the foot robot is completed from the end point to the ground clearance return in the swing stage, and the stage is not suitable for planning the track in a Cartesian coordinate system. On the other hand, because the stage moves in a 3-dimensional space, in the inverse kinematics solution process, particularly for a multi-joint leg and foot structure, there may be problems of no solution, multiple solutions, and the like, for example, if the space of the trajectory planned by the conventional trajectory planning method is limited to a hemisphere, if a user wants to lift the leg vertically in the swing stage, the inverse kinematics solution may possibly occur as a case of no solution, and the reliability is insufficient. To solve the above problem, in this embodiment, from the perspective of positive kinematics, a trajectory is planned in a joint space in a superposition manner, and based on an inverse time sequence of a first joint angle function in a support phase, a joint angle variation interpolation function obtained according to a joint angle variation of each joint is superposed to plan a trajectory in a swing phase, so as to implement an action of ground-based return in the swing phase, specifically, the method includes the following steps:
s3: interpolating the joint angle variation by adopting a polynomial interpolation method to obtain a joint angle variation interpolation function;
s3 may specifically use a fourth-order polynomial interpolation method to interpolate the joint angle variation to obtain a joint angle variation interpolation function, as shown in fig. 3, which includes the following steps:
s31: acquiring a second known parameter of the swing stage; the second known parameter comprises a preset joint angle variation of a swing stage;
preset joint angle variation delta theta of swing stage [ delta theta ]1，Δθ2，...，Δθi，...Δθn]，ΔθiThe joint angle variation is set for the ith joint, and n is the number of joints of the leg-foot structure.
S32: establishing an initial joint angle variation interpolation function; the function of each joint in the initial joint angle variation interpolation function is a quartic polynomial equation;
the initial joint angle change interpolation function can be expressed as:
θP(t)＝[θP_1(t)，θP_2(t)，...，θP_i(t)，...，θP_n(t)]。
function θ P _ i (t) P of i-th joint4t4+p3t3+p2t2+p1t+p0，p4，p3，p2，p1，p0Are all correlation coefficients.
S33: solving the initial joint angle variation interpolation function according to the second known parameter and a second limiting condition to obtain a joint angle variation interpolation function; the second limitation includes a start and end joint angle change amount, an intermediate joint angle change amount, and a start and end joint angle change speed.
And solving the functions of the n joints to obtain a specific expression of the function, and combining to obtain the joint angle change interpolation function. Taking the function of the ith joint as an example, the solution process is explained in detail:
the second limiting condition is as follows:
in the formula 3, the 1 st formula and the 2 nd formula are constraint on the variation of the joint angles at the beginning and the end, and the variation of the joint angles at the beginning and the end is taken as 0 so as to ensure the continuity of the variation of the joint angles; the 3 rd and 4 th formulas are constraint of the change speed of the joint angle at the beginning and the end, and the change speed of the joint angle at the beginning and the end is taken as 0 to ensure the stability of the joint rotation; the 5 th formula is the joint angle change amount constraint at the middle moment so as to realize the symmetry of angle change.
Obtaining the specific values of all the correlation coefficients of the function of the ith joint according to the second known parameter and the formula 3, further obtaining the specific expression of the function of the ith joint, and then obtaining the joint angle change interpolation function theta by synthesizing the specific expressions of the functions of all the jointsP(t)。
S4: calculating a second joint angle function of each joint of the single leg-foot structure in a swing stage according to the first joint angle function and the joint angle variation interpolation function;
s4 may include: and calculating a joint angle reverse time sequence function of the swing stage according to the first joint angle function. Specifically, the joint angle inverse time sequence function required by the swing stage and the first joint angle function of the support stage are T ═ TfSymmetry, and therefore the joint angle negative timing function, can be found as:
θB(t)＝θS(2Tf-t)＝[θ1(2Tf-t)，θ2(2Tf-t)，...，θi(2Tf-t)，...，θn(2Tf-t)]。
and after the joint angle reverse time sequence function is obtained, summing the joint angle reverse time sequence function and the joint angle change interpolation function to obtain a second joint angle function of each joint of the single leg-foot structure in the swing stage.
Second joint angle function thetaT(t)＝θB(t)+θP(t)。
The first joint angle function and the second joint angle function are representations of the foot end locus in joint space within a single period.
That is, the whole foot end trajectory plan is expressed in the joint space as
In the embodiment, from the angle of positive kinematics, a trajectory planning is performed in a joint space in a superposition mode in a swing stage, a reverse time sequence of a first joint angle function in a support stage is used as a basis, a joint angle change interpolation function obtained according to joint angle changes of all joints is superposed, a trajectory of the swing stage is planned, and an action of ground return in the swing stage is realized. Compared with the trajectory planning only in the joint space, the uncertainty of the trajectory of the end effector in the Cartesian space, which may occur only after the joint space planning, is avoided due to the fact that the joint angle inverse time sequence curve is used as the basis in the stage, and the trajectory planning method is more intuitive physically.
The embodiment provides a flexible foot end trajectory planning method for a foot-type robot, a linear function with parabola mixing is adopted to plan a support stage of a foot end trajectory, and a constant acceleration or deceleration braking process is provided for a joint in a head-tail parabola mode. Planning in joint space in a swing stage of foot end tracks, planning tracks in a superposition mode, namely, using the reverse time sequence of joint angle functions in a support stage as a basis, superposing quartic polynomial interpolation functions obtained according to joint angle variation of each joint, planning the tracks in the swing stage, and realizing the action of suspension motion in the swing stage. The planning method of the embodiment plans the multi-degree-of-freedom track, compresses the multi-degree-of-freedom track into three elements, can generate the required track by adjusting the three elements, and is convenient and practical. And the planning method of the embodiment can also obviously simplify the operation and improve the speed of foot end trajectory planning.
In order to make the trajectory planning method described in this embodiment more clear to those skilled in the art, a leg-foot structure composed of three joints is taken as an example, the leg-foot structure is shown in fig. 4, and before describing the trajectory planning method of the leg-foot structure, several basic concepts are described:
1. D-H parameter method:
the D-H parameter method is called as the Denavit-Hartenberg parameter method, and is a standard method for representing the robot and modeling the robot motion. This example uses the Craig version of the D-H parametric method to model the single foot and leg structure shown in fig. 4. The D-H parameters for this example are shown in table 1 below.
TABLE 1
In table 1, a subscript i denotes an ith leg-foot structure of the foot end robot, and a subscript j denotes a jth joint of the ith leg-foot structure. a isj-1The common perpendicular distance between the joint axis of the j-1 th joint and the joint axis of the j joint; alpha is alphaj-1Is the included angle between the joint axis of the j-1 th joint and the joint axis of the j joint; djIs the offset distance of the connecting rod; thetajIs the joint angle.
Each link coordinate system (e.g., coordinate system O in FIG. 4)i0}, coordinate system { Oi1}, coordinate system { Oi2And a coordinate system Oi3}) is also established by adopting a Craig version description method, and the adjacent connecting rods transform a matrix into
2. Positive kinematics analysis
And positive kinematic analysis, namely, given the angle of each joint of the robot, and solving the position and the posture of the robot end effector. The positive kinematic equation for the three joint leg-foot structure employed in this example is as follows:
the parameters in formula 5 are shown in the D-H parameter table of Table 1,
is the foot end F point relative to the coordinate system { O }i3The coordinate component of { C };0PFis the foot end F point relative to the coordinate system { O }i0The coordinates of { C };3PFis the foot end F point relative to the coordinate system { O }i3The coordinates of the points.
3. Inverse kinematics analysis
And (4) performing inverse kinematics analysis, namely calculating the angle value of each joint by the inverse kinematics solution given the expected posture. The present example uses geometric methods to solve the inverse kinematics equation, and the geometric model of fig. 4 is shown in fig. 5.
The respective parameters of fig. 5 are explained: coordinate system { O } in FIG. 5BThe frame coordinate system is used as the frame coordinate system; point a of fig. 5 corresponds to point O of fig. 4i0Dot (O)i1And Oi0Coincident); point B of fig. 5 corresponds to point O of fig. 4i2Point; point C of fig. 5 corresponds to point O of fig. 4i3Point; point D of fig. 5 corresponds to point D of fig. 4; point F of fig. 5 corresponds to point F of fig. 4; α in fig. 5 represents an angle which is an angle with B as a vertex, and with an extension of the line segment AB and the line segment BF as sides; β in fig. 5 represents the size of < CBF in Δ BCF; gamma in fig. 5 represents the size of < BCF in Δ BCF; σ in FIG. 5 represents the magnitude of < FCD in Δ CDF.
The inverse kinematics equation for this example is:
the toe trajectory planning of the present example starts based on the basic concepts described above.
Setting Tb＝0.2s,TfThe initial point coordinates are (154.6,229.6, -66.1), the end point coordinates are (154.6,189.6, -66.1) and the preset joint angle variation delta theta is [0 deg., 10 deg. ° ═]The first joint angle function in the support phase is acquired using S1 and S2, and the second joint angle function in the sway phase is acquired using S3 and S4. Fig. 6 shows a graph of coordinate components of the foot end trajectory, fig. 7 shows a spatial curve of the foot end trajectory, and fig. 8 shows a graph of joint angle functions of the joints.
As can be seen from fig. 6, in the supporting stage, the X and Z coordinates of the foot end are kept unchanged, only the Y coordinate is changed, the foot falling point is a linear motion parallel to the Y axis, and the spatial trajectory curve of the foot end in fig. 7 can also be verified. In the swing phase, as shown in fig. 6 and 7, the foot end raises the leg back in the air, completing the entire orbital motion. This proves the correctness and feasibility of the generated trajectory by the foot end trajectory planning method designed by the embodiment. In the whole movement process, as shown in fig. 8, the time curve of each joint is continuous and gentle, and the change of the angular velocity of each joint is stable, which fully proves that the method can effectively reduce the loss of the moving element as the joint and make the movement of the leg and foot structure smoother.
Example 2:
the present embodiment is configured to provide a foot end trajectory planning system for a legged robot, which plans a foot end trajectory of a single leg and foot structure in a single cycle, as shown in fig. 9, and the planning system includes:
a support trajectory planning module M1, configured to plan a foot end trajectory of the support phase: interpolating the track by adopting a linear function with parabolic transition to obtain a track parameter equation of a support stage; calculating a first joint angle function of each joint of the single leg-foot structure in a supporting stage according to the track parameter equation;
a swing trajectory planning module M2, configured to plan a foot end trajectory of a swing phase: interpolating the joint angle variation by adopting a polynomial interpolation method to obtain a joint angle variation interpolation function; calculating a second joint angle function of each joint of the single leg-foot structure in a swing stage according to the first joint angle function and the joint angle variation interpolation function; the first joint angle function and the second joint angle function are representations of the foot end locus in joint space within a single period.
Example 3:
the embodiment is used for providing a foot end trajectory control method of a legged robot, which is used for controlling the motion of a single-leg foot structure in a single cycle, and the control method includes:
controlling the motion of the foot robot according to the first joint angle function obtained in the embodiment 1 to finish the motion in the supporting stage; and controlling the motion of the legged robot according to the second joint angle function obtained in the embodiment 1 to finish the motion in the swing stage.
Example 4:
the present embodiment is configured to provide a foot end trajectory control system for a legged robot, which controls the motion of a single leg and foot structure in a single cycle, and the control system includes:
the support control module is used for controlling the motion of the legged robot according to the first joint angle function obtained in the embodiment 1 to finish the motion in a support stage;
and the swing control module is used for controlling the motion of the foot type robot according to the second joint angle function obtained in the embodiment 1 to finish the motion in a swing stage.
The embodiments in the present description are described in a progressive manner, each embodiment focuses on differences from other embodiments, and the same and similar parts among the embodiments are referred to each other. For the system disclosed by the embodiment, the description is relatively simple because the system corresponds to the method disclosed by the embodiment, and the relevant points can be referred to the method part for description.
The principles and embodiments of the present invention have been described herein using specific examples, which are provided only to help understand the method and the core concept of the present invention; meanwhile, for a person skilled in the art, according to the idea of the present invention, the specific embodiments and the application range may be changed. In view of the above, the present disclosure should not be construed as limiting the invention.
Claims (10)
1. A foot end trajectory planning method of a foot robot plans a foot end trajectory of a single leg and foot structure in a single period, and is characterized by comprising the following steps:
1) planning the foot end track of the supporting stage:
interpolating the track by adopting a linear function with parabolic transition to obtain a track parameter equation of a support stage;
calculating a first joint angle function of each joint of the single leg-foot structure in a supporting stage according to the track parameter equation;
2) planning the foot end track of the swing stage:
interpolating the joint angle variation by adopting a polynomial interpolation method to obtain a joint angle variation interpolation function;
calculating a second joint angle function of each joint of the single leg-foot structure in a swing stage according to the first joint angle function and the joint angle variation interpolation function;
the first joint angle function and the second joint angle function are representations of the foot end locus in joint space within a single period.
2. The planning method according to claim 1, wherein the interpolating the trajectory using the linear function with the parabolic transition to obtain the trajectory parameter equation of the support phase specifically comprises:
acquiring a first known parameter of a support stage; the first known parameter comprises a starting point coordinate and an end point coordinate of the support stage;
establishing an initial linear function with a parabolic transition;
solving the initial linear function according to the first known parameter and a first limiting condition to obtain a track parameter equation; the first constraint includes a start-end position, a start-end velocity, a trajectory continuation, and a velocity continuation.
3. The planning method according to claim 1, wherein the calculating a first joint angle function of each joint of the single-leg foot structure in the support phase according to the trajectory parameter equation specifically includes:
obtaining an inverse kinematics equation of the single leg-foot structure;
and calculating a first joint angle function of each joint of the single leg-foot structure in a supporting stage by using the inverse kinematics equation by taking the track parameter equation as input.
4. The planning method according to claim 3, wherein the obtaining the inverse kinematics equation of the single-legged foot structure specifically comprises:
acquiring a D-H parameter table of the single leg and foot structure;
calculating a transformation matrix of adjacent connecting rods according to the D-H parameter table;
and acquiring an inverse kinematics equation of the single leg and foot structure by using a geometric method according to the adjacent connecting rod transformation matrix.
5. The planning method according to claim 1, wherein the interpolating the joint angle variation by a polynomial interpolation method to obtain the joint angle variation interpolation function specifically includes: and (4) interpolating the joint angle change by adopting a quartic polynomial interpolation method to obtain a joint angle change interpolation function.
6. The planning method according to claim 5, wherein the interpolating the joint angle variation by using a fourth-order polynomial interpolation method to obtain the joint angle variation interpolation function specifically includes:
acquiring a second known parameter of the swing stage; the second known parameter comprises a preset joint angle variation of a swing stage;
establishing an initial joint angle variation interpolation function; the function of each joint in the initial joint angle variation interpolation function is a quartic polynomial equation;
solving the initial joint angle variation interpolation function according to the second known parameter and a second limiting condition to obtain a joint angle variation interpolation function; the second limitation includes a start and end joint angle change amount, an intermediate joint angle change amount, and a start and end joint angle change speed.
7. The planning method according to claim 1, wherein the calculating a second joint angle function of each joint of the single-legged foot structure in the swing phase according to the first joint angle function and the joint angle variation interpolation function specifically includes:
calculating a joint angle reverse time sequence function of a swing stage according to the first joint angle function;
and summing the joint angle inverse time sequence function and the joint angle variation interpolation function to obtain a second joint angle function of each joint of the single leg-foot structure in a swing stage.
8. A foot end trajectory planning system of a foot robot plans a foot end trajectory of a single leg and foot structure in a single period, and is characterized by comprising:
a support trajectory planning module for planning the foot end trajectory in the support phase: interpolating the track by adopting a linear function with parabolic transition to obtain a track parameter equation of a support stage; calculating a first joint angle function of each joint of the single leg-foot structure in a supporting stage according to the track parameter equation;
the swing track planning module is used for planning the foot end track in the swing stage: interpolating the joint angle variation by adopting a polynomial interpolation method to obtain a joint angle variation interpolation function; calculating a second joint angle function of each joint of the single leg-foot structure in a swing stage according to the first joint angle function and the joint angle variation interpolation function; the first joint angle function and the second joint angle function are representations of the foot end locus in joint space within a single period.
9. A foot end track control method of a foot robot is used for controlling the motion of a single leg and foot structure in a single period, and is characterized by comprising the following steps:
controlling the motion of the legged robot according to the first joint angle function obtained in claim 1 to complete the motion in the support phase;
the second joint angle function obtained according to claim 1 controls the motion of the legged robot to complete the motion in the swing phase.
10. A foot end trajectory control system for a legged robot for controlling the motion of a single leg and foot structure in a single cycle, the control system comprising:
a support control module, which is used for controlling the motion of the legged robot according to the first joint angle function obtained in the claim 1 to complete the motion in the support stage;
and the swing control module is used for controlling the motion of the legged robot according to the second joint angle function obtained in the claim 1 to complete the motion in the swing stage.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110752263.8A CN113377113B (en) | 2021-07-02 | 2021-07-02 | Foot end trajectory planning method and system of foot type robot and control method and system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110752263.8A CN113377113B (en) | 2021-07-02 | 2021-07-02 | Foot end trajectory planning method and system of foot type robot and control method and system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113377113A true CN113377113A (en) | 2021-09-10 |
| CN113377113B CN113377113B (en) | 2022-01-04 |
Family
ID=77580789
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110752263.8A Active CN113377113B (en) | 2021-07-02 | 2021-07-02 | Foot end trajectory planning method and system of foot type robot and control method and system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113377113B (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114888802A (en) * | 2022-05-16 | 2022-08-12 | 军事科学院系统工程研究院卫勤保障技术研究所 | Data processing method and device for constant-level control of parallel robot |
Citations (19)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20090187275A1 (en) * | 2005-02-03 | 2009-07-23 | Keisuke Suga | Legged Robot and Control Method Thereof |
| US20100042256A1 (en) * | 2003-06-27 | 2010-02-18 | Honda Motor Co., Ltd. | Control device for legged mobile robot |
| CN102897245A (en) * | 2012-10-30 | 2013-01-30 | 同济大学 | Robot body modularization linkage device for single-drive multi-legged robot |
| CN104192221A (en) * | 2014-09-26 | 2014-12-10 | 哈尔滨工业大学 | Motion control system and method for electrically-driven hexapod robot |
| CN104331081A (en) * | 2014-10-10 | 2015-02-04 | 北京理工大学 | Gait planning method for walking of biped robot along slope |
| CN104527830A (en) * | 2014-11-12 | 2015-04-22 | 哈尔滨工业大学 | Motion planning method for decreasing power and traffic consumption of hydraulic-driven six-foot robot |
| CN106681341A (en) * | 2016-12-12 | 2017-05-17 | 杭州宇芯机器人科技有限公司 | Multi-foot robot gait optimization control method based on multidimensional workspace coupling algorithm |
| CN106826813A (en) * | 2016-12-12 | 2017-06-13 | 杭州宇芯机器人科技有限公司 | The robot stabilized motion control method of the foot point assessment that falls with multiple constraint is modeled based on curve matching |
| CN108572553A (en) * | 2018-05-16 | 2018-09-25 | 清华大学深圳研究生院 | A kind of movement closed loop control method of quadruped robot |
| CN109298682A (en) * | 2018-11-16 | 2019-02-01 | 重庆邮电大学 | A kind of dual robot collaboration track optimizing method based on parabola interpolation algorithm |
| CN109521771A (en) * | 2018-11-22 | 2019-03-26 | 西北工业大学 | A kind of hexapod robot motion control arithmetic |
| CN109807901A (en) * | 2019-03-30 | 2019-05-28 | 华南理工大学 | A kind of hexapod robot and its planing method of sufficient end track |
| CN109946974A (en) * | 2019-04-12 | 2019-06-28 | 山东大学 | A kind of control system of electric drive quadruped robot |
| CN110018635A (en) * | 2019-01-18 | 2019-07-16 | 北方工业大学 | A kind of parallel robot Pick-and-Place operations method for planning track and system |
| CN111290272A (en) * | 2020-02-15 | 2020-06-16 | 中国海洋大学 | Attitude stationarity adjusting method based on multi-legged robot |
| CN111891249A (en) * | 2020-06-19 | 2020-11-06 | 浙江大学 | Hydraulic hexapod robot and walking gait control method based on centroid fluctuation |
| CN112193345A (en) * | 2020-09-30 | 2021-01-08 | 江苏集萃智能制造技术研究所有限公司 | Sectional type quintic polynomial curve obstacle crossing gait planning method |
| CN112558622A (en) * | 2020-11-30 | 2021-03-26 | 华南理工大学 | Control method of foot end trajectory tracking controller of wall-climbing hexapod robot |
| CN112697149A (en) * | 2020-12-10 | 2021-04-23 | 华南理工大学 | Planning method for rhythm gait foot end track of hexapod robot |
-
2021
- 2021-07-02 CN CN202110752263.8A patent/CN113377113B/en active Active
Patent Citations (19)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20100042256A1 (en) * | 2003-06-27 | 2010-02-18 | Honda Motor Co., Ltd. | Control device for legged mobile robot |
| US20090187275A1 (en) * | 2005-02-03 | 2009-07-23 | Keisuke Suga | Legged Robot and Control Method Thereof |
| CN102897245A (en) * | 2012-10-30 | 2013-01-30 | 同济大学 | Robot body modularization linkage device for single-drive multi-legged robot |
| CN104192221A (en) * | 2014-09-26 | 2014-12-10 | 哈尔滨工业大学 | Motion control system and method for electrically-driven hexapod robot |
| CN104331081A (en) * | 2014-10-10 | 2015-02-04 | 北京理工大学 | Gait planning method for walking of biped robot along slope |
| CN104527830A (en) * | 2014-11-12 | 2015-04-22 | 哈尔滨工业大学 | Motion planning method for decreasing power and traffic consumption of hydraulic-driven six-foot robot |
| CN106681341A (en) * | 2016-12-12 | 2017-05-17 | 杭州宇芯机器人科技有限公司 | Multi-foot robot gait optimization control method based on multidimensional workspace coupling algorithm |
| CN106826813A (en) * | 2016-12-12 | 2017-06-13 | 杭州宇芯机器人科技有限公司 | The robot stabilized motion control method of the foot point assessment that falls with multiple constraint is modeled based on curve matching |
| CN108572553A (en) * | 2018-05-16 | 2018-09-25 | 清华大学深圳研究生院 | A kind of movement closed loop control method of quadruped robot |
| CN109298682A (en) * | 2018-11-16 | 2019-02-01 | 重庆邮电大学 | A kind of dual robot collaboration track optimizing method based on parabola interpolation algorithm |
| CN109521771A (en) * | 2018-11-22 | 2019-03-26 | 西北工业大学 | A kind of hexapod robot motion control arithmetic |
| CN110018635A (en) * | 2019-01-18 | 2019-07-16 | 北方工业大学 | A kind of parallel robot Pick-and-Place operations method for planning track and system |
| CN109807901A (en) * | 2019-03-30 | 2019-05-28 | 华南理工大学 | A kind of hexapod robot and its planing method of sufficient end track |
| CN109946974A (en) * | 2019-04-12 | 2019-06-28 | 山东大学 | A kind of control system of electric drive quadruped robot |
| CN111290272A (en) * | 2020-02-15 | 2020-06-16 | 中国海洋大学 | Attitude stationarity adjusting method based on multi-legged robot |
| CN111891249A (en) * | 2020-06-19 | 2020-11-06 | 浙江大学 | Hydraulic hexapod robot and walking gait control method based on centroid fluctuation |
| CN112193345A (en) * | 2020-09-30 | 2021-01-08 | 江苏集萃智能制造技术研究所有限公司 | Sectional type quintic polynomial curve obstacle crossing gait planning method |
| CN112558622A (en) * | 2020-11-30 | 2021-03-26 | 华南理工大学 | Control method of foot end trajectory tracking controller of wall-climbing hexapod robot |
| CN112697149A (en) * | 2020-12-10 | 2021-04-23 | 华南理工大学 | Planning method for rhythm gait foot end track of hexapod robot |
Non-Patent Citations (8)
| Title |
|---|
| ARBIA AYARI;JILANI KNANI: "Kinematics velocity and dynamic modeling of biped robot", 《2017 4TH INTERNATIONAL CONFERENCE ON CONTROL, DECISION AND INFORMATION TECHNOLOGIES (CODIT)》 * |
| YANQIN WANG ETAL.: "An Optimized Turning Gait Strategy for Walking Quadruped Robot", 《PROCEEDINGS OF THE 39TH CHINESE CONTROL CONFERENCE》 * |
| ZHIHUACHEN ETAL.: "Control strategy of stable walking for a hexapod wheel-legged robot", 《ISA TRANSACTIONS》 * |
| 刘瑞轩: "四足机器人结构设计与仿真优化", 《中国优秀硕士学位论文全文数据库》 * |
| 周荣荻: "多指灵巧手的抓取规划策略研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
| 徐维超: "小型六足机器人运动控制系统设计与实现", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
| 罗天洪等: "基于过路径点的三次多项式插值函数的仿蜘蛛机器人足路径规划", 《机械传动》 * |
| 雷震宇: "六足机器人行走步态的协调控制研究", 《中国优秀硕士学位论文全文数据库》 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114888802A (en) * | 2022-05-16 | 2022-08-12 | 军事科学院系统工程研究院卫勤保障技术研究所 | Data processing method and device for constant-level control of parallel robot |
| CN114888802B (en) * | 2022-05-16 | 2023-08-08 | 军事科学院系统工程研究院卫勤保障技术研究所 | Data processing method and device for parallel robot constant level control |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113377113B (en) | 2022-01-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107490965B (en) | Multi-constraint trajectory planning method for space free floating mechanical arm | |
| Girard | Interactive design of 3-D computer-animated legged animal motion | |
| CN109871018B (en) | Control method of quadruped robot with waist degree of freedom | |
| CN103149933A (en) | Closed-loop control-based humanoid robot omnidirectional walking method | |
| CN104192221A (en) | Motion control system and method for electrically-driven hexapod robot | |
| CN113377113B (en) | Foot end trajectory planning method and system of foot type robot and control method and system | |
| CN109623812A (en) | Consider the mechanical arm method for planning track of spacecraft ontology attitude motion | |
| Zhao et al. | Kinematics mapping and similarity evaluation of humanoid motion based on human motion capture | |
| Kulpa et al. | Fast inverse kinematics and kinetics solver for human-like figures | |
| CN114384918A (en) | Diagonal gait compliance control method of four-footed robot based on drop foot point adjustment | |
| Li et al. | An efficient motion generation method for redundant humanoid robot arm based on the intrinsic principles of human arm motion | |
| Szumowski et al. | Preview control applied for humanoid robot motion generation | |
| Qiu et al. | A hierarchical framework for realizing dynamically-stable motions of humanoid robot in obstacle-cluttered environments | |
| CN112256028B (en) | Method, system, equipment and medium for controlling compliant gait of biped robot | |
| Nguyen et al. | Controlling center of mass in humanoid robot using sliding mode control | |
| CN108717267B (en) | Central mode reverse control method of hexapod robot | |
| Mu et al. | Sagittal gait synthesis for a five-link biped robot | |
| Toscano et al. | A new kinetostatic model for humanoid robots using screw theory | |
| Kim et al. | Stair climbing task of humanoid robot by phase composition and phase sequence | |
| Lu et al. | A novel multi-configuration quadruped robot with redundant DOFs and its application scenario analysis | |
| Moubarak et al. | Reconfiguring a modular robot into a humanoid formation: A multi-body dynamic perspective on motion scheduling for modules and their assemblies | |
| Wang et al. | Design and Analysis of a Novel Tree Climbing Robot Mechanism | |
| Li et al. | Dynamic Locomotion of a Quadruped Robot with Active Spine via Model Predictive Control | |
| Turetta et al. | Distributed control architecture for self-reconfigurable manipulators | |
| CN114454983B (en) | Turning control method and system for quadruped robot |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |