Agricultural biped robot and pose control algorithm

Abstract

The invention discloses an agricultural biped robot and a pose control algorithm. The pose control algorithm comprises the following steps that basic parameters of a model prediction control algorithm are initialized; establishing a speed model, an angular speed model, an angle change model and a position change model; establishing a spatial state discrete model; building a model predictive control algorithm, wherein the model predictive control algorithm comprises a space state equation, a cost function and constraints; and performing attitude planning through the model prediction control algorithm, wherein the attitude planning algorithm comprises degree of freedom, standing, squatting, stepping, walking attitude, pause, translation and steering. According to the technical scheme, by optimizing the motion model, the spatial state discrete model, attitude planning and other control algorithms, the control efficiency of the agricultural biped robot is improved, the robot is prevented from sensing confusion and adapting to the complexity of the agricultural production environment, and the motion control capacity is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of robot posture control, and in particular to an agricultural biped robot and a posture control algorithm. Background Art

[0002] The open source robot operating system ROS has been widely used in robotic arms, underwater robots, humanoid robots, composite robots, etc., and has become a universal standard in the field of robotics. Simulation tools based on webots, Matlab, gazebo, and RVIZ can quickly debug, evaluate, iterate, and optimize robot algorithms and controllers. ChatGPT has triggered a wave of generative artificial intelligence. For the first time, computers have truly understood human language, which will revolutionize the ability of robots to summarize and inductively reason. The development of robotics technology will surely empower all walks of life. At present, the research and application of agricultural bipedal robots has become a hot topic of concern. In this context, posture control is one of the important issues in the application of agricultural bipedal robots. Traditional posture control algorithms perform poorly in dealing with the complexity of agricultural environments and cannot meet the posture control requirements of agricultural bipedal robots. Therefore, designing a new posture control algorithm suitable for agricultural bipedal robots is of great significance and value to improve the stability and work efficiency of agricultural bipedal robots. Summary of the invention

[0003] The main purpose of the present invention is to provide an agricultural bipedal robot posture control algorithm, aiming to solve the problem that the posture control algorithms of existing bipedal robots perform poorly when dealing with the complexity of agricultural environments.

[0004] To achieve the above purpose, the present invention proposes an agricultural biped robot posture control algorithm, which includes the following steps:

[0005] Initialize the basic parameters of the model predictive control algorithm;

[0006] Establishing a velocity model, an angular velocity model, an angle change model and a position change model; establishing a spatial state discrete model through the velocity model, the angular velocity model, the angle change model, the position change model, the forward coordinate system, the prediction period and the control input;

[0007] Complete the construction of a model predictive control algorithm according to the spatial state discrete model, wherein the model predictive control algorithm includes a spatial state equation, a cost function and constraints;

[0008] Performing posture planning through the model predictive control algorithm, the posture planning algorithm includes degrees of freedom, standing, squatting, stepping, walking posture, pausing, translation and turning;

[0009] The motion trajectory and state of the robot are calculated through the model predictive control algorithm and the attitude planning algorithm, so as to achieve smooth and stable output of position and attitude changes and realize the stable control of the robot's pose.

[0010] Preferably, the step of initializing the basic parameters of the model predictive control algorithm includes the following steps:

[0011] Coordinate system initialization, the coordinate system includes a world coordinate system, a body coordinate system, and a forward coordinate system;

[0012] Initialization of the sampling period and the prediction period;

[0013] Initialization of the control input, the control input includes the forces and torques at the left and right foot ends;

[0014] Initialization of the target state; the target state includes the current state measured by the sensor.

[0015] Preferably, the spatial state discrete model expression includes:

[0016] x i+1 = Ax i + Bu i + c( - );

[0017]

[0018] In the formula: m0 represents the torque of the left leg; m1 represents the torque of the right leg; m represents the mass of the robot; f0 represents the force of the left leg; f1 represents the force of the right leg; g represents the acceleration due to gravity; ω i represents the body three-axis angular velocity of the robot at time point i; v i represents the body three-axis motion velocity of the robot at time point i; x i represents the state of the robot at time point i; Δθ i represents the angular change from time point i - 1 to time point i of the robot, Δθ i = ω * Δt; Δp i represents the position change amount from time point i - 1 to time point i of the robot; I represents the moment of inertia; u i represents the input variable at time point i; l0 represents the length of the left leg, l1 represents the length of the right leg, E is used to represent the identity matrix, A is used to represent the matrix above the letter A in the second formula, B i is used to represent the matrix above B in the second formula i above matrix, c is used to represent the matrix above c in the second formula.

[0019] Preferably, the spatial state equation includes:

[0020] x = A + B·u (three);

[0021]

[0022] In the formula, x represents the state recursive sequence, The symbol above x is a stacked matrix expression; A represents the stacked matrix above in the fourth formula, B represents the stacked matrix above the fourth formula; x n represents the state variable at time n, u n-1 represents the input variable at time n-1, n represents the forecast period, A 0 = Ax0 + g; A i =A. A i-1 +g.

[0023] Preferably, the constraints include: equality constraints and inequality constraints, the equality constraints include: when only one leg of the robot is in contact with the ground, the contact leg is called the standing leg, and the state is in the support phase at this time; the other leg is called the swing leg, and the state is in the swing phase at this time; the inequality constraints include that the sole of the robot has an upward supporting force, horizontal friction force and rotational friction force, and the equivalent effect point of the torque does not exceed the sole polygon.

[0024] Preferably, the equality constraint comprises:

[0025]

[0026] The inequality constraints include:

[0027] L i ·u i +M i ≥0(vi);

[0028]

[0029] In the sixth formula, L i Indicates that L in the seventh formula i The matrix above; the sixth formula M i In the seventh formula, M i The matrix above; in the sixth formula, u i Indicates that u in the seventh formula i The matrix above; m x represents the moment in the x direction; m y represents the moment in the y direction; m z represents the moment in the z direction; f z represents the force in the z direction; f x represents the force in the x direction; f yIndicates the force in the y direction; max(f z ) represents the maximum force in the z direction; l y represents the force arm in the y direction; l x represents the force arm in the x direction; u z represents the friction coefficient in the z direction; u represents the friction coefficient.

[0030] Preferably, the model predictive control algorithm is constructed according to the spatial state discrete model, and the model predictive control algorithm includes the spatial state equation, cost function and constraint steps:

[0031] The algebraic functions include:

[0032] minJ=u T Hu+2u T h+C;

[0033] H= B T QB +R;

[0034]

[0035] Among them, C is a constant; Q is a diagonal matrix, and each element in Q is the error weight coefficient; R is also a diagonal matrix, and each element in R is the input weight coefficient; is the gravity feedforward given to the corresponding leg according to the future support phase; minJ represents the minimum value of the cost function; u T Indicates the input parameter matrix transpose; x d Indicates a desired state.

[0036]

[0037] Preferably, the posture planning includes the steps of freedom, standing, squatting, stepping, walking posture, pausing, translating and turning, and the squatting posture planning includes the following steps;

[0038] The joint angles of each degree of freedom in the initial and squatting postures;

[0039] The joint angles in the process are optimized through periodic changes of the cosine function;

[0040]

[0041] Wherein: j represents the real-time posture, j1 represents the state of each degree of freedom joint in the squatting posture, j0 represents the state of each degree of freedom joint in the standing posture, t represents time, t∈[0,1);

[0042] The speed of the joint angles during the process is optimized through the sine function;

[0043]

[0044] Wherein: ω represents the rotation speed of the joint angle, and T represents the length of the posture cycle.

[0045] Preferably, the posture planning includes the steps of freedom, standing, squatting, stepping, walking posture, pausing, translating and turning, and the stepping posture planning includes the following steps;

[0046] Establish the position formula, speed formula and angular velocity formula of the foot,

[0047] Among them: The foot position formula includes:

[0048]

[0049]

[0050] Where: p0 and p1 represent the starting point and end point of the foot, p x Indicates the position of the foot on the x-axis, p y Indicates the position of the foot on the y-axis, p z It represents the position of the foot on the z-axis, s represents the phase in the motion, and takes the value [0,1]; n represents the sampling period; T represents the stepping period; R represents the proportion of static time in the period; and h represents the height of the lifted foot.

[0051] An agricultural bipedal robot comprises a controller and a body and bipeds connected to the controller, wherein the controller is used to execute executable instructions, and the executable instructions contain program codes for implementing an agricultural bipedal robot posture control algorithm as described in any of the above items when executed.

[0052] In the technical solution of the present invention, by optimizing the motion model, spatial state discrete model, posture planning and other control algorithms, the control efficiency of the agricultural bipedal robot is improved, the robot perception confusion is avoided, the complexity of the agricultural production environment is adapted, the motion control capability is improved, and the needs of agricultural production are better met, so that the bipedal robot can be more widely used in agricultural production, which can reduce manual labor, improve production efficiency, and alleviate the problem of insufficient manpower; adapt to the diversity and complexity of different terrains and environments, accurately perform agricultural production tasks, and improve the environmental friendliness of production and the quality and safety of agricultural products. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the structures shown in these drawings without paying creative work.

[0054] Figure 1 The present invention is a flowchart of a posture control algorithm for an agricultural biped robot.

[0055] Figure 2 The figure is a schematic diagram of a model of an agricultural biped robot according to the present invention.

[0056] The realization of the purpose, functional features and advantages of the present invention will be further explained in conjunction with embodiments and with reference to the accompanying drawings. DETAILED DESCRIPTION

[0057] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0058] It should be noted that all directional indications in the embodiments of the present invention (such as up, down, left, right, front, back, etc.) are only used to explain the relative position relationship, movement status, etc. between the components under a certain specific posture (as shown in the accompanying drawings). If the specific posture changes, the directional indication will also change accordingly.

[0059] In addition, in the present invention, descriptions such as "first", "second", etc. are only used for descriptive purposes and cannot be understood as indicating or implying their relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" or "second" may explicitly or implicitly include at least one of the features. In the description of the present invention, the meaning of "plurality" is at least two, such as two, three, etc., unless otherwise clearly and specifically defined.

[0060] In the present invention, unless otherwise clearly specified and limited, the terms "connection", "fixation", etc. should be understood in a broad sense. For example, "fixation" can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection or an indirect connection through an intermediate medium, it can be the internal connection of two elements or the interaction relationship between two elements, unless otherwise clearly defined. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0061] In addition, the technical solutions between the various embodiments of the present invention can be combined with each other, but it must be based on the fact that ordinary technicians in the field can implement it. When the combination of technical solutions is contradictory or cannot be implemented, it should be deemed that such combination of technical solutions does not exist and is not within the scope of protection required by the present invention.

[0062] Please refer to Figure 1 The present invention proposes an agricultural biped robot posture control algorithm, comprising the following steps:

[0063] S100, initializing basic parameters of the model predictive control algorithm;

[0064] S200, establishing a velocity model, an angular velocity model, an angle change model and a position change model; establishing a spatial state discrete model through the velocity model, the angular velocity model, the angle change model, the position change model, the forward coordinate system, the prediction period and the control input;

[0065] S300, completing the construction of a model predictive control algorithm according to the spatial state discrete model, wherein the model predictive control algorithm includes a spatial state equation, a cost function and constraints;

[0066] S400, performing posture planning through the model predictive control algorithm, the posture planning algorithm including degrees of freedom, standing, squatting, stepping, walking posture, pause, translation and turning;

[0067] S500, calculating the motion trajectory and state of the robot through the model prediction control algorithm and the posture planning algorithm, so as to achieve smooth and stable output position and posture changes and realize stable control of the robot's posture.

[0068] In the technical solution of the present invention, by optimizing the motion model, spatial state discrete model, posture planning and other control algorithms, the control efficiency of the agricultural bipedal robot is improved, the robot perception confusion is avoided, the complexity of the agricultural production environment is adapted, the motion control capability is improved, and the needs of agricultural production are better met, so that the bipedal robot can be more widely used in agricultural production, which can reduce manual labor, improve production efficiency, and alleviate the problem of insufficient manpower; adapt to the diversity and complexity of different terrains and environments, accurately perform agricultural production tasks, and improve the environmental friendliness of production and the quality and safety of agricultural products.

[0069] Specific:

[0070] 1. The continuous model solution process is complicated and the amount of calculation is large. Discretization processing will introduce errors and cause the control system to be unstable. Computers can only process discrete models. Although the continuous model can be constructed into a discrete model through exponential transformation, it is more convenient to directly use the discrete model to avoid matrix conversion and waste the computer's computing power. The present invention uses a discrete model, the solution process is relatively simple, the amount of calculation is small, and the control system instability problem caused by excessive control cycle is effectively avoided, and it can be easily applied to actual control systems.

[0071] 2. The world coordinate system is used as a real-time motion modeling control, which can easily confuse the robot's perception:

[0072] In existing robot modeling, most of them choose the world coordinate system as the reference when moving forward. Frequent turning can easily confuse the robot's perception and make it difficult to determine the east, south, west and north. The present invention selects a forward coordinate system that is the same as the robot's body planning direction. The current planning point is also the origin, which not only simplifies the modeling, but also eliminates the floating interference caused by the IMU sensor. This method can effectively simplify robot modeling, improve control efficiency, and avoid the perception confusion problem existing in traditional methods.

[0073] 3. Error state modeling is complex, the program is difficult to implement, and it is prone to errors.

[0074] In the world coordinate system, the current actual state is measured by the sensor as s0, and the current target state is calculated by the last model as s1, so the current error state is (s1-s0). After conversion to the forward coordinate system, the current target state is automatically reset to zero each time it is calculated, so the current error state is the current actual state (-s0), making modeling simpler and programming more convenient.

[0075] 4. The program implements redundant code structure packaging, low matrix operation efficiency, and deep binding to third-party dependent libraries such as ROS.

[0076] The present invention preprocesses the motion control model, improves the filter and posture planning algorithms, abandons the XML, YAML, JSON and other data file formats, and selects a lightweight packaged third-party library, which improves the calculation efficiency, eliminates repeated jumps at the code level, and is easy to maintain and expand.

[0077] In another embodiment of the present invention, the step S100 includes the following steps:

[0078] S110, initializing a coordinate system, wherein the coordinate system includes a world coordinate system, a body coordinate system, and a forward coordinate system;

[0079] S120, initializing the sampling period and the prediction period;

[0080] S130, initializing control input, wherein the control input includes force and torque of the left and right foot ends;

[0081] S140, target state initialization; the target state includes the current state measured by the sensor.

[0082] Specifically, in order to facilitate real-time response to the robot's current motion posture, three sets of coordinate systems are established to convert each other.

[0083] 1.1 Coordinate system

[0084] World coordinate system (W system for short): fixed at a point on the ground, used to define the absolute position of the robot, where the three-dimensional coordinates x, y, and z are all 0;

[0085] Body coordinate system (B system for short): fixed at the center of mass of the robot body, the direction in which the x-axis points to the front of the robot body is positive, and the angle of rotation around the x-axis is the roll angle (roll); the direction in which the y-axis points to the left of the robot body is positive, and the angle of rotation around the y-axis is the pitch angle (pitch); the direction in which the z-axis points to the top of the robot body is positive, and the angle of rotation around the z-axis is the yaw angle (yaw);

[0086] Forward coordinate system (F system for short): The coordinate system of the robot when it is moving forward. The coordinates of each calculation start from 0. The model control prediction is calculated based on the F system.

[0087] 1.2 Forecast period

[0088] The control process of the robot is not continuous. The present invention directly adopts a discrete model. The discrete time interval is the sampling period of the control signal (abbreviated as dt). Generally speaking, the smaller the dt, the faster the controller responds, but the amount of calculation will increase and the prediction accuracy is limited, so the dt value here is 1ms.

[0089] During each control process, the controller must solve multiple control input parameters (u for short) in real time to determine the future position of the robot. The time span of multiple u is called the prediction period (pt for short). If pt is too large, the prediction error will gradually accumulate, resulting in a decrease in the performance of the controller. Here, pt=10, that is, each prediction will get a u sequence of length 10.

[0090] 1.3 Control Input

[0091] That is, u mentioned above. According to practical experience, the force and torque of the left and right foot ends of the robot are both three-dimensional space vectors, and the total dimension is "2*2*3=12". u is a control input sequence {u} with a length of 10 in the prediction cycle. Because of external interference and model uncertainty, only the first value solved is applied to the system.

[0092] Torque: The three-dimensional torque of the left and right soles is M=I*a, where:

[0093] Where: a is the rotational acceleration; L1 is the length of the foot; L2 is the width of the foot; I is the moment of inertia. For the convenience of description, the left leg torque is referred to as m0 and the right leg torque is referred to as m1.

[0094] Force: three-way support force of left and right soles F = m*g;

[0095] Where g is the acceleration due to gravity and m is the mass of the robot.

[0096] 1.4 Target state

[0097] There are three states that are easy to cause confusion in the model predictive control process:

[0098] Current actual state (abbreviated as x): can be measured by the fuselage sensor, such as the velocity ν, angular velocity ω, angle θ, position p, etc. at the center of mass;

[0099] Current target state: the future target state calculated by the previous step;

[0100] Future target state or expected state (xd for short): For the future target position, it is (current target position + ν*Δt); the speed v can change its value through the remote control during movement.

[0101] The target state is the output parameter calculated by the model predictive control algorithm (MPC), including the future target state and the current target state. For the sake of simplicity, the MPC calculations are based on the F system, so the current target state is 0. The following four types of xd are commonly used:

[0102] ω: The future three-dimensional angular velocity of the body, in radians per second.

[0103] ν: Future body three - dimensional movement speed.

[0104] Δθ: Future offset angle, whose value is ω * Δt.

[0105] Δp: Future position change amount, whose value is ν * Δt.

[0106] Corresponding to u, 10 groups of xd will also be output in each prediction period, called the target state sequence {xd};

[0107]

[0108] 1.5 Current state

[0109] The current state is the state shown in the actual movement process of the robot, that is, x described above, which can be obtained by actual measurement with sensors, and are all three - dimensional space vectors, including:

[0110] ω: Body three - dimensional angular velocity measured in real - time by the gyroscope, with the unit of radian / second.

[0111] ν: Body three - dimensional movement speed measured in real - time each time by the speed sensor installed on the robot.

[0112] Δθ: Current offset angle, whose value is ω * Δt.

[0113] Δp: Current position change amount, whose value is ν * Δt.

[0114] Corresponding to u, 10 groups of current state values will be output in each prediction period, called the state recurrence sequence {x}.

[0115]

[0116] In another embodiment of the present invention, the expression of the spatial state discrete model includes:

[0117] x i+1 = Ax i + Bu i + c(one);

[0118]

[0119] In the formula: m0 represents the left - leg torque; m1 represents the right - leg torque; m represents the mass of the robot; f0 represents the left - leg force; f1 represents the right - leg force; g represents the acceleration due to gravity; ω i represents the body three - dimensional angular velocity of the robot at time point i; v i represents the body three - dimensional movement speed of the robot at time point i; x i represents the state of the robot at time point i; Δθ irepresents the angle change of the robot from time i-1 to time i, Δθ i =ω*Δt;Δp i represents the change in the robot's position from time point i-1 to time point i; I represents the moment of inertia; u i represents the input variable at time point i; l0 represents the length of the left leg, l1 represents the length of the right leg, E is used to represent the unit matrix, A is used to represent the matrix above the letter A in the second formula, and B i Used to represent the second formula B i The upper matrix, c is used to represent the second formula c upper matrix.

[0120] Specifically, 2.1, speed model:

[0121] x-direction velocity ν x : The current forward speed of the robot in the x direction.

[0122] Y-speed ν y : The translation speed of the robot in the y direction.

[0123] Speed ​​in z direction ν y : The vertical speed is 0 when the biped robot is walking:

[0124]

[0125] F = f0 + f1 - m × g;

[0126]

[0127] Where: m is the mass of the robot; a is the acceleration; f0 is the left leg force; f1 is the right leg force.

[0128] 2.2 Angular velocity model:

[0129] Angular velocity in x direction ω x : The roll angular velocity of the robot around the x-axis. To simplify the calculation, it is set to 0.

[0130] Y-axis angular velocity ωy: The pitch angular velocity of the robot rotating around the y-axis. To simplify the calculation, it is set to 0.

[0131] Z-axis angular velocity ωz: The yaw angular velocity of the robot rotating around the z-axis. The initial value is measured by the sensor in real time.

[0132]

[0133] m=m0+m1+f0×L0+f1×L 1；

[0134]

[0135] Where: m is the torque of the robot; f0 represents the left leg force; f1 represents the right leg force; m0 represents the left leg torque; m1 represents the right leg torque; I represents the moment of inertia.

[0136] 2.3 Angle change model

[0137] X-axis angle change Δθx: The angle of rotation of the robot around the x-axis, i.e. roll.

[0138] Y-axis angle change Δθy: The angle of rotation of the robot around the y-axis, that is, pitch.

[0139] Angle change in z direction Δθz: The angle of rotation of the robot around the z axis, i.e. yaw:

[0140]

[0141] Where: m0 represents the left leg torque; m1 represents the right leg torque; I represents the moment of inertia.

[0142] 2.4 Position change model

[0143] Change in x-position Δpx:

[0144] Change in y-direction position Δpy:

[0145] Change in z-direction position Δpz:

[0146]

[0147] In the formula: m represents torque; f0 represents left leg force; f1 represents right leg force; s represents movement distance, and ν represents movement speed.

[0148] 2.5 Spatial state equation

[0149] In the F system, the above four models of velocity, angular velocity, angle change, and position change are integrated, and the prediction period pt and the control input sequence {u} are considered to establish a unified spatial state discrete model.

[0150] In another embodiment of the present invention, the space state equation includes:

[0151] x= A + B ·u (three);

[0152]

[0153] In the formula, x represents the state recursive sequence, The symbol above x is a stacked matrix expression; A represents the stacked matrix above in the fourth formula, Brepresents the stacked matrix above the fourth formula; x n represents the state variable at time n, u n-1 represents the input variable at time n-1, n represents the forecast period, A 0 = Ax0 + g; A i =A. A i-1 +g.

[0154] Specifically, expand the above state recursive sequence {x}:

[0155] x1=Ax0+c+B0u0;

[0156] x2=Ax1+B1u1=A(Ax0+g)+c+AB0u0+B1u1;

[0157] x3=A(A(Ax0+c)+c)+c+A 2 B0u0+AB1u1+B2u2;

[0158]

[0159] Pick:

[0160] A 0 = Ax0 + c;

[0161] A i =A. A i-1 +c;

[0162] but:

[0163] x= A + B u;

[0164]

[0165] Where x is the state recursive sequence, and below it is the stacking matrix expression. n is the state variable at time n, u n-1 is the input variable at time n-1, and n is the forecast period.

[0166] In another embodiment of the present invention, the constraints include: equality constraints and inequality constraints. The equality constraints include: when only one leg of the robot is in contact with the ground, the contact leg is called the standing leg, and the state is in the support phase at this time; the other leg is called the swing leg, and the state is in the swing phase at this time; the inequality constraints include that the sole of the robot has upward supporting force, horizontal friction force and rotational friction force, and the equivalent effect point of the torque does not exceed the sole polygon.

[0167] In another embodiment of the present invention,

[0168] The equality constraints include:

[0169]

[0170] The inequality constraints include:

[0171] L i ·u i +M i ≥0(vi);

[0172]

[0173] In the sixth formula, L i Indicates that L in the seventh formula i The matrix above; the sixth formula M i In the seventh formula, M i The matrix above; in the sixth formula, u i Indicates that u in the seventh formula i The matrix above; m x represents the moment in the x direction; m y represents the moment in the y direction; m z represents the moment in the z direction; f z represents the force in the z direction; f x represents the force in the x direction; f y Indicates the force in the y direction; max(f z ) represents the maximum force in the z direction; l y represents the force arm in the y direction; l x represents the force arm in the x direction; u z represents the friction coefficient in the z direction; u represents the friction coefficient.

[0174] In another embodiment of the present invention, in step S300:

[0175] The algebraic functions include:

[0176] minJ=u T Hu+2u T h+C;

[0177] H= B T QB +R;

[0178]

[0179] Among them, C is a constant; Q is a diagonal matrix, and each element in Q is the error weight coefficient; R is also a diagonal matrix, and each element in R is the input weight coefficient; It is the gravity feed-forward given to the corresponding leg according to the future support phase;

[0180]

[0181] Specifically, we optimize {u} by first converting it into a cost function and then finding the minimum value (minJ) of the cost function within the prediction period. To simplify the calculation, minJ takes the sum of the error weight and the input weight, and discards the terminal error.

[0182]

[0183] Substituting into the above formula:

[0184] x= A + B u;

[0185]

[0186] The result is a new cost function where:

[0187] H= B T QB +R;

[0188]

[0189] Among them, C is a constant; Q is a diagonal matrix, and each element in the matrix is ​​the error weight coefficient; R is also a diagonal matrix, and each element in the matrix is ​​the input weight coefficient; It is the gravity feedforward given to the corresponding leg according to the future support phase. In the bipedal MPC analysis, the cost term of u will cause the predicted trajectory to drop with gravity. The introduction of can offset this trend; x d represents the future target state or expected state; x represents the state actually detected by the sensor.

[0190] In another embodiment of the present invention, the S400 includes the following steps:

[0191] S410, joint angles of each degree of freedom in the initial posture and squatting posture;

[0192] S420, optimizing the joint angles during the process by periodic variation of the cosine function;

[0193]

[0194] Wherein: j represents the real-time posture, j1 represents the state of each degree of freedom joint in the squatting posture, j0 represents the state of each degree of freedom joint in the standing posture, t represents time, t∈[0,1);

[0195] S430, optimizing the speed of the joint angle during the process by using a sine function;

[0196]

[0197] Wherein: ω represents the rotation speed of the joint angle, and T represents the length of the posture cycle.

[0198] In another embodiment of the present invention, a foot position formula, a speed formula and an angular velocity formula are established.

[0199] Among them: The foot position formula includes:

[0200]

[0201] Where: p0 and p1 represent the starting point and the end point respectively; p x Indicates the position of the foot on the x-axis; p y Indicates the position of the foot on the y-axis; p z Indicates the position of the foot on the z-axis; p 1z Indicates the position of the end point on the z-axis; p 1x Indicates the position of the end point on the x-axis; p 1y Indicates the position of the end point on the y-axis; p 0z Indicates the position of the starting point on the z-axis; p 0x Indicates the position of the starting point on the x-axis; p 0y It represents the position of the starting point on the y-axis; s represents the phase in the motion, with a value of [0,1]; n represents the sampling period; T represents the stepping period; R represents the proportion of static time in the period; and h represents the height of the lifted foot.

[0202] Specifically, posture planning includes:

[0203] 4.1 Degrees of freedom:

[0204] To simplify the model, the bipedal robot only considers bipedal movement in the early stage, and the whole body has 12 degrees of freedom, which are:

[0205] The left and right thighs respectively rotate around the x, y, and z axis joints: 6 in total;

[0206] The left and right calves have two joints around the y axis.

[0207] The left and right ankles have joints around the y and z axes respectively: 4 in total;

[0208] 4.2. Standing:

[0209] When designing the robot, the standing posture is its initial posture, so you only need to set the joint angle of each degree of freedom to 0. It should be noted that the unit here is radian, and 1 radian is approximately 57.3 degrees.

[0210] J0={0,0,0,0,0,0,0,0,0,0,0,0};

[0211] 4.3. Squat:

[0212] In a standing position, to allow your legs to squat naturally, then:

[0213] The left and right thighs rotate upward around the y-axis by 0.5 radians, that is, 0.5*180 / π=28.6 degrees, with negative values ​​taken upwards;

[0214] The left and right calves are rotated downward around the y-axis by 1 arc, that is, the calves rotate in the opposite direction by 2 times the angle of the thighs to keep the ankle joint force point unchanged;

[0215] The left and right ankles are rotated upward by 0.5 arcs around the y-axis to keep the soles of the left and right feet flush with the ground;

[0216] The final angle position assignments of the 12 joints are: j1 = {0, 0, -0.5, 1, -0.5, 0, 0, 0, -0.5, 1, -0.5, 0};

[0217] In order to make the gait smooth during squatting, the joint motor must smoothly change from the initial joint angle j0 to the final joint angle j1, which can be optimized by the periodic change of the cosine function:

[0218]

[0219] When t = 0, cos(πt) = 1, j = j0, indicating that the robot's joint angle is in the initial state; when t = 0.5, cos(πt) = 0, j = (j1 + j0) / 2, indicating that the robot's joint angle is between the initial value and the final value; when t = 1, cos(πt) = -1, j = j1, indicating that the robot's joint angle has reached the final state. Therefore, by adjusting the change of t, the change process of the robot's joint angle from the initial value to the final value can be smoothly controlled.

[0220] Similarly, the velocity of the joint is processed by the sine function. The formula is as follows, where T is the length of the posture cycle:

[0221]

[0222] 4.4, Stepping:

[0223] A complete cycle of marching in place includes two stages: single-leg support and single-leg swing. The position formula of single-leg swing is a one-dimensional cubic curve:

[0224]

[0225] Among them: p0 and p1 represent the starting point and end point of the foot, px Indicates the position of the foot on the x-axis, p y Indicates the position of the foot on the y-axis, p z Indicates the position of the foot on the z-axis, p 0x Indicates the starting position of the foot on the x-axis, p 1x Indicates the end position of the foot on the x-axis, p 0y Indicates the starting position of the foot on the y-axis, p 1y Indicates the end position of the foot on the y-axis, p 0z Indicates the starting position of the foot on the z-axis, p 1z It represents the end position of the foot on the z-axis, s represents the phase in the motion, and takes the value [0,1]; n represents the sampling period; T represents the stepping period; R represents the proportion of static time in the period; and h represents the height of the lifted foot.

[0226] p0 and p1 are the starting and ending points of the foot;

[0227] s is the phase in motion, with a value of [0,1];

[0228] When s=0 (starting position), the position of the foot on the x-axis is Px=P0x, the position of the foot on the y-axis is Py=P0y, and the position of the foot on the z-axis is Pz=P0z;

[0229] When s=0.5, Px=0.5(P1x+P0x), Py=0.5(P1y+P1y), Pz=0.5h+0.5(p1z+p0z);

[0230] When s=1 (end point), the position of the foot on the x-axis is Px=P1x, the position of the foot on the y-axis is Py=P1y, and the position of the foot on the z-axis is Pz=P1z;

[0231] n represents the sampling period, which is 0.001 seconds;

[0232] T represents the stepping cycle, which is 0.4s;

[0233] R represents the proportion of static state to the period, generally half for swing and half for support, which is 0.5;

[0234] h represents the height of the raised foot. If it is too high, it is easy to lose balance. Generally, it is 1 / 4 of the calf length.

[0235] Speed ​​formula:

[0236]

[0237] in:

[0238] When s=0 (starting position), the speed of the foot on the X-axis Vx=0; the speed of the foot on the Y-axis Vy=0; the speed of the foot on the Z-axis Vz=0;

[0239] When s = 0.5, Px = 1.5 (P1x - P0x), Py = 1.5 (P1y - P1y), Pz = 1.5 (p1z - p0z);

[0240] When s=1, Vx=0, Vy=0, Vz=0;

[0241] Angular velocity formula:

[0242] ω=6×(ss 2 )×(3-2s)×s 2 ×r0×R T ;

[0243] in:

[0244] R represents the direction vector of the current foot end position in the global coordinate system;

[0245] r0 represents the initial angle of the ankle joint;

[0246] When the initial s=0, ω=0;

[0247] When s = 0.5, ω = 0.75r0;

[0248] When s=1, ω=0;

[0249] 4.5 Walking:

[0250] The motion control of a single leg when the robot is walking calculates the speed of the foot end according to the current operation stage. Then, it is converted to the target reference system through the rotation matrix, and the influence of the target linear speed and the angular velocity of the foot end in the target reference system are subtracted to convert the speed control input into the speed relative to the forward direction of the target reference system. Finally, the angle, angular velocity, and torque of other joints are calculated through the tooth-comparable matrix. These joint values ​​are assigned to the motor to achieve steady walking.

[0251] First define the step size and step width:

[0252] Step width: determined by the robot's hip width, 0.1 on each side of the center of mass, so the width is 0.2 and remains unchanged;

[0253] Step length: cmdVx*T*(1-R), where: T represents the stepping cycle; R represents the direction vector of the current foot end position in the global coordinate system; changes with speed. cmdVx is the speed in the forward direction.

[0254] The controller issues a walking command:

[0255] When the W key is pressed, move forward and increase cmdVx by a small value.

[0256] When the S key is pressed, move backwards, decreasing cmdVx by a small value.

[0257] Accepts controller button input and converts it into walking speed:

[0258] double kkk=0.005;

[0259] cmdVx=0.5*kkk+cmdVx*(1-kkk);

[0260] 4.6. Suspension:

[0261] The controller issues a pause command to stop the robot's movement and sets cmdVx and cmdVy to 0, where cmdVy is the translation speed in the y-axis direction.

[0262] 4.7. Translation:

[0263] The controller issues a translation command:

[0264] When the A key is pressed, pan to the left and increase cmdVy by a small value.

[0265] While the D key is pressed, pan to the right and decrease cmdVy by a small value.

[0266] 4.8. Steering:

[0267] The controller issues a steering command, where cmdWz is the speed at which the robot's center of mass rotates around the z-axis.

[0268] When the J key is pressed, rotate left, increasing cmdWz by a small value.

[0269] When the L key is pressed, rotate right, decreasing cmdWz by a small value.

[0270] An agricultural bipedal robot comprises a controller and a body and bipeds connected to the controller, wherein the controller is used to execute executable instructions, and the executable instructions contain program codes for implementing an agricultural bipedal robot posture control algorithm as described in any of the above items when executed.

[0271] In one embodiment, the main logic of the program is implemented, and the computing framework is:

[0272] The code structure strives to be concise. The cost function optimization problem is solved by using the third-party open source library eiquadprog and other libraries to connect to eigen3. It is LGPL-licensed and easy to use. The rbdl library is used to solve kinematics and dynamics. The robot's motion balance uses the mpc framework, and the coordinate system uses three types: world coordinate system, body coordinate system, and forward coordinate system. The simulation program can use mujoco, webots, coppelia, etc. Webots is recommended, which is based on C++ to directly simulate real-time control.

[0273] In one embodiment, the single-leg motion control algorithm implementation includes:

[0274]

[0275]

[0276] In another embodiment of the present invention, the model simulation, the bipedal robot is designed to be life-size, the torso is a cuboid of 200mm*400mm*500mm, and the center of mass is located at the center of the cuboid. The thigh is 0.5m long, the calf is 0.4m long, and the foot is 0.1m high. The thigh root is designed with 3 degrees of freedom, which can rotate around the x, y, and z axes respectively. The connection between the calf and the thigh has 1 degree of freedom, which can rotate around the y axis. The connection between the calf and the foot has 2 degrees of freedom, which can rotate around the y and z axes.

[0277] In one implementation, the following is the core function of the MPC process, which calculates the control input parameter u based on the current state and the desired trajectory. The process is:

[0278] S610, assign gravity to the ci matrices of the left and right legs, for a total of 10 rounds of data;

[0279] S620, assign values ​​to A and xd, a total of 10 rounds of data

[0280] S630, predict the future state and destination according to the cycle (pcT) and the initial state (stT), and then calculate the B0 matrix and the B matrix according to the state and the destination

[0281] S640, and calculate the control target h and H matrix.

[0282] S650. Use the control target h and H matrix as the objective function and constraints of the quadratic programming problem, and use the qp.solve_quadprog function to solve the optimal control input.

[0283] S660, save the control input into the mf array, and calculate the next state x1 based on the control input.

[0284]

[0285] if (stStage[i2 + 1]) { ci[i * 22 + 21] = maxFz;

[0286] } else { ci[i * 22 + 21] = 0;};

[0287] } g0[5] = -10 * pcT * pcT * 0.5;

[0288] g0

[11] = -10 * pcT;

[0289] A.head<12>() << stt.aDlt + stt.w * pcT, stt.pDlt + stt.v * pcT, stt.w, stt.v;

[0290] A.head<12>() += g0;

[0291] xd.head<12>() << stt.w * pcT, stt.v * pcT, stt.w, stt.v;

[0292] for (int i = 1; i < 10; i++) {;

[0293] int i12{i * 12};

[0294] A.segment<12>(i12) = A.segment<12>(i12 - 12);

[0295] A.segment<6>(i12) += A.segment<6>(i12 + 6) * pcT;

[0296] A.segment<12>(i12) += g0;

[0297] xd.segment<12>(i12) = xd.segment<12>(i12 - 12);

[0298] xd.segment<6>(i12) += xd.segment<6>(i12 + 6) * pcT;

[0299] }

[0300] vec3d tip[2]{stt.leg0, stt.leg1}; / / Right leg [x, y, z], left leg [x, y, z];

[0301] B0.block<3, 3>(6, 0) = InertiaInv * pcT;

[0302] B0.block<3,3>(6,3)=InertiaInv*Ei::skew(tip[0])*pcT;

[0303] B0.block<3,3>(6,6)=InertiaInv*pcT;

[0304] B0.block<3,3>(6,9)=InertiaInv*Ei::skew(type[1])*pcT;

[0305] B0.topRows <3> ()=B0.middleRows <3> (6)*pcT*0.5;

[0306] double tmp=pcT / mass;

[0307] B0(9,3)=tmp;B0(10,4)=tmp;B0(11,5)=tmp;

[0308] B0(9,9)=tmp;B0(10,10)=tmp;B0(11,11)=tmp;

[0309] tmp=0.5*pcT*tmp;

[0310] B0(3,3)=tmp;B0(4,4)=tmp;B0(5,5)=tmp;

[0311] B0(3,9)=tmp;B0(4,10)=tmp;B0(5,11)=tmp;

[0312] B.topLeftCorner<12,12>()=B0; / / first round;

[0313] for(int i=1;i<10;i++){int i12{i*12};

[0314] mat2d Rz2d=Ei::rot2R2d(stt.wz*pcT*i); / / convert rotation to angle

[0315]

[0316]

[0317] Then use webots (open source, multi-platform robot simulation software) to provide a development environment for modeling, programming and simulation. Add a node to the webots canvas and specify the Transform local coordinate system; add boundingObject, physics, shape, etc. to the children node; select hingejoint in the solid node and define the node of the rotation joint parameters;

[0318] Specify the motor under device; add the gyroscope, IMU, position, and acceleration sensors at the center of mass of the torso; and finally complete the simulation model construction.

[0319] Please refer to Figure 2 The simulation model controller is written in C++. After it is compiled in Ubuntu 20 (an open source, Linux-based operating system), the controller is specified in the model to perform simulation tests.

[0320] Step 2: Model Predictive Control (MPC) Construction:

[0321] Through the first four steps of the method of this application, namely initializing the basic parameters of model predictive control, establishing a spatial state discrete model, and constructing model predictive control and posture planning.

[0322] Step 3: Software development and testing includes the following steps:

[0323] The basic idea of ​​program processing is:

[0324] 3.1. Initialize the robot simulation model;

[0325] 3.2. Start the Model Predictive Control (MPC) processing thread;

[0326] 3.3. Start processing multiple postures such as stepping in place, walking, translation, turning, etc., let the two legs swing alternately, set the foot end speed, and calculate the movement trajectory of the legs and ankle joints;

[0327] 3.4. The MPC thread calculates the terminal velocity and force based on the trajectory of the ankle joint;

[0328] 3.5. Based on the actual joint angles and target positions fed back by the robot, inverse kinematics is used to solve the target angles, target angular velocities, and target torques of each joint;

[0329] 3.6. The target parameters are passed to the controller of the simulation model. The controller uses any of the three closed loops of position, speed and torque to achieve smooth control of the robot's posture.

[0330] Step 4: Body manufacturing and integration testing:

[0331] According to the size and material of the simulation model, a bill of materials (BOM) is listed, and then a series of processes such as hardware assembly and testing, software installation and debugging, power-on hardware and software joint debugging, defect repair and improvement are carried out.

[0332] The bill of materials (BOM) is as follows:

[0333] 4.1. Motor: specify brushless servo robot joint motor reducer, drive, encoder;

[0334] 4.2. Control board: Select STM32 (STM32 stands for STMicroelectronics' (ST) 32-bit microcontroller based on the ARM Cortex-M series core) development board, AI visual computing development board, expansion board, IMU, etc.

[0335] 4.3, Standard parts: including carbon tubes, tube clamps, ball heads, tie rods, flange bearings, nuts, half teeth, wiring, power supplies, etc.;

[0336] 4.4, CNC processed parts: including torso, hips, legs, feet, various connectors, extensions, output parts, etc.;

[0337] 4.5. Carbon plate cutting: including front plate, back plate, leg inner plate, etc.;

[0338] 4.6. Soft rubber processing: including soft rubber on the soles of feet, etc.

[0339] The above are only preferred embodiments of the present invention, and are not intended to limit the patent scope of the present invention. All equivalent structural changes made using the contents of the present invention's specification and drawings, or directly / indirectly applied in other related technical fields, are included in the patent protection scope of the present invention.

Claims

1. An agricultural bipedal robot posture control algorithm, characterized in that: The steps include: Initialize the basic parameters of the model predictive control algorithm; Establishing a velocity model, an angular velocity model, an angle change model and a position change model; establishing a spatial state discrete model through the velocity model, the angular velocity model, the angle change model, the position change model, the forward coordinate system, the prediction period and the control input; Complete the construction of a model predictive control algorithm according to the spatial state discrete model, wherein the model predictive control algorithm includes a spatial state equation, a cost function and constraints; Performing posture planning through the model predictive control algorithm, the posture planning algorithm includes degrees of freedom, standing, squatting, stepping, walking posture, pausing, translation and turning; The motion trajectory and state of the robot are calculated by the model predictive control algorithm and the posture planning algorithm, so as to achieve smooth and stable output position and posture changes and realize stable control of the robot's posture.

2. The agricultural biped robot posture control algorithm according to claim 1, characterized in that: The step of initializing the basic parameters of the model predictive control algorithm comprises the following steps: Initializing a coordinate system, wherein the coordinate system includes a world coordinate system, a body coordinate system, and a forward coordinate system; Initialize sampling period and prediction period; Initializing control input, wherein the control input includes force and torque of the left and right foot ends; The target state is initialized; the target state includes the current state measured by the sensor.

3. The agricultural biped robot posture control algorithm according to claim 1, characterized in that: The spatial state discrete model expression includes: x i+1 = Ax i + Bu i + c(一); Where: m0 represents the left leg torque; m1 represents the right leg torque; m represents the mass of the robot; f0 represents the left leg force; f1 represents the right leg force; g represents the gravitational acceleration; ω i represents the three-dimensional angular velocity of the robot at time i; v i represents the three-dimensional motion speed of the robot at time point i; x i represents the state of the robot at time i; Δθ i represents the angle change of the robot from time i-1 to time i, Δθ i =ω*Δt;Δp i represents the change in the robot's position from time point i-1 to time point i; I represents the moment of inertia; u i represents the input variable at time point i; l0 represents the length of the left leg, l1 represents the length of the right leg, E is used to represent the unit matrix, A is used to represent the matrix above the letter A in the second formula, and B i Used to represent the second formula B i The upper matrix, c is used to represent the second formula c upper matrix.

4. The agricultural biped robot posture control algorithm as claimed in claim 2, characterized in that: The spatial state equation includes: x= A + B ·u(three); In the formula, x represents the state recursive sequence, The symbol above x is a stacked matrix expression; A represents the stacked matrix above in the fourth formula, B represents the stacked matrix above the fourth formula; x n represents the state variable at time n, u n-1 represents the input variable at time n-1, n represents the forecast period, A 0 = Ax0 + g; A i =A. A i-1 +g.

5. The agricultural biped robot posture control algorithm according to claim 1, characterized in that: Constraints include: Equality constraints and inequality constraints, the equality constraints include: when only one leg of the robot is in contact with the ground, the contact leg is called the standing leg, and the state is in the support phase at this time; the other leg is called the swing leg, and the state is in the swing phase at this time; the inequality constraints include that the robot's sole has an upward support force, horizontal friction force and rotational friction force, and the equivalent effect point of the torque does not exceed the sole polygon.

6. The agricultural biped robot posture control algorithm as claimed in claim 5, characterized in that: The equality constraints include: The inequality constraints include: L i ·u i +M i ≥0(vi); In the sixth formula, L i Indicates that L in the seventh formula i The matrix above; the sixth formula M i In the seventh formula, M i The matrix above; in the sixth formula, u i Indicates that u in the seventh formula i The matrix above; m x represents the moment in the x direction; m y represents the moment in the y direction; m z represents the moment in the z direction; f z represents the force in the z direction; f x represents the force in the x direction; f y Indicates the force in the y direction; max(f z ) represents the maximum force in the z direction; l y represents the force arm in the y direction; l x represents the force arm in the x direction; u z represents the friction coefficient in the z direction; u represents the friction coefficient.

7. The agricultural biped robot posture control algorithm according to claim 4, characterized in that: The model predictive control algorithm is constructed according to the spatial state discrete model, and the model predictive control algorithm includes the spatial state equation, the cost function and the constraint step: The algebraic functions include: minJ=u T Hu+2u T h+C; H= B T QB +R; Among them, C is a constant; Q is a diagonal matrix, and each element in Q is the error weight coefficient; R is also a diagonal matrix, and each element in R is the input weight coefficient; is the gravity feedforward given to the corresponding leg according to the future support phase; minJ represents the minimum value of the cost function; u T Indicates the input parameter matrix transpose; x d Indicates a desired state; 8. The agricultural biped robot posture control algorithm according to claim 1, characterized in that: The posture planning includes the squatting posture planning in the steps of freedom, standing, squatting, stepping, walking posture, pausing, translating and turning, and includes the following steps; The joint angles of each degree of freedom in the initial and squatting postures; The joint angles in the process are optimized through periodic changes of the cosine function; Wherein: j represents the real-time posture, j1 represents the state of each degree of freedom joint in the squatting posture, j0 represents the state of each degree of freedom joint in the standing posture, t represents time, t∈[0,1); The speed of the joint angles during the process is optimized through the sine function; Wherein: ω represents the rotation speed of the joint angle, and T represents the length of the posture cycle.

9. An agricultural biped robot posture control algorithm as described in any one of claims 1 to 8, characterized in that: The posture planning includes the following steps: the posture planning includes the steps of freedom, standing, squatting, stepping, walking posture, pausing, translating and turning, and the stepping posture planning; Establish the position formula, speed formula and angular velocity formula of the foot, Among them: The foot position formula includes: Where: p0 and p1 represent the starting point and end point of the foot; p x Indicates the position of the foot on the x-axis; p y Indicates the position of the foot on the y-axis; p z represents the position of the foot on the z-axis; s represents the phase in the motion, with a value of [0,1]; n represents the sampling period; T represents the stepping period; R represents the proportion of static in the period; h represents the height of the raised foot; p 1z Indicates the position of the end point on the z-axis; p 1x Indicates the position of the end point on the x-axis; p 1y Indicates the position of the end point on the y-axis; p 0z Indicates the position of the starting point on the z-axis; p 0x Indicates the position of the starting point on the x-axis; p 0y Indicates the position of the starting point on the y-axis.

10. An agricultural bipedal robot, characterized in that: It comprises a controller and a body and two feet connected to the controller, wherein the controller is used to execute executable instructions, and the executable instructions contain program codes for implementing an agricultural biped robot posture control algorithm as described in any one of claims 1-9 when executed.

Images