CN111605642B - Free fault-tolerant gait planning method, device and storage medium for hexapod robot - Google Patents

Abstract

The invention provides a free fault-tolerant gait planning method, device and storage medium of a hexapod robot. The method includes the following steps: acquiring state information and ground landing point information of the hexapod robot; determining the swing of the hexapod robot according to the state information. The combination of legs and the movement step of the body's center of gravity; combined with the movement step of the body's center of gravity and state information, respectively determine the trajectory of the body's center of gravity and the landing area of each swinging leg of the hexapod robot; The target footfall point in the foot area; for the swinging leg that has no target footfall point or cannot land the foot, control the swinging leg to hover at a predetermined position; for the swinging leg that can land on the corresponding target footfall point, according to the status information and the target foot landing point to determine the foot end trajectory of the swinging leg. The technical solution of the present invention can plan the gait of the hexapod robot when the swinging leg has no landing point or cannot land, and improves the ability of the hexapod robot to pass through the terrain with sparse landing points.

Description

Free fault-tolerant gait planning method, device and storage medium for hexapod robot

technical field

The invention relates to the technical field of robot control, in particular to a free fault-tolerant gait planning method, device and storage medium for a hexapod robot.

Background technique

Since the wheeled robot needs to move on a continuous ground, and the footed robot can choose the foothold to move in the discrete foothold terrain, when facing the complex field environment, the footed robot has better performance. capacity. Among many legged robots, hexapod robots stand out due to their strong environmental adaptability and high fault tolerance. They are widely used in industrial, The fields of aerospace, military and disaster relief have broad prospects for development.

Gait planning refers to planning the phase relationship between the legs of a footed robot when it walks from one position to another. In the existing gait planning methods, when planning the gait of a hexapod robot, it usually only considers the situation when the hexapod robot passes through the terrain with dense landing points, and each leg of the hexapod robot can land on the foot during the movement process. on the foothold. However, when faced with terrain with sparse landing points or the legs of the hexapod robot fail, the legs of the hexapod robot may have no landing points or fail to land in the motion cycle. At this time, the existing technology is not very good. solution.

SUMMARY OF THE INVENTION

The problem solved by the present invention is how to plan the gait of the hexapod robot when the legs of the hexapod robot are faulty or have no footholds, so as to improve the ability of the hexapod robot to pass through the terrain with sparse footholds.

To solve the above problems, the present invention provides a free fault-tolerant gait planning method, device and storage medium for a hexapod robot.

In a first aspect, the present invention provides a free fault-tolerant gait planning method for a hexapod robot, the method comprising the following steps:

Obtain the status information and ground landing point information of the hexapod robot.

According to the state information, a combination of swinging legs and a moving step length of the center of gravity of the hexapod robot are determined, and the combination of swinging legs includes zero or at least one swinging leg.

The center of gravity trajectory of the hexapod robot and the foot landing area of each of the swing legs are respectively determined in combination with the moving step length of the center of gravity of the body and the state information.

The target footfall point of each swing leg in the corresponding footfall area is determined according to the ground footfall point information, and whether each swing leg can land on the corresponding footfall area is determined according to the state information Target landing point.

For the swinging legs that do not have the target footing point or cannot land, control the swinging leg to hover at a predetermined position; for the swinging legs that can land on the corresponding target footing point, according to the The state information and the target foot landing point determine the foot end trajectory of the swing leg.

In a second aspect, the present invention provides a free fault-tolerant gait planning device for a hexapod robot, including:

The acquisition module is used to acquire the status information and ground landing point information of the hexapod robot.

The first processing module is configured to determine, according to the state information, a combination of swinging legs and a moving step length of the center of gravity of the hexapod robot, where the combination of swinging legs includes zero or at least one swinging leg.

The first planning module is configured to combine the movement step length of the body's center of gravity and the state information to respectively determine the trajectory of the body's center of gravity of the hexapod robot and the landing area of each of the swing legs.

The second processing module is configured to determine the target footfall point of each of the swing legs in the corresponding footfall area according to the ground footfall point information, and to determine whether each of the swing legs can landing on the corresponding target landing point.

The second planning module is configured to control the swinging leg to hover at a predetermined position for the swinging leg that does not have the target footing point or cannot land; for the swinging leg that can land at the corresponding target footing point For the swing leg, the foot end trajectory of the swing leg is determined according to the state information and the target foot landing point.

In a third aspect, the present invention provides a free fault-tolerant gait planning device for a hexapod robot, including a memory and a processor.

The memory is used to store computer programs.

The processor, when executing the computer program, implements the above-mentioned free fault-tolerant gait planning method for a hexapod robot.

In a fourth aspect, the present invention provides a computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, the above-mentioned free fault-tolerant gait of the hexapod robot is realized planning method.

The free and fault-tolerant gait planning method, device and storage medium of the hexapod robot of the present invention have the following beneficial effects: firstly, according to the acquired state information of the hexapod robot and the ground landing point information, the moving step length of the swinging leg and the center of gravity of the body is determined, and the swinging step length is determined. The legs of the hexapod robot other than the legs are the supporting legs; then plan the trajectory of the body's center of gravity according to the body's center of gravity moving step and state information, determine the target landing point of each swinging leg, and determine whether each swinging leg can land on the foot. The corresponding target falls on the foothold. For a swinging leg that has no landing point or is unable to land, controlling the swinging leg to hover at a predetermined position can reduce the interference between the swinging legs, wherein the swinging leg that cannot land the foot includes the swinging leg that cannot move due to a fault , no footfall point includes the situation that there is no footfall point in the footfall area of the swinging leg due to bad environment; for the swinging leg that can land on the corresponding target footfall point, plan the foot end trajectory of the swinging leg. Finally, control the body of the hexapod robot to move to the next position according to the trajectory of the body's center of gravity, and control each swinging leg that can land on the corresponding target landing point to move according to the trajectory of the foot end, so as to complete the hexapod robot in one motion cycle. The motion process of the hexapod is repeated, and after several motion cycles, the hexapod robot can reach the target position. In the technical solution of the present application, the foot-end trajectory is planned for the swinging leg that can land on the target footing point, and the swinging leg that has no target footing point or cannot be landed is controlled to hover at a predetermined position, The gait planning of the hexapod robot is completed, and the fault-tolerant processing of the leg errors of the hexapod robot is realized, which can improve the traffic ability of the hexapod robot when facing the terrain with sparse landing points.

Description of drawings

1 is a schematic flowchart of a free fault-tolerant gait planning method for a hexapod robot according to an embodiment of the present invention;

2 is a schematic diagram of the trajectory of the center of gravity of an airframe according to an embodiment of the present invention;

3 is a schematic diagram of a swinging leg dropping process according to an embodiment of the present invention;

4 is a schematic diagram of a swinging leg dropping process according to another embodiment of the present invention;

5 is a schematic diagram of a swinging leg dropping process according to still another embodiment of the present invention;

6 is a schematic diagram of a foot end trajectory of a swing leg according to an embodiment of the present invention;

Fig. 7 is the state schematic diagram when the swing leg slips or falls in the embodiment of the present invention;

FIG. 8 is a schematic diagram of a process of attempting to disengage the swing leg according to an embodiment of the present invention;

9 is a phase sequence diagram of a leg support state of a hexapod robot according to an embodiment of the present invention;

10 is a schematic diagram of a simulation of a hexapod robot passing through a foothold area according to an embodiment of the present invention;

FIG. 11 is a schematic top view of the hexapod robot in the partial state of FIG. 10;

12 is a schematic structural diagram of a free fault-tolerant gait planning device for a hexapod robot according to an embodiment of the present invention.

Detailed ways

In order to make the above objects, features and advantages of the present invention more clearly understood, the specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

It should be noted that the coordinate system X _W Y _W Z _W in this paper is the body coordinate system of the hexapod robot. The direction of the arrow in the drawings is the direction of movement of the body of the hexapod robot. At the same time, it should be noted that the terms "first" and "second" in the description and claims of the present invention and the above drawings are used to distinguish similar objects, but not necessarily to describe a particular order or sequence. In the description of the present invention, "plurality" means at least two, such as two, three, etc., unless otherwise expressly and specifically defined. It is to be understood that the data so used may be interchanged under appropriate circumstances such that the embodiments of the invention described herein can be practiced in sequences other than those illustrated or described herein.

As shown in FIG. 1 , a free fault-tolerant gait planning method for a hexapod robot provided by an embodiment of the present invention includes the following steps:

100. Obtain state information and ground landing point information of the hexapod robot.

Specifically, the state information may include body posture information, leg posture information, and fault status information of the hexapod robot, and the ground landing point information may be obtained by taking a picture of the ground with the camera of the hexapod robot.

200. Determine, according to the state information, a swing leg combination of the hexapod robot and a movement step length of the body's center of gravity, where the swing leg combination includes zero or at least one swing leg.

Specifically, the support state of the hexapod robot may be determined in a preset support state table according to the state information, and the support state corresponds to the combination of support legs and swing legs of the hexapod robot. When the hexapod robot moves, the body is supported by each supporting leg in the supporting leg assembly, and each swinging leg in the swinging leg assembly swings to the target position and falls. The hexapod robot may also be unable to determine the combination of swing legs in a certain motion cycle, that is, the number of swing legs is zero and the number of support legs is six. The state information at the position determines the swing leg combination of the hexapod robot. If the number of swing legs in the swing leg combination is still zero, the hexapod robot stops moving forward. If the swing leg combination includes at least one swing leg, then the hexapod robot stops moving forward. Continue planning the foot-end trajectory of each swinging leg.

300. Determine a center of gravity trajectory of the hexapod robot and a foot drop area of each of the swing legs in combination with the moving step length of the center of gravity of the body and the state information.

Specifically, the foot drop area of the swinging leg may be the reachable working area of the foot end of the swinging leg, and the reachable area of the swinging leg is limited by the physical parameters of the swinging leg. During the movement of the hexapod robot, when the swinging leg swings to the target position, the center of gravity of the robot should also move forward accordingly. Planning the trajectory of the center of gravity of the body, and controlling the hexapod robot to move according to the trajectory of the center of gravity of the body, can make the hexapod robot maintain static stability during the movement process.

400. Determine the target landing point of each swing leg in the corresponding foot landing area according to the ground foot landing point information, and determine whether each swing leg can land on the corresponding foot landing area according to the state information. the target landing point.

Specifically, the swinging leg can accurately land on the target foot-falling point only when the target foot-falling point is satisfied and the foot-falling is possible at the same time.

500. Control the swing leg to hover at a predetermined position for the swing leg that does not have the target foot drop point or cannot land the foot; for the swing leg that can land on the corresponding target foot drop point, The foot end trajectory of the swing leg is determined according to the state information and the target foot landing point.

Specifically, the swinging legs that cannot fall may include a swinging leg with a fault, etc., and the predetermined position may be a designated position close to the body of the hexapod robot, which can reduce the interference between the swinging legs.

In this embodiment, firstly, according to the acquired state information of the hexapod robot and the ground landing point information, the step lengths of the swing legs and the center of gravity of the body are determined, and the legs of the hexapod robot other than the swing legs are the support legs; The step length and state information are used to plan the trajectory of the body's center of gravity, and determine the target landing point of each swinging leg, and at the same time determine whether each swinging leg can land on the corresponding target landing point. For a swinging leg that does not have a footing point or cannot land, controlling the swinging leg to hover at a predetermined position can reduce the interference between the swinging legs, wherein the swinging leg that cannot land the foot includes a malfunctioning swinging leg, etc.; For the swinging leg that can land on the corresponding target footing point, the trajectory of the foot end of the swinging leg is planned. Finally, control the body of the hexapod robot to move to the next position according to the trajectory of the body's center of gravity, and control each swinging leg that can land on the corresponding target landing point to move according to the trajectory of the foot end, so as to complete the hexapod robot in one motion cycle. The motion process of the hexapod is repeated, and after several motion cycles, the hexapod robot can reach the target position. In the technical solution of the present application, the foot-end trajectory is planned for the swinging leg that can land on the target footing point, and the swinging leg that has no target footing point or cannot be landed is controlled to hover at a predetermined position, The gait planning of the hexapod robot is completed, and the fault-tolerant processing of the leg errors of the hexapod robot is realized, which can improve the traffic ability of the hexapod robot when facing the terrain with sparse landing points.

Preferably, the state information includes fault information of the hexapod robot; and the determining, according to the state information, the combination of swing legs and the movement step length of the center of gravity of the hexapod robot includes:

According to the fault information, it is determined that the legs of the hexapod robot other than the faulty legs are preselected support legs.

Specifically, the faulty legs include the legs with no target landing point in the current state and the legs with faults. The faulty legs cannot support the hexapod robot, so other legs except the faulty legs are selected as preselected supporting legs.

The preselected support state of the hexapod robot is determined according to the corresponding relationship between the preselected support leg and the preset support state, wherein the corresponding relationship of the preselected support state includes at least one of the preselected support state and the corresponding relationship with each of the preselected support states. The state corresponds to the stability margin, the maximum forward length of the body, and the preselected combination of the swinging leg.

Specifically, the preset support state corresponding relationship may be in the form of a table, that is, a preset support state table, and the states of a plurality of preselected support legs can be freely combined according to the preset support state table, and the states of the preselected support legs include support and swing. , to determine all possible preselected support states, and the set of all preselected support states is the set of preselected support states.

The hexapod robot can maintain static stability in each pre-selected support state. The premise of maintaining static stability is that the number of support legs is greater than or equal to three during the movement process, and the center of gravity of the body is within the support polygon, and can meet the calibration stability margin. Spend. Specifically, the supporting polygon is the polygon formed by the connecting lines of the supporting legs and feet. The stability margin represents the distance between the projection of the body's center of gravity on the supporting polygon and any side of the supporting polygon. To maintain static stability, the hexapod robot needs a stability margin greater than or equal to the preset threshold, the typical value of the threshold is 0.1m. If the six legs of the hexapod robot are sorted in turn, the support leg is represented by 1, and the swing leg is represented by 0, then the support state table including all possible support states is shown in Table 1:

Table 1 Support status table

The supporting state in Table 1 shows the state of each leg in this state. The polygon in the schematic diagram of the supporting state is the supporting polygon, and the dot is the projection of the center of gravity of the body on the supporting polygon.

It should be noted that, the stability margin, the maximum forward length of the body, and the preselected combination of swing legs corresponding to each of the preselected support states are not shown in the support state table of this embodiment.

The stable support state of the hexapod robot is determined according to the stability margin and the maximum forward step length of the body, wherein the maximum forward step length of the body corresponding to the stable support state is the movement step length of the center of gravity of the body, The preselected combination of the swing legs corresponding to the stable support state is the swing leg combination.

Specifically, the stability margin corresponds to the stability of the hexapod robot during motion, and the maximum forward length of the body corresponds to the walking speed of the hexapod robot during motion. The stable support state is determined according to the stability margin and the maximum forward length of the body, and the corresponding stable support state can be freely selected according to needs. It can make the hexapod robot as fast as possible while maintaining static stability.

In this preferred embodiment, a stable support state can be freely selected according to the needs of the specific terrain to plan the gait. Compared with the periodic three-legged gait, quadrupedal gait or five-legged gait adopted in the prior art, the gait can be Give full play to the multi-legged advantages of the hexapod robot, greatly improve the ability of the hexapod robot to deal with complex terrain, and improve the movement speed of the hexapod robot on the premise of maintaining static stability.

Preferably, the determining the stable support state of the hexapod robot according to the stability margin and the maximum forward length of the body includes:

The first formula is used to determine the evaluation function of each of the preselected support states, and the first formula is:

为所述预选支撑状态对应的所述机体最大前进步长，ω₁为稳定裕度的权重，ω₂为机体最大前进步长的权重。Wherein, f(state) is the evaluation function, S is the set of preselected support states, state is any one of the preselected support states in the set of preselected support states, and BM _state is the corresponding state of the preselected support state. Said stability margin,

is the maximum forward progress of the body corresponding to the preselected support state, ω ₁ is the weight of the stability margin, and ω ₂ is the weight of the maximum forward progress of the body.

According to the evaluation function, the stable support state is determined using a second formula, and the second formula is:

Wherein, state ₀ is the stable support state.

Specifically, during the movement, the larger ω ₁ indicates that the hexapod robot is more stable, and the larger ω ₂ indicates that the hexapod robot walks faster. Therefore, the values of ω ₁ and ω ₂ can be selected according to specific needs. For example, in the terrain with dense footholds, the stability of the hexapod robot is better, and ω ₂ can be appropriately increased and ω ₁ can be decreased to improve the walking speed of the hexapod robot; while in the terrain with sparse footfall points, it can be Appropriately increase ω ₁ , decrease ω ₂ , and under the condition of appropriately sacrificing walking speed, the ability of the hexapod robot to pass through the terrain with sparse landing points is improved.

Preferably, the state information includes the current position of the body center of gravity of the hexapod robot, the equipment parameters of each of the swing legs, and the preset body motion parameters; the combination of the body center of gravity movement step length and the state information Determining the trajectory of the center of gravity of the hexapod robot and the landing area of each of the swing legs includes:

Determine the next position of the center of gravity of the hexapod robot according to the current position of the center of gravity of the body and the moving step length of the center of gravity of the body; The footfall area of each of the swing legs is determined.

Specifically, the next position of the body's center of gravity can be obtained by moving the body's center of gravity forward on the current position of the body's center of gravity. Assuming that the body's center of gravity of the hexapod robot moves to the next position of the body's center of gravity, it is determined according to the equipment parameters of the swinging legs. At this time, in the footfall area of the swinging leg, the equipment parameters include physical parameters such as swinging angle and length of the swinging leg.

Based on the method of polynomial fitting, the trajectory of the center of gravity of the body is determined according to the current position of the center of gravity of the body, the next position of the center of gravity of the body, and the motion parameters of the body.

Specifically, as shown in Figure 2, the trajectory planning of the center of gravity of the body is to plan the center of gravity of the body from the current position of the center of gravity of the body to the center of gravity of the body when the support legs are combined to support the hexapod robot, that is, when the position of the foot end of the support leg is unchanged. The trajectory of the next position.

In this embodiment, the trajectory of the center of gravity of the body is regarded as a straight line, and a quintic polynomial is used to plan the trajectory of the center of gravity of the body, so as to ensure the continuous smoothness of the position, speed and acceleration of the body's center of gravity. Let the current position of the body's center of gravity be ^W p _b1 , the next position of the body's center of gravity is ^W p _b3 , and the preset motion parameters of the body include the speed ^W v _b1 , the acceleration ^W a _b1 and the corresponding time t ₁ when the body's center of gravity is at the current position of the body's center of gravity , and the velocity ^W v _b3 , the acceleration ^W a _b3 , and the corresponding time t ₃ when the center of gravity of the body is at the next position of the center of gravity of the body. The known quantities when planning the trajectory of the center of gravity of the aircraft are shown in Table 2:

Table 2 The known quantities of the trajectory planning of the center of gravity of the body

The position polynomial of the trajectory of the center of gravity of the fuselage is established according to the position of the center of gravity of the fuselage and the corresponding time. The position polynomial of the trajectory of the center of gravity of the fuselage is as follows:

^W p _b =A _b0 +A _b1 t+A _b2 t ² +A _b3 t ³ +A _b4 t ⁴ +A _b5 t ⁵ ,

Wherein, ^W p _b is ^W p _b1 or ^W p _b3 , t corresponding to ^W p _b1 is t ₁ , and t corresponding to ^W p _b3 is t ₃ .

^W p _b1 can be ^W p _x1 or ^W p _y1 or ^W p _z1 , ^W p _x1 is the X and _W axis coordinates of the current position of the body’s center of gravity in the body coordinate system, and ^W p _y1 is the current position of the body’s center of gravity in the body coordinate system. Y and _W axis coordinates, ^W p _z1 is the Z _W coordinate of the current position of the center of gravity of the body in the body coordinate system. ^W p _b3 can be ^W p _x3 or ^W p _y3 or ^W p _z3 , ^W p _x3 is the X and _W axis coordinates of the next position of the center of gravity of the body in the body coordinate system, ^W p _y3 is the next position of the center of gravity of the body in the body coordinate system The Y and _W axis coordinates in , ^W p _z3 is the Z and _W axis coordinates of the next position of the center of gravity of the body in the body coordinate system. In this embodiment, ^W p _z3 is equal to ^W p _z1 .

The velocity polynomial of the trajectory of the center of gravity of the fuselage is established according to the velocity of the center of gravity of the fuselage and the corresponding time. The velocity polynomial of the trajectory of the center of gravity of the fuselage is shown as follows:

^W v _b =A _b1 +2A _b2 t+3A _b3 t ² +4A _b4 t ³ +5A _b5 t ⁴ ,

Wherein, ^W v _b is ^W v _b1 or ^W v _b3 , t corresponding to ^W v _b1 is t ₁ , and t corresponding to ^W v _b3 is t ₃ .

^W v _b1 can be ^W v _x1 or ^W v _y1 or ^W v _z1 , ^W v _x1 is the X _W axis component of ^W v _b1 , ^W v _y1 is the Y _W axis component of ^W v _b1 , ^W v _z1 is ^W v _b1 The Z and _W axis components of . ^W v _b3 can be ^W v _x3 or ^W v _y3 or ^W v _z3 , ^W v _x3 is the X _W axis component of ^W v _b3 , ^W v _y3 is the Y _W axis component of ^W v _b3 , ^W v _z3 is ^W v _b3 The Z and _W axis components of .

The acceleration polynomial of the trajectory of the center of gravity of the fuselage is established according to the acceleration of the center of gravity of the fuselage and the corresponding time. The acceleration polynomial of the trajectory of the center of gravity of the fuselage is as follows:

^W a _b =2A _b2 +6A _b3 t+12A _b4 t ² +20A _b5 t ³ ,

Wherein, ^W a _b is ^W a _b1 or ^W a _b3 , t corresponding to ^W a _b1 is t ₁ , and t corresponding to ^W a _b3 is t ₃ .

^W a _b1 can be ^W a _x1 or ^W a _y1 or ^W a _z1 , ^W a _x1 is the X _W axis component of ^W a _b1 , ^W a _y1 is the Y _W axis component of ^W a _b1 , ^W a _z1 is ^W a _b1 The Z and _W axis components of . ^W a _b3 can be ^W a _x3 or ^W a _y3 or ^W a _z3 , ^W a _x3 is the X _W axis component of ^W a _b3 , ^W a _y3 is the Y _W axis component of ^W a _b3 , ^W a _z3 is ^W a _b3 The Z and _W axis components of .

Substitute each parameter into the corresponding polynomial, t ₁ can be set to 0, and the following formula can be obtained:

According to the known ^W p _b1 , ^W p _b3 , ^W v _b1 , ^W v _b3 , ^W a _b1 , ^W a _b3 and t ₃ , the polynomial coefficients [A _b0 , A _b1 , A _b2 , A _b3 , A _b4 can be obtained ,A _b5 ] ^T , where ^W v _b1 , ^W v _b3 , ^W a _b1 and ^W a _b3 can be 0, and t ₃ can be specifically set according to the situation.

In this preferred embodiment, after the polynomial coefficients are determined, any position in the process of the center of gravity of the body moving from the current position of the center of gravity of the body to the next position of the center of gravity of the body can be obtained by setting the time, and the speed and acceleration of the body corresponding to this position can be obtained , to determine the trajectory of the center of gravity of the fuselage. The trajectory planning process is smooth and can meet the stability requirements and energy consumption constraints of the hexapod robot.

Preferably, the state information further includes the current position of the foot end of each of the swinging legs; the target footfall point of each of the swinging legs in the corresponding footfall area is determined according to the ground footfall point information include:

A set of landing points in each of the landing areas is determined according to the ground landing point information, where the set of landing points includes zero or at least one of the pre-selected landing points.

For any of the swinging legs, let the set of footfall points be P, and determine the cost function for selecting the target footfall point according to a third formula. The third formula is:

为所述六足机器人机体重心的运动方向上的单位向量，f(p_e)为所述代价函数。Wherein, _pi is the current position of the foot end of the swinging leg, and p _e is the preselected footing point corresponding to the swinging leg,

is the unit vector in the movement direction of the center of gravity of the hexapod robot, and f( _pe ) is the cost function.

Based on the cost function, the target footfall point of each of the swing legs is determined according to a fourth formula, where the fourth formula is:

Wherein, p is the target footing point of the swing leg.

Specifically, as shown in FIG. 3 , the dotted line frame on the left is the foot drop area of any swinging leg before the center of gravity of the body moves, the broken line segment corresponding to K1 is the posture _of the swinging leg before swinging, and p _i is the swinging leg at this time. The foot end position of the leg; the solid line box on the right is the foot drop area of the swing leg when the center of gravity of the body moves to the next position of the body center of gravity, the dotted line box and the circle in the solid line box represent the foot drop point, and the fold corresponding to K ₂ The line segment is the posture of the swinging leg after _swinging , and _pe is the foot end position of the _swinging leg at this time, that is, the pre-selected landing point _. The dotted line is the trajectory of the foot end of the swinging leg. The arrows indicate the body movement direction of the hexapod robot.

As shown in Figure 4, there is a circle in the dotted box before the center of gravity of the body moves, but there is no circle in the solid line box after the center of gravity of the body moves, that is, the swinging leg does not have a footing point in the footfall area after the center of gravity of the body moves forward. At this time, the swinging leg is controlled to retract and hover to a predetermined position, that is, the pose shown by K ₂ in FIG. 4 .

As shown in Figure 5, there is no circle in the dotted box before the center of gravity of the fuselage moves, while there is a circle in the solid line box after the center of gravity of the fuselage moves, that is, the swinging leg does not have a footing point in the foot-drop area before the center of gravity of the fuselage moves forward , the swinging leg hovers at a predetermined position at this time, that is, the pose shown by K ₁ in FIG. 5 . After the center of gravity of the fuselage moves forward, there is a foot drop point in the corresponding foot drop area, and at this time, the swing leg is controlled to drop to the corresponding target foot drop point p _e .

Preferably, the fault information includes leg fault information; determining whether each of the swing legs can land on the corresponding target landing point according to the state information includes: determining each of the swing legs according to the leg fault information Whether the swing leg is a swing leg that cannot fall.

Specifically, it is determined whether each swinging leg is a faulty leg according to the leg fault information, and the faulty leg is the swinging leg that cannot fall. The leg fault information may include driver failure, motor damage, joint lock, encoder and other sensor failures, etc. .

Preferably, the state information further includes preset motion parameters of the swinging leg; the determining the foot end trajectory of the swinging leg according to the current state information and the target footfall point includes:

Based on the method of polynomial fitting, the foot end trajectory of the swing leg is determined according to the current position of the foot end, the target foot landing point and the motion parameter of the swing leg.

Specifically, as shown in Fig. 6 , a multi-order continuous hexadecimal polynomial is used to plan the foot end trajectory of the swinging leg. Let the current position of the foot end be ^W p _r1 , the position of the target foot landing point is ^W p _r3 , and the swing leg's current position is W p r3 . The midpoint position of the foot end during the movement is ^W p _r2 . The preset motion parameters of the swinging leg include the speed ^W v _r1 , the acceleration ^W a _r1 and the corresponding time t ₄ when the foot end of the swing leg is at the current position of the foot end, and the speed ^W when the foot end of the swing leg is at the target footing point v _r3 , acceleration ^W a _r3 , corresponding time t ₆ , and let the time at the midpoint position be t ₅ . The known quantities when planning the trajectory of the center of gravity of the aircraft are shown in Table 2:

Table 3 Known quantities for trajectory planning of the foot

The foot end position polynomial of the swing leg is established according to the foot end position of the swing leg and the corresponding time, and the foot end position polynomial is as follows:

^W _{pr =A r0 +} A _r1 t+A _r2 t ² +A _r3 t ³ +A _r4 t ⁴ +A _r5 t ⁵ ,

_Wherein , _Wpr is ^Wpr1 or _Wpr2 or ^Wpr3 , ^t corresponding to _Wpr is _t4 , _{t corresponding} to ^Wpr2 is _t5 , and _{t corresponding} to ^Wpr3 is ^{t6 .}

Similarly, ^W p _r1 can be ^W p rx1 or ^W p _ry1 or ^W p _rz1 , ^W p _rx1 is the X and _W axis coordinates of the current position of the foot in the body coordinate system, and ^W p _ry1 is the current position _of the foot in the body coordinate The Y and _W axis coordinates in the system, ^W p _rz1 is the Z and _W coordinates of the current position of the foot end in the body coordinate system. ^W p _r3 can be ^W p _rx3 or ^W p _ry3 or ^W p _rz3 , ^W p _rx3 is the X and _W axis coordinates of the target landing point in the body coordinate system, ^W p _ry3 is the target landing point in the body coordinate system Y and _W -axis coordinates, and ^W p _rz3 are the Z and _W -axis coordinates of the target landing point in the body coordinate system. In this embodiment, both ^W p _rz1 and ^W p _rz3 are 0. ^W p _r2 can be ^W p _rx2 or ^W p _ry2 or ^W p _rz2 , ^W p _rx2 is the X and _W axis coordinates of the intermediate point position in the body coordinate system, ^W p _ry2 is the Y _W of the intermediate point position in the body coordinate system Axis coordinates, ^W p _rz2 is the Z and _W axis coordinates of the midpoint position in the body coordinate system, in this embodiment, ^W p _rz2 is the height of the swinging leg, ie h in FIG. 6 .

According to the speed and the corresponding time of the foot end of the swinging leg, the foot end speed polynomial of the swing leg is established, and the foot end speed polynomial is as follows:

^W v _r =A _r1 +2A _r2 t+3A _r3 t ² +4A _r4 t ³ +5A _r5 t ⁴ +6A _r6 t ⁵ ,

Wherein, ^W v _r is ^W v _r1 or ^W v _r3 , t corresponding to ^W v _r1 is t ₄ , and t corresponding to ^W v _r3 is t ₆ .

Similarly, ^W v _r1 can be the X _W axis component or Y _W axis component or Z _W axis component of ^W v _r1 , ^W v _r3 can be the X _W axis component or Y _W axis component or Z _W axis component of ^W v _r3 .

The foot end acceleration polynomial of the swinging leg is established according to the acceleration and the corresponding time of the swinging leg when the foot end moves, and the foot end acceleration polynomial is as follows:

^W a _r = 2A _r2 +6A _r3 t+12A _r4 t ² +20A _r5 t ³ +30A _r6 t ⁴ ,

Wherein, ^W a _r is ^W a _r1 or ^W a _r3 , t corresponding to ^W a _r1 is t ₄ , and t corresponding to ^W a _r3 is t ₆ .

Similarly, ^W a _r1 can be the X _W axis component or Y _W axis component or Z _W axis component of ^W a _r1 , ^W a _r3 can be the X _W axis component or Y _W axis component or Z _W axis component of ^W a _r3 .

By substituting the parameters into the corresponding polynomials, t ₄ can be set to 0, and the following formula can be obtained:

_The ^{polynomial coefficients} _{[ A} ^r0 _, ^A _{r1 , A} ^_ _{_} ^_ _{_ _} ^_ _{_ r2} , A _r3 , A _r4 , A _r5 , A _r6 ] ^T , wherein, ^W v _r1 , ^W v _r3 , ^W a _r1 and ^W a _r3 can be 0, and t ₅ and t ₆ can be specifically set according to the situation.

In this preferred embodiment, after the polynomial coefficients are determined, by setting the time, it is possible to obtain any position of the foot end of the swinging leg during the movement from the current position of the foot end to the target foot landing point, and the corresponding foot end of the position. Speed and acceleration to determine the trajectory of the foot end. The foot end trajectory is planned by the method of polynomial fitting, which can not only ensure the smooth and continuous position trajectory curve, but also ensure that the speed curve and acceleration curve are smooth and continuous, so that the motor will not have sudden changes in jitter when controlling the movement of the swinging leg, and can save energy.

Preferably, after the foot end trajectory of the swinging leg is determined according to the state information and the target footfall point, the method further includes the following steps:

The hexapod robot is controlled to move to the next position of the body's center of gravity according to the trajectory of the body's center of gravity, and the swing leg is controlled to move to the corresponding target landing point according to the trajectory of the foot end.

If it is determined that the swinging leg slips or sinks when the foot falls to the corresponding target footfall point, the swing leg is controlled to try to land on the preselected footfall point adjacent to the target footfall point .

When it is determined that the pre-selected foot landing point cannot be smoothly landed after many attempts to land the foot, the swing leg is controlled to hover at a predetermined position.

Specifically, when the foot end of the swing leg is controlled to fall to the target foot end point, when the characteristics of the ground are poor, such as the ground is slippery and soft, the foot end may slip or fall. As shown in Figure 7, the direction of the arrow is the movement direction of the body, _K1 is the posture before the swinging leg swings, _pi is the foot end position of the swinging leg at this time _; K2 is the posture of the swinging leg falling after the swinging leg, p _e1 is the position of the foot end of the swing leg at this time; the circle in the figure is the foot drop point where the foot can be dropped, and the black dot is the foot drop point that cannot drop the foot, that is, the foot end of the swing leg will slip or sink. point. That is, when the swinging leg moves from pi to p _e1 in FIG. 7 and _falls , the foot end of the swinging leg may slip or sink.

At this time, the swing leg is controlled to fall to other footfall points adjacent to p _e1 , and the other footfall points are shown as p _e2 in Fig. 8, because p _e2 is also a black point, that is, the foot drop point that cannot fall. , so continue to control the swinging leg to try to land on the foot landing point adjacent to p _e2 , and so on, try n times. The size of n can be set as required. The larger n is, the greater the probability of the swinging leg falling, and the greater the probability of the hexapod robot passing through the terrain, but the slower the passing speed. The smaller n is, the less likely the swinging leg will land, and the less likely the hexapod will pass through the terrain.

If after n times of attempts, the swinging leg still fails to land stably, the swinging leg is regarded as an error leg, and the swinging leg is controlled to hover to a predetermined position. Among them, n can be set larger than the number of footfall points in the footfall area, that is, when the swinging leg fails to land, try all the footfall points in the footfall area one by one, until the swinging leg stably drops, or all the footfall points are Unable to stably fall, control the swinging leg to hover to a predetermined position at this time.

In this preferred embodiment, when the physical characteristics of the terrain are poor, and the foot end of the swing leg is prone to slipping or sinking, the swing leg is controlled to try to land on another foot drop point in the foot drop area, and re-search for the target foot drop The error correction of the landing point can improve the probability of the swinging leg landing stably, thereby improving the passing ability of the hexapod robot in complex terrain.

Hereinafter, a free fault-tolerant gait planning method for a hexapod robot according to an embodiment of the present invention will be further described by taking a hexapod robot passing through a terrain with a footing point density of 11.3/m ² as an example.

The phase sequence diagram of the leg support state of the hexapod robot is shown in Figure 9. The six legs of the hexapod robot are named Leg1, Leg2, Leg3, Leg4, Leg5 and Leg6 in turn. The black part in the figure indicates that the corresponding legs are used as Supporting legs, the white part indicates that the corresponding leg is used as a swinging leg. In the first motion cycle of the hexapod robot, Leg1, Leg2, Leg4 and Leg5 are used as support legs, and Leg3 and Leg6 are used as swing legs; in the second motion cycle, Leg3, Leg4 and Leg6 are used as support legs, Leg1, Leg2, and Leg5 are used as swing legs, and so on. It can be seen that when the free fault-tolerant gait planning method of the hexapod robot according to the embodiment of the present invention is used to plan the gait, the support state is freely selected. It can be switched freely between different terrains, and the appropriate gait mode can be selected according to different terrains, which improves the passing ability in various complex terrains.

As shown in Figure 10, the simulation diagram of the hexapod robot passing through the terrain with a foothold density of 11.3/m ² , the hexapod robot realizes the task of moving from coordinate point [0,0] to coordinate point [8.4,0] . Figure 10 corresponds to the 15 states of the hexapod robot during motion from left to right and from top to bottom. The black dots in the figure are the footfall points, and the white area is the area without footfall points. The simulation results show that, The free and fault-tolerant gait planning method of the present application can help the hexapod robot to smoothly pass through the sparse footfall area. FIG. 11 shows a schematic top view of the hexapod robot corresponding to the first nine states in FIG. 10 respectively.

As shown in FIG. 12 , a free fault-tolerant gait planning device for a hexapod robot provided by an embodiment of the present invention includes:

The acquisition module is used to acquire the status information and ground landing point information of the hexapod robot.

The first processing module is configured to determine, according to the state information, a combination of swinging legs and a moving step length of the center of gravity of the hexapod robot, where the combination of swinging legs includes zero or at least one swinging leg.

The first planning module is configured to combine the movement step length of the body's center of gravity and the state information to respectively determine the trajectory of the body's center of gravity of the hexapod robot and the landing area of each of the swing legs.

The second processing module is configured to determine the target footfall point of each of the swing legs in the corresponding footfall area according to the ground footfall point information, and to determine whether each of the swing legs can landing on the corresponding target landing point.

The second planning module is configured to control the swinging leg to hover at a predetermined position for the swinging leg that does not have the target footing point or cannot land; for the swinging leg that can land at the corresponding target footing point For the swing leg, the foot end trajectory of the swing leg is determined according to the state information and the target foot landing point.

A free fault-tolerant gait planning device for a hexapod robot provided by another embodiment of the present invention includes a memory and a processor; the memory is used for storing a computer program; the processor is used when executing the computer program , to realize the free fault-tolerant gait planning method of the hexapod robot as described above. The device can be an industrial computer, a server, or the like.

Still another embodiment of the present invention provides a computer-readable storage medium with a computer program stored thereon. When the computer program is executed by a processor, the above-mentioned free fault-tolerant gait planning method for a hexapod robot is implemented.

Those of ordinary skill in the art can understand that all or part of the processes in the methods of the above embodiments can be implemented by instructing relevant hardware through a computer program, and the program can be stored in a computer-readable storage medium. During execution, the processes of the embodiments of the above-mentioned methods may be included. The storage medium may be a magnetic disk, an optical disk, a read-only memory (Read-Only Memory, ROM), or a random access memory (Random Access Memory, RAM) or the like. In this application, the units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place, or may be distributed to multiple locations. on a network unit. Some or all of the units may be selected according to actual needs to achieve the purpose of the solutions in the embodiments of the present invention. In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically alone, or two or more units may be integrated into one unit. The above-mentioned integrated units may be implemented in the form of hardware, or may be implemented in the form of software functional units.

Although the disclosure of the present invention is as above, the protection scope of the disclosure of the present invention is not limited thereto. Those skilled in the art can make various changes and modifications without departing from the spirit and scope of the present disclosure, and these changes and modifications will fall within the protection scope of the present invention.

Claims (10)

1. A free fault-tolerant gait planning method of a hexapod robot is characterized by comprising the following steps:

acquiring state information and ground foot-landing point information of the hexapod robot;

determining a swing leg combination and a body gravity center moving step length of the hexapod robot according to the state information, wherein the swing leg combination comprises zero or at least one swing leg;

respectively determining the body gravity center track of the hexapod robot and the foot falling area of each swing leg by combining the body gravity center moving step length and the state information;

determining a target foot falling point of each swing leg in the corresponding foot falling area according to the ground foot falling point information, and determining whether each swing leg can fall to the corresponding target foot falling point according to the state information;

for the swing leg without the target foot drop point or failing to drop foot, controlling the swing leg to hover at a predetermined position; and determining the foot end track of the swing leg capable of falling at the corresponding target foot falling point according to the state information and the target foot falling point.

2. The free fault tolerant gait planning method of a hexapod robot according to claim 1, characterized in that the state information includes fault information of the hexapod robot; the step of determining the swing leg combination and the body gravity center moving step length of the hexapod robot according to the state information comprises the following steps:

determining legs of the hexapod robot except for the fault leg as preselected support legs according to the fault information;

determining a preselected support state of the hexapod robot according to a corresponding relation between the preselected support legs and a preset support state, wherein the corresponding relation between the support states comprises at least one preselected support state, and a preselected combination of a stability margin, a maximum body advancing step length and a swing leg, which respectively correspond to each preselected support state;

and determining a stable supporting state of the hexapod robot according to the stability margin and the maximum body advancing step length, wherein the maximum body advancing step length corresponding to the stable supporting state is the body gravity center moving step length, and the preselected combination of the swing legs corresponding to the stable supporting state is the swing leg combination.

3. The free fault tolerant gait planning method of a hexapod robot according to claim 2, wherein said determining the stable support state of the hexapod robot from the stability margin and the maximum body forward step size comprises:

determining a merit function for each of the preselected support states using a first formula, the first formula being:

wherein f (state) is the evaluation function, S is a preselected set of support states, the preselected set of support states is a set of all the preselected support states, and state is any one of the preselected set of support states, BM_stateThe stability margin for the preselected support condition,

the maximum advance step length, omega, of the body corresponding to the preselected support state₁Weight of stability margin, ω₂The weight of the maximum advancing step length of the machine body;

determining the stable supporting state by adopting a second formula according to the evaluation function, wherein the second formula is as follows:

wherein, state₀The stable support state.

4. The free fault-tolerant gait planning method of a hexapod robot according to claim 2, characterized in that the state information includes a current position of a body center of gravity of the hexapod robot, equipment parameters of each of the swing legs, and preset body motion parameters; the determining the body gravity center track of the hexapod robot and the foot falling area of each swing leg by combining the body gravity center moving step length and the state information comprises:

determining the next position of the body gravity center of the hexapod robot according to the current position of the body gravity center and the body gravity center moving step length;

enabling the gravity center of the body of the hexapod robot to be located at the position below the gravity center of the body, and respectively determining the foot falling area of each swing leg according to the equipment parameters of each swing leg;

and determining the gravity center track of the body according to the current position of the gravity center of the body, the next position of the gravity center of the body and the motion parameters of the body based on a polynomial fitting method.

5. The free fault tolerant gait planning method of a hexapod robot according to claim 4, characterized in that the state information further includes a current position of a foot end of each of the swing legs; the determining a target foot-landing point of each swing leg in the corresponding foot-landing area according to the ground foot-landing point information comprises:

determining a set of foot placement points in each of the foot placement regions from the ground foot placement point information, the set of foot placement points including zero or at least one preselected foot placement point;

for any swing leg, the set of the foot falling points is set to be P, and a cost function for selecting the target foot falling point is determined according to a third formula, wherein the third formula is as follows:

wherein p is_iIs the current position of the foot end, p_eFor the pre-selected foot drop point,

is a unit vector in the direction of motion of the center of gravity of the body, f (p)_e) Is the cost function;

based on the cost function, determining the target drop foot point of the swing leg according to a fourth formula, the fourth formula being:

wherein p is the target drop foot point of the swing leg.

6. The free fault tolerant gait planning method of a hexapod robot according to claim 5, characterized in that the fault information comprises leg fault information, the state information further comprises preset swing leg motion parameters;

the determining whether each swing leg can fall to the corresponding target foot falling point according to the state information includes:

determining whether each swing leg cannot fall to the foot according to the leg fault information;

the determining the foot end trajectory of the swing leg according to the state information and the target foot drop point comprises:

and determining the foot end track of the swing leg according to the current position of the foot end, the target foot drop point and the motion parameters of the swing leg based on a polynomial fitting method.

7. The free fault tolerant gait planning method of a hexapod robot according to claim 5 or 6, characterized in that after said determining the foot end trajectory of the swing leg from the state information and the target drop foot point, further comprises:

controlling the hexapod robot to move to a position below the gravity center of the robot body according to the gravity center track of the robot body, and controlling each swing leg to move to the corresponding target foot falling point according to the foot end track;

if the swing leg is determined to be slipped or sunk when falling to the corresponding target foot falling point, controlling the swing leg to try to fall to the preselected foot falling point adjacent to the target foot falling point;

controlling the swing leg to hover at a predetermined location when it is determined that none of the plurality of attempted foot drops to the preselected foot drop point results in a smooth foot drop.

8. A free fault-tolerant gait planning device of a hexapod robot, comprising:

the acquisition module is used for acquiring the state information and the ground foot-landing point information of the hexapod robot;

the first processing module is used for determining a swing leg combination and an organism gravity center moving step length of the hexapod robot according to the state information, wherein the swing leg combination comprises zero or at least one swing leg;

the first planning module is used for respectively determining the gravity center track of the six-legged robot and the foot falling area of each swing leg by combining the gravity center moving step length of the robot body and the state information;

the second processing module is used for determining a target foot falling point of each swing leg in the corresponding foot falling area according to the ground foot falling point information and determining whether each swing leg can fall to the corresponding target foot falling point according to the state information;

a second planning module for controlling the swing leg to hover at a predetermined position for the swing leg without the target foot drop point or failing to drop foot; and determining the foot end track of the swing leg capable of falling at the corresponding target foot falling point according to the state information and the target foot falling point.

9. A free fault-tolerant gait planning device of a hexapod robot is characterized by comprising a memory and a processor;

the memory for storing a computer program;

the processor, when executing the computer program, for implementing a free fault tolerant gait planning method of a hexapod robot as claimed in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that the storage medium has stored thereon a computer program which, when executed by a processor, implements a free fault tolerant gait planning method of a hexapod robot according to any of claims 1 to 7.

Images