Quadruped robot foot end track motion control method based on local terrain self-adaption

Abstract

The invention discloses a quadruped robot foot end track motion control method based on local terrain self-adaption, and relates to the field of robot motion control. According to the robot, firstly, a foot end three-dimensional force sensor and a machine body IMU sensor of the quadruped robot are combined with a quadruped robot machine body kinematics forward solution algorithm model to classify current motion terrains, and self-adaptive control based on the terrains is carried out according to different terrains; the slope angle of the terrain is calculated, the output angle of a joint actuator is adjusted to complete self-adaptive control over the slope terrain, three attitude angles of the quadruped robot are measured through an IMU sensor of a robot body, and therefore a rotation matrix BWR from a coordinate system of the robot body of the quadruped robot to a world coordinate system is obtained, and the motion trail of the legs of the quadruped robot is planned. The stability of the robot in the moving process can be guaranteed, impact on parts of a robot body due to topographic changes is reduced, and the effective working range of the robot is widened.

Description

Technical Field

[0001] The present invention relates to the technical field of robot motion control, and particularly to a foot-end trajectory motion control method for a quadruped robot based on local terrain adaptation, which enables the quadruped robot to perceive changes in the motion terrain through multi-sensor information fusion technology during motion, and complete terrain adaptive motion control by changing the motion foot-end trajectory. Background Art

[0002] Quadruped robots generally perform local detection and precise modeling of the terrain environment through self-carried environmental perception devices to obtain terrain parameters, and make optimal choices for the robot's foot landing points based on this terrain information. Most existing research schemes adopt the method of modeling the current motion terrain by vision to control the motion of quadruped robots.

[0003] In order to further improve the terrain adaptation control ability of quadruped robots, the quadruped robots must accurately estimate the current terrain information, their own motion and attitude information during motion and make corresponding adaptive adjustment strategies to adapt to terrain changes and enhance the stability of robot motion.

[0004] Therefore, the present invention proposes a foot-end trajectory motion control method for a quadruped robot based on local terrain adaptation, which improves the stability and obstacle-crossing ability of the robot's motion. Summary of the Invention

[0005] To solve the deficiencies of existing terrain adaptation methods, the present invention proposes a foot-end trajectory motion control method for a quadruped robot based on local terrain adaptation. This method classifies the current motion terrain through detection devices such as the three-dimensional force sensor at the robot's foot end and the body IMU sensor, and combines the forward kinematic solution algorithm model of the quadruped robot body. Based on different terrains, it changes the robot's motion state, and provides a quadruped robot motion control method to complete the adaptation to the current terrain.

[0006] The technical solution of the present invention is as follows:

[0007] A foot-end trajectory motion control method for a quadruped robot based on local terrain adaptation includes the following steps:

[0008] S1: Step of obtaining external terrain information: Classify the current motion terrain through detection devices such as the three-dimensional force sensor at the robot's foot end and the body IMU sensor, and combine the forward kinematic solution algorithm model of the quadruped robot body into flat terrain, concave-convex terrain, and slope terrain.

[0009] S2: Robot body attitude perception step: Obtain the touchdown state of the robot's feet, body attitude angles, leg joint encoder information, spinal joint angle information, and foot end position information, and fuse the information from multiple sensors to estimate the robot's own motion attitude and motion state.

[0010] S3: Slope terrain slope angle estimation step: Solve the forward kinematics to obtain the foot end position of the robot in the body coordinate system, and solve the rotation matrix from the body coordinate system to the world coordinate system according to the body attitude angles. Solve the representation of the robot's foot end position in the world coordinate system, and use the least squares method to solve the plane coefficients based on the terrain plane estimation formula and the robot's foot end information. Calculate the terrain angle pitch angle θ and roll angle

[0011] S4: Flat terrain motion control step: When it is judged that the current terrain is flat terrain, the robot adopts a diagonal trot gait to quickly pass through the current flat terrain according to the motion control requirements.

[0012] Rough terrain motion control step: When it is judged that the current terrain is rough terrain, the robot controls the foot end motion trajectory of the robot by combining the feedback quantity and the CPG output signal to complete the motion control strategy for rough terrain.

[0013] Slope terrain motion control step: When it is judged that the current terrain is slope terrain, the robot controls the motion of the hip motors of the quadruped robot according to the pitch angle and roll angle information of the current terrain, adjusts the hip angles of the quadruped robot to adapt to the slope of the terrain, locks the spinal joint motor when going uphill to ensure that the robot's body is parallel to the slope of the current terrain. When climbing over a step terrain, unlock the spinal joint motor and adjust the spinal joint angle to enhance the climbing and obstacle-crossing ability of the quadruped robot.

[0014] S5: Detection step: Set the thresholds of the yaw angle, pitch angle, and roll angle of the robot's body attitude angle to be α f , β f , γ f , and judge the effectiveness of the adaptive foot end trajectory motion control method of the quadruped robot according to the feedback of the robot's motion state information.

[0015] The terrain classification method in S1 is: Based on the three-dimensional force sensing sensor installed at the foot end of the quadruped robot, judge the contact situation between the foot end and the ground in the current motion state. The higher the pressure value of the foot end pressure sensor, the higher the probability of the foot end contacting the ground. By setting the foot end pressure threshold p f to judge whether the foot end is in full contact with the ground. When the foot end pressure p z ≥ p f indicates that the foot end corresponding to this pressure value is already in full contact with the ground. When pz ≤ p f When it indicates that the foot end corresponding to this pressure value is not fully in contact with the ground, the contact information is not credible at this time; judge the next terrain information according to the state of the foot end being fully in contact with the ground: set the initial motion state of the robot, obtain the Z-axis coordinate of the robot in the world coordinate system at this time. When the pressure sensor shows a contact state and the gait is in the support phase for several consecutive motion cycles and the Z-axis coordinate does not change, it is judged that the current terrain is flat terrain; when the foot end pressure sensor signal shows early or delayed contact during the motion gait cycle, the Z-axis coordinate value changes, and it is judged that the current terrain is uneven terrain or slope terrain; at this time, apply the foot end force perception mechanism of 4 one-dimensional force sensors FSS. When the external force F t acts on the foot end three-dimensional force perception mechanism, it is decomposed into 3 components along the coordinate axes (i.e., F x , F y , F z ). In the X direction, the component F x of the external force is balanced by the components of the support force changes of the first FSS and the third FSS in the X direction; in the Y direction, the component F y of the external force is balanced by the components of the support force changes of the second FSS and the fourth FSS in the Y direction; in the Z direction, the component F z of the external force is balanced by the components of the support force changes of the 4 FSS sensors in the Z direction. According to the installation layout scheme and working principle of the sensors, F t can be obtained:

[0016]

[0017] where F1, F2, F3, and F4 represent the changes in the support forces of the 4 FSS force sensors. According to the output signals of the 4 force sensors, the magnitude and direction of the foot end contact force can be calculated through the decoupling matrix. If the magnitude and direction of the detected force remain unchanged in each motion gait cycle, it is judged that the current terrain is slope terrain; if the magnitude and direction of the detected force are continuously changing in each motion gait cycle, it is judged that the current terrain is uneven terrain; classify the current motion terrain in combination with the position information of the foot end point to provide feedforward information for the adaptive control of the quadruped robot.

[0018] The method for fuselage attitude perception in S2 is: when it is judged that the four foot ends of the robot are fully in contact with the ground and in the support state according to the information of the foot end three-dimensional pressure sensor, rely on the joint motor encoders of the robot, and combine with the kinematic model of the robot to obtain the foot end position and motion speed of the robot at this moment. Three attitude angles are output externally through the attitude fusion algorithm built in the fuselage IMU sensor of the quadruped robot. The attitude angles are the yaw angle α, the pitch angle β, and the roll angle γ respectively.

[0019] The steps for estimating the terrain slope angle in S3 are as follows: Solve the rotation matrix from the body coordinate system to the world coordinate system based on the yaw angle α, pitch angle β, and roll angle γ of the robot's attitude. The solution formula is:

[0020]

[0021] In the above formula, c represents cos and s represents sin. Then the representation of the foot end in the world coordinate system is:

[0022]

[0023] Where W C x W C y W C z T is the representation of the body centroid coordinate system in the world coordinate system, B x B y B z] T represents the representation of the position of the robot's foot end in the body coordinate system. The latest touchdown position of each leg of the quadruped robot in the world coordinate system can be updated as: W p i =[x i y i z i T , i = 1, 2, 3, 4. According to the terrain plane estimation formula: z = b0 + b1x + b2y to fit the terrain plane, where b0, b1, and b2 in the formula are plane coefficients. Use the least squares method to minimize the residual of the coordinate z value, and the regression coefficients obtained by solving are the coefficients of the plane expression. The solution formula is: A and Z in the formula are:

[0024]

[0025] Based on the coefficients of the fitted terrain plane formula, calculate the terrain angles of the pitch angle θ and the roll angle which are respectively

[0026]

[0027]

[0028] The specific method of S4 control is: Control the motion gait of the quadruped robot through the rhythm signal output by the central pattern generator of the quadruped robot, and complete the control of the four legs by constructing four Hopf oscillators. The CPG topology network constructed by the Hopf oscillator is as follows:

[0029] ​​

[0030] In the above formula: x hi is the signal output by the oscillator for controlling the pitch hip joint of the robot, and y ki is the signal output by the oscillator for controlling the knee joint of the robot, where the second term represents the coupling term of the oscillator is the rotation matrix, used to represent the phase coupling relationship between each Hopf oscillator. By changing the value of, the switching of different gait patterns can be achieved.

[0031] The adaptive control method for uneven terrain is as follows: The robot performs feedback control on the foot end trajectory of the robot's movement according to the actual terrain information, and adjusts the movement by superimposing the CPG output signal and the feedback control signal. The mathematical model is as follows:

[0032]

[0033] In the formula, X i , Y i are the input quantities of the actual hip joint and knee joint angle signals, x hi , y hi are the hip joint and knee joint angle signals output by the original CPG oscillator, a i , k i are the feedback quantities of the hip joint and knee joint trajectory planning, used to control the interaction with the outside world according to the current uneven terrain change, so as to better achieve terrain adaptability.

[0034] The solution process of the knee joint and hip joint angles of the quadruped robot is as follows:

[0035] Table 1-1 D-H parameter table of a single leg of the quadruped robot

[0036]

[0037] Solve the single-leg 0-4 coordinate transformation matrix according to the single-leg coordinate parameter table

[0038]

[0039] In the formula: s i =sinθ i , c i =cosθ i , s ij =sin(θ i +θ j ), c ij =cos(θ i +θ j ), i, j = 1, 2, 3

[0040] The relationship expression between the position P of the end of the right front leg in the hip joint coordinate system and the joint angles is solved by forward kinematics:

[0041]

[0042] Through the inverse kinematics solution of the robot's leg, if the position of the end of the right front leg of the quadruped robot in the hip joint coordinate system is expressed as: RF P = RF P X RF P Y RF P Z T , then the joint rotation angle expression of the right front leg of the quadruped robot can be calculated:

[0043]

[0044]

[0045]

[0046] where θ1, θ2, and θ3 are the angles of the yaw joint, hip joint, and knee joint respectively, RF p x , RF P y , RF p z are the x-axis, y-axis, and z-axis coordinates of point P at the end of the right front leg in the right front leg hip joint center coordinate system respectively. l1, l2, and l3 are the leg lengths between the yaw joint and the hip joint, between the hip joint and the knee joint, and between the knee joint and the end of the leg of the quadruped robot respectively. The changes in the knee joint and hip joint angles of the other three legs are solved similarly.

[0047] The trajectory planning completes the terrain adaptation process: The specific implementation process is that the knee joint movement control process is to swing up and lift the leg, keep the stride unchanged, and fall back to the ground; the hip joint movement control process is to swing back and lift the leg, swing forward to cross an obstacle / cross a pit, and swing forward to land. The knee joint angle trajectory planning feedback formula is:

[0048]

[0049] The hip joint angle trajectory planning feedback formula is:

[0050]

[0051] In the above formula: t fi is the moment of the reflection point of the i-th leg; T0 is the reflection control time, that is, the time of the phase change point from the swing phase to the support phase: t is the current time; a i , k i ​The angular feedback quantity for the hip joint and knee joint trajectory planning of the i-th leg; a fi , k fi are respectively the angular values of the hip joint and knee joint of the reflected leg at the reflection point of the i-th leg; a pi is the angular value of the hip joint at the commutation point of the i-th leg; feed i is the feedback term of the i-th leg; C1 is the change in knee joint angle; D1, D2 are the changes in hip joint angle. In the text, i = 1, 2, 3, 4.

[0052] The movement process of the foot-end trajectory planning is as follows: For the swing leg's leg-lifting stage: The knee joint swings upward, and the hip joint swings backward to lift the leg. The foot-end of the swing leg vertically lifts off the ground from the starting point, and after vertically lifting a certain distance, it reaches the highest point. For the forward-striding stage of the swing leg: The knee joint angle remains unchanged, and the hip joint swings forward to a certain angle. After the swing foot has experienced a swing distance of one step length to complete crossing an obstacle or a small pit, it reaches the farthest point. For the landing stage of the swing leg: The knee joint falls back, and the foot-end starts to vertically lower the leg from the farthest point. There may be situations of early landing and delayed landing, and the feedback quantity is added to adjust the landing preparation of the foot-end.

[0053] The slope adaptive control method of the robot: According to the calculated terrain angles, the pitch angle θ and the roll angle adjust the control signals of the robot's joint actuators, so that the attitude angle of the robot adapts to the slope change of the terrain. The control strategy is to achieve the adaption to the sloped terrain by adjusting the output angle signals of the robot's joint actuators:

[0054] β(k + 1) = A1 * β(k) + B1 * u1(k) + w1(k)

[0055] γ(k + 1) = A2 * γ(k) + B2 * u2(k) + w2(k)

[0056] In the formula, β(k) and γ(k) are respectively the angles of the hip joint and the side-swing joint at time k, u1(k) and u2(k) are the inputs of the pitch angle and roll angle of the quadruped robot body at time k. Among them, β(k + 1) and γ(k + 1) are the predicted values of the joint angles of the robot's hip joint and side-swing joint at time k + 1. A1, B1, A2, B2 are the coefficient matrices of the system model, and w1(k), w2(k) are the random noises representing the model prediction errors;

[0057] The optimization process of the above formula is as follows:

[0058] Minimize ∑||β(k) - θ(k)|| 2 + ∑||u1(k)|| 2 , k = 0, 1, 2, 3,...., T

[0059]

[0060] where θ(k), represents the set target at time k, that is, the pitch angle and roll angle of the calculated terrain angle at time k, and T is the optimization time series length. By solving this optimization problem, the optimal joint angle control sequence can be obtained to achieve the adaptive control of the joint angle for the slope terrain.

[0061] The detection step in S5 is as follows: When the quadruped robot is moving, the changes in the pitch angle and roll angle can best reflect the stability of the robot's movement. According to the body attitude angles of the robot during the movement state, the yaw angle α, pitch angle β, and roll angle γ are compared with the set body attitude angles of the yaw angle α f , pitch angle β f , roll angle γ f threshold values. When the values of the yaw angle α, pitch angle β, and roll angle γ detected by the IMU sensor of the robot body are all less than the set body attitude angle threshold values, it indicates that the movement of the robot body is stable.

[0062] The beneficial effects of the present invention are as follows:

[0063] 1) The present invention combines the foot-end three-dimensional force sensor and the body IMU sensor with the forward kinematics solution information of the quadruped robot body to sense the terrain information in the current movement state and classify the terrain, which can effectively provide feedforward information for the local terrain adaptive movement control of the robot.

[0064] 2) By controlling the leg movement trajectory of the quadruped robot, the quadruped robot walks more smoothly on the uneven terrain, has good stability and passability on the uneven terrain, realizes the effect of adaptive control of the uneven terrain, and makes the robot have better adaptability to the environment.

[0065] 3) The present invention adopts the slope terrain adaptive control strategy. The robot adjusts the pitch angle and roll angle of the body by combining the slope information of the terrain to adapt to walking on the slope terrain, which improves the stability of the robot's movement on the slope terrain.

[0066] 4) The effectiveness detection method of the adaptive control strategy adopted by the present invention can effectively provide feedback information for the movement control of the robot, so as to timely adjust the movement control parameters of the robot and ensure the accuracy and timeliness of the movement control. Description of the Drawings

[0067] Figure 1 is a schematic diagram of the robot coordinate system in the specific implementation manner of the present invention;

[0068] Figure 2It is the flowchart of the foot-end trajectory planning of the terrain-adaptive robot in the specific implementation manner of the present invention;

[0069] Figure 3 It is the schematic diagram of the working principle of the foot-end pressure sensor in the specific implementation manner of the present invention. Figure 3 In it: S1, S2, S3, and S4 represent 4 FSS force sensors;

[0070] Figure 4 It is the schematic diagram of the foot-end motion control of the bumpy terrain robot in the specific implementation manner of the present invention;

[0071] Figure 5 It is the schematic diagram of the motion control of the slope terrain robot in the specific implementation manner of the present invention;

[0072] Figure 6 It is the simulation effect diagram of the foot-end trajectory of the right front leg of the robot in the bumpy terrain in the specific implementation manner of the present invention;

[0073] Figure 7 It is the simulation diagram of the foot-end motion control of the quadruped robot in the bumpy terrain in the specific implementation manner of the present invention. Specific implementation manner

[0074] The following further describes the present invention in conjunction with the drawings of the specification.

[0075] As Figure 1 Shown in the schematic diagram of the robot coordinate system, the foot-end trajectory motion control method of the quadruped robot based on local terrain adaptation of the present invention is described in detail.

[0076] Establish the body coordinate system {B}, the origin of the coordinate system is located at the centroid of the body, the X-axis points to the side direction of the robot body, the Y-axis points to the forward direction of the robot, and the Z-axis is perpendicular to the plane of the body and points upward. Establish the world coordinate system {W}, the X-axis direction of this coordinate system is aligned with the side direction of the quadruped robot's body at the starting moment, the Z-axis is parallel to the direction of the gravitational acceleration and points vertically upward, and establish the right front leg side swing joint base coordinate system {RF}, the origin is at the center of the right front leg side swing joint, the Y-axis points to the forward positive direction of the body, and the Z-axis points to the direction perpendicular to the body and upward.

[0077] As Figure 2 It is the flowchart of the foot-end trajectory motion control method of the quadruped robot based on local terrain adaptation provided by the present invention, mainly including the following steps:

[0078] S1: Classify the current motion terrain through detection devices such as the foot-end three-dimensional force sensor and the body IMU sensor of the robot in combination with the forward kinematic solution algorithm model of the quadruped robot body: classify it into flat terrain, bumpy terrain, and slope terrain.

[0079] S2: Obtain the ground contact state of the robot's feet, the body attitude angle, the information of the leg joint encoders, the information of the spinal joint angles, and the information of the foot end positions. Fuse the information of multiple sensors to estimate the self-motion attitude and motion state of the robot.

[0080] S3: Solve the rotation matrix from the body coordinate system to the world coordinate system through the body attitude angle of the robot obtained in S2 , solve the foot end position of the robot in the world coordinate, estimate the plane coefficient using the least squares method in combination with the foot end information of the robot according to the terrain plane estimation formula, and calculate the terrain angle pitch angle θ and roll angle When it is judged that the current terrain is a slope terrain, the robot controls the motion of the four-legged robot joint motors according to the pitch angle and roll angle information of the current terrain, adjusts the joint angles of the four-legged robot to adapt to the slope of the terrain. When going uphill, lock the spinal joint motor to ensure that the robot's body is parallel to the slope of the current terrain. When climbing over a step terrain, unlock the spinal joint motor and adjust the spinal joint angle to enhance the climbing and obstacle-crossing ability of the four-legged robot.

[0081] S4: When it is judged that the current terrain is a flat terrain, the robot adopts a diagonal trot gait and quickly passes through the current flat terrain according to the motion control requirements. When it is judged that the current terrain is an uneven terrain, the robot plans the foot end motion trajectory of the robot by combining the feedback quantity with the CPG output signal to complete the control strategy for the motion on the uneven terrain.

[0082] S5: Set the thresholds of the yaw angle, pitch angle, and roll angle of the robot's body attitude angle to be α f , β f , γ f , and judge the effectiveness of the adaptive foot end trajectory motion control method of the four-legged robot according to the feedback of the robot motion state information.

[0083] In an embodiment of the present invention, the selected is the combined simulation platform of Webots and Simulink. By establishing the mechanical structure model of the four-legged robot in Webots, establishing the external environment model of the robot motion and installing various sensors for the robot, a simulation environment is provided for algorithm verification. Among them, the software Webots can perform combined simulation with Simulink. Webots provides a physical engine to execute instructions and generate motion data. And Simulink receives the data generated in Webots, performs data operations on the motion control algorithm, and provides the operation results to the Webots platform. In the example of the present invention, a Simulink control model is established, where the input of Simulink is the relevant gait and control parameters, and the output signal is the angles of each joint.

[0084] Build the mechanical structure model of the quadruped robot on the Webots platform, configure the robot body sensors on the Webots platform to collect information during the robot's movement, and build a CPG network for robot motion control in Simulink to output rhythmic joint angle control signals. This section introduces the mathematical model of CPG and does not constitute a limitation to the invention:

[0085]

[0086]

[0087]

[0088]

[0089] In the formula: x hi is the signal output by the oscillator to control the pitch hip joint of the robot, and y ki is the signal output by the oscillator to control the knee joint of the robot, where the second term represents the coupling term of the oscillator is the rotation matrix, which is used to represent the phase coupling relationship between each Hopf oscillator. By changing value, different gait pattern switching can be achieved. w sw and w st are the swing phase frequency and the stance phase frequency of the quadruped robot respectively; the parameter α determines the change speed of w between w sw and w st and is a positive constant; β is the load factor.

[0090] As Figure 3 shown in the schematic diagram of the working principle of the foot-end pressure sensor: Four FSS force sensors are installed in the card slots at the foot end at a 45-degree inclination angle, and the four FSS force sensors are arranged in a circular array at the foot end; before the robot motion planning, it is necessary to perceive the current motion environment information. According to the fusion algorithm of the three-dimensional force sensor information and the foot-end Z-axis coordinate information at the robot's foot end, the current motion terrain is classified into flat terrain, uneven terrain, and slope terrain, providing feedforward information for the robot motion control.

[0091] When it is judged according to the information of the foot-end three-dimensional pressure sensor that all four foot-ends of the robot are in full contact with the ground and in the support state, relying on the joint motor encoders of the robot and combining with the kinematic model of the robot, the foot-end position and motion speed of the robot at this moment are obtained. Through the attitude fusion algorithm built in the body IMU sensor of the quadruped robot, three attitude angles are output externally, and the attitude angles are the yaw angle α, the pitch angle β, and the roll angle γ respectively.

[0092] Furthermore, estimate the slope of the terrain for the robot's movement. Solve the rotation matrix from the body coordinate system to the world coordinate system based on the yaw angle α, pitch angle β, and roll angle γ of the robot's attitude. The solution formula is:

[0093]

[0094] In the above formula, cα = cosα, sα = sinα, etc. Then the representation of the foot end in the world coordinate system is:

[0095]

[0096] where W C x W C y W C z T is the representation of the body centroid coordinate system in the world coordinate system, B x B y B z] T represents the representation of the robot's foot end position in the body coordinate system. The latest touchdown position of each leg of the quadruped robot in the world coordinate system can be updated as: W p i =[x i y i z i T , i = 1, 2, 3, 4. According to the terrain plane estimation formula: z = b0 + b1x + b2, fit the terrain plane. b0, b1, and b2 in the formula are plane coefficients. Use the least squares method to minimize the residual of the coordinate z value and solve the regression

[0097] The coefficients are the coefficients of the plane expression, and the solution formula is: A and Z in the formula are:

[0098]

[0099] Based on the coefficients of the fitted terrain plane formula, calculate the terrain angles pitch angle θ and roll angle respectively as:

[0100]

[0101]

[0102] Furthermore, perform the motion control of the quadruped robot. Complete the control of the four legs by constructing four Hopf oscillators. The CPG topology network constructed by the Hopf oscillator is as follows: ​​

[0103]

[0104] When it is determined that the current terrain is flat, the robot adopts a free gait control method and passes through the current flat terrain at a relatively fast speed or in a more energy-efficient manner according to actual needs.

[0105] When it is determined that the current terrain is uneven, the robot plans the foot-end motion trajectory of the robot by combining the feedback quantity with the CPG output signal, and the control model is as follows:

[0106]

[0107] In the formula, X i , Y i are the input quantities of the actual hip and knee joint angle signals, x hi , y ki are the hip and knee joint angle signals output by the original CPG oscillator, a i , k i are the angle feedback quantities for the hip and knee joint trajectories of the i-th leg, which are used to control the interaction with the outside world according to the changes in the current uneven terrain, so as to better achieve terrain adaptability.

[0108] The solution process of the joint angles of the right front leg of the quadruped robot is as follows:

[0109] Table 1-1 D-H parameter table of a single leg of the quadruped robot

[0110]

[0111] Solve the single-leg 0-4 coordinate transformation matrix according to the single-leg coordinate parameter table

[0112]

[0113] In the formula: s i = sinθ i , c i = cosθ i , s ij = sin(θ i + θ j ), c ij = cos(θ i + θ j ), i, j = 1, 2, 3

[0114] The relationship expression between the foot-end position P of the right front leg in the hip joint coordinate system and the joint angles is solved by forward kinematics:

[0115]

[0116] Through the inverse kinematics solution of the robot leg, if the position of the foot end of the right front leg of the quadruped robot in the hip joint coordinate system is expressed as: RF P = RF P X RF P Y RF P Z T , then the joint angle expressions of the right front leg of the quadruped robot can be calculated:

[0117]

[0118]

[0119]

[0120] where θ1, θ2, and θ3 are the angles of the yaw joint, hip joint, and knee joint respectively, RF p x , RF P y , RF p z are the x-axis, y-axis, and z-axis coordinates of point P at the foot end of the right front leg in the right front leg hip joint center coordinate system respectively. The joint angles of other legs can be obtained in the same way.

[0121] The specific implementation process of the trajectory motion control is as follows: The knee joint motion control process is lifting the leg upward, maintaining the stride, and landing back. The hip joint motion control process is lifting the leg backward, swinging forward to cross an obstacle / a pit, and landing forward. In the feedback formula, the knee joint angle trajectory planning formula is:

[0122]

[0123] The hip joint angle trajectory planning formula is:

[0124]

[0125] In the above formula: t fi is the moment of the reflection point of the i-th leg; T0 is the reflection control time, that is, the time from the swing phase to the support phase commutation point: t is the current time; a i , k i are the angle feedback amounts of the hip joint and knee joint trajectory planning of the i-th leg; a fi , k fi are the angle values of the hip joint and knee joint of the reflected leg at the reflection point of the i-th leg respectively; a pi is the angle value of the hip joint at the commutation point of the i-th leg; feed i ​is the feedback term for the i-th leg; C1 is the change in knee joint angle; D1 and D2 are the changes in hip joint angles, where i = 1, 2, 3, 4 as mentioned in the text.

[0126] As Figure 4 shown in the schematic diagram of the foot-end motion control of the robot on uneven terrain: The foot-end trajectory planning motion process on uneven terrain is as follows: For the swing leg's lifting phase: The knee joint swings upward, and the hip joint swings backward to lift the leg. The foot-end of the swing leg lifts vertically off the ground from the starting point, reaches the highest point after lifting vertically a certain distance. For the swing leg's forward striding phase: The knee joint angle remains unchanged, and the hip joint swings forward to a certain angle. After the swing foot experiences a swing distance of one step length, it reaches the farthest distance point to complete crossing an obstacle or a small pit. For the swing leg's landing phase: The knee joint drops, and the foot-end starts to drop vertically from the farthest distance point. There may be situations of early landing and delayed landing, and the feedback quantity is added to adjust the foot-end's preparation for landing.

[0127] As Figure 5 shown in the schematic diagram of the robot's motion control on slope terrain: When it is judged that the current terrain is slope terrain, according to the pitch angle θ and roll angle estimated from the terrain slope, the control signals of the robot's joint actuators are adjusted so that the attitude angle of the robot adapts to the slope change of the terrain. The control strategy is to achieve the adaption to the slope terrain by adjusting the output angle signals of the joint actuators:

[0128] β(k + 1) = A1 * β(k) + B1 * u1(k) + w1(k)

[0129] γ(k + 1) = A2 * γ(k) + B2 * u2(k) + w2(k)

[0130] where β(k) and γ(k) are the angles of the hip joint and the side-swing joint at time k respectively, u1(k) and u2(k) are the inputs of the pitch angle and roll angle of the quadruped robot's fuselage at time k. Among them, β(k + 1) and γ(k + 1) are the predicted values of the joint angles of the robot's hip joint and side-swing joint at time k + 1, A1, B1, A2, B2 are the coefficient matrices of the system model, and w1(k), w2(k) are the random noises representing the model prediction errors;

[0131] The optimization process of the above formula is:

[0132] Minimize ∑||β(k) - θ(k)|| 2 + ∑||u1(k)|| 2 , k = 0, 1, 2, 3,...., T

[0133]

[0134] where θ(k), Denote the set target at time k, which is the pitch angle and roll angle of the terrain angle calculated in step (3) at time k. T is the optimization time sequence length. By solving this optimization problem, the optimal joint angle control sequence can be obtained to achieve the adaptive control of joint angles for sloped terrains.

[0135] Further, after completing the adaptive control of flat terrains, uneven terrains, or sloped terrains, the effectiveness of the adaptive control algorithm is detected according to the IMU sensor mounted on the robot body, providing feedback information for the motion control of the robot: According to the yaw angle α, pitch angle β, and roll angle γ of the body attitude angle of the robot in the motion state and the set body attitude angle yaw angle α f , pitch angle β f , roll angle γ f thresholds for comparison. When the values of the yaw angle α, pitch angle β, and roll angle γ detected by the IMU sensor of the robot body are all less than the set body attitude angle threshold, it indicates that the motion of the robot body is stable. That is, the above-mentioned local terrain adaptive motion control strategy is effective.

[0136] Such as Figure 6 is the motion effect diagram of the foot end trajectory of the quadruped robot in the joint simulation of Matlab and Webots, Figure 7 is the simulation diagram of the foot end motion control of the quadruped robot in the uneven terrain in the joint simulation of Matlab and Webots.

[0137] The above embodiments are only the preferred embodiments of the present invention and do not limit the technical solutions of the present invention. Any technical solutions that can be implemented on the basis of the above embodiments without creative labor shall be regarded as falling within the scope of the patent rights of the present invention.

Claims

1. A foot-end trajectory motion control method for a quadruped robot based on local terrain adaptation, characterized in that The following steps are involved: S1: Classification of external terrain information: The current terrain is classified into flat terrain, concave-convex terrain, and slope terrain through the robot's foot-end three-dimensional force sensor, the fuselage IMU sensor, and the quadruped robot's fuselage kinematics forward solution algorithm model; S2: Perception of the robot's body posture: by acquiring the robot's foot contact status, body posture angle, leg joint encoder information, spine joint angle information, and foot position information, the information of multiple sensors is integrated to estimate the robot's own motion posture and motion state; S3: Estimation of the slope angle of the slope terrain: Solve the foot position of the robot in the body coordinate system through forward kinematics, and solve the rotation matrix from the body coordinate system to the world coordinate system according to the body attitude angle Solve the representation of the robot's foot position in the world coordinate system, estimate the plane coefficient using the least squares method based on the terrain plane estimation formula and the foot information of the robot, and calculate the terrain angle pitch angle θ and roll angle S4: When the current terrain is determined to be flat, the robot adopts a diagonal trot gait to pass through the current flat terrain; when the current terrain is determined to be concave and convex, the robot plans the robot's foot end motion trajectory by combining the feedback amount with the CPG output signal to complete the control strategy for the concave and convex terrain movement; when the current terrain is determined to be a slope terrain, the robot controls the movement of the hip joint motor of the quadruped robot according to the pitch angle and roll angle information of the current terrain, adjusts the hip joint angle of the quadruped robot to adapt to the slope of the terrain, and locks the spine joint motor when climbing uphill to ensure that the robot body is parallel to the current terrain slope; when climbing over step terrain, the spine joint motor is unlocked, and the spine joint angle is adjusted to enhance the climbing and obstacle-crossing ability of the quadruped robot; S5: Set the thresholds of the robot's body attitude angle yaw angle, pitch angle, and roll angle to be α respectively f , β f , γ f ,The effectiveness of the adaptive foot trajectory motion control method of the quadruped robot is judged based on the feedback of the robot motion state information.

2. The method for controlling the foot-end trajectory movement of a quadruped robot based on local terrain adaptation according to claim 1, wherein, The terrain classification process in S1 is as follows: The three-dimensional force sensors at the four feet of the quadruped robot are used to judge the contact situation between the feet and the ground in the current motion state. The higher the pressure value of the foot-end pressure sensor, the higher the probability of the foot-end contacting the ground; by setting the foot-end pressure threshold p f to judge whether the foot-end is in full contact with the ground; When the pressure p at the foot end z ≥ p f it indicates that the foot end corresponding to this pressure value has fully contacted the ground. When p z ≤ p f it indicates that the foot end corresponding to this pressure value has not fully contacted the ground; The next step of terrain information judgment is performed according to the ground contact status of the foot end: the robot's initial motion posture is set, and the robot's Z-axis coordinate in the world coordinate system is obtained. When the pressure sensor shows contact and the gait is in the support phase for N consecutive motion cycles, the Z-axis coordinate value does not change, and the current motion terrain is judged to be flat terrain; when the signal of the foot-end pressure sensor shows early or delayed contact in the gait cycle of the movement, the Z-axis coordinate value changes, and the current terrain is judged to be concave and convex terrain or slope terrain; based on the three-dimensional force sensor installed at the foot end, the size and direction of the foot-end contact force are sensed, and the current terrain is classified. If the size and direction of the detected force remain unchanged in each gait cycle of the movement, the current terrain is judged to be a slope terrain. If the size and direction of the detected force continue to change in each gait cycle of the movement, the current terrain is judged to be a concave and convex terrain; the current motion terrain is classified in combination with the position information of the foot end point to provide feedforward information for the adaptive control of the quadruped robot.

3. The method for controlling the foot-end trajectory movement of a quadruped robot based on local terrain adaptation according to claim 1, wherein In S2, the robot's joint motor encoders are combined with the robot's kinematic model and body size information to solve the robot's foot position and movement speed in the current state. The attitude fusion algorithm of the quadruped robot's body IMU sensor outputs three attitude angles, namely the yaw angle α, pitch angle β and roll angle γ.

4. The method for controlling the foot-end trajectory motion of a quadruped robot based on local terrain adaptation according to claim 3, characterized in that The specific process of terrain slope angle estimation in S3 is as follows: When the current four feet are fully in contact with the ground, the rotation matrix from the body coordinate system to the world coordinate system is solved according to the yaw angle α, pitch angle β, and roll angle γ of the robot's pose. Where W is represented in the world coordinate system and B is represented in the body coordinate system. The solution formula is: Where c represents cosine and s represents sin; Solve for the representation of the foot end in the world coordinate system: Among them W C x W C y W C z T is the representation of the centroid coordinate system of the fuselage in the world coordinate system, B x B y B z] T represents the representation of the position of the robot's foot end in the fuselage coordinate system;​ The latest touchdown position of each leg of the quadruped robot in the world coordinate system is updated to: W p i =[x i y i z i T , i = 1, 2, 3, 4. According to the terrain plane estimation formula: z = b0 + b1x + b2 to fit the terrain plane, where b0, b1, and b2 in the formula are plane coefficients; use the least squares method to minimize the residual of the coordinate z value, and the regression coefficient obtained by solving is the coefficient of the plane expression. The solution formula is: A and Z in the formula are:​ Calculate the terrain angle pitch angle θ and roll angle according to the coefficients of the fitted terrain plane formula They are respectively:

5. The method for controlling the foot-end trajectory movement of a quadruped robot based on local terrain adaptation according to claim 2, wherein When it is determined in S4 that the current moving terrain is an uneven terrain, the movement trajectory of the foot end is controlled to adapt to the pit or convex terrain. During the movement trajectory planning process, in the leg-lifting stage of the swinging leg: the knee joint swings upward, the hip joint swings backward to lift the leg, and the foot end of the swinging leg vertically lifts off the ground from the starting point and vertically lifts to the highest point. In the forward-striding stage of the swinging leg: the knee joint angle remains unchanged, the hip joint swings forward to complete crossing an obstacle or a pit, and after the swinging foot experiences a swinging distance of one step pitch, it reaches the farthest point. In the leg-landing stage of the swinging leg: the knee joint falls back, and the foot end starts to vertically lower the leg from the farthest point. There are cases of early landing and delayed landing, and the landing preparation of the foot end is adjusted by adding a feedback amount; The specific control method is as follows: The movement is adjusted by superimposing the CPG output signal and the feedback control signal. The mathematical model is as follows: Among them, X i , Y i are the input quantities of the actual hip and knee joint angle signals, x hi , y hi are the hip and knee joint angle signals output by the original CPG oscillator, a i (i = 1, 2, 3, 4), k i (i = 1, 2, 3, 4) are the feedback quantities for the hip and knee joint trajectory planning, which are used to control the interaction with the outside world according to the current undulating terrain changes, so as to better achieve terrain adaptability; The feedback planning formula for the knee joint movement angle is: The feedback planning formula for the hip joint movement angle is: In the above formula: is the moment of the reflection point of the i-th leg; T0 is the reflection control time, that is, the time from the swing phase to the commutation point of the support phase: t is the current time; a i , k i are the feedback quantities for the trajectory planning of the hip joint and knee joint of the i-th leg; are respectively the angle values of the hip joint and knee joint of the reflected leg at the reflection point of the i-th leg; is the angle value of the hip joint at the commutation point of the i-th leg; feed i is the feedback term of the i-th leg; C1 is the change in knee joint angle; D1, D2 are the changes in hip joint angle, i = 1, 2, 3, 4; Taking the right front leg as an example, the angle change amounts of the knee joint and the hip joint are obtained according to the inverse kinematics of the robot leg: Swing joint angle: Hip joint angle: Knee joint angle: Among them RF p x 、 RF P y 、 RF p z are the x-axis, y-axis, and z-axis coordinates of point P at the end of the right front leg in the coordinate system of the right front leg hip joint center, respectively. l1, l2, and l3 are the leg lengths between the side-sway joint and the hip joint, between the hip joint and the knee joint, and between the knee joint and the end of the leg of the quadruped robot, respectively. The changes in the knee joint and hip joint angles of the other three legs are solved in the same way.

6. The method for controlling the foot-end trajectory movement of a quadruped robot based on local terrain adaptation according to claim 4, characterized in that The terrain angle pitch angle θ and roll angle obtained by calculation Adjust the control signals of the robot's yaw joint and hip joint actuator so that the attitude angle of the robot adapts to the slope change of the terrain. The control strategy is to achieve the adaptability to the slope terrain by adjusting the output angle signals of the yaw joint and hip joint actuator: β(k + 1) = A1 * β(k) + B1 * u1(k) + w1(k) γ(k + 1) = A2 * γ(k) + B2 * u2(k) + w2(k) In the formula, β(k) and γ(k) are the angles of the hip joint and the side-swing joint at time k respectively, u1(k) and u2(k) are the inputs of the pitch angle and the roll angle of the quadruped robot body at time k. Among them, β(k + 1) and γ(k + 1) are the predicted values of the joint angles of the robot hip joint and the side-swing joint at time k + 1, A1, B1, A2, and B2 are the coefficient matrices of the system model, and w1(k), w2(k) are the random noises representing the model prediction errors; The optimization process of the above formula is: Minimize ∑||β(k) - θ(k)|| 2 + ∑||u1(k)|| 2 , k = 0, 1, 2, 3,...., T where θ(k), represents the set target at time k, that is, the pitch angle and roll angle of the terrain angle at time k, and T is the length of the optimization time series; by solving the optimization problem, the optimal joint angle control sequence is obtained to achieve the adaptive control of the joint angle for the slope terrain.

7. The method for controlling the foot-end trajectory movement of a quadruped robot based on local terrain adaptation according to claim 1, wherein In S5, according to the motion state of the robot, the magnitudes of the yaw angle α, pitch angle β, and roll angle γ of the fuselage attitude angle are compared with the set yaw angle α of the fuselage attitude angle f , pitch angle β f , roll angle γ f threshold values. When the yaw angle α, pitch angle β, and roll angle γ values detected by the IMU sensor of the robot fuselage are all less than the set fuselage attitude angle threshold values, it indicates that the fuselage of the robot is moving smoothly, and this is used to judge the effectiveness of the local terrain adaptive foot-end trajectory motion control method for the quadruped robot.

Images