A static gait planning system for quadruped detectors

Abstract

The present invention discloses a static gait planning system for a quadruped detector. The quadruped detector consists of a fuselage platform and moving legs. The platform is connected to four sets of moving legs in the same state and bears the weight of the detector structure at the same time. The moving legs have three degrees of freedom, and their ends can move arbitrarily in three-dimensional space. The fast static gait planning method for the quadruped detector proposed in the present invention can move the center of gravity of the fuselage in an "8"-shaped curve while taking steps under the premise of ensuring the stability of the fuselage, eliminating the process of adjusting the center of gravity of the fuselage four times under the support of four legs in the traditional static gait planning method, thereby significantly improving the travel speed of the detector.

Description

Technical Field

[0001] The invention belongs to the technical field of spacecraft and relates to a fast static gait planning technology for a quadruped detector. Background Art

[0002] When the quadrupedal probe moves in a static gait, the probe's center of gravity can always remain within the triangle formed by the projection points of the supporting legs on the ground, thereby maintaining the stability of the probe's body. However, in order to adjust the position of the probe's center of gravity, the current quadrupedal probe needs to move its center of gravity while supported by its four legs before each step, and this movement process cannot move the body forward. Therefore, every time the probe body moves forward, it needs to adjust its center of gravity four times while supported by its four legs, which not only greatly reduces the probe's travel speed, but also increases ineffective energy consumption.

[0003] Existing research results mainly focus on the adaptability of the static gait of quadruped detectors to rugged terrain, and there is little research on how to improve the speed of static gait. For example, the Chinese patent "A static gait motion control method for quadruped detectors climbing stairs" CN111483532A proposes a static gait planning method for climbing stairs. However, this method still needs to adjust the center of gravity of the fuselage when the four legs are supported at the same time, and the walking speed is slow. In "A load-type quadruped walking platform complex terrain static gait walking posture adjustment method", Chao Zhiqiang et al. predicted the terrain by detecting the relative position of the supporting foot to the fuselage, and then adjusted the fuselage posture. However, it is still necessary to adjust the center of gravity under the four-legged support state. Tan Yongying et al. proposed a static gait in "Static gait analysis of quadruped walking mobile platform based on supporting polygons". By controlling the center of gravity of the fuselage to the common area of ​​the supporting triangle, the four fuselage center of gravity adjustments of the traditional static gait are reduced to two times, which improves the walking speed of the static gait to a certain extent.

[0004] In addition, the above-mentioned documents and patents lack descriptions on how to adjust the center of gravity of the fuselage, or adopt a linear motion strategy. Although the calculation is simple, it will make the angular velocity of the corresponding joints of the supporting legs discontinuous, resulting in a "stuttering" phenomenon, which is not conducive to the smooth rotation of the joints and easily damages the transmission mechanism. Summary of the invention

[0005] The technical problem solved by the present invention is: to overcome the deficiencies of the prior art, and to provide a fast static gait planning system for a quadruped detector. In the process of detector movement, a dynamic "8"-shaped fuselage center of gravity position moving curve is designed according to the projection point of the foot end of the moving leg in the swinging state on the ground, and the dynamically changing quadrilateral formed by the contact point between the foot end of the moving leg in the supporting state and the ground. When the supporting state and the swinging state of the moving leg are alternately switched, the configuration of the moving leg in the supporting state is controlled in real time, so that the fuselage center of gravity tracks the "8"-shaped curve. In the three-leg supporting state, the fuselage center of gravity position is adjusted along with the swinging of the foot end of the moving leg in the swinging state, so that the fuselage center of gravity position adjustment and the stepping action are carried out simultaneously, thereby eliminating the process of adjusting the fuselage center of gravity four times under the support of four legs in the traditional static gait planning method, which not only improves the traveling speed of the detector, but also the designed fuselage center of gravity moving curve is continuous and smooth, which is conducive to the smooth operation of the joints of the moving legs of the detector.

[0006] In order to achieve the above object, the technical solution of the present invention is:

[0007] A fast static gait planning system for a quadruped detector, comprising: a foot end position solving module, a fuselage center of gravity curve generating module, a walking phase switching module, a swing foot end trajectory planning module, a four-leg foot end trajectory planning module, and a joint angle inverse solving module;

[0008] The foot end position calculation module calculates the position of the foot end in the coordinate system of the detector body, i.e., the foot end coordinate, in real time according to the leg joint angle, and inputs the result into the body center of gravity curve generation module;

[0009] The fuselage center of gravity curve generation module generates a dynamically changing "8"-shaped fuselage center of gravity movement curve according to the foot end coordinates and movement time;

[0010] The walking phase switching module switches the moving leg between the swinging state and the supporting state according to the ground contact state of the moving leg in the swinging state, and obtains the starting foot end position of the moving leg in the swinging state;

[0011] The swing foot end trajectory planning module plans the foot end motion trajectory of the moving leg in the swing state according to the fourth-order Bezier curve;

[0012] The four-leg foot-end trajectory planning module plans the final desired motion trajectory of the foot-ends of the four moving legs in real time according to the foot-end motion trajectory of the moving leg in the swinging state and the center of gravity movement curve of the fuselage, and inputs it into the joint angle inverse solution module;

[0013] The joint angle inverse solution module obtains the desired joint angle according to the desired motion trajectory, and realizes the static gait control of the quadruped detector through the joint angle closed-loop control.

[0014] Furthermore, the foot end position calculation module calculates the position of the foot end in the detector body coordinate system in real time according to the leg joint angle, including:

[0015] Joint angle measurement: measure the angle θ of each joint of the moving leg l of the detector i =[θ i_1 θ i_2 θ i_3 ], i=2, 3, 4, 5 are the moving leg numbers; the thigh length of each moving leg is L upper , calf length is L lower ; Measure the rolling angle ψ and pitch angle β of the detector relative to the inertial coordinate system;

[0016] Calculate the position P of the foot end of the moving leg i of the detector in the leg body coordinate system i =[p i_x p i_y p i_z ];

[0017] Calculate the mounting rotation matrix C of mobile leg i a_i ;

[0018] Calculate the rotation matrix C from the body coordinate system to the inertial coordinate system;

[0019] Calculate the position P of the foot end of the moving leg i in the inertial coordinate system g_i =[p g_i_x p g_i_y p g_i_z ].

[0020] Further, calculate the position P of the foot end of the moving leg i of the detector in the leg body coordinate system i =[p i_x p i_y p i_z ], specifically:

[0021] p i_x =L upper cosθ i_2 +L lower cosθ i_3

[0022] p i_y =(L upper sinθ i_2 +L lower sinθ i_3 )cosθ i_1

[0023] p i_z =(L upper sinθ i_2 +L lower sinθi_3 )sinθ i_1

[0024] Install the rotation matrix where α i is the installation angle of the mobile leg i relative to the fuselage;

[0025] Rotation Matrix

[0026] The position of the foot end of the moving leg i in the inertial coordinate system is P g_i =C·(P a_i +C a_i ·P i ), where P a_i is the installation position matrix of mobile leg i, which is a fixed value obtained through measurement.

[0027] Furthermore, the fuselage center of gravity curve generation module generates a dynamically changing "8"-shaped fuselage center of gravity movement curve according to the foot end coordinates and the movement time, including:

[0028] Calculate the intersection position P of the diagonal lines of the quadrilateral formed by the projection points of the first moving leg, the second moving leg, the third moving leg, and the fourth moving leg on the ground cross =[x cross y cross ] T ;

[0029] Calculate the incenter P of the triangle formed by the projection points of the first moving leg and the fourth moving leg and the intersection point of the diagonal of the quadrilateral inner_1 =[p inner1_x p inner1_y ] T ;

[0030] Calculate the incenter P of the triangle formed by the projection points of the first moving leg, the second moving leg and the intersection point of the diagonal of the quadrilateral inner_2 =[p inner2_x p inner2_y ] T ;

[0031] Calculate the angle bisector vector A lr =[x lr y lr ]、A fb =[x fb y fb ];

[0032] Calculate the center of gravity position P of the fuselage on the "8" shaped trajectory cor =[x cor y cor h w ], where h wIt is the height of the fuselage during walking, which is the preset value.

[0033] Furthermore, the intersection position P of the diagonal lines of the quadrilateral formed by the projection points of the first moving leg, the second moving leg, the third moving leg, and the fourth moving leg on the ground is cross =[x cross y cross ] T for:

[0034]

[0035] in, b 11 =p g_1_y -p g_1_x k1, b 12 =p g_2_y -p g_2_x k2;

[0036] The incenter P of the triangle formed by the projection points of the first moving leg and the fourth moving leg and the intersection point of the diagonal line of the quadrilateral inner_1 =[p inner1_x p inner1_y ] T for:

[0037]

[0038]

[0039]

[0040]

[0041]

[0042] The incenter P of the triangle formed by the projection points of the first moving leg, the second moving leg and the intersection point of the diagonal line of the quadrilateral inner_2 =[p inner2_x p inner2_y ] T for:

[0043]

[0044]

[0045]

[0046]

[0047] Angle bisector vector A lr =[x lr ylr ]、A fb =[x fb y fb ]for:

[0048]

[0049] in

[0050]

[0051] in

[0052] The position of the center of gravity of the fuselage on the "8" shape trajectory P cor =[x cor y cor h w ]for:

[0053] x cor =x cross +f lr ·x lr ·sin(θ-π / 2)+f fb ·x fb sin(2θ)

[0054] y cor =y cross +f lr ·y lr ·sin(θ-π / 2)+f fb ·y fb sin(2θ)

[0055] where f lr and f fb It is the preset value, used to control the size of the "8" shaped track; Where T a is the total time required for all four legs of the detector to take a step, which is the preset value; t a is the time elapsed during the journey, and is the measured value.

[0056] Further, the walking phase switching module switches the swinging state and the supporting state of the moving leg according to the ground contact state of the moving leg in the swinging state, and obtains the starting foot end position of the moving leg in the swinging state, including:

[0057] Measure the joint angle θ of the moving leg i of the detector i =[θ i_1 θ i_2 θ i_3 ], i is the moving leg number; measure the joint torque τ of moving leg i i =[τ i_1τ i_2 τ i_3 ] T ;

[0058] Calculate the Jacobian matrix J of each moving leg i ;

[0059] Calculate the foot-end force matrix F of each moving leg i =[f x f y f z ] T ; There is F i =(J i T ) -1 ·τ i ;

[0060] Determine whether the moving leg in the swing state touches the ground:

[0061] The foot force F of the moving leg currently in the swing state i =[f x f y f z ] T , if f z ≤f threh , then the moving leg is considered to have touched the ground, where f threh is the default value;

[0062] If the moving leg currently in the swing state touches the ground, the walking phase is switched and the swing starting point position P of the new moving leg in the swing state is calculated. s_0 and the swing end position P s_d , and calculate the starting point position P of the foot end of the first moving leg, the second moving leg, the third moving leg, and the fourth moving leg z1_0 , P z2_0 , P z3_0 , P z4_0 ,

[0063]

[0064] Among them, d x is the distance traveled in the x direction, d y The distance forward in the y direction is the preset value.

[0065] Furthermore, the Jacobian matrix J i for:

[0066]

[0067] The starting point position P of the foot end of the first moving leg, the second moving leg, the third moving leg, and the fourth moving leg z1_0 , Pz2_0 , P z3_0 , P z4_0 , the calculation method is as follows:

[0068] For walking phase state:

[0069] If the current state is 1, update the state to 3, P s_0 =P z3_0 =P g_3 , P z1_0 =P g_1 , P z2_0 =P g_2 , P z4_0 =P g_4 ;

[0070] If the current state is 2, update the state to 4, P s_0 =P z4_0 =P g_4 , P z1_0 =P g_1 , P z2_0 =P g_2 , P z3_0 =P g_3 ;

[0071] If the current state is 3, update the state to 2, P s_0 =P z2_0 =P g_2 , P z1_0 =P g_1 , P z3_0 =P g_3 , P z4_0 =P g_4 ;

[0072] If the current state is 4, update the state to 1, P s_0 =P z1_0 =P g_1 , P z2_0 =P g_2 , P z3_0 =P g_3 , P z4_0 =P g_4 .

[0073] Furthermore, the swing foot end trajectory planning module plans the foot end motion trajectory of the moving leg in the swinging state according to the fourth-order Bezier curve, including:

[0074] Calculate the midpoint P s_1 , P s_2 , P s_3 Location:

[0075] P s_1 =P s_0

[0076] P s_2 =[x s_0 +d x / 2 y s_0 +d y / 2 h]

[0077] P s_3 =P s_d

[0078] Where P s_0 is the swing starting point position, P s_d is the end position of the swing, d x is the distance traveled in the x direction, d y is the distance traveled in the y direction, h is the step height, which is the preset value;

[0079] Calculate the planned trajectory point position P of the foot end of the moving leg in the swing state swing :

[0080] P swing =(1-u) 4 ·P s_0 +4(1-u) 3 u·P s_1 +6(1-u) 2 u 2 ·P s_2 +4(1-u)u 3 P s_3 +u 4 ·P s_d

[0081] Wherein u=t / T, t is the time elapsed during the swing process, which is a measured value; T is the swing period, which is a preset value.

[0082] Furthermore, the four-leg foot-end trajectory planning module plans the final desired motion trajectory of the four-leg foot-end in real time according to the foot-end motion trajectory of the moving leg in the swinging state and the center of gravity movement curve of the fuselage, specifically:

[0083] The desired foot position of the four legs is P c_i =[p c_i_x p c_i_y p c_i_z ]:

[0084] If the current state is 1, then:

[0085] P c_1 =P swing -P cor , Pc_2 =P z2_0 -P cor , P c_3 =P z3_0 -P cor , P c_4 =P z4_0 -P cor

[0086] If the current state is 2, then:

[0087] P c_2 =P swing -P cor , P c_1 =P z1_0 -P cor , P c_3 =P z3_0 -P cor , P c_4 =P z4_0 -P cor

[0088] If the current state is 3, then:

[0089] P c_3 =P swing -P cor , P c_1 =P z1_0 -P cor , P c_2 =P z2_0 -P cor , P c_4 =P z4_0 -P cor

[0090] If the current state is 4, then:

[0091] P c_4 =P swing -P cor , P c_1 =P z1_0 -P cor , P c_2 =P z2_0 -P cor , P c_3 =P z3_0 -P cor

[0092] Fuselage center of gravity position P cor =[x cor y cor h w ].

[0093] Furthermore, the joint angle inverse solution module obtains the expected joint angle according to the expected motion trajectory by inverse solution, including:

[0094] Calculate the foot end distance of the detector moving leg i, the calculation method is as follows:

[0095] The expected foot position P of the detector moving leg i c_i =[p c_i_x p c_i_y p c_i_z ], foot end distance

[0096] Calculate the virtual angle θ i_4 and virtual height h i ;

[0097]

[0098]

[0099] Calculate the joint inverse angle θ i_c =[θ i_1_c θ i_2_c θ i_3_c ],

[0100] θ i_1_c =a tan 2(h i ·p i_z ,h i ·p i_y )

[0101] θ i_2_c =a tan 2(h i , p i_x )+θ i_4 .

[0102] θ i_3_c =θ i_2_c -2θ i_4

[0103] The advantages of the present invention compared with the prior art are:

[0104] The fast static gait planning method for the quadruped detector proposed in the present invention can, under the premise of ensuring the stability of the fuselage, move the center of gravity of the fuselage in an "8"-shaped curve while the foot end of the moving leg is swinging in a swinging state under the three-leg support state, eliminating the process of adjusting the center of gravity of the fuselage four times under the support of four legs in the traditional static gait planning method, thereby significantly improving the travel speed of the detector and reducing energy consumption. BRIEF DESCRIPTION OF THE DRAWINGS

[0105] Figure 1 This is a schematic diagram of the composition of the quadruped detector;

[0106] Figure 2 A block diagram of the gait planning method for a quadruped detector is provided;

[0107] Figure 3 This is the flow chart for foot end position calculation;

[0108] Figure 4 Develop a flow chart for the fuselage center of gravity curve planning;

[0109] Figure 5 It is a walking phase switching flow chart;

[0110] Figure 6 Flowchart for planning the trajectory of the four-legged foot ends;

[0111] Figure 7 This is the flow chart for inverse solution of joint angles;

[0112] Figure 8 It is a schematic diagram of the movement trajectory of the center of gravity of the "8"-shaped fuselage. DETAILED DESCRIPTION

[0113] The present invention realizes the fast static gait planning of the quadruped detector. During the movement of the detector, an "8"-shaped fuselage center of gravity position movement curve is dynamically generated according to the projection point of the foot end of the moving leg in the swinging state on the ground and the dynamically changing quadrilateral formed by the contact point between the foot end of the moving leg in the supporting state and the ground. The fuselage center of gravity tracks the "8"-shaped curve by real-time control of the configuration of the moving leg in the supporting state. In the three-legged supporting state, the fuselage center of gravity position is adjusted along with the swinging of the foot end of the moving leg in the swinging state, so that the fuselage center of gravity position adjustment and the stepping action are carried out simultaneously, eliminating the process of adjusting the fuselage center of gravity four times under the four-legged support in the traditional static gait planning method, and improving the moving speed of the detector.

[0114] The quadruped detector corresponding to the present invention is as follows Figure 1 As shown, the quadruped detector consists of a fuselage platform and moving legs. The platform connects 4 sets of moving legs in the same state and bears the weight of the detector structure at the same time. The moving legs have 3 degrees of freedom, and their ends can move arbitrarily in three-dimensional space. Specifically, the quadruped detector includes a fuselage 1, a first moving leg 2, a second moving leg 3, a third moving leg 4, and a fourth moving leg 5. The first moving leg 2, the second moving leg 3, the third moving leg 4, and the fourth moving leg 5 are connected to the outer peripheral surface of the fuselage 1 and are evenly distributed along the outer peripheral surface of the platform 1. The states of the first moving leg 2, the second moving leg 3, the third moving leg 4, and the fourth moving leg 5 are completely consistent.

[0115] The components of the fast static gait planning system of the quadruped detector are as follows: Figure 2The system includes: a foot end position solving module, a fuselage center of gravity curve generating module, a walking phase switching module, a swing foot end trajectory planning module, a four-leg foot end trajectory planning module, and a joint angle inverse solving module;

[0116] The foot end position calculation module calculates the position of the foot end in the coordinate system of the detector body, i.e., the foot end coordinate, in real time according to the leg joint angle, and inputs the result into the body center of gravity curve generation module;

[0117] The fuselage center of gravity curve generation module generates a dynamically changing "8"-shaped fuselage center of gravity movement curve according to the foot end coordinates and movement time;

[0118] The walking phase switching module switches the moving leg between the swinging state and the supporting state according to the ground contact state of the moving leg in the swinging state, and obtains the starting foot end position of the moving leg in the swinging state;

[0119] The swing foot end trajectory planning module plans the swing leg foot end motion trajectory according to the fourth-order Bezier curve;

[0120] The four-leg foot end trajectory planning module plans the final desired motion trajectory of the four-leg foot end in real time according to the foot end motion trajectory of the moving leg in the swinging state and the center of gravity movement curve of the fuselage, and inputs it into the joint angle inverse solution module;

[0121] The joint angle inverse solution module obtains the desired joint angle according to the desired motion trajectory, and realizes the static gait control of the quadruped detector through the joint angle closed-loop control.

[0122] The calculation process of the foot end position solution module is as follows: Figure 3 shown.

[0123] Step (1), measure the joint angle θ of the detector moving leg i i =[θ i_1 θ i_2 θ i_3 ], i is the moving leg number (i = 2, 3, 4, 5). The thigh length of each moving leg is L upper , calf length is L lower . Measure the roll angle ψ and pitch angle β of the detector relative to the inertial coordinate system.

[0124] Step (2), calculate the position P of the foot end of the moving leg i of the detector in the leg body coordinate system i =[p i_x p i_y p i_z ], i is the moving leg number (i=2, 3, 4, 5).

[0125] p i_x =L upper cosθi_2 +L lower cosθ i_3

[0126] p i_y =(L upper sinθ i_2 +L lower sinθ i_3 )cosθ i_1

[0127] p i_z =(L upper sinθ i_2 +L lower sinθ i_3 )sinθ i_1

[0128] Step (3), calculate the installation rotation matrix C of the mobile leg i a_i , i is the moving leg number (i=2, 3, 4, 5).

[0129]

[0130] where α i is the installation angle of the mobile leg i relative to the fuselage.

[0131] Step (4), calculate the rotation matrix C from the body coordinate system to the inertial coordinate system.

[0132]

[0133] Step (5), calculate the position P of the foot end of the moving leg i in the inertial coordinate system g_i =[p g_i_x p g_i_y p g_i_z ], i is the moving leg number (i=2, 3, 4, 5).

[0134] P g_i =C·(P a_i +C a_i ·P i )(i=2,3,4,5)

[0135] Where P a_i is the installation position matrix of mobile leg i, which is a fixed value obtained through measurement.

[0136] The calculation method of the fuselage center of gravity curve generation module is as follows: Figure 4 shown.

[0137] Step (1), calculate the intersection position P of the diagonal lines of the quadrilateral formed by the projection points of the feet of the first moving leg 2, the second moving leg 3, the third moving leg 4, and the fourth moving leg 5 on the groundcross =[x cross y cross ] T .

[0138]

[0139] in, b 11 =p g_1_y -p g_1_x k1, b 12 =p g_2_y -p g_2_x ·k2.

[0140] Step (2), calculate the incenter P of the triangle formed by the projection point of the foot end of the first moving leg (2) and the fourth moving leg (5) and the intersection point of the diagonal line inner_1 =[p inner1_x p inner1_y ] T .

[0141]

[0142]

[0143]

[0144]

[0145]

[0146] Step (3), calculate the incenter P of the triangle formed by the projection point of the foot end of the first moving leg (2) and the intersection point of the diagonal lines of the second moving leg (3) inner_2 =[p inner2_x p inner2_y ] T .

[0147]

[0148]

[0149]

[0150]

[0151]

[0152] Step (4), calculate the angle bisector vector A lr =[x lr y lr ]、A fb =[xfb y fb ].

[0153]

[0154] in

[0155]

[0156] in

[0157] Step (5), calculate the center of gravity position P of the fuselage on the "8"-shaped trajectory cor =[x cor y cor h w ], where h w It is the height of the fuselage during walking, which is the preset value.

[0158] x cor =x cross +f lr ·x lr ·sin(θ-π / 2)+f fb ·x fb sin(2θ)

[0159] y cor =y cross +f lr ·y lr ·sin(θ-π / 2)+f fb ·y fb sin(2θ)

[0160] where f lr and f fb It is the preset value, used to control the size of the "8" shaped track; Where T a is the total time required for all four legs of the detector to take a step, which is the preset value; t a is the time elapsed during the journey, and is the measured value.

[0161] The calculation process of the walking phase switching module is as follows: Figure 5 shown.

[0162] Step (1), measure the joint angle θ of the detector moving leg i i =[θ i_1 θ i_2 θ i_3 ], i is the moving leg number (i = 2, 3, 4, 5). Measure the joint torque τ of the moving leg i i =[τ i_1 τ i_2 τi_3 ] T , the thigh length of each moving leg is L upper , calf length is L lower .

[0163] Step (2), calculate the Jacobian matrix J of each leg i

[0164]

[0165] Step (3), calculate the foot end force matrix F of each leg i =[f x f y f z ] T

[0166] F i =(J i T ) -1 ·τ i

[0167] Step (4), determine whether the swinging leg touches the ground.

[0168] The foot force F of the moving leg currently in the swing state i =[f x f y f z ] T , if f z ≤f threh , then the moving leg is considered to have touched the ground, where f threh is the default value.

[0169] Step (5): If the moving leg in the swinging state touches the ground, the walking phase is switched and the swing starting point position P of the new moving leg in the swinging state is calculated. s_0 and the swing end position P s_d , and calculate the foot end starting point position P of the first moving leg 2, the second moving leg 3, the third moving leg 4, and the fourth moving leg 5 z1_0 , P z2_0 , P z3_0 , P z4_0 , the calculation method is as follows:

[0170] For walking phase state:

[0171] If the current state is 1, update the state to 3, P s_0 =P z3_0 =P g_3 , P z1_0 =P g_1 , P z2_0 =Pg_2 , P z4_0 =P g_4 ;

[0172] If the current state is 2, update the state to 4, P s_0 =P z4_0 =P g_4 , P z1_0 =P g_1 , P z2_0 =P g_2 , P z3_0 =P g_3 ;

[0173] If the current state is 3, update the state to 2, P s_0 =P z2_0 =P g_2 , P z1_0 =P g_1 , P z3_0 =P g_3 , P z4_0 =P g_4 ;

[0174] If the current state is 4, update the state to 1, P s_0 =P z1_0 =P g_1 , P z2_0 =P g_2 , P z3_0 =P g_3 , P z4_0 =P g_4 .

[0175]

[0176] Among them, d x is the distance traveled in the x direction, d y The forward distance in the y direction is the preset value.

[0177] The calculation process of the swing leg foot end trajectory planning part is as follows Figure 6 shown.

[0178] Step (1), calculate the middle point P s_1 , P s_2 , P s_3 Location.

[0179] P s_1 =P s_0

[0180] P s_2 =[x s_0 +d x / 2 y s_0+d y / 2 h]

[0181] P s_3 =P s_d

[0182] Where P s_0 is the swing starting point position, P s_d is the end position of the swing, d x is the distance traveled in the x direction, d y is the distance moved forward in the y direction, and h is the step height, which is the preset value.

[0183] Step (2), calculate the planned trajectory point position P of the foot end of the moving leg in the swinging state swing .

[0184] P swing =(1-u) 4 ·P s_0 +4(1-u) 3 u·P s_1 +6(1-u) 2 u 2 ·P s_2 +4(1-u)u 3 P s_3 +u 4 ·P s_d

[0185] Where u = t / T, t is the time elapsed during the swing process, is the measured value, and T is the swing period, is the preset value.

[0186] The calculation method of the four-leg foot trajectory planning module is as follows:

[0187] The desired foot position P for the four legs c_i =[p c_i_x p c_i_y p c_i_z ], i is the moving leg number (i=2\3\4\5):

[0188] If the current state is 1, then:

[0189] P c_1 =P swing -P cor , P c_2 =P z2_0 -P cor , P c_3 =P z3_0 -P cor , P c_4 =P z4_0 -P cor

[0190] If the current state is 2, then:

[0191] P c_2 =P swing -P cor , P c_1 =P z1_0 -P cor , P c_3 =P z3_0 -P cor , P c_4 =P z4_0 -P cor

[0192] If the current state is 3, then:

[0193] P c_3 =P swing -P cor , P c_1 =P z1_0 -P cor , P c_2 =P z2_0 -P cor , P c_4 =P z4_0 -P cor

[0194] If the current state is 4, then:

[0195] P c_4 =P swing -P cor , P c_1 =P z1_0 -P cor , P c_2 =P z2_0 -P cor , P c_3 =P z3_0 -P cor

[0196] The calculation process of the joint angle inverse solution module is as follows: Figure 7 shown.

[0197] Step (1), calculate the foot end distance of the detector moving leg i, the calculation method is as follows:

[0198] The expected foot position P of the detector moving leg i c_i =[p c_i_x p c_i_y p c_i_z ], foot end distance i is the moving leg number (i=2\3\4\5).

[0199] Step (2), calculate the virtual angle θi_4 and virtual height h i , i is the moving leg number (i=2\3\4\5). The thigh length of each moving leg is L upper , calf length is L lower .

[0200]

[0201]

[0202] Step (3), calculate the joint inverse angle θ i_c =[θ i_1_c θ i_2_c θ i_3_c ], i is the moving leg number (i=2\3\4\5).

[0203] θ i_1_c =a tan 2(h i ·p i_z ,h i ·p i_y )

[0204] θ i_2_c =a tan 2(h i , p i_x )+θ i_4

[0205] θ i_3_c =θ i_2_c -2θ i_4

[0206] Example:

[0207] The "8" shaped center of gravity movement curve of the fuselage is as follows Figure 8 As shown in the figure, f1, f2, f3, f4 are the projection points of the feet of the first moving leg 2, the second moving leg 3, the third moving leg 4, and the fourth moving leg 5 of the quadruped detector on the ground, point C is the projection of the center of gravity of the fuselage on the ground, point SL is the top of the "8" shaped curve, point SR is the bottom of the "8" shaped curve, and the detector moves in the direction of the arrow in the figure. The "8" shaped curve is equally divided into four parts: 1, 2, 3, and 4. The movement process of the center of gravity on the curve is as follows:

[0208] Step (1), the center of gravity of the fuselage moves from the starting point C along the first part of the "8"-shaped curve to the SR point;

[0209] Step (2), the center of gravity of the fuselage moves from point SR along the first part of the "8"-shaped curve to point C, during which the first moving leg 2 is taken;

[0210] Step (3), the center of gravity of the fuselage moves from point C along the second part of the "8"-shaped curve to point SL, during which the third moving leg 4 is stepped;

[0211] Step (4), the center of gravity of the fuselage moves from point SL along the third part of the "8"-shaped curve to point C, during which the second moving leg 3 is stepped;

[0212] Step (4), the center of gravity of the fuselage moves from point C along the fourth part of the "8"-shaped curve to point SR. During this process, the fourth moving leg 5 is stepped forward, and the detector completes a one-step forward movement;

[0213] Step (5), the center of gravity of the fuselage continues to the next step according to step (2).

[0214] This embodiment provides a specific process of fast static gait planning of a quadruped detector as follows:

[0215] Step 1: The center of gravity of the quadruped detector platform 1 moves to above the center point of the triangle formed by the foot ends of the second moving leg 3, the third moving leg 4, and the fourth moving leg 5 contacting the ground;

[0216] Step 2: The first moving leg 2 steps forward with its foot end hanging in the air. The foot ends of the second moving leg 3, the third moving leg 4, and the fourth moving leg 5 remain in contact with the ground. The center of gravity of the platform 1 moves in an "8"-shaped curve within the quadrilateral formed by the projection point of the foot end of the first moving leg 2 on the ground and the contact points of the foot ends of the second moving leg 3, the third moving leg 4, and the fourth moving leg 5 with the ground. When the foot end of the first moving leg 2 touches the ground, the center of gravity of the platform 1 moves to above the intersection of the diagonals of the quadrilateral formed by the contact points of the foot ends of the first moving leg 2, the second moving leg 3, the third moving leg 4, and the fourth moving leg 5 with the ground;

[0217] Step 3: The third moving leg 4 steps forward with its foot end hanging in the air. The foot ends of the first moving leg 2, the second moving leg 3, and the fourth moving leg 5 remain in contact with the ground. The center of gravity of the platform 1 is within the quadrilateral formed by the projection point of the foot end of the third moving leg 4 on the ground and the contact points of the foot ends of the first moving leg 2, the second moving leg 3, and the fourth moving leg 5 with the ground, and continues to move in an "8"-shaped curve. When the foot end of the third moving leg 4 touches the ground, the center of gravity of the platform 1 moves to the area adjacent to the center point of the triangle formed by the contact points of the foot ends of the first moving leg 2, the second moving leg 3, and the fourth moving leg 5 with the ground;

[0218] Step 4: The second moving leg 3 steps forward with its foot end hanging in the air. The foot ends of the first moving leg 2, the third moving leg 4, and the fourth moving leg 5 remain in contact with the ground. The center of gravity of the platform 1 is within the quadrilateral formed by the projection point of the foot end of the second moving leg 3 on the ground and the contact points of the foot ends of the first moving leg 2, the third moving leg 4, and the fourth moving leg 5 with the ground, and continues to move in an "8"-shaped curve. When the foot end of the second moving leg 3 touches the ground, the center of gravity of the platform 1 moves to above the intersection of the diagonals of the quadrilateral formed by the contact points of the foot ends of the first moving leg 2, the second moving leg 3, the third moving leg 4, and the fourth moving leg 5 with the ground;

[0219] Step 5: The fourth moving leg 5 steps forward, with its foot end hanging in the air. The foot ends of the first moving leg 2, the second moving leg 3, and the third moving leg 4 remain in contact with the ground. The center of gravity of the platform 1 is within the quadrilateral formed by the projection point of the foot end of the fourth moving leg 5 on the ground and the contact points of the foot ends of the first moving leg 2, the second moving leg 3, and the third moving leg 4 with the ground, and continues to move in an "8"-shaped curve. When the fourth moving leg 5 touches the ground, the center of gravity of the platform 1 returns to the area near the center point of the triangle formed by the contact points of the first moving leg 2, the second moving leg 3, and the third moving leg 4 with the ground.

[0220] Parts of the present invention that are not described in detail belong to common knowledge among those skilled in the art.

Claims

1. A fast static gait planning system for quadruped detectors, characterized in that include: Foot position calculation module, fuselage center of gravity curve generation module, walking phase switching module, swing foot trajectory planning module, four-leg foot trajectory planning module, joint angle inverse solution module; The foot end position calculation module calculates the position of the foot end in the coordinate system of the detector body, i.e., the foot end coordinate, in real time according to the leg joint angle, and inputs the result into the body center of gravity curve generation module; The fuselage center of gravity curve generation module generates a dynamically changing "8"-shaped fuselage center of gravity movement curve according to the foot end coordinates and movement time; The walking phase switching module switches the moving leg between the swinging state and the supporting state according to the ground contact state of the moving leg in the swinging state, and obtains the starting foot end position of the moving leg in the swinging state; The swing foot end trajectory planning module plans the foot end motion trajectory of the moving leg in the swing state according to the fourth-order Bezier curve; The four-leg foot-end trajectory planning module plans the final desired motion trajectory of the foot-ends of the four moving legs in real time according to the foot-end motion trajectory of the moving leg in the swinging state and the center of gravity movement curve of the fuselage, and inputs it into the joint angle inverse solution module; The joint angle inverse solution module obtains the desired joint angle according to the desired motion trajectory, and realizes the static gait control of the quadruped detector through the joint angle closed-loop control; The foot end position calculation module calculates the position of the foot end in the detector body coordinate system in real time according to the leg joint angle, including: Joint angle measurement: Measure the angle θ of each joint of the moving leg i of the detector i =[θ i_1 θ i_2 θ i_3 q, i = 2, 3, 4, 5 are the moving leg numbers; the thigh length of each moving leg is L upper , the calf length is L lower ; Measure the rolling angle ψ and pitch angle β of the detector relative to the inertial coordinate system; Calculate the position P of the foot end of the moving leg i of the detector in the leg body coordinate system i =[p i_x p i_y p i_z ]; Calculate the mounting rotation matrix C of mobile leg i a_i ; Calculate the rotation matrix C from the body coordinate system to the inertial coordinate system; Calculate the position P of the foot end of the moving leg i in the inertial coordinate system g_i =[pg_i_x pg_i_y pg_i_z]; The fuselage center of gravity curve generation module generates a dynamically changing "8"-shaped fuselage center of gravity movement curve according to the foot end coordinates and movement time, including: Calculate the intersection position P of the diagonal lines of the quadrilateral formed by the projection points of the first moving leg, the second moving leg, the third moving leg, and the fourth moving leg on the ground cross =[x cross y cross ] T ; Calculate the incenter P of the triangle formed by the projection points of the first moving leg and the fourth moving leg and the intersection point of the diagonal of the quadrilateral inner_1 =[p inner1_x p inner1_y ] T ; Calculate the incenter P of the triangle formed by the projection points of the first moving leg, the second moving leg and the intersection point of the diagonal of the quadrilateral inner_2 =[p inner2_x p inner2_y ] T ; Calculate the angle bisector vector A lr =[x lr y lr ]、A fb =[x fb y fb ]; Calculate the center of gravity position P of the fuselage on the "8" shaped trajectory cor =[x cor y cor h w ], where h w is the height of the fuselage during walking, which is the preset value; The walking phase switching module switches the swing state and the supporting state of the moving leg according to the ground contact state of the moving leg in the swing state, and obtains the starting foot end position of the moving leg in the swing state, including: Measure the joint angle θ of the moving leg i of the detector i =[θ i_1 θ i_2 θ i_3 ], i is the moving leg number; measure the joint torque τ of moving leg i i =[τ i_1 τ i_2 τ i_3 ] T ; Calculate the Jacobian matrix J of each moving leg i ; Calculate the foot-end force matrix F of each moving leg i =[f x f y f z ] T ; There is F i =(J i T ) -1 ·τ i ; Determine whether the moving leg in the swinging state touches the ground: The foot force F of the moving leg currently in the swing state i =[f x f y f z ] T , if f z ≤f thre , then the moving leg is considered to have touched the ground, where f threh is the default value; If the moving leg currently in the swing state touches the ground, the walking phase is switched and the swing starting point position P of the new moving leg in the swing state is calculated. s_0 and the swing end position P s_d , and calculate the starting point position P of the foot end of the first moving leg, the second moving leg, the third moving leg, and the fourth moving leg z1_0 , P z2_0 , P z3_0 , P z4_0 , Among them, d x is the distance traveled in the x direction, d y The forward distance in the y direction is the preset value.

2. A quadruped detector fast static gait planning system according to claim 1, characterized in that: Calculate the position P of the foot end of the moving leg i of the detector in the leg body coordinate system i =[p i_x p i_y p i_z ], specifically: p i_x =L upper cosθ i_2 +L lower cosθ i_3 p i_y =(L upper sinθ i_2 +L lower sinθ i_3 )cosθ i_1 p i_z =(L upper sinθ i_2 +L lower sinθ i_3 )sinθ i_1 Install the rotation matrix where α i is the installation angle of the mobile leg i relative to the fuselage; Rotation Matrix The position of the foot end of the moving leg i in the inertial coordinate system is P g_i =C(P a_i +C a_i ·P i ), where P a_i is the installation position matrix of mobile leg i, which is a fixed value obtained through measurement.

3. A quadruped detector fast static gait planning system according to claim 1, characterized in that: The intersection position P of the diagonal lines of the quadrilateral formed by the projection points of the first moving leg, the second moving leg, the third moving leg and the fourth moving leg on the ground cross =[x cross y cross ] T for: in, b 11 =p g_1_y -p g_1_x k1, b 12 =p g_2_y -p g_2_x k2; The incenter P of the triangle formed by the projection points of the first moving leg and the fourth moving leg and the intersection point of the diagonal line of the quadrilateral inner_1 =[p inner1_x p inner1_y ] T for: The incenter P of the triangle formed by the projection points of the first moving leg, the second moving leg and the intersection point of the diagonal line of the quadrilateral inner_2 =[p inner2_x p inner2_y ] T for: Angle bisector vector A lr =[x lr y lr ]、A fb =[x fb y fb ]for: in in The position of the center of gravity of the fuselage on the "8" shape trajectory P cor =[x cor y cor h w ]for: x cor =x cross +f lr ·x lr ·sin(θ-π / 2)+f fb ·x fb ·sin(2θ) y cor =y cross +f lr ·y lr ·sin(θ-π / 2)+f fb ·y fb ·sin(2θ) where f lr and f fb It is the preset value, used to control the size of the "8" shaped track; Where T a is the total time required for all four legs of the detector to take a step, which is the preset value; t a is the time elapsed during the journey, and is the measured value.

4. A quadruped detector fast static gait planning system according to claim 1, characterized in that: Jacobian matrix J i for: The starting point position P of the foot end of the first moving leg, the second moving leg, the third moving leg, and the fourth moving leg z1_0 , P z2_0 , P z3_0 , P z4_0 , the calculation method is as follows: For walking phase state: If the current state is 1, update the state to 3, P s_0 =P z3_0 =P g_3 , P z1_0 =P g_1 , P z2_0 =P g_2 , P z4_0 =P g_4 ; If the current state is 2, update the state to 4, P s_0 =P z4_0 =P g_4 , P z1_0 =P g_1 , P z2_0 =P g_2 , P z3_0 =P g_3 ; If the current state is 3, update the state to 2, P s_0 =P z2_0 =P g_2 , P z1_0 =P g_1 , P z3_0 =P g_3 , P z4_0 =P g_4 ; If the current state is 4, update the state to 1, P s_0 =P z1_0 =P g_1 , P z2_0 =P g_2 , P z3_0 =P g_3 , P z4_0 =P g_4 .

5. A quadruped detector fast static gait planning system according to claim 4, characterized in that: The swing foot end trajectory planning module plans the foot end motion trajectory of the moving leg in the swinging state according to the fourth-order Bezier curve, including: Calculate the midpoint P s_1 , P s_2 , P s_3 Location: P s_1 =P s_0 P s_2 =[x s_0 +d x / 2y s_0 +d y / 2h] P s_3 =P s_d Where P s_0 is the swing starting point position, P s_d is the end position of the swing, d x is the distance traveled in the x direction, d y is the distance traveled in the y direction, h is the step height, which is the preset value; Calculate the planned trajectory point position P of the foot end of the moving leg in the swing state swing : P swing =(1-u) 4 ·P s_0 +4(1-u) 3 uP s_1 +6(1-u) 2 u 2 ·P s_2 +4(1-u)u 3 P s_3 +in 4 ·P s_d Wherein u=t / T, t is the time elapsed during the swing process, which is a measured value; T is the swing period, which is a preset value.

6. A quadruped detector fast static gait planning system according to claim 5, characterized in that: The four-leg foot-end trajectory planning module plans the final desired motion trajectory of the four-leg foot-end in real time according to the foot-end motion trajectory of the moving leg in the swinging state and the center of gravity movement curve of the fuselage, specifically: The desired foot position of the four legs is P c_i =[p c_i_x p c_i_y p c_i_z ]: If the current state is 1, then: P c_1 =P swing -P cor ,P c_2 =P z2_0 -P cor ,P c_3 =P z3_0 -P cor ,P c_4 =P z4_0 -P cor If the current state is 2, then: P c_2 =P swing -P cor ,P c_1 =P z1_0 -P cor ,P c_3 =P z3_0 -P cor ,P c_4 =P z4_0 -P cor If the current state is 3, then: P c_3 =P swing -P cor ,P c_1 =P z1_0 -P cor ,P c_2 =P z2_0 -P cor ,P c_4 =P z4_0 -P cor If the current state is 4, then: P c_4 =P swing -P cor ,P c_1 =P z1_0 -P cor ,P c_2 =P z2_0 -P cor ,P c_3 =P z3_0 -P cor Fuselage center of gravity position P cor =[x cor y cor h w ].

7. A quadruped detector fast static gait planning system according to claim 6, characterized in that: The joint angle inverse solution module obtains the expected joint angle according to the expected motion trajectory by inverse solution, including: Calculate the foot end distance of the detector moving leg i, the calculation method is as follows: The expected foot position P of the detector moving leg i c_i =[p c_i_x p c_i_y p c_i_z ], foot end distance Calculate the virtual angle θ i_4 and virtual height h i ; Calculate the joint inverse angle θ i_c =[θ i_1_c θ i_2_c θ i_3_c ], θ i_1_c =atan2(h i ·p i_z ,h i ·p i_y ) i i_2_c =atan2(h i ,p i_x )+θ i_4 i i_3_c =θ i_2_c -2θ i_4 。

Images