CN110597267A - Local optimal foot drop point selection method for foot type robot - Google Patents
Local optimal foot drop point selection method for foot type robot Download PDFInfo
- Publication number
- CN110597267A CN110597267A CN201910926924.7A CN201910926924A CN110597267A CN 110597267 A CN110597267 A CN 110597267A CN 201910926924 A CN201910926924 A CN 201910926924A CN 110597267 A CN110597267 A CN 110597267A
- Authority
- CN
- China
- Prior art keywords
- foot
- point
- robot
- falling
- candidate
- 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
- 206010034701 Peroneal nerve palsy Diseases 0.000 title claims abstract description 40
- 238000010187 selection method Methods 0.000 title claims abstract description 10
- 238000011156 evaluation Methods 0.000 claims abstract description 43
- 210000002414 leg Anatomy 0.000 claims description 57
- 238000000034 method Methods 0.000 claims description 27
- 230000007613 environmental effect Effects 0.000 claims description 10
- 210000000689 upper leg Anatomy 0.000 claims description 9
- 238000004422 calculation algorithm Methods 0.000 claims description 7
- 238000000605 extraction Methods 0.000 claims description 4
- 238000013461 design Methods 0.000 claims description 3
- 230000006870 function Effects 0.000 description 40
- 230000008569 process Effects 0.000 description 12
- 238000010586 diagram Methods 0.000 description 9
- 230000005484 gravity Effects 0.000 description 5
- 238000004458 analytical method Methods 0.000 description 4
- 238000004088 simulation Methods 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 3
- 230000003068 static effect Effects 0.000 description 3
- 238000012706 support-vector machine Methods 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 210000001503 joint Anatomy 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 238000012216 screening Methods 0.000 description 2
- 230000004888 barrier function Effects 0.000 description 1
- 230000006399 behavior Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000002131 composite material Substances 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
Abstract
The invention discloses a local optimal foot drop point selection method for a foot type robot, which comprises the following steps: step 1, extracting topographic information of a foot drop point and analyzing the foot drop performance; step 2, establishing a single-leg motion space of the foot type robot; and 3, analyzing the stability of the organism. Step 4, establishing a target function of the foot-falling point planning; step 5, a constraint bar of the foot-falling point evaluation function; and 6, selecting an optimal foot falling point. The evaluation function of the touchdown point is provided by considering in multiple aspects, the contents in multiple aspects are included, the touchdown performance of the robot is comprehensively evaluated, and the walking stability of the foot type robot in the complex terrain is improved.
Description
Technical Field
The invention belongs to the technical field of robots, and particularly relates to a local optimal foot drop point selection method for a foot type robot.
Background
The ground mobile robot is mainly divided into a wheel type, a crawler type, a foot type, a composite type and the like according to the structural characteristics and different motion forms of the ground mobile robot. Foot robots are more advantageous than wheeled or tracked robots in dealing with complex environments with rough terrain in the field, because the motion trajectory is made up of a series of discrete points (footfall points) rather than a continuous line segment of the latter. The foot robot needs to move from the current path point to the next path point through the swing of the leg, and the contact point of the swing leg and the ground is the foot drop point. The feasible path is planned in the working environment by the robot through the path planning method, so the foot-falling point of the robot is selected according to the feasible path, and the importance of the selection of the foot-falling point is self-evident in order to ensure that the robot safely and stably walks in the working environment.
The foot robot acquires surrounding environment information according to environment perception sensors such as a depth camera and a laser radar, and plans an optimal walking path according to a path planning algorithm. However, the path is only the moving track of the center of gravity of the trunk of the robot, and the robot needs to move from the current path point to the next path point through the swing of the legs, and the set of all the foot-falling points also forms the moving path of the robot. However, how to improve the walking stability of the legged robot in complex terrains is still a problem to be solved.
Disclosure of Invention
In order to solve the problems, the invention provides a local optimal foot-falling point selection method for a foot-type robot, which can effectively complete the planning of optimal foot-falling points in a local range and improve the terrain adaptability of the robot on the premise of ensuring the stability and safety of a machine body.
In order to achieve the above purpose, the present invention provides a local optimal foot-falling point selection method for a foot robot, comprising the following steps:
step 1: extracting the terrain feature of each candidate foot landing point, and generating a terrain drop footability evaluation function E (F) according to the terrain feature;
step 2: solving the single-leg motion space of the foot robot, and obtaining a first-level candidate foot-falling point set C according to the single-leg motion space and the terrain features obtained by the extraction in the step 1;
and step 3: establishing an objective function of a foot drop point plan, wherein the objective function of the foot drop point plan is constructed by a landform footfall evaluation function E (F), a support triangular area and a step length of a swing leg;
and 4, step 4: determining constraint conditions of an objective function of the foot-falling point plan, wherein the constraint conditions of the foot-falling evaluation function comprise foot end motion space constraint and swing leg step length constraint;
and 5: the first-level candidate foot-falling point set C obtained in the step 2 is larger than a set safe foot-falling threshold value EsafeRemoving the points, wherein the remaining points in the primary candidate foot-falling point set C form a secondary candidate foot-falling point set C1; and in the secondary candidate foot-falling point set C1, determining a local optimal foot-falling point according to the target function of the foot-falling point plan established in the step 3 and the constraint conditions of the target function of the foot-falling point plan determined in the step 4.
Further, step 1 comprises the steps of:
step 1.1, performing quadric surface fitting on a terrain area to be processed by using data contained in a moving window, extracting environmental features according to mathematical properties of a fitted surface to serve as footability evaluation information of a window center, and acquiring the terrain features of each candidate footability point in the whole environment when the moving window traverses any grid in a grid map, wherein a mathematical expression of the quadric surface is f (x, y) ═ a0+a1x+a2y+a3xy+a4x2+a2y2In the formula a0,a1,a2,a3,a4,a5All are surface coefficients; the expression of the terrain feature vector F of the candidate foot-landing point is as follows: f ═ F'x(x,y),f′y(x,y),f″xx(x,y),f″yy(x,y));
Step 1.2, evaluating candidate foot falling points and constructing a landform foot falling performance evaluation functionThe number of topographic features extracted by E (F) is counted E (F) ═ F-TIn the formula, oc (. alpha.) - (. alpha.)1,∝2,∝3,∝4) Is a terrain feature weight coefficient.
Further, step 2 comprises the steps of:
step 2.1, solving a motion space of a foot end of the robot, and then solving an intersection of the motion space of a single leg of the robot and a terrain to find an overlapping area of the motion space and the terrain, wherein the overlapping area is a foot falling area;
the expression of the foot end coordinate (x, y, z) of the single leg of the robot is as follows:
mixing L with2、L3And theta1、θ2、θ3The value range of (1) is substituted for the formula (1) to obtain the motion space of the foot end of the robot, and the thigh length L in the formula (1)2And shank length L3Are all known quantities, theta1Is the deflection angle of a single leg in the lateral space, theta2Is the angle between the body and the thigh, θ3Is the included angle between the thigh and the shank;
and 2.2, gradually substituting the candidate point coordinates (X, Y and Z) into a formula (1), judging whether the point can be used as a foot drop point by judging whether the obtained joint angle is in a design range, if so, bringing the point into a primary candidate foot drop point set C, and otherwise, discarding the point.
Further, in step 3, the landform footfall evaluation function is: e1(xi,yi)=F(xi,yi)∝T(ii) a The support triangle area function is: e2(xi,yi)=S(xi,yi) (ii) a Step function of the swing leg is L (x)i,yi)=xi-x3(ii) a Then, E3(xi,yi)=L(xi,yi)。
Further, in step 3, the area S (x) of the triangle is supportedi,yi) The expression of (a) is:
further, in step 4, the foot end motion space constraint is as follows: candidate drop foot point Pi(xi,yi) E.g. C, the swing leg step length constraint is: e3(xi,yi)≥0。
7. The method for selecting the local optimal foot-falling point of the foot-type robot according to claim 4, wherein in the step 5, the safe foot-falling threshold E is set according to the topographic characteristics of the working environment of the robotsafeIf E is1(xi,yi)≤EsafeIt holds that point (x)i,yi) Can fall to the foot and point (x)i,yi) Stored in the set C of second-level candidate footfall points1Performing the following steps; otherwise, rejecting point (x)i,yi);
The maximum area expression of the supporting triangle is: e2_max=max(S(xi,yi));
The maximum step size expression for the swing leg is: e3_max=max(L(xi,yi));
The evaluation function E (x) of the foot-falling point can be obtainedi,yi) The expression is as follows:
the local optimal foot-drop point evaluation algorithm can be obtained as follows:
compared with the prior art, the invention has at least the following beneficial technical effects:
the evaluation function of the foot drop point is provided by integrating multiple aspects, the evaluation function comprises the contents of the terrain foot drop performance evaluation function, the supporting triangular area and the step length of the swing leg, the foot drop performance of the robot is comprehensively evaluated, and the walking stability of the foot type robot in the complex terrain is improved.
The method for extracting the terrain information of the landing foot point is provided, the evaluation index of any contact point can be obtained according to the evaluation function, the index is used as the most intuitive judgment basis of the landing foot performance of the robot relative to the contact point, the terrain judgment efficiency of the robot is improved to a certain extent, and the movement real-time performance of the robot is enhanced.
The static stability margin is selected as a measuring method of the body stability, so that the body stability can be accurately measured, the calculation process is relatively simple, and the method is easy to realize.
Aiming at the problem that the influence of each factor needs to be comprehensively expressed in the foot-falling point planning, a foot-falling point selection strategy of a multi-constraint objective function is provided, the strategy can convert the complex multi-constraint foot-falling point planning into a single-target optimization problem with the minimum multi-constraint objective function value for solving a weight coefficient, and the foot-falling point selection efficiency is improved.
The idea of obtaining the environmental topographic features is that the data contained in the moving window is utilized to perform quadratic surface fitting on the area, and then the environmental features are extracted according to the mathematical properties of the fitted surface to be used as the footability evaluation information of the window center, so that the topographic features of each candidate footfall point in the whole environment can be obtained when the moving window traverses any grid in the grid map.
The invention provides a multi-constraint local optimal foot-falling point selection algorithm, which can effectively complete local optimal foot-falling point planning, better ensure the stability and safety of a machine body and improve the terrain adaptability of a robot.
Drawings
FIG. 1 is a topographical information analysis flow chart;
FIG. 2 is a moving window diagram;
FIG. 3a is a graph of environmental features before fitting;
FIG. 3b is a graph of the environment characteristics after the quadratic surface fitting;
FIG. 4a is a diagram of the motion of the foot end in the xoy plane;
FIG. 4b is a view of the foot end movement in the plane xoz;
FIG. 5 is a space diagram of candidate footfall points;
FIG. 6 is a graph of the results of a foot drop point planning experiment;
FIG. 7 is an analysis chart of a drop foot point planning experiment;
fig. 8 is a diagram of body stability margin during robot motion.
Detailed Description
In order to make the objects and technical solutions of the present invention clearer and easier to understand. The present invention will be described in further detail with reference to the following drawings and examples, wherein the specific examples are provided for illustrative purposes only and are not intended to limit the present invention.
In the description of the present invention, it is to be understood that the terms "center", "longitudinal", "lateral", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", and the like, indicate orientations or positional relationships based on those shown in the drawings, and are used only for convenience in describing the present invention and for simplicity in description, and do not indicate or imply that the referenced devices or elements must have a particular orientation, be constructed and operated in a particular orientation, and thus, are not to be construed as limiting the present invention. Furthermore, the terms "first", "second" and "first" are used for descriptive purposes only and are not to be construed as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature. In the description of the present invention, "a plurality" means two or more unless otherwise specified. In the description of the present invention, it should be noted that, unless otherwise explicitly specified or limited, the terms "mounted," "connected," and "connected" are to be construed broadly, e.g., as meaning either a fixed connection, a removable connection, or an integral connection; can be mechanically or electrically connected; they may be connected directly or indirectly through intervening media, or they may be interconnected between two elements. The specific meanings of the above terms in the present invention can be understood in specific cases to those skilled in the art.
Referring to fig. 1, a local optimal foot drop point selection method for a foot robot includes the following steps:
step 1: analysis of topographical information of foot landing sites
The process of analyzing the topographic information is mainly divided into two steps of topographic feature extraction and footability evaluation, and the process is shown in fig. 1.
Topographic feature extraction
The idea of obtaining the environmental topographic features is that a depth camera is used for collecting environmental information, a grid method is used for environment modeling, the foot end deformation amount, the foot end round cake structure size, the robot motion control precision and the computing capacity of the robot are integrated, the grid size of a grid map is finally selected to be 25x25mm, and a moving window method is used for limiting a value area, as shown in fig. 2. With NijTo represent each small grid, the environmental characteristics contained in the entire grid map are represented by the set omega, i.e.
Ω=∑Nij|(i∈[1,m],j∈[1,n]) (1)
The whole grid map is composed of barrier areas, free areas and uncertain areas, so that grid information can be expressed as
The data contained in the moving window is fitted to the quadratic surface of the region, as shown in fig. 3a and 3 b; and then extracting environmental features as footfall evaluation information of the center of the mobile window according to the mathematical properties of the fitted surface, so that when the mobile window traverses any grid in the grid map, the topographic features of each candidate footfall point in the whole environment can be obtained. Wherein the topographical features comprise: inclination and unevenness.
f(x,y)=a0+a1x+a2y+a3xy+a4x2+a5y2 (2);
In the formula (a)0,a1,a2,a3,a4,a5And) are surface coefficients.
Equation 2 is a quadratic mathematical expression known from analytic geometry knowledge: the gradient of a certain region can be described by a first-order partial derivative of a fitted quadric surface of the region, the concave-convex property can be characterized by a second-order partial derivative, and the combination of the first-order partial derivative and the second-order partial derivative is the curvature change condition of the region. Thus, the droppable at the center of the window is evaluated herein by calculating the following partial derivatives of the quadric resulting from the fitting of the moving window data.
By solving for the first derivative f 'of the quadric surface in the x direction'x(x, y), first derivative in the y-direction, f'y(x, y), second derivative in the x-direction, f ″)xx(x, y) and the second derivative in the y-direction, f ″)yyThe (x, y) four topographic features may reflect the environmental characteristics more comprehensively and finely, so the four parameters are recorded as a four-dimensional feature vector F to characterize the topographic features of any contact point.
F=(f′x(x,y),f′y(x,y),f″xx(x,y),f″yy(x,y)) (3);
(ii) evaluation of the touchdown behavior of the terrain
And evaluating candidate foot-falling points by adopting an algebraic analysis method, and comprehensively evaluating the extracted topographic features by constructing a topographic foot-falling evaluation function E (F), thereby generating a final evaluation index of the robot for selecting the optimal foot-falling point.
Let a landform footfall evaluation function be e (f), which is a linear combination of terrain features, and the mathematical expression is:
E(F)=F∝T (4);
wherein, oc (. alpha.) - (. alpha.)1,∝2,∝3,∝4) Weighting coefficients for the terrain features;
F=(f′x(x,y),f′y(x,y),f″xx(x,y),f″yy(x, y)) is a terrain feature vector.
The terrain feature weight coefficient in the formula (4) is obtained by learning of a Support Vector Machine (SVM), and the learning process of the SVM is completed through an LIBSVM software package in an MATLAB2017a environment. The evaluation index of any contact point can be obtained by the formula (4), and the evaluation index can be used as the most intuitive judgment basis of the touchdown property of the robot about the contact point, so that the terrain judgment efficiency of the robot is improved to a certain extent, and the motion real-time property of the robot is enhanced.
Step 2: single leg movement space of foot type robot
The foot falling point of the robot is required to be located in the motion range of a single leg of the robot, so that the motion space of the single leg of the robot is required to be solved, then the intersection of the motion space and the terrain is solved to find an overlapping area of the motion space and the terrain, the overlapping area can be regarded as a foot falling area, and the point in the foot falling area is a candidate point. The acquisition of the single-leg motion space of the robot is a positive kinematics calculation process of the robot, namely, the position of the foot end is solved according to the known rotation angles of the joints of the leg, and the embodiment utilizes a D-H model to solve the motion of the single leg.
The expression of the coordinates (x, y, z) of the foot end of the robot obtained by the D-H model and the parameters of the single leg joint of the robot is as follows:
thigh Length L in formula (5)2And shank length L3Are all known quantities, theta1Is the deflection angle of a single leg in the lateral space, theta2Is the angle between the body and the thigh, θ3The angle between thigh and shank is also known, so L is the angle of rotation of each joint2、L3And theta1、θ2And (3) substituting the value range of (5) into the equation (5) to obtain the motion space of the robot foot end, as shown in fig. 4a and 4b, fig. 4a is a foot end motion diagram on the xoy plane, fig. 4b is a foot end motion diagram on the xoz plane, and the two diagrams are collectively called an xyz space diagram.
In the process of screening the foot falling points, substituting candidate point coordinates (X, Y, Z) into an equation (5), judging whether the point can be used as the foot falling point by judging whether the obtained joint angle is in a design range, if so, including the candidate point into a candidate foot falling point set C, carrying out next evaluation, and otherwise, abandoning.
Step 3 organism stability
In the embodiment, the static stability margin is selected as the measuring method of the body stability, the method not only can accurately measure the body stability, but also has relatively simple calculation process and is easy to realize. The idea of the static stability margin is: calculating delta P from projection point of gravity center of robot on support plane to support triangle1P3PiThe minimum value of the respective side distances is set to be closer to the inscribed circle radius of the support polygon, and the stability margin of the robot is set to be larger. Wherein, three vertexes of the supporting triangle are: candidate drop foot point Pi(xi,yi) And the foot falling points of the other two legs which are not on the same side of the robot as the candidate foot falling points.
FIG. 5 is a schematic diagram of a process for selecting a foot drop point of No. 4 swing leg, in which a dotted rectangle is a first-level candidate foot drop point set C of No. 4 leg, and an "x" in the rectangle is a candidate foot drop point Pi(xi,yi). The stepping sequence of the robot selected in the embodiment is 4-2-3-1 with the optimal stability margin, and the stepping sequence can enable the center of gravity of the robot to move forwards all the time in the advancing process of the robot. Therefore, when selecting the foot-drop point, leg No. 4 should make the robot move in the next step, i.e. when leg No. 2 is used as the swing leg, the stability margin as large as possible should be kept, i.e. the area of the supporting triangle in this state is as large as possible.
As can be seen from FIG. 5, the three vertices of the support triangle are the coordinates P of leg No. 11(x1,y1) Coordinate P of leg No. 13(x3,y3) And foot drop point P of leg No. 4i(xi,yi) Supporting triangle delta P1P3PiArea of (d) is denoted as S (x)i,yi) Due to P1(x1,y1),P3(x3,y3) The coordinates of the two vertices are known, so S (x) can be obtainedi,yi) The expression of (a) is:
p in formula (6)iThe coordinate of (2) is a variable, and the value range of the variable is the motion space of the No. 4 leg. The stability margin values of all candidate drop foot points of leg No. 4 can be obtained by equation (6). Then, in order to maximize the stability margin of the robot when selecting the foot drop point for leg No. 4, i.e. to make the support triangle Δ P1P3PiThe area of (C) is the maximum, that is, a point in the primary candidate foot-drop point set C is found so that the point satisfies formula (7), and the point is the most stable foot-drop point:
SMAX=max(S(xi,yi)) 1∈[1,2,...,n] (7)
where n is the number of candidate footfall points for leg number 4.
The embodiment provides a multi-constraint objective function drop point selection strategy aiming at the problem that the drop point planning needs to comprehensively express the influence of each factor, and the strategy can convert the complex multi-constraint drop point planning into a single-objective optimization problem which is used for solving the weight coefficient and has the minimum multi-constraint objective function value.
And 4, step 4: objective function for footfall point planning
The objective function constructed in the present embodiment includes three parts, namely, a landform footability evaluation function, a support triangle area, and a step length of a swing leg, and the specific contents of each part are described below.
Landform footfall evaluation function
Let Pi(xi,yi) For the ith point in the primary candidate foot-falling point set C, order E1(xi,yi) The landform footability evaluation function value of the ith point is expressed as follows:
F(xi,yi)=(f′x(xi,yi),f′y(xi,yi),f″xx(xi,yi),f″yy(xi,yi)) 1∈[1,2,...,n] (8)
E1(xi,yi)=F(xi,yi)∝T (9)
in the formula, F (x)i,yi) Is a topographic feature vector, oc ═ (. alpha.), (1,∝2,∝3,∝4) Is a terrain feature weight coefficient.
(ii) supporting triangular area function
As can be seen from fig. 5, the area of the supporting triangle formed by the projection points of the three supporting legs on the projection plane can be derived from equation (6), and the expression is as follows:
where (x ', y') and (x ', y') are projected point coordinates of known support legs.
E2(xi,yi)=S(xi,yi) (11)
In the above formula, E2(xi,yi) The function value is evaluated for the support triangle area at the ith point.
(iii) step function of the swing leg
The step size of the swing leg should be as large as possible, i.e. as far as possible in the target direction, as allowed by the swing leg kinematics, so that the swing leg can obtain as large a kinematic margin as possible. As shown in fig. 5, when the robot advances in the X direction, the step size of the swing leg in the advancing direction is as shown in equation (12).
L(xi,yi)=xi-xs i∈[1,2,...,n] (12)
In the formula, xsIs the X-axis coordinate of the projected point of the swing leg.
E3(xi,yi)=L(xi,yi) (13)
In equations 12 and 13, E3(xi,yi) For the evaluation function value of the step size, L (x)i,yi) Is a step function.
And 5: constraints of an objective function for a footfall point plan
The foot-falling point selection constraint conditions mainly comprise two conditions, namely a foot end motion space and a leg swinging step length. All the drop foot points can become candidate drop foot points only after the screening of the two constraints.
Foot end motion space constraint
Foot drop point PiMust be located within the range of motion of the foot end of the swing leg, i.e., PiAs elements in the set C of first-level candidate footfall points, i.e.
Pi(xi,yi)∈C (14)
(ii) swing leg step size constraint
In order to improve the energy utilization efficiency of the robot, it is necessary to move the center of gravity of the robot toward the target point all the time, and therefore, it is desirable that the step length of the swing leg is not less than 0, that is, it is desirable that the step length of the swing leg is not less than 0 during the movement of the robot
E3(xi,yi)≥0 (15)
Step 6: selection algorithm of local optimal foot drop point
And selecting the local optimal foot drop point according to the evaluation function and the constraint condition. Firstly, any point (x) in the candidate foot-falling point set C is calculated by utilizing a landform foot-falling evaluation functioni,yi) Evaluation function value E of1(xi,yi) Setting a safe footfall threshold value E according to the topographic characteristics of the working environment of the robotsafeIf E is1(xi,yi) If the formula 16 is satisfied, the point is considered to be sufficient to fall, and is stored in the secondary candidate foot-falling point set C1Performing the following steps; otherwise, the feet can not fall and the feet can be removed.
E1(xi,yi)≤Esafe (16)
In order to better reflect the influence of each evaluation function on the foot-falling pointIt needs to be normalized first. According to the formula (11) and a secondary candidate foot-falling point set C1The maximum area of the supporting triangle can be obtained, as shown in formula (17).
E2_max=max(S(xi,yi)) 1∈[1,2,...,n] (17)
Similarly, the second-level candidate foot-falling point set C is obtained according to the formula (11)1The maximum step size of the swing leg can be obtained as shown in equation (18).
E3_max=max(L(xi,yi)) 1∈[1,2,...,n] (18)
Then, based on the equations (17) and (18), an evaluation function of the footfall point can be obtained, as shown in equation (19).
In the formula: e1_max=Esafe,(β1,β2,β3) The weight coefficients for each objective function. Beta is a1,β2,β3∈[0,1]
Finally, a local optimal footfall evaluation algorithm can be obtained according to the equations (15) to (19), as shown in equation (20).
Set C according to secondary candidate foot-falling points1And (20) obtaining a comprehensive evaluation function value of each candidate foot-falling point, and then determining the point with the minimum function value in all the candidate points as a local optimal foot-falling point, namely the target foot-falling position of the next step of the swing leg.
A second part:
in order to verify the feasibility of the above-mentioned footfall point planning algorithm, a set of simulation experiments was designed. Firstly, a feasible path is drawn by using a path planning method, and finally a local optimal foot-falling point sequence of the robot is generated according to the obtained path and a local optimal foot-falling point selection strategy, a simulation experiment result is shown in fig. 7, a rectangular frame in the figure is a circumscribed rectangle of an obstacle, an equipotential line taking the center of the rectangular frame as the circle center represents the obstacle and the difficulty degree of landform foot-falling performance around the obstacle, a curve between two rectangular frames in the figure is the planned feasible path, and the 'marks' distributed on two sides of the feasible path represent the local optimal foot-falling point of the robot.
The robot is decomposed into multiple steps from the starting point to the target point, and as can be seen from fig. 7, the local optimal foot-falling point of each step constitutes the movement track of the robot. When the robot walks according to the foot-falling points, the stability and the safety of the robot must be ensured at all times, and as shown in fig. 6, a support triangle formed by support legs always contains a gravity center projection point of the robot inside. In order to further quantify the motion stability of the robot, the simulation experiment also gives the result of the change of the stability margin of the robot in the motion process, as shown in fig. 8. The body stability margin of the robot in the moving process must be not less than the minimum body stability margin S determined according to the self structural characteristics and the moving characteristics of the robotminAs can be seen from fig. 8, when the robot moves according to the selected local optimal foot-drop point, the stability of the robot itself can be effectively ensured, and in addition, since the robot does not collide with an obstacle in the process, the safety of the robot is better. In conclusion, the foot drop point planning method provided by the invention has the capability of enabling the robot to stably and safely reach the destination according to the planned foot drop point in the non-structural environment through simulation and reality verification.
The above-mentioned contents are only for illustrating the technical idea of the present invention, and the protection scope of the present invention is not limited thereby, and any modification made on the basis of the technical idea of the present invention falls within the protection scope of the claims of the present invention.
Claims (7)
1. A local optimal foot drop point selection method for a foot robot is characterized by comprising the following steps:
step 1: extracting the terrain feature of each candidate foot landing point, and generating a terrain drop footability evaluation function E (F) according to the terrain feature;
step 2: solving the single-leg motion space of the foot robot, and obtaining a first-level candidate foot-falling point set C according to the single-leg motion space and the terrain features obtained by the extraction in the step 1;
and step 3: establishing an objective function of a foot drop point plan, wherein the objective function of the foot drop point plan is constructed by a landform footfall evaluation function E (F), a support triangular area and a step length of a swing leg;
and 4, step 4: determining constraint conditions of an objective function of the foot-falling point plan, wherein the constraint conditions of the foot-falling evaluation function comprise foot end motion space constraint and swing leg step length constraint;
and 5: the first-level candidate foot-falling point set C obtained in the step 2 is larger than a set safe foot-falling threshold value EsafeRemoving the points, wherein the remaining points in the primary candidate foot-falling point set C form a secondary candidate foot-falling point set C1; and in the secondary candidate foot-falling point set C1, determining a local optimal foot-falling point according to the target function of the foot-falling point plan established in the step 3 and the constraint conditions of the target function of the foot-falling point plan determined in the step 4.
2. The local optimal foot-drop point selection method for the foot robot according to claim 1, wherein the step 1 comprises the following steps:
step 1.1, performing quadric surface fitting on a terrain area to be processed by using data contained in a moving window, extracting environmental features according to mathematical properties of a fitted surface to serve as footability evaluation information of a window center, and acquiring the terrain features of each candidate footability point in the whole environment when the moving window traverses any grid in a grid map, wherein a mathematical expression of the quadric surface is f (x, y) ═ a0+a1x+a2y+a3xy+a4x2+a5y2In the formula a0,a1,a2,a3,a4,a5All are surface coefficients; the expression of the terrain feature vector F of the candidate foot-landing point is as follows: f ═ F'x(x,y),f′y(x,y),f″xx(x,y),f″yy(x,y));
Step 1.2, evaluating candidate footfall points, and comprehensively evaluating the topographic features extracted by constructing a topographic footfall evaluation function e (F) ═ F-TIn the formula, oc (. alpha.) - (. alpha.)1,∝2,∝3,∝4) Is a terrain feature weight coefficient.
3. The local optimal foot-drop point selection method for the foot robot as claimed in claim 1, wherein the step 2 comprises the following steps:
step 2.1, solving a motion space of a foot end of the robot, and then solving an intersection of the motion space of a single leg of the robot and a terrain to find an overlapping area of the motion space and the terrain, wherein the overlapping area is a foot falling area;
the expression of the foot end coordinate (x, y, z) of the single leg of the robot is as follows:
mixing L with2、L3And theta1、θ2、θ3The value range of (1) is substituted for the formula (1) to obtain the motion space of the foot end of the robot, and the thigh length L in the formula (1)2And shank length L3Are all known quantities, theta1Is the deflection angle of a single leg in the lateral space, theta2Is the angle between the body and the thigh, θ3Is the included angle between the thigh and the shank;
and 2.2, gradually substituting the candidate point coordinates (X, Y and Z) into a formula (1), judging whether the point can be used as a foot drop point by judging whether the obtained joint angle is in a design range, if so, bringing the point into a primary candidate foot drop point set C, and otherwise, discarding the point.
4. The method for selecting the local optimal foot-drop point of the foot robot according to claim 1, wherein in the step 3, the landform footability evaluation function is as follows: e1(xi,yi)=F(xi,yi)∝T(ii) a The support triangle area function is: e2(xi,yi)=S(xi,yi) (ii) a Step function of the swing leg is L (x)i,yi)=xi-xs(ii) a Then, E3(xi,yi)=L(xi,yi)。
5. The method according to claim 4, wherein in step 3, the area S (x) of the supporting triangle is selectedi,yi) The expression of (a) is:
6. the method for selecting the local optimal foot-falling point of the legged robot according to claim 4, wherein in the step 4, the spatial constraint of the foot end motion is as follows: candidate drop foot point Pi(xi,yi) E.g. C, the swing leg step length constraint is: e3(xi,yi)≥0。
7. The method for selecting the local optimal foot-falling point of the foot-type robot according to claim 4, wherein in the step 5, the safe foot-falling threshold E is set according to the topographic characteristics of the working environment of the robotsafeIf E is1(xi,yi)≤EsafeIt holds that point (x)i,yi) Can fall to the foot and point (x)i,yi) Stored in the set C of second-level candidate footfall points1Performing the following steps; otherwise, rejecting point (x)i,yi);
The maximum area expression of the supporting triangle is: e2_max=max(S(xi,yi));
The maximum step size expression for the swing leg is: e3_max=max(L(xi,yi));
Can obtain the foot fallingEvaluation function E (x) of pointsi,yi) The expression is as follows:
the local optimal foot-drop point evaluation algorithm can be obtained as follows:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910926924.7A CN110597267B (en) | 2019-09-27 | 2019-09-27 | Local optimal foot drop point selection method for foot type robot |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910926924.7A CN110597267B (en) | 2019-09-27 | 2019-09-27 | Local optimal foot drop point selection method for foot type robot |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110597267A true CN110597267A (en) | 2019-12-20 |
CN110597267B CN110597267B (en) | 2023-01-10 |
Family
ID=68864309
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910926924.7A Active CN110597267B (en) | 2019-09-27 | 2019-09-27 | Local optimal foot drop point selection method for foot type robot |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110597267B (en) |
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2022048266A1 (en) * | 2020-09-07 | 2022-03-10 | 腾讯科技(深圳)有限公司 | Method, apparatus and device for controlling movement of legged robot, and medium |
CN114633825A (en) * | 2022-05-16 | 2022-06-17 | 深圳鹏行智能研究有限公司 | Movement control method, legged robot, and computer-readable storage medium |
CN114683289A (en) * | 2022-05-09 | 2022-07-01 | 中兵智能创新研究院有限公司 | Attitude pre-adaptive control method for quadruped robot |
CN114859947A (en) * | 2022-03-21 | 2022-08-05 | 哈尔滨理工大学 | Fault-tolerant gait planning method of hexapod robot based on rollover performance index analysis |
CN115840453A (en) * | 2023-02-10 | 2023-03-24 | 季华实验室 | Adaptive foot end obstacle avoidance method and device for quadruped robot, electronic equipment and medium |
CN115857328A (en) * | 2022-11-07 | 2023-03-28 | 哈尔滨理工大学 | Hexapod robot fault-tolerant gait foot-falling point planning method based on cooperative optimization of moment balance point and motion stability |
CN116775849A (en) * | 2023-08-23 | 2023-09-19 | 成都运荔枝科技有限公司 | On-line problem processing system and method |
Citations (17)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2003340763A (en) * | 2002-05-24 | 2003-12-02 | Mitsubishi Heavy Ind Ltd | Biped walking robot step elevating/lowering method and biped walking robot |
EP1642687A1 (en) * | 2003-06-27 | 2006-04-05 | HONDA MOTOR CO., Ltd. | Control device for legged mobile robot |
US20070241713A1 (en) * | 2004-01-28 | 2007-10-18 | Takashi Yamamoto | Legged Robot and Legged Robot Walking Control Method |
JP2008183701A (en) * | 2007-01-30 | 2008-08-14 | Kochi Univ Of Technology | Structure model of outdoor horse-riding robot for health promotion and its walking method |
JP2011059071A (en) * | 2009-09-14 | 2011-03-24 | Univ Of Electro-Communications | Device and method for recognition of movement environment |
CN103085070A (en) * | 2013-01-15 | 2013-05-08 | 上海交通大学 | Quadruped robot motion planning method for facing complex terrain |
CN104267720A (en) * | 2014-10-24 | 2015-01-07 | 山东大学 | Method for generating free gaits for four-footed bionic robot |
JP2015175664A (en) * | 2014-03-13 | 2015-10-05 | 三菱重工業株式会社 | Landform determination device, and leg type mobile robot |
US9387896B1 (en) * | 2014-08-25 | 2016-07-12 | Google Inc. | Slip avoidance |
US9586316B1 (en) * | 2015-09-15 | 2017-03-07 | Google Inc. | Determination of robotic step path |
US9594377B1 (en) * | 2015-05-12 | 2017-03-14 | Google Inc. | Auto-height swing adjustment |
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 |
CN106886155A (en) * | 2017-04-28 | 2017-06-23 | 齐鲁工业大学 | A kind of quadruped robot control method of motion trace based on PSO PD neutral nets |
KR20180052457A (en) * | 2016-11-10 | 2018-05-18 | 한국수력원자력 주식회사 | Multiplex walking robot |
CN108333931A (en) * | 2018-01-25 | 2018-07-27 | 北京理工大学 | A kind of quadruped robot double-layer structure gait planning method towards rugged topography |
CN108614421A (en) * | 2018-05-17 | 2018-10-02 | 长安大学 | A kind of quadruped robot motion control method based on central pattern generator (cpg) |
CN109696909A (en) * | 2017-10-23 | 2019-04-30 | 深圳市优必选科技有限公司 | Foot type robot path planning method and device |
-
2019
- 2019-09-27 CN CN201910926924.7A patent/CN110597267B/en active Active
Patent Citations (17)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2003340763A (en) * | 2002-05-24 | 2003-12-02 | Mitsubishi Heavy Ind Ltd | Biped walking robot step elevating/lowering method and biped walking robot |
EP1642687A1 (en) * | 2003-06-27 | 2006-04-05 | HONDA MOTOR CO., Ltd. | Control device for legged mobile robot |
US20070241713A1 (en) * | 2004-01-28 | 2007-10-18 | Takashi Yamamoto | Legged Robot and Legged Robot Walking Control Method |
JP2008183701A (en) * | 2007-01-30 | 2008-08-14 | Kochi Univ Of Technology | Structure model of outdoor horse-riding robot for health promotion and its walking method |
JP2011059071A (en) * | 2009-09-14 | 2011-03-24 | Univ Of Electro-Communications | Device and method for recognition of movement environment |
CN103085070A (en) * | 2013-01-15 | 2013-05-08 | 上海交通大学 | Quadruped robot motion planning method for facing complex terrain |
JP2015175664A (en) * | 2014-03-13 | 2015-10-05 | 三菱重工業株式会社 | Landform determination device, and leg type mobile robot |
US9387896B1 (en) * | 2014-08-25 | 2016-07-12 | Google Inc. | Slip avoidance |
CN104267720A (en) * | 2014-10-24 | 2015-01-07 | 山东大学 | Method for generating free gaits for four-footed bionic robot |
US9594377B1 (en) * | 2015-05-12 | 2017-03-14 | Google Inc. | Auto-height swing adjustment |
US9586316B1 (en) * | 2015-09-15 | 2017-03-07 | Google Inc. | Determination of robotic step path |
KR20180052457A (en) * | 2016-11-10 | 2018-05-18 | 한국수력원자력 주식회사 | Multiplex walking robot |
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 |
CN106886155A (en) * | 2017-04-28 | 2017-06-23 | 齐鲁工业大学 | A kind of quadruped robot control method of motion trace based on PSO PD neutral nets |
CN109696909A (en) * | 2017-10-23 | 2019-04-30 | 深圳市优必选科技有限公司 | Foot type robot path planning method and device |
CN108333931A (en) * | 2018-01-25 | 2018-07-27 | 北京理工大学 | A kind of quadruped robot double-layer structure gait planning method towards rugged topography |
CN108614421A (en) * | 2018-05-17 | 2018-10-02 | 长安大学 | A kind of quadruped robot motion control method based on central pattern generator (cpg) |
Non-Patent Citations (2)
Title |
---|
王瑞霄,等: "六足机器人爬坡性能分析及步态选择", 《现代制造技术与装备》 * |
陈杰等: "仿生六足机器人面向非结构化地形的落足点选取策略", 《江南大学学报(自然科学版)》 * |
Cited By (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2022048266A1 (en) * | 2020-09-07 | 2022-03-10 | 腾讯科技(深圳)有限公司 | Method, apparatus and device for controlling movement of legged robot, and medium |
CN114859947A (en) * | 2022-03-21 | 2022-08-05 | 哈尔滨理工大学 | Fault-tolerant gait planning method of hexapod robot based on rollover performance index analysis |
CN114859947B (en) * | 2022-03-21 | 2024-04-05 | 哈尔滨理工大学 | Six-foot robot fault-tolerant gait planning method based on tipping performance index analysis |
CN114683289A (en) * | 2022-05-09 | 2022-07-01 | 中兵智能创新研究院有限公司 | Attitude pre-adaptive control method for quadruped robot |
CN114633825A (en) * | 2022-05-16 | 2022-06-17 | 深圳鹏行智能研究有限公司 | Movement control method, legged robot, and computer-readable storage medium |
CN115857328A (en) * | 2022-11-07 | 2023-03-28 | 哈尔滨理工大学 | Hexapod robot fault-tolerant gait foot-falling point planning method based on cooperative optimization of moment balance point and motion stability |
CN115857328B (en) * | 2022-11-07 | 2023-10-17 | 哈尔滨理工大学 | Six-foot robot fault-tolerant gait foot drop point planning method based on cooperative optimization of moment balance points and motion stability |
CN115840453A (en) * | 2023-02-10 | 2023-03-24 | 季华实验室 | Adaptive foot end obstacle avoidance method and device for quadruped robot, electronic equipment and medium |
CN116775849A (en) * | 2023-08-23 | 2023-09-19 | 成都运荔枝科技有限公司 | On-line problem processing system and method |
CN116775849B (en) * | 2023-08-23 | 2023-10-24 | 成都运荔枝科技有限公司 | On-line problem processing system and method |
Also Published As
Publication number | Publication date |
---|---|
CN110597267B (en) | 2023-01-10 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110597267B (en) | Local optimal foot drop point selection method for foot type robot | |
CN106599108A (en) | Method for constructing multi-mode environmental map in three-dimensional environment | |
CN105843222B (en) | A kind of six-wheel/leg robot complex motion paths planing method | |
KR20220083666A (en) | Constrained Mobility Mapping | |
Menna et al. | Real-time autonomous 3D navigation for tracked vehicles in rescue environments | |
Jian et al. | Putn: A plane-fitting based uneven terrain navigation framework | |
CN110361026A (en) | A kind of anthropomorphic robot paths planning method based on 3D point cloud | |
Zhang et al. | Fast active aerial exploration for traversable path finding of ground robots in unknown environments | |
de Waard et al. | Analysis of flat terrain for the atlas robot | |
Toupet et al. | A ros-based simulator for testing the enhanced autonomous navigation of the mars 2020 rover | |
Hardouin et al. | Surface-driven Next-Best-View planning for exploration of large-scale 3D environments | |
CN113096190A (en) | Omnidirectional mobile robot navigation method based on visual map building | |
Haddeler et al. | Explore bravely: Wheeled-legged robots traverse in unknown rough environment | |
Wang et al. | Towards efficient trajectory generation for ground robots beyond 2d environment | |
CN117289301A (en) | Air-ground unmanned platform collaborative path planning method under unknown off-road scene | |
Cafaro et al. | Terrain traversability in rescue environments | |
Jordan et al. | Real-time model based path planning for wheeled vehicles | |
Zhang et al. | Supervoxel plane segmentation and multi-contact motion generation for humanoid stair climbing | |
Wettergreen et al. | Developing a framework for reliable autonomous surface mobility | |
Walas et al. | Supporting locomotive functions of a six-legged walking robot | |
CN115619953A (en) | Rugged terrain-oriented mobile robot terrain mapping method and system | |
Wang et al. | Aerial-Ground Collaborative Continuous Risk Mapping for Autonomous Driving of Unmanned Ground Vehicle in Off-Road Environments | |
CN115129059A (en) | Continuous potential field avoiding construction method of assembly adapting to real shape | |
Guo et al. | Autonomous legged robot navigation with environment awareness system in complex outdoor environments | |
Gassmann et al. | Real-time 3d map building for local navigation of a walking robot in unstructured terrain |
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 |