Quadruped robot rapid motion control method under discrete terrain

Abstract

The invention provides a quick motion control method for a quadruped robot in a discrete terrain, which is used for improving the quick passing performance of the quadruped robot in a complex discrete terrain. According to the method, a multi-dimensional constraint model including dynamic constraints, kinematics constraints and the like is constructed, and limiting conditions of the robot during fast passing are systematically analyzed; an optimal safe landing point is searched by adopting an efficient optimization algorithm, and a dynamic response rapid motion controller is established on the basis of the optimal safe landing point, so that the posture and the foot end track of the robot are accurately regulated and controlled, and finally, the quadruped robot is ensured to have rapid and stable motion capability in discrete terrains.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of robot motion control, and particularly to a fast motion control method of a quadruped robot in discrete terrain. BACKGROUND

[0002] Quadruped robots can precisely control the landing points by virtue of the autonomous foot selection of discrete swing legs, and thus exhibit stronger environmental adaptability in complex terrains such as plateaus and mountains, as compared with traditional mobile platforms such as wheeled and tracked platforms. At present, quadruped robots mainly rely on perception sensors to collect terrain elevation data, and then complete landing point planning and motion control. For typical discrete terrains such as continuous concave barriers and plum-blossom stakes, the path planning method based on a geometric safety area and the traditional control model can still meet the operation requirements when the robot travels at low speed under normal working conditions. However, when the robot needs to quickly cross the discrete terrain, the kinetic disturbance caused by the landing impact is significantly aggravated, which limits the moving speed of the robot and greatly restricts the improvement of the motion efficiency of the quadruped robot in complex terrain environments. SUMMARY

[0003] Therefore, the present application provides a fast motion control method of a quadruped robot in discrete terrain, which is used to improve the fast passing performance of the quadruped robot in complex discrete terrain. The present application systematically analyzes the limiting conditions of the robot when it quickly passes through by constructing a multi-dimensional constraint model including kinetic constraints, kinematic constraints, etc. An efficient optimization algorithm is used to search for the optimal safe landing point, and a dynamic response fast motion controller is established based on the optimal safe landing point, so as to accurately regulate the posture and foot trajectory of the robot, and finally ensure that the quadruped robot has fast and stable motion capability in discrete terrain.

[0004] A fast motion control method of a quadruped robot in discrete terrain, the implementation steps of the method are as follows:

[0005] Step one, determining the optimal landing point in discrete terrain;

[0006] Step two, constructing a robot stable controller based on the optimal landing point, and iteratively optimizing the foot end desired force F d that enables the robot body to stably adapt based on the optimal landing point, and designing the swing leg trajectory;

[0007] Step three, force and position control of the joints to realize the swing and support of the robot.

[0008] Further, the specific implementation process of step one is as follows:

[0009] Step 11: constructing a search area based on the balanced landing point;

[0010] Step 12: search for a safe landing point meeting the constraint condition in the search area;

[0011] Step 13: quickly search for a safe landing point closest to the distance balance landing point, which is the optimal landing point.

[0012] Further, the constraint conditions include: leg length constraint condition, terrain constraint condition, and collision constraint condition with adjacent support legs, wherein the leg length constraint condition specifically refers to that the distance between the safe landing point and the leg hip joint is not greater than the maximum leg length.

[0013] Further, the specific implementation process of step two is as follows:

[0014] Step 21: establish a center of mass adaptation model under large steps to obtain a position compensation amount and a speed compensation amount of the center of mass of the robot, correct the center of mass position and the center of mass speed of the robot, and obtain the expected state X ref_k of the robot.

[0015] Step 22: establish a fuselage stabilization controller based on model prediction, and iteratively optimize the foot force based on the optimal landing point to enable the robot fuselage to stably adapt.

[0016] Step 23: swing leg trajectory design: taking the current foot position as the starting point and the optimal landing point as the ending point, the swing leg is smoothly swung to the optimal landing position through polynomial planning.

[0017] Further, the step 22 includes: predicting the state of the robot by establishing a single rigid body dynamics model of the robot; using the predicted state of the robot and the expected state X ref_k , according to the established fuselage stabilization controller, iteratively optimizing the foot force based on the optimal landing point to enable the robot fuselage to stably adapt, that is, the control input U k .

[0018] Further, swing leg trajectory re-planning design is used to consider the swing inertia disturbance of the fast walking leg under discrete terrain, to ensure that the robot can accurately land in the safe area, including:

[0019] In 0~T1 time, a quintic polynomial is used to plan the swing trajectory; when it is detected that the actual landing position deviates from the safe landing position by more than a set threshold, the position of the swing expected foot is re-planned in T1~T2 time.

[0020] Further, the specific implementation process of step three is as follows:

[0021] Step 31: establish a joint control model for swing leg trajectory mapping to obtain a joint position control amount.

[0022] Step 32: establish a joint control model of support leg force mapping, and obtain a joint force control quantity.

[0023] Further, the step 31 further comprises obtaining a desired joint angle vector according to the swing leg trajectory, and obtaining a joint position control quantity by using the desired joint angle vector, an actual joint angle vector, a desired joint angle velocity vector and an actual joint angle velocity vector.

[0024] Further, the joint control model of support leg force mapping is specifically as follows,

[0025] u τ =τ ff

[0026] In the formula: τ ff =-J T F d is a joint force feedforward term, F d is a foot end desired force, J is a joint force Jacobian matrix, u τ is a joint force controller input, i.e., a joint force control quantity.

[0027] Advantageously:

[0028] 1、The application fully considers the constraint conditions of safe landing and uses the kdTree search method to quickly select the optimal landing point.

[0029] 2、The application fully considers the influence of large-step landing and adjusts the centroid to adapt, which can improve the stability of the robot support.

[0030] 3、The application integrates the secondary re-planning design of the robot swing leg, which can further improve the control accuracy of large-step landing.

[0031] 4、The application can quickly realize stable adaptation to discrete terrain and improve the passing efficiency of the quadruped robot.

[0032] 5、The application realizes stable adaptation to discrete terrain by establishing a robot fast motion controller, which improves the passability of the quadruped robot under complex terrain.

[0033] 6、The application fully considers the various constraint conditions of the robot fast passing, searches the optimal safe landing efficiently and further establishes a fast motion controller, so as to realize the fast and stable adaptation of the robot to discrete terrain. BRIEF DESCRIPTION OF DRAWINGS

[0034] Figure 1 is a flow chart of the steps of the quadruped robot fast motion control method;

[0035] Figure 2is the optimal foot placement selection schematic diagram;

[0036] Figure 3 is the centroid stable adaptation schematic diagram;

[0037] Figure 4 is the swing quadratic programming schematic diagram;

[0038] Wherein, P c Indicates the foot starting position before quadratic programming. DETAILED DESCRIPTION

[0039] The application will be described in detail below with reference to the accompanying drawings and examples.

[0040] The application provides a fast motion control method of a four-legged robot on discrete terrain, and the steps of the method are as shown in the accompanying Figure 1 The steps are as follows:

[0041] Step 1, optimal foot placement selection on discrete terrain: a search area is constructed around the balance foot placement; a safe foot placement meeting the constraint condition is searched in the area; the nearest safe foot placement to the balance foot placement is searched quickly through kdTree, which is the optimal foot placement.

[0042] Further comprising:

[0043] Step 11: constructing a search area based on the balance foot placement;

[0044] Firstly, the balance point position is calculated, and the balance point is calculated as follows:

[0045]

[0046] In the formula, H z is the height of the robot, g is the acceleration of gravity, is the actual forward and lateral speed of the robot, is the expected forward and lateral speed of the robot, k v,x , k v,y is the experimental tuning coefficient, p x_offset , p y_offset is the compensation amount corresponding to the deviation of the centroid of the robot and the geometric center position of the body, which can be obtained by calibration.

[0047] Take the balance point p slip As the reference, search forward a distance, search backward a distance b, search left a distance c, and search right a distance d to form a rectangular search area, and the length and width of the rectangle correspond to (a+b, c+d).

[0048] Step 12: as shown in the accompanying Figure 2 , the constraint condition of searching for a safe foot placement is constructed, and is constructed as follows:

[0049] First, satisfy the constraint condition for leg length: ||p σ -p hip ||≤l leg_max

[0050] In the formula, p σ For a safe landing point, p hip The hip joint of the leg, l leg_max This is the maximum leg length.

[0051] Secondly, the constraint condition of terrain undulation must be satisfied: σ≤σ * ,

[0052] In the formula, N is the number of elevation grid cells, x i Let i be the elevation value of the i-th grid. σ is the average elevation of N grids. * x is the threshold for terrain undulation information. * This is the terrain elevation threshold.

[0053] Furthermore, the constraint condition of not colliding with adjacent support legs must be met: ||p σ -p s ||≥d *

[0054] In the formula, p s d represents the position of the foot of the adjacent supporting leg. * The set collision distance threshold.

[0055] Step 13: Under the above constraints, the kdTree search obtains the safest landing point closest to the equilibrium point. The search process is as follows:

[0056] First, p slip From the root node Begin, according to p slip and safe landing point The distance comparison result is used to traverse the kdTree downwards until the leaf node is reached. At this point, calculate p. slip leaf nodes Calculate the distance between them and record the current nearest neighbor position. Where i represents the index of the safe landing point, and j represents the index of the leaf node.

[0057] Secondly, perform a bottom-up backtracking operation to find the distance p. slip Closer neighbor This process is repeated recursively until the search set is empty, at which point the search is terminated. Considering that the closer the robot is to the equilibrium landing point, the more stable it is, therefore, p that satisfies the termination condition is selected.slip Neighboring points as optimal landing points

[0058] Step 2, by constructing a robot stability controller based on the optimal landing point, iteratively optimize the optimal landing point based on the foot force that enables the robot body to adapt stably.

[0059] As shown in Figure 3 and Figure 4 , specifically includes:

[0060] Step 21: By establishing the mass center adaptation model under large strides, the position compensation and velocity compensation of the robot mass center are obtained, and then the corrected mass center position and mass center velocity of the robot are obtained, wherein the mass center adaptation model is established as follows:

[0061]

[0062] In the formula, p com_offset , is the position compensation and velocity compensation of the mass center, N l is the number of legs, k p , k v , λ are tuning coefficients, is the actual linear velocity of the robot.

[0063] Further, the position and velocity of the robot are corrected by using the position compensation and velocity compensation of the mass center, and X ref_k , that is, the tuned mass center compensation part is superimposed into X ref_k corresponding mass center position and velocity elements, specifically as follows:

[0064] , p x_d , is the expected position and expected velocity of the mass center in the x direction, p y_d , is the expected position and expected velocity of the mass center in the y direction, p z_d , is the expected position and expected velocity of the mass center in the z direction, is the position compensation of p com_offset corresponding to the x and y directions, is the velocity compensation corresponding to the x and y directions, Θ d is the expected body attitude of the robot, ω d is the expected angular velocity of the robot.

[0065] Step 22: By establishing a single rigid body dynamics model of the robot, the state variables of the robot are predicted; using the state variables and X ref_k, according to the constructed body stability controller, iteratively optimize the foot force based on the optimal landing point, that is, the control input U k .

[0066] wherein a single rigid body dynamics model of the robot is established, and the following is established

[0067]

[0068] wherein m is the mass of the robot body, p is the center of mass position of the robot, are the linear acceleration and the angular acceleration of the robot, respectively, and ω is the angular velocity of the robot; (r i -p) × , ω × respectively represent the anti-symmetric matrix of (r i -p), ω; F i is the ground reaction force of the ith leg; r i is the position of the contact point of the ith support leg; I is the moment of inertia of the robot body, g is the acceleration of gravity, and n is the number of support legs.

[0069] Secondly, the system dynamics is discretized and expressed as:

[0070] X k+1 = A k X k +B k U k

[0071] is the system state variable, A k is the state matrix, B k is the input matrix, and U k =[F1 F2 F3F4] T is the control input, k is the current iteration number, is the linear velocity of the robot, and Θ is the actual body attitude of the robot.

[0072] Further, the body stability controller is used to rollingly optimize the control input U k , and the body stability controller is specifically as follows:

[0073]

[0074]

[0075] wherein Q and R are weighting matrices; X ref_k is the expected system state, is the normal force of the ith contact point. is the tangential force of the ith contact point; μ is the sliding friction coefficient, N is the maximum number of iterations, is the minimum value of the normal force.

[0076] Step 23: Swing leg trajectory re-planning design: taking the current foot position as the starting point and the optimal landing foot as the ending point, the swing leg is smoothly swung to the optimal landing foot position through polynomial planning, considering the swing inertia disturbance of the fast walking leg on the discrete terrain, in order to ensure that the robot can accurately land in the safe area, swing leg re-planning design is carried out, and the following is established:

[0077] In 0~T1 time, the swing trajectory is planned by using quintic polynomial, and the planning is as follows:

[0078] p ref_f =a5t 5 +a4t 4 +a3t 3 +a2t 2 +a1t+a0

[0079] In the formula, p ref_f is the expected position of the foot, a i is the polynomial coefficient, and t is the swing time. According to the optimal landing point, swing height, starting point and intermediate position point information, the above polynomial can be solved.

[0080] When it is detected that the actual landing position deviates from the safe landing by more than the set threshold p * , the position of the swing expected foot is planned in T1~T2 time, that is:

[0081] p ref_f =a′5t 5 +a′4t 4 +a′3t 3 +a′2t 2 +a′1t+a′0

[0082] In the formula, a i ′ is the polynomial coefficient of the quadratic programming.

[0083] Step 3, joint force position control to realize the swing and support of the robot.

[0084] Step 31: Establish the joint control model of swing leg trajectory mapping, and establish as follows:

[0085] θ d =IK(p ref_f )

[0086]

[0087] In the formula: IK represents leg inverse kinematics, K pθ , Kvθ K, C are controller stiffness, damping coefficient matrices; θ d is the desired joint angle vector; θ is the actual joint angle vector; is the desired joint angular velocity vector; is the actual joint angular velocity vector; u p is the joint position controller input.

[0088] Step 32: Establish the joint control model of the support leg force mapping, which is established as follows:

[0089] u τ = τ ff

[0090] In the formula: τ ff = -J T F d is the joint force feedforward term; F d is the foot end desired force; J is the joint force Jacobian matrix; u τ is the joint force controller input.

[0091] To sum up, the above is only a preferred embodiment of the present application, and is not used to limit the protection scope of the present application. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A method for rapid motion control of a quadruped robot in discrete terrain, characterized in that: Step 1: Determine the optimal landing point under discrete terrain. Step two involves constructing a robot stabilization controller based on the optimal foot placement point, and iteratively optimizing the foot force F based on the optimal foot placement point to enable the robot body to stably adapt. d And design the trajectory of the swing leg; Step 3: Perform force and position control on the joints to enable the robot to swing and support.

2. The method for rapid motion control of a quadruped robot in discrete terrain as described in claim 1, characterized in that, The specific implementation process of step one is as follows: Step 11: Construct the search area based on the balance foot points; Step 12: Search for safe landing points that meet the constraints within the search area; Step 13: Quickly search for the safest landing point closest to the equilibrium landing point, which is the optimal landing point.

3. The rapid motion control method for a quadruped robot in discrete terrain as described in claim 2, characterized in that, The constraints include: leg length constraints, terrain undulation constraints, and constraints to avoid collisions with adjacent supporting legs. Specifically, the leg length constraint means that the distance between the safe landing point and the hip joint of the leg is not greater than the maximum leg length.

4. The method for rapid motion control of a quadruped robot in discrete terrain as described in any one of claims 1, characterized in that, The specific implementation process of step two is as follows: Step 21: Establish a centroid adaptation model under large stride conditions, obtain the position and velocity compensation values ​​of the robot's centroid, correct the robot's centroid position and velocity, and obtain the robot's desired state X. ref_k ; Step 22: Establish a model-predictive-based body stability controller and iteratively optimize the foot force based on the optimal footing point to enable the robot body to adapt stably; Step 23: Swing leg trajectory design: Starting from the current foot position and ending at the optimal landing point, the swing leg is smoothly swung to the optimal landing position through polynomial programming.

5. The method for rapid motion control of a quadruped robot in discrete terrain as described in claim 4, characterized in that, Step 22 includes: predicting the robot's state by establishing a single rigid body dynamics model of the robot; and using the predicted robot state and the desired state X. ref_k Based on the constructed body stability controller, the foot force that enables the robot body to adapt stably is iteratively optimized based on the optimal foot placement point, i.e., the control input U. k .

6. The method for rapid motion control of a quadruped robot in discrete terrain as described in claim 4, characterized in that, Swing leg trajectory replanning design, used to account for the swing inertial disturbances of the fast-walking leg in discrete terrain, ensures that the robot can land accurately in a safe area, including: During the time interval 0 to T1, a fifth-order polynomial is used to plan the swing trajectory; when the deviation between the actual landing position and the safe landing position is detected to be greater than the set threshold, a second-order planning is performed during the time interval T1 to T2 to plan the desired swing position.

7. The method for rapid motion control of a quadruped robot in discrete terrain as described in any one of claims 1-6, characterized in that, The specific implementation process of step three is as follows: Step 31: Establish a joint control model for the swing leg trajectory mapping to obtain the joint position control variables; Step 32: Establish a joint control model for the force mapping of the supporting leg to obtain the joint force control quantity.

8. The method for rapid motion control of a quadruped robot in discrete terrain as described in claim 7, characterized in that, Step 31 further includes obtaining the desired joint angle vector based on the swing leg trajectory, and using the desired joint angle vector, the actual joint angle vector, the desired joint angular velocity vector, and the actual joint angular velocity vector to obtain the joint position control quantity.

9. The method for rapid motion control of a quadruped robot in discrete terrain as described in claim 7, characterized in that, The joint control model for the force mapping of the supporting leg is as follows. you τ =t ff In the formula: τ ff =-J T F d For the joint force feedforward term, F d Let J be the expected force at the foot, and J be the Jacobian matrix of the joint forces. τ This is the input to the joint force controller, i.e., the joint force control quantity.

Images