Crawling state planning method and system for quadruped robot capable of crossing large obstacles

Abstract

The invention discloses a crawling state planning method and system for a quadruped robot capable of crossing large obstacles. A nonlinear trajectory optimizer, a swing phase trajectory planner and a state machine send control signals to a motion controller, and the motion controller controls the movement of the quadruped robot; a state estimator collects the positions of each foot of the quadruped robot, and estimates the slope of an unknown terrain according to the positions of each foot of the quadruped robot; the state estimator sends the collected data and the estimated slope to a contact detector and a motion controller respectively; the contact detector estimates the probability of the foot end touching the ground, and sends the probability of the foot end touching the ground to the state machine; the state machine calculates the gait timing of the next state phase according to the probability of the foot end touching the ground and a set time interval; the state machine sends the calculation result to the nonlinear trajectory optimizer and the swing phase trajectory planner; the quadruped robot starts the movement of the next state phase until the quadruped robot crosses the obstacle.

Description

Technical Field

[0001] The present invention relates to the technical field of quadruped robot motion planning, and in particular to a crawling state planning method and system for a quadruped robot capable of crossing large obstacles. Background Art

[0002] The statements in this section merely mention background art related to the present invention and do not necessarily constitute prior art.

[0003] Compared with wheeled robots and tracked robots, legged robots are favored by many researchers and research institutions because of their strong adaptability to unstructured terrain and high stability. Among them, quadruped robots are the typical representatives of legged robots because of their light weight and strong stability compared with bipedal or hexapedal robots.

[0004] Quadruped robots are limited by their leg length when climbing rough terrain, and it is difficult to achieve scene movement with obstacles that are more than 60% of their leg length. Among the current robots, ANYmal has the highest performance and can climb obstacles 21cm high with the help of visual sensors, which is 40% of its leg length. HyQ can climb 15cm high stairs and steps, which is 20% of its leg length. LittleDog can climb obstacles as high as its leg length with the help of high-resolution maps and motion capture systems, but this method cannot be applied in real scenes because it requires scanning accurate terrain models in advance.

[0005] Chinese patent document CN107065867A discloses a method for motion planning of a quadruped robot on unknown rugged terrain, which uses a cart-table ZMP model to implement online trajectory planning of the robot and realizes the motion planning of the quadruped robot on rugged terrain. However, the motion scenarios and control methods for large obstacles are not clearly described.

[0006] The "Gait planning method for a quadruped robot climbing and crossing steep slopes or high obstacles" disclosed in Chinese patent document CN110842921A uses the normalized energy stability margin method to plan the movement of the center of mass to ensure stability at steep slope angles. However, this method does not take into account the situation where the robot becomes unstable due to excessive foot landing difference when the time-based state machine switches under large obstacles, and does not consider the situation where the foot touches the ground with delay or in advance. Summary of the invention

[0007] In order to solve the deficiencies of the prior art, the present invention provides a method and system for planning the crawling state of a quadruped robot that can cross large obstacles; the quadruped robot can pass a large obstacle of 38 cm in height that is more than 63% of its leg length. The nonlinear trajectory optimizer is used to realize the online planning of the optimal motion trajectory of the center of mass during the movement, and the stability margin of the center of mass during the movement is guaranteed; the probability-based sole contact estimation algorithm is used to realize the accurate detection of the foot-end contact state of the quadruped robot; the state machine based on events and time is used to realize the state switching of the quadruped robot based on the foot-end contact situation and the time state machine; the foot-end drop is used to estimate the terrain slope, and the quadruped robot is self-adaptive to the foot-end terrain.

[0008] In a first aspect, the present invention provides a crawling state planning method for a quadruped robot that can cross large obstacles;

[0009] A crawling state planning method for a quadruped robot capable of crossing large obstacles includes:

[0010] Setting basic data of the quadruped robot; the basic data includes: expected position, expected speed and expected step height;

[0011] Based on the basic data, a nonlinear trajectory optimizer and a swing phase trajectory planner are constructed; the nonlinear trajectory optimizer is used to optimize the motion trajectory of the center of gravity of the quadruped robot; the swing phase trajectory planner is used to plan the swing trajectory of the foot end of the quadruped robot; a state machine is constructed, and the state machine includes: a preset gait timing sequence of the quadruped robot; the initial state phase of the quadruped robot is a quadruped support phase;

[0012] The quadruped robot starts to move, and the nonlinear trajectory optimizer, swing phase trajectory planner and state machine send control signals to the motion controller, and the motion controller controls the movement of the quadruped robot; the state estimator collects the positions of each foot of the quadruped robot, and estimates the slope of the unknown terrain according to the positions of each foot of the quadruped robot; the state estimator sends the collected data and the estimated slope to the contact detector and the motion controller respectively; the contact detector estimates the probability of the foot end touching the ground, and sends the probability of the foot end touching the ground to the state machine; the state machine calculates the gait timing of the next state phase according to the probability of the foot end touching the ground and the set time interval; the state machine sends the calculation results to the nonlinear trajectory optimizer and the swing phase trajectory planner;

[0013] Repeat the previous step, and the quadruped robot starts moving to the next state phase until the quadruped robot crosses the obstacle.

[0014] In a second aspect, the present invention provides a quadruped robot crawling state planning system that can cross large obstacles;

[0015] A quadruped robot crawling planning system that can cross large obstacles includes:

[0016] An initialization module is configured to: set basic data of the quadruped robot; the basic data includes: a desired position, a desired speed, and a desired step height;

[0017] A construction module is configured to: construct a nonlinear trajectory optimizer and a swing phase trajectory planner based on basic data; the nonlinear trajectory optimizer is used to optimize the motion trajectory of the center of gravity of the quadruped robot; the swing phase trajectory planner is used to plan the swing trajectory of the foot end of the quadruped robot; construct a state machine, the state machine includes: a preset gait timing sequence of the quadruped robot; the initial state phase of the quadruped robot is a quadruped support phase;

[0018] The planning module is configured as follows: the quadruped robot starts to move, the nonlinear trajectory optimizer, the swing phase trajectory planner and the state machine send a control signal to the motion controller, and the motion controller controls the movement of the quadruped robot; the state estimator collects the positions of each foot of the quadruped robot, and estimates the slope of the unknown terrain according to the positions of each foot of the quadruped robot; the state estimator sends the collected data and the estimated slope to the contact detector and the motion controller respectively; the contact detector estimates the probability of the foot end touching the ground, and sends the probability of the foot end touching the ground to the state machine; the state machine calculates the gait timing of the next state phase according to the probability of the foot end touching the ground and the set time interval; the state machine sends the calculation result to the nonlinear trajectory optimizer and the swing phase trajectory planner;

[0019] The motion module is configured to repeat the work of the planning module, and the quadruped robot starts the motion of the next state phase until the quadruped robot crosses the obstacle.

[0020] Compared with the prior art, the present invention has the following beneficial effects:

[0021] 1) The present invention proposes an online trajectory optimizer based on composite constraints such as energy to achieve online planning of the center of gravity trajectory of the quadruped robot, generates a reference center of gravity trajectory according to the standard energy stability margin, constructs a nonlinear optimization function based on the supporting triangle boundary constraints and the curve continuity constraints, and optimizes the optimal motion trajectory of the center of gravity online, thereby ensuring the stability and continuity of the center of gravity trajectory to the greatest extent, and ensuring the stability margin of the trunk when climbing rugged terrain.

[0022] 2) The present invention proposes a time and event-based state machine to achieve foot state switching of a quadruped robot under a large obstacle, and realizes single-leg state switching based on the ground contact of the swinging leg. When the swinging leg touches the ground, it immediately switches to the support phase. Compared with the time state machine, it effectively reduces the foot plantar force impact caused by the large obstacle landing difference, prevents the trunk from tilting backward or rolling over due to the foot impact when the inclination angle is too large, and enhances stability when climbing high and rugged terrain.

[0023] 3) The present invention proposes a foot touchdown detection algorithm based on a probability model to achieve accurate detection of foot sliding, early touchdown or delayed touchdown of a quadruped robot, improves the accuracy of plantar force estimation based on the generalized momentum method, and estimates the probability of single-leg foot touchdown by fusing the probability model of foot height, plantar force and time state machine through Kalman filtering. The foot touchdown can be accurately detected without the need for a plantar force sensor, thereby improving the robustness of the entire quadruped robot movement. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] The accompanying drawings in the specification, which constitute a part of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0025] Figure 1 This is a schematic diagram of the coordinate system definition of a quadruped robot;

[0026] Figure 2 It is a control block diagram of the motion control method of a quadruped robot;

[0027] Figure 3(a)-Figure 3(d) 3 is a schematic diagram illustrating the timing of the crawling motion of a quadruped robot, wherein FIG3(a) is a stage of adjusting the length of the center of mass and the swinging of the left hind leg, FIG3(b) is a stage of adjusting the length of the center of mass and the swinging of the left front leg, FIG3(c) is a stage of adjusting the length of the center of mass and the swinging of the right hind leg, and FIG3(d) is a stage of adjusting the length of the center of mass and the swinging of the right front leg;

[0028] Figure 4 It is the flow chart of the optimal landing strategy execution of the quadruped robot;

[0029] Figure 5 This is a schematic diagram of the center of gravity movement adjustment of a quadruped robot taking the RF leg as an example;

[0030] Figure 6 It is a schematic diagram of the inequality constraints for the dynamic stability of the optimization function;

[0031] Figure 7 It is a schematic diagram of the terrain adaptation process during the movement of the quadruped robot;

[0032] Figure 8 This is a state machine switching diagram of a quadruped robot based on events and time;

[0033] Figure 9(a)-Figure 9(d) This is a data diagram of the ground contact detection during the movement of the RH legs of the quadruped robot;

[0034] Fig.10 This is a schematic diagram of the motion trajectory of the swinging legs of a quadruped robot;

[0035] Figure 11(a)-Figure 11(i) This is a screenshot of the experiment of a quadruped robot climbing a 38cm high staircase;

[0036] Figure 12(a)-Figure 12(i) This is a screenshot of the experiment of a quadruped robot climbing a 22.5cm high staircase. DETAILED DESCRIPTION

[0037] It should be noted that the following detailed descriptions are exemplary and are intended to provide further explanation of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meanings as those commonly understood by those skilled in the art to which the present invention belongs.

[0038] It should be noted that the terms used herein are only for describing specific embodiments, and are not intended to limit exemplary embodiments according to the present invention. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that the terms "include" and "have" and any of their variations are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device comprising a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0039] In the absence of conflict, the embodiments of the present invention and the features of the embodiments may be combined with each other.

[0040] In this embodiment, all data is obtained in compliance with laws and regulations and based on the user's consent, and is used legally.

[0041] Embodiment 1

[0042] This embodiment provides a crawling state planning method for a quadruped robot that can cross large obstacles;

[0043] A crawling state planning method for a quadruped robot capable of crossing large obstacles includes:

[0044] S101: Setting basic data of the quadruped robot; the basic data includes: expected position, expected speed and expected step height;

[0045] S102: Based on the basic data, construct a nonlinear trajectory optimizer and a swing phase trajectory planner; the nonlinear trajectory optimizer is used to optimize the motion trajectory of the center of gravity of the quadruped robot; the swing phase trajectory planner is used to plan the swing trajectory of the foot end of the quadruped robot; construct a state machine, the state machine includes: a preset gait timing of the quadruped robot; the initial state phase of the quadruped robot is a quadruped support phase;

[0046] S103: The quadruped robot starts to move, and the nonlinear trajectory optimizer, the swing phase trajectory planner and the state machine send control signals to the motion controller, and the motion controller controls the movement of the quadruped robot; the state estimator collects the positions of each foot of the quadruped robot, and estimates the slope of the unknown terrain according to the positions of each foot of the quadruped robot; the state estimator sends the collected data and the estimated slope to the contact detector and the motion controller respectively; the contact detector estimates the probability of the foot end touching the ground, and sends the probability of the foot end touching the ground to the state machine; the state machine calculates the gait timing of the next state phase according to the probability of the foot end touching the ground and the set time interval; the state machine sends the calculation results to the nonlinear trajectory optimizer and the swing phase trajectory planner;

[0047] S104: Repeat step S103, and the quadruped robot starts moving to the next state phase until the quadruped robot crosses the obstacle.

[0048] Furthermore, the state machine includes: two triggering modes, one of which is a time-based triggering mode, when the set time interval is reached, the robot starts the next state phase; the other is an event-based triggering mode, when the robot's swinging foot touches the ground, the robot starts the next state phase.

[0049] Furthermore, the S101: setting basic data of the quadruped robot; the basic data includes: expected position, expected speed and expected step height;

[0050] Construct the trunk coordinate system {B}: The trunk coordinate system is fixed on the robot's trunk, with the origin at the center of mass of the robot's trunk, and the X B Pointing forward to the torso, Y B Point to the left of the robot, Z B Vertically upwards from the torso;

[0051] Construct the leg coordinate system {H}: The leg coordinate system is fixed on the robot's hip joint, with the origin at the center of mass of the robot's leg swing joint. h Pointing forward to the torso, Y h Point to the left of the robot, Z h Upward perpendicular to the side swing joint;

[0052] Construct the world coordinate system {W}: The world coordinate system is fixed on the ground, Z w Opposite to the direction of gravity, X w Point and X b The projection on the horizontal plane is the same, Y w Pointing and Y b The projection on the horizontal plane is the same;

[0053] Among them, the expected position refers to the target position that the robot expects to reach during the movement;

[0054] Among them, the expected speed refers to the expected movement speed of the robot during the movement;

[0055] The expected step height refers to the expected leg lift height of the robot when it swings its legs during movement.

[0056] A large obstacle refers to an obstacle whose height is higher than 60% of the robot's leg length.

[0057] Furthermore, the S102: constructing a nonlinear trajectory optimizer and a swing phase trajectory planner based on the basic data, wherein the nonlinear trajectory optimizer includes:

[0058] S102-a1: Calculate the standard energy stability margin around each support axis of the support triangle according to the current foot end position of the support leg in the trunk coordinate system {B};

[0059] S nesm =h max -h com

[0060] Among them, S nesm Standard energy stability margin, h max is the maximum height that the center of mass of the quadruped robot can reach when it rotates around the supporting triangle support axis, h com is the height of the center of mass of the quadruped robot.

[0061] According to the Normalized Energy Stability Margin (NESM), the position of the center of gravity (COG) with the maximum stability margin of the supporting triangle formed by the remaining supporting legs is obtained when a certain leg swings in the next cycle;

[0062] Calculate the distance of the COG reference movement in the x and y directions in the torso coordinate system {B}; the x and y directions refer to the forward direction along the robot torso and the left direction of the torso;

[0063] Among them, when the right front leg swings, calculate the moving distance of COG:

[0064] Δx cog =k cogx (k x S nesm23 -S nesm12 )

[0065] Δy cog =k cogy (k y S nesm12 -S nesm13 )

[0066] Among them, kcogx , k cogy is the proportional gain in the x and y directions relative to the torso coordinate system COG, k x and k y is the lateral and transverse tilt coefficient set according to the NESM criterion, S nesm12 It represents the stability margin of the left front leg and the right rear leg, S nesm13 It represents the stability margin of the left front leg and the left rear leg, S nesm23 represents the stability margin between the left and right hind legs, Δx cog , Δy cog The distance the COG moves in the x and y directions in the torso coordinate system {B};

[0067] According to the adjustment of the center of gravity in the x and y directions, the displacement generated per unit time at the expected speed of the trunk in the world coordinate system {W} is superimposed, and the motion trajectory p of the quadruped robot from the current COG to the target position in the world coordinate system {W} is planned. ref =[c x c y ] T As a reference motion trajectory;

[0068] The process of superposition:

[0069] c x =Δx cog +v x T

[0070] c y =Δy cog +v y T

[0071] Among them, v x 、v y represents the expected speed of the trunk in the world coordinate system {W}, T represents the gait cycle time of the robot movement, c x represents the reference motion trajectory of the center of gravity of the quadruped robot from the current position to the target position in the x direction in the world coordinate system after superposition, c y It means the reference motion trajectory of the center of gravity of the quadruped robot from the current position to the target position in the y direction of the world coordinate system after superposition;

[0072] S102-a2: Plan the x and y motion trajectories of the quadruped robot's trunk mass center in the world coordinate system {W} as quintic spline curves:

[0073]

[0074] Among them, the height of the center of gravity in the Z direction is a constant c z = constant, p x 、py represents the motion trajectory of the center of gravity planning, a k is the coefficient of the spline curve, t is the current motion time of the robot in the gait cycle;

[0075] The center-of-mass motion trajectory generated according to the Normalized Energy Stability Margin (NESM) criterion equality constraint is used as the reference motion trajectory:

[0076] p ref =[c x c y ] T ;

[0077] Among them, c x 、c y It represents the reference motion trajectory of the quadruped robot's center of gravity from the current position to the target position in the x, y direction in the world coordinate system.

[0078] Construct a nonlinear trajectory optimization cost function:

[0079] δ=p(t,a)-p ref ;

[0080] Among them, p ref The reference motion trajectory from the current position to the target position in the x,y direction in the world coordinate system, p(t,a) is the motion trajectory generated by the quintic spline curve in the x,y direction in the world coordinate system.

[0081] It satisfies the dynamic stability inequality and the continuity trajectory equality constraints of the center of mass motion trajectory, and is transformed into an optimization problem. The optimal trajectory is solved using the quadratic programming (QP) optimization library, so it is expressed as:

[0082] δ T Qδ

[0083] stp l x cog +q l y cog +r l >m,l=1,2,3;

[0084] p k (T k )=p k+1 (0), k = 1, ..., n;

[0085] S i >S min , i=1,2,3;

[0086] Where Q is a positive definite weight matrix, p l ,q l 、r l is the coefficient of the straight line formed by the single side of the supporting triangle, m is the minimum distance from the boundary of the supporting triangle, and p k (T k ) is the final position of the center of gravity adjustment at the last moment T of the k-th quadruped support stage, p k+1 (0) is the initial position of the center of gravity at the initial moment of the k+1 quadruped support stage, S min is the minimum standard energy stability margin, S i is the standard energy stability margin of the i-th supporting axis during motion.

[0087] p l x cog +q l y cog +r l >m is a dynamic stability constraint, which constrains the center of mass to be within a triangle whose three sides form a straight line and whose distance is greater than m, ensuring the stability of the center of mass during foot swing and trunk movement, such as Figure 6 Schematic diagram of the inequality constraints for the dynamic stability of the optimization function, p k (T k )=p k+1 (0) is the trajectory continuity constraint. The final position T of the k-th quadruped support stage is the initial position of the k+1th stage. S i >S min The minimum standard energy stability margin is greater than S min , to prevent the robot from tipping over during movement.

[0088] S102-a3: Generate the optimal motion trajectory p of the center of gravity through online optimization based on the cost function opt =[xyz] T , the centroid adjustment process, such as Figure 5 As shown, the continuity and stability margin of the center of gravity (COG) trajectory position, velocity and acceleration during the movement are guaranteed to be maximum, and the center of mass of the robot trunk is moved to the position with the maximum stability margin to enter the next swinging stage.

[0089] Furthermore, the S102: constructing a nonlinear trajectory optimizer and a swing phase trajectory planner based on the basic data, wherein the swing phase trajectory planner includes:

[0090] S102-b1: Fig.10 As shown, the foot end motion trajectory swings during the process; during the foot end lifting phase, the robot foot end retreats and lifts upward to prevent collision with the protruding part of the obstacle;

[0091] S102-b2: During the forward swinging phase of the foot, the robot maintains a constant height in the Z direction. According to the set step length, the foot moves forward horizontally and maintains a set distance from obstacles in the Z direction to prevent tipping or instability due to collision. The Z direction refers to the direction opposite to the gravity direction of the robot.

[0092] S102-b3: During the vertical drop phase of the foot, the robot's X direction remains unchanged, and the Z direction height gradually decreases until it contacts the obstacle ground; when the contact detection probability is greater than the set threshold, it is judged as touching the ground, the swing phase ends, and the leg enters the next cycle; the X direction refers to the front of the robot's torso.

[0093] It should be understood that if the current single leg is in the swinging stage, the step length and step height parameters during the swinging process are set according to the height of the obstacle and the distance to the obstacle; based on the set step length and step height parameters, the trajectory during the movement is planned according to the step height, step length and movement speed of each step.

[0094] Furthermore, the S102: constructing a state machine includes:

[0095] S102-c1: The predetermined stepping sequence during the crawling movement is: four-legged support long adjustment phase, LH (left hind) swing, four-legged support short adjustment phase, LF (left front) swing, four-legged support long adjustment phase, RH (right hind) swing, four-legged support short adjustment phase, RF (right front) swing, a total of eight states, at least three legs are in the support state at each moment; the gait timing is as follows Figure 3(a)-Figure 3(d) As shown, in the quadruped support phase, the center of gravity moves to the position with the maximum stability margin of the support triangle according to the constructed nonlinear trajectory optimizer; in the single-leg swing phase, the foot end swings according to the planned swing leg foot end trajectory;

[0096] S102-c2: Set the optimal landing strategy: After the foot of the swinging leg touches the ground, estimate the virtual center of gravity position of the next leg swing according to the Normalized Energy Stability Margin (NESM) criterion, and estimate the minimum normalized energy stability margin S around each rotation axis of the support triangle. nesm size;

[0097] If S nesm Less than the set minimum stability margin value S min, the trunk will tip over when the center of gravity is moved at the next moment, so the gait sequence is changed, the current leg is still used as the swing leg, and the foot is re-landed to increase the stability margin of the support polygon to prevent the trunk from tipping over due to the reduction of the stable area caused by accidental contact with the ground;

[0098] If S nesm Greater than the set minimum stability margin value S min , then at the next moment, continue to move the center of gravity and swing the legs according to the predetermined gait sequence;

[0099] Figure 4 This is the process of the optimal landing strategy. The stable area is the supporting triangle area formed by the foot end.

[0100] S102-c3: In the next gait cycle, the default landing sequence is restored, and the trunk of the robot is moved according to the predetermined gait sequence. The landing strategy of S102-c2 is used to continue to judge the stability margin to prevent the robot from colliding with obstacles during movement, causing it to touch the ground prematurely and reduce the support polygon area, thereby tipping over.

[0101] Furthermore, the S103: the quadruped robot starts to move, the nonlinear trajectory optimizer, the swing phase trajectory planner and the state machine send a control signal to the motion controller, and the motion controller controls the movement of the quadruped robot, including:

[0102] The nonlinear trajectory optimizer sends the desired position, motion velocity and acceleration commands of the optimized center of gravity motion trajectory in the x, y and z directions in the world coordinate system {W} to the motion controller;

[0103] The swing phase trajectory planner sends to the motion controller the desired position, motion velocity and acceleration commands of the optimized hindfoot end in the x, y and z directions in the hip joint coordinate system {H};

[0104] The state machine sends the swing sequence of each leg, the phase time of the stance phase and the phase time of the swing phase to the motion controller;

[0105] The motion controller controls the movement of the quadruped robot.

[0106] Furthermore, the S103: motion controller comprises:

[0107] S103-a1: simulate the spring damping model according to the planned expected COG position and posture information and the COG position and posture information fed back by the state estimator, calculate the virtual force and virtual torque required by the robot according to the virtual stiffness and damping, and use the virtual spring principle to increase the flexibility of the system and reduce the foot end impact;

[0108]

[0109]

[0110] Among them, p d and θ d is the desired COG position and attitude angle of the torso, p f and θ f is the actual torso COG position and posture, K p , K d , K p,θ and K d,θ are the positive definite diagonal matrices of proportional gain and derivative gain respectively;

[0111] S103-a2: Construct the state space equation of the virtual model. Since the system is not rank-satisfied, the least square method is used to transform it into an optimization problem. The optimal plantar force is solved using the quadratic programming (QP) optimization library QuadProg++. The optimal plantar force satisfies the friction cone constraint.

[0112]

[0113]

[0114] st

[0115] |f x,i |<μf z,i

[0116] |f y,i |<μf z,i

[0117] f min ≤f z,i ≤f max

[0118] Among them, F i =[f x f y f z ] T represents the optimal contact force of the ith leg, G b is the gravity of the quadruped robot, I is the unit matrix, p i =[x i y i z i ] T is the position vector of the foot end in the trunk coordinate system during the support phase, S and W are the weights of the diagonal semi-positive definite matrix, and f x,i 、f y,i 、f y,i is the friction force at the foot end in the x, y, and z directions, f min f maxare the minimum and maximum friction constraints, and μ is the friction coefficient of the ground.

[0119] S103-a3: Based on the calculated optimal plantar force F i , through the Jacobian matrix J of the leg b according to The joint torque τ is solved and sent to the joint motor for execution, thereby controlling the leg motion of the quadruped robot and tracking the desired motion trajectory of the trunk.

[0120] Furthermore, the state estimator collects the positions of the feet of the quadruped robot and estimates the slope of the unknown terrain according to the positions of the feet of the quadruped robot in S103, which specifically includes:

[0121] S103-b1: The state estimator obtains the foot end position in the current four-legged or three-legged support phase, where z RF 、z LF 、z RH 、z LH They represent the positions of the RF leg, LF leg, RH leg, and LH leg in the z direction of the hip joint coordinate system {H}, respectively. Therefore, the average height of the first two legs is estimated:

[0122] z front =(z LF +z RF ) / 2;

[0123] Estimate the average height of the back legs:

[0124] z hind =(z LH +z RH ) / 2;

[0125] Estimate the average height of the two left legs:

[0126] z left =(z LF +z LH ) / 2;

[0127] Estimate the average height of the right legs:

[0128] z right =(z RF +z RH ) / 2;

[0129] S103-b2: Estimate the slope of unknown terrain based on the foot end drop, such as Figure 7 As shown in the figure, the pitch angle calculation of the quadruped robot during terrain adaptation is calculated by the difference between the front and rear feet:

[0130]

[0131] The desired roll angle is calculated from the difference in footfall between the left and right feet:

[0132]

[0133] Among them, x front =(x LF +x RF ) / 2 represents the position of the front leg in the x direction of the {H} coordinate system, x hin = d (x LH +x RH ) / 2 represents the position of the hind legs in the x direction of the {H} coordinate system, and y left =(y LF +x LH ) / 2 represents the position of the left leg in the y direction of the {H} coordinate system, y right =(y RF +y RH ) / 2 represents the position of the right leg in the y direction of the {H} coordinate system;

[0134] S103-b3: The calculated desired trunk posture angle θ pitch ,θ roll It is converted into a rotation matrix to obtain the foot height of the supporting leg in the trunk coordinate system {B}. The height difference of the legs is adjusted to adjust the robot's trunk posture and trunk position, so that the robot can adapt to the height difference of rugged terrain.

[0135] Furthermore, the step S103: the state estimator sends the collected data and the estimated slope to the contact detector and the motion controller respectively, specifically includes:

[0136] The state estimator sends the collected foot end position, velocity, plantar force parameters and estimated slope to the contact detector;

[0137] The state estimator sends the acquired foot position, velocity, plantar force parameters and estimated slope to the motion controller.

[0138] Furthermore, the contact detector estimates the probability of the foot touching the ground and sends the probability of the foot touching the ground to the state machine in S103. The specific calculation process includes:

[0139] S103-c1: The joint velocity and joint torque of the robot are obtained according to the state estimator of the quadruped robot, and the estimated value of the joint torque is solved by the generalized momentum theorem:

[0140]

[0141] in, is the coefficient related to the cutoff frequency β, rk , r represents generalized momentum, T step is the robot's gait cycle, τ is the joint motor torque, S is the selection matrix, C is the Coss force and centrifugal force, G is the gravity term, is the joint velocity.

[0142] Then, based on the estimated value of the joint torque, the mapping relationship between the joint torque and the plantar force is used to obtain the real-time contact force at the foot end.

[0143] S103-c2: Establish the quadruped robot phase time φ g , foot end height z h and foot contact force f z The probability distribution model of the foot end height and contact force conforms to the standard Gaussian normal distribution;

[0144]

[0145]

[0146]

[0147] Among them, erf is the representation of the error function, c φ represents the contact state of gait planning, c φ =1 is in the support phase, c φ = 0 is in the swing phase; φ is the duration of the sub-phase corresponding to the support phase or swing. c0 , μ c1 , σ c0 , σ c1 are the corresponding mean and variance of the sub-stages of the support phase, respectively. s0 , μ s1 , σ s0 , σ s1 are the corresponding means and variances of the sub-phases of the swing phase, respectively. is the mean and variance of height and plantar force, z h Indicates the real-time foot height, f z Indicates the real-time Z-direction plantar force.

[0148] S103-c3: Kalman filtering is used to realize the fusion of multi-source proprioceptive information, and the foot contact probability model P(φ g ) as the input of the Kalman filter The standard normal Gaussian distribution model P(z h )、P(f z ) as the measured variable The predicted touchdown probability Calibrate to calculate foot contact probability Estimates.

[0149] Among them, the prediction model of the Kalman filter is:

[0150]

[0151]

[0152] Among them, only the existing measurement parameters are considered to estimate the contact state of the system, the state matrix and the input matrix represents the current forecast value, represents the last best estimate, and represents the covariance of the posterior and prior estimates, represents the covariance of the observation noise;

[0153] The measurement model of the Kalman filter is:

[0154]

[0155]

[0156]

[0157] in, Represents the output matrix, corresponding to the identity matrix of the four legs, Indicates the measured value, K kf (n) represents the Kalman gain, represents the covariance of the measurement noise and I represents the identity matrix.

[0158] Through the Kalman filter measurement model, the contact probability of the foot touching the ground is estimated, and the early touchdown, delayed touchdown or sliding of the foot is detected. Figure 9(a)-Figure 9(d) This is the detection of the foot contact during the process of climbing high steps. The foot contact situation is consistent with the changing trends of the foot height, plantar force and phase time.

[0159] Furthermore, in S103, the state machine calculates the gait timing of the next state phase according to the probability of the foot touching the ground and the set time interval. The specific calculation process includes:

[0160] S103-d1: Figure 8 The figure shows the switching process of the state machine. When the supporting leg reaches the specified time T of the state machine, contact Afterwards, the leg is transformed into the swing phase state and executes the swing trajectory expected by the swing leg;

[0161] S103-d2: When a single leg is planned to swing, the foot of the single leg will continue to follow the desired trajectory until the state estimator detects that the foot touches the ground;

[0162] like Figure 8 As shown, if the state machine specified time T is not reached fall If the foot touches the ground, the state is switched to the support phase and the next phase is entered;

[0163] If the state machine reaches the specified time T fall , when it is detected that the foot end still has not touched the ground, the foot end continues to move downward until it detects that it has touched the ground;

[0164] S103-d3: When the foot end of the swinging leg is detected to touch the ground during the swinging process, the single leg immediately switches to the support phase state, executes the desired trajectory of the support leg according to the motion plan during the center of gravity adjustment process, waits to enter the state cycle of the next cycle, and switches the support and swinging of the single leg.

[0165] Below Figure 1 The twelve-degree-of-freedom quadruped robot shown is an embodiment of the present invention, in which the directions of the world coordinate system {W}, the trunk coordinate system {B}, and the hip joint coordinate system {H} are defined in accordance with those in the figure.

[0166] In order to cope with the movement of large obstacles and rugged terrain in nature, the quadruped robot needs to design an advanced planner and controller to maintain stability and accurate following during the movement. This paper proposes a quadruped robot motion planning and control method that can be applied to rugged terrain such as steps and stairs that are more than 63% of its leg length. The specific process is as follows Figure 2 As shown in the figure, a nonlinear trajectory optimizer is used to realize online planning of the optimal motion trajectory of the center of mass during the movement, thereby ensuring the stability margin of the center of mass during the movement; a probability-based plantar contact estimation algorithm is used to realize accurate detection of the contact state of the foot end of the quadruped robot; a multi-trigger state machine is used to realize state switching of the quadruped robot based on the contact state of the foot end; the foot end drop is used to estimate the terrain slope, thereby realizing the adaptation of the quadruped robot to the foot end terrain; a trapezoidal foot end swing trajectory is used to prevent the foot end from colliding with obstacles.

[0167] like Figure 11(a)-Figure 11(i) and Figure 12(a)-Figure 12(i) The quadruped robot moves in scenarios with large obstacles such as high steps and continuous stairs. The trajectory is accurately followed and the trunk remains stable during the movement.

[0168] The present invention comprises the following steps: planning the gait timing of a quadruped robot during crawling motion; planning the motion trajectory of the center of gravity during the quadruped support phase; estimating the unknown terrain slope according to the height of the foot end of the supporting foot; estimating the probability of the foot end touching the ground according to a probability model of Kalman filtering fusing multi-source information, and designing a multi-triggering state machine based on time and events; planning the swing trajectory of the foot end of the quadruped robot, and designing a trapezoidal swing motion trajectory; and implementing the motion trajectory execution by a whole-body motion controller based on a virtual model according to the planned motion trajectory.

[0169] Embodiment 2

[0170] This embodiment provides a quadruped robot crawling state planning system that can cross large obstacles;

[0171] A quadruped robot crawling planning system that can cross large obstacles includes:

[0172] An initialization module is configured to: set basic data of the quadruped robot; the basic data includes: a desired position, a desired speed, and a desired step height;

[0173] A construction module is configured to: construct a nonlinear trajectory optimizer and a swing phase trajectory planner based on basic data; the nonlinear trajectory optimizer is used to optimize the motion trajectory of the center of gravity of the quadruped robot; the swing phase trajectory planner is used to plan the swing trajectory of the foot end of the quadruped robot; construct a state machine, the state machine includes: a preset gait timing sequence of the quadruped robot; the initial state phase of the quadruped robot is a quadruped support phase;

[0174] The planning module is configured as follows: the quadruped robot starts to move, the nonlinear trajectory optimizer, the swing phase trajectory planner and the state machine send a control signal to the motion controller, and the motion controller controls the movement of the quadruped robot; the state estimator collects the positions of each foot of the quadruped robot, and estimates the slope of the unknown terrain according to the positions of each foot of the quadruped robot; the state estimator sends the collected data and the estimated slope to the contact detector and the motion controller respectively; the contact detector estimates the probability of the foot end touching the ground, and sends the probability of the foot end touching the ground to the state machine; the state machine calculates the gait timing of the next state phase according to the probability of the foot end touching the ground and the set time interval; the state machine sends the calculation result to the nonlinear trajectory optimizer and the swing phase trajectory planner;

[0175] The motion module is configured to repeat the work of the planning module, and the quadruped robot starts the motion of the next state phase until the quadruped robot crosses the obstacle.

[0176] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A crawling planning method for a quadruped robot capable of crossing large obstacles, characterized in that: include: S101: Set the basic data of the quadruped robot; The basic data includes: expected position, expected speed and expected step height; S102: Based on the basic data, construct a nonlinear trajectory optimizer and a swing phase trajectory planner; the nonlinear trajectory optimizer is used to optimize the motion trajectory of the center of gravity of the quadruped robot; the swing phase trajectory planner is used to plan the swing trajectory of the foot end of the quadruped robot; construct a state machine, the state machine includes: a preset gait timing of the quadruped robot; the initial state phase of the quadruped robot is a quadruped support phase; S103: The quadruped robot starts to move, and the nonlinear trajectory optimizer, the swing phase trajectory planner and the state machine send control signals to the motion controller, and the motion controller controls the movement of the quadruped robot; the state estimator collects the positions of each foot of the quadruped robot, and estimates the slope of the unknown terrain according to the positions of each foot of the quadruped robot; the state estimator sends the collected data and the estimated slope to the contact detector and the motion controller respectively; the contact detector estimates the probability of the foot end touching the ground, and sends the probability of the foot end touching the ground to the state machine; the state machine calculates the gait timing of the next state phase according to the probability of the foot end touching the ground and the set time interval; the state machine sends the calculation results to the nonlinear trajectory optimizer and the swing phase trajectory planner; The state estimator collects the positions of each foot of the quadruped robot and estimates the slope of the unknown terrain based on the positions of each foot of the quadruped robot. Specifically, it includes: S103-b1: The state estimator obtains the foot end position in the current four-legged or three-legged support phase, and thus estimates the average height of the first two legs: ； Estimate the average height of the back legs: ； Estimate the average height of the two left legs: ； Estimate the average height of the right legs: ； in, , , , Respectively represent the RF leg, LF leg, RH leg, and LH leg in the hip joint coordinate system { H }Down z Direction location; S103-b2: Estimate the slope of the unknown terrain based on the foot end drop, calculate the pitch angle of the quadruped robot during terrain adaptation, and calculate the expected pitch angle based on the front and rear foot end drop: ； The desired roll angle is calculated from the difference in footfall between the left and right feet: ； in, Represented in { H Coordinate system x Direction of the front leg position, Represented in { H Coordinate system x Direction of the hind legs, Represented in { H Coordinate system y Direction of the left leg position, Represented in { H Coordinate system y Direction: Position of right leg; S103-b3: The calculated desired posture angle of the torso Converted into a rotation matrix, and then the torso coordinate system is obtained { B The height of the foot of the lower support leg is adjusted to adjust the height difference of the legs to adjust the posture and position of the robot's trunk, so that the robot can adapt to the height difference of rough terrain; S104: Repeat step S103, and the quadruped robot starts moving to the next state phase until the quadruped robot crosses the obstacle.

2. The method for planning the crawling state of a quadruped robot capable of crossing large obstacles as claimed in claim 1, characterized in that: The state machine includes two triggering modes, one of which is a time-based triggering mode, when a set time interval is reached, the robot starts the next state phase; the other is an event-based triggering mode, when the robot's swinging foot touches the ground, the robot starts the next state phase.

3. The crawling state planning method of a quadruped robot capable of crossing large obstacles as claimed in claim 1, characterized in that: Based on the basic data, a nonlinear trajectory optimizer and a swing phase trajectory planner are constructed, wherein the swing phase trajectory planner includes: During the foot end lifting phase, the robot's foot end moves backward and lifts upward to prevent collision with the protruding part of the obstacle; During the forward swing phase of the foot, the robot maintains Z The direction and height remain unchanged. According to the set step length, the foot moves forward horizontally. Z direction and maintain a set distance from obstacles to prevent tipping or instability due to collision; Z Direction refers to the direction opposite to the robot's gravity direction; In the vertical drop phase of the foot end, the robot X The direction remains unchanged. Z The direction height gradually decreases until it contacts the obstacle ground; when the contact detection probability is greater than the set threshold, it is judged as touching the ground, the swing phase ends, and the leg enters the next cycle; X The direction refers to the front of the robot's torso.

4. The crawling state planning method of a quadruped robot capable of crossing large obstacles as claimed in claim 1, characterized in that: Build a state machine, including: S102-c1: The predetermined stepping sequence during the crawling movement is: four-legged support long adjustment phase, left hind leg swing, four-legged support short adjustment phase, left front leg swing, four-legged support long adjustment phase, right hind leg swing, four-legged support short adjustment phase, right front leg swing, a total of eight states, at each moment at least three legs are in the support state; in the four-legged support phase, according to the constructed nonlinear trajectory optimizer, the center of gravity is planned online to move to the position with the maximum stability margin of the support triangle; in the single-leg swing phase, the foot end swings according to the planned swing leg foot end trajectory; S102-c2: Set the optimal landing strategy: After the foot of the swinging leg touches the ground, estimate the virtual standard energy stability margin of the next leg swing according to the standard energy stability margin criterion S nesm size; if S nesm Less than the set minimum stability margin value S min , then the trunk will tip over when the center of gravity is moved at the next moment, so the gait sequence is changed, and the current leg is still used as the swing leg to land again, increasing the stability margin of the support polygon to prevent the trunk from tipping over due to the reduction of the stable area caused by accidental contact with the ground; if S nesm Greater than the set minimum stability margin value S min , then at the next moment, continue to move the center of gravity and swing the legs according to the predetermined gait sequence; S102-c3: In the next gait cycle, the default landing sequence is restored, and the trunk of the robot is moved according to the predetermined gait sequence. The landing strategy of S102-c2 is used to continue to judge the stability margin to prevent the robot from colliding with obstacles during movement, causing it to touch the ground prematurely and reduce the support polygon area, thereby tipping over.

5. The crawling state planning method of a quadruped robot capable of crossing large obstacles as claimed in claim 1, characterized in that: The quadruped robot starts to move, and the nonlinear trajectory optimizer, swing phase trajectory planner, and state machine send control signals to the motion controller. The motion controller controls the movement of the quadruped robot, including: The nonlinear trajectory optimizer sends the motion controller a W }Down x, y, z The expected position, motion speed and acceleration command of the center of gravity motion trajectory after direction optimization; The swing phase trajectory planner sends the hip joint coordinate system to the motion controller. H }Down x, y, z Direction optimization of the desired position, movement speed and acceleration commands of the hindfoot; The state machine sends the swing sequence of each leg, the phase time of the support phase and the phase time of the swing phase to the motion controller; the motion controller controls the movement of the quadruped robot.

6. The method for planning the crawling state of a quadruped robot capable of crossing large obstacles as claimed in claim 5, characterized in that: Motion controller, including: S103-a1: simulate the spring damping model according to the planned expected COG position and posture information and the COG position and posture information fed back by the state estimator, calculate the virtual force and virtual torque required by the robot according to the virtual stiffness and damping, and use the virtual spring principle to increase the flexibility of the system and reduce the foot end impact; in, and is the desired COG position and attitude angle of the torso, and is the actual torso COG position and posture, , , and are the positive definite diagonal matrices of proportional gain and derivative gain respectively; S103-a2: Construct the state space equation of the virtual model. Since the system is not rank-satisfied, the least square method is used to transform it into an optimization problem. The optimal plantar force is solved using the quadratic programming optimization library. The optimal plantar force satisfies the friction cone constraint. in, represents the optimal contact force of the i-th leg, is the gravity of the quadruped robot, 𝑰 is the unit matrix, is the position vector of the foot end in the trunk coordinate system during the support phase, 𝑺 and 𝑾 are the weights of the diagonal semi-positive definite matrix, , , The foot end is x, y, z The friction force in the direction are the minimum and maximum friction constraints, is the coefficient of friction of the ground; S103-a3: Based on the calculated optimal plantar force , through the Jacobian matrix of the leg according to Solving for joint torque , and sends the obtained joint torque to the joint motor for execution, thereby controlling the leg motion of the quadruped robot and completing the tracking of the desired motion trajectory of the torso.

7. The method for planning the crawling state of a quadruped robot capable of crossing large obstacles as claimed in claim 1, characterized in that: The contact detector estimates the probability of the foot touching the ground and sends the probability of the foot touching the ground to the state machine. The specific calculation process includes: S103-c1: The joint velocity and joint torque of the robot are obtained according to the state estimator of the quadruped robot, and the estimated value of the joint torque is solved by the generalized momentum theorem: in, is related to the cut-off frequency The coefficient of correlation, , represents the generalized momentum, is the robot's gait period, is the joint motor torque, is the selection matrix, It is the Cosmic force and the centrifugal force. is the gravity term, is the joint velocity; Then, based on the estimated value of the joint torque, the mapping relationship between the joint torque and the plantar force is used to obtain the real-time contact force at the foot end. ; S103-c2: Establish the phase time of the quadruped robot , foot end height and foot contact force The probability distribution model of the foot end height and contact force conforms to the standard Gaussian normal distribution; in, erf is the representation of the error function, represents the contact state of gait planning, In the support phase, is in the swing phase; 𝜙 is the duration of the sub-phase corresponding to the support phase or swing; , , , are the corresponding means and variances of the sub-stages of the support phase, respectively; , , , are the corresponding means and variances of the sub-phases of the swing phase, respectively; , , , are the mean and variance of height and plantar force, Indicates the real-time foot height, Indicates the real-time Z-direction plantar force; S103-c3: Fusion of multi-source proprioceptive information through Kalman filtering, and foot contact probability model established by state machine As the input of Kalman filter , the standard normal Gaussian distribution model established by foot end height and plantar force , As a measured variable The predicted touchdown probability Calibrate to calculate foot contact probability estimates; Among them, the prediction model of the Kalman filter is: Among them, only the existing measurement parameters are considered to estimate the contact state of the system, the state matrix and the input matrix ; represents the current forecast value, represents the last best estimate, and represents the covariance of the posterior and prior estimates, represents the covariance of the observation noise; The measurement model of the Kalman filter is: in, Represents the output matrix, corresponding to the identity matrix of the four legs, Indicates the measured value, represents the Kalman gain, represents the covariance of the measurement noise, Represents the identity matrix.

8. The method for planning the crawling state of a quadruped robot capable of crossing large obstacles as claimed in claim 1, characterized in that: The state machine calculates the gait timing of the next state phase according to the probability of the foot touching the ground and the set time interval. The specific calculation process includes: S103-d1: When the supporting leg reaches the time specified by the state machine Afterwards, the leg is transformed into the swing phase state and executes the swing trajectory expected by the swing leg; S103-d2: When a single leg is planned to swing, the foot of the single leg will continue to follow the desired trajectory until the state estimator detects that the foot touches the ground; If the state machine specified time is not reached T fall If the foot touches the ground, the state is switched to the support phase and the next phase is entered; If the state machine reaches the specified time T fall , when it is detected that the foot end still has not touched the ground, the foot end continues to move downward until it detects that it has touched the ground; S103-d3: When the foot end of the swinging leg is detected to touch the ground during the swinging process, the single leg immediately switches to the support phase state, executes the desired trajectory of the support leg according to the motion plan during the center of gravity adjustment process, waits to enter the state cycle of the next cycle, and switches the support and swinging of the single leg.

9. A quadruped robot crawling planning system capable of crossing large obstacles, characterized by: include: An initialization module, which is configured to: set basic data of the quadruped robot; The basic data includes: expected position, expected speed and expected step height; A construction module is configured to: construct a nonlinear trajectory optimizer and a swing phase trajectory planner based on basic data; the nonlinear trajectory optimizer is used to optimize the motion trajectory of the center of gravity of the quadruped robot; the swing phase trajectory planner is used to plan the swing trajectory of the foot end of the quadruped robot; construct a state machine, the state machine includes: a preset gait timing sequence of the quadruped robot; the initial state phase of the quadruped robot is a quadruped support phase; The planning module is configured as follows: the quadruped robot starts to move, the nonlinear trajectory optimizer, the swing phase trajectory planner and the state machine send a control signal to the motion controller, and the motion controller controls the movement of the quadruped robot; the state estimator collects the positions of each foot of the quadruped robot, and estimates the slope of the unknown terrain according to the positions of each foot of the quadruped robot; the state estimator sends the collected data and the estimated slope to the contact detector and the motion controller respectively; the contact detector estimates the probability of the foot end touching the ground, and sends the probability of the foot end touching the ground to the state machine; the state machine calculates the gait timing of the next state phase according to the probability of the foot end touching the ground and the set time interval; the state machine sends the calculation result to the nonlinear trajectory optimizer and the swing phase trajectory planner; The state estimator collects the positions of each foot of the quadruped robot and estimates the slope of the unknown terrain based on the positions of each foot of the quadruped robot. Specifically, it includes: S103-b1: The state estimator obtains the foot end position in the current four-legged or three-legged support phase, and thus estimates the average height of the first two legs: ； Estimate the average height of the back legs: ； Estimate the average height of the two left legs: ； Estimate the average height of the right legs: ； in, , , , Respectively represent the RF leg, LF leg, RH leg, and LH leg in the hip joint coordinate system { H }Down z Direction location; S103-b2: Estimate the slope of the unknown terrain based on the foot end drop, calculate the pitch angle of the quadruped robot during terrain adaptation, and calculate the expected pitch angle based on the front and rear foot end drop: ； The desired roll angle is calculated from the difference in footfall between the left and right feet: ； in, Represented in { H Coordinate system x Direction of the front leg position, Represented in { H Coordinate system x Direction of the hind legs, Represented in { H Coordinate system y Direction of the left leg position, Represented in { H Coordinate system y Direction: Position of right leg; S103-b3: The calculated desired posture angle of the torso Converted into a rotation matrix, and then the torso coordinate system is obtained { B The height of the foot of the lower support leg is adjusted to adjust the height difference of the legs to adjust the posture and position of the robot's trunk, so that the robot can adapt to the height difference of rough terrain; The motion module is configured to repeat the work of the planning module, and the quadruped robot starts the motion of the next state phase until the quadruped robot crosses the obstacle.

Images