An agricultural bipedal robot and its pose control algorithm

Abstract

This invention discloses an agricultural bipedal robot and its pose control algorithm, comprising the following steps: initializing 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; completing the construction of the model predictive control algorithm, which includes a spatial state equation, a cost function, and constraints; and performing pose planning through the model predictive control algorithm, which includes degrees of freedom, standing, squatting, stepping, walking postures, pausing, translation, and turning. In the technical solution of this invention, by optimizing the motion model, the spatial state discrete model, and the pose planning control algorithm, the control efficiency of the agricultural bipedal robot is improved, robot perception confusion is avoided, it adapts to the complexity of the agricultural production environment, and its motion control capability is enhanced.

Description

Technical Field

[0001] This invention relates to the field of robot pose control technology, specifically to an agricultural bipedal robot and its pose control algorithm. Background Technology

[0002] The open-source robot operating system ROS has been widely used in robotic arms, underwater robots, humanoid robots, and hybrid robots, becoming a common standard in the robotics field. Simulation tools based on Webots, Matlab, Gazebo, and RVIZ allow for rapid debugging, evaluation, iteration, and optimization of robot algorithms and controllers. ChatGPT sparked a surge in generative artificial intelligence, marking the first time computers truly understood human language, revolutionizing robots' ability to summarize and reason inductively. The development of robotics technology will undoubtedly empower various industries. Currently, the research and application of agricultural bipedal robots has become a hot topic. In this context, posture control is one of the key 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 novel posture control algorithm suitable for agricultural bipedal robots is of great significance and value for improving their stability and working efficiency. Summary of the Invention

[0003] The main objective of this invention is to provide a pose control algorithm for agricultural bipedal robots, aiming to solve the problem that existing posture control algorithms for bipedal robots perform poorly when dealing with the complexity of agricultural environments.

[0004] To achieve the above objectives, this invention proposes a pose control algorithm for an agricultural bipedal robot, comprising the following steps:

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

[0006] Establish a velocity model, an angular velocity model, an angle change model, and a position change model; and establish a spatial state discrete model using 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] Based on the spatial state discrete model, a model predictive control algorithm is constructed, which includes spatial state equations, cost functions, and constraints.

[0008] The model predictive control algorithm is used for attitude planning, which 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 posture planning algorithm, so as to achieve smooth and stable output of position and posture 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] Initializing the coordinate system, which includes the world coordinate system, the body coordinate system, and the forward coordinate system;

[0012] Initializing the sampling period and the prediction period;

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

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

[0015] Preferably, the expression of the spatial state discrete model 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's three-way angular velocity at the i-th time point of the robot; v i represents the body's three-way motion velocity at the i-th time point of the robot; x i represents the state of the robot at the i-th time point; Δθ i represents the angular change from the (i - 1)-th time point to the i-th time point of the robot, Δθ i = ω * Δt; Δp i represents the position change amount from the (i - 1)-th time point to the i-th time point of the robot; I represents the moment of inertia; u i represents the input variable at the i-th time point; 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, and 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 recursive sequence of states. The symbol above 'x' in the middle represents a stacked matrix expression; A This represents the stacked matrix at the top in the fourth formula. B This represents the stacked matrix above the fourth formula; x n Let u represent the state variable at time n. n-1 This represents the input variable at time n-1, where n represents the prediction 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 contacting leg is called the standing leg, and the state at this time is the support phase; the other leg is called the swinging leg, and the state at this time is the swinging phase. The inequality constraints include: the robot's foot has an upward supporting force, a horizontal friction force, and a rotational friction force, and the point of equivalent force application of the torque does not exceed the polygon of the foot.

[0024] Preferably, the equality constraint includes:

[0025]

[0026] The inequality constraints include:

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

[0028]

[0029] In the formula: L in the sixth formula i This indicates that L in the seventh formula i The matrix above; the sixth formula M i The middle represents M in the seventh formula. i The matrix above; u in the sixth formula i In the seventh formula, u i The matrix above; m x Indicates the torque in the x-direction; m y Indicates the torque in the y-direction; m z f represents the torque in the z-direction; z f represents the force in the z-direction; x f represents the force in the x-direction. yThis represents the force in the y-direction; max(f) z ) represents the maximum force in the z-direction; l y Indicates the lever arm in the y-direction; l x Indicates the lever arm in the x-direction; u z represents the friction coefficient in the z-direction; u represents the friction coefficient.

[0030] Preferably, the step of constructing a model predictive control algorithm based on the spatial state discrete model includes the following steps: 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] Where C is a constant; Q is a diagonal matrix, and each element in Q is an error weight coefficient; R is also a diagonal matrix, and each element in R is an input weight coefficient; It is the gravity feedforward of the corresponding leg according to the future support phase; minJ represents the minimum value of the cost function; u T Indicates the transpose of the input parameter matrix; x d This indicates the desired state.

[0036]

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

[0038] Joint angles for each degree of freedom in the initial and squatting postures;

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

[0040]

[0041] In the formula: j represents the real-time posture, j1 represents the state of each degree of freedom joint when squatting, j0 represents the state of each degree of freedom joint when standing, and t represents time, t∈[0,1).

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

[0043]

[0044] In the formula: ω represents the rotational speed of the joint angle, and T represents the length of the attitude cycle.

[0045] Preferably, the posture planning includes steps such as standing, squatting, stepping, walking, pausing, translating, and turning. The stepping posture planning includes the following steps:

[0046] Establish the formulas for foot position, velocity, and angular velocity.

[0047] Among them, the formula for foot position includes:

[0048]

[0049]

[0050] In the formula: p0 and p1 represent the starting and ending points of the foot, p x p represents the position of the foot on the x-axis. y p represents the position of the foot on the y-axis. z The z-axis represents the position of the foot, s represents the phase of the movement, with a value of [0,1], n represents the sampling period, T represents the stepping period, R represents the proportion of the period when the foot is stationary, and h represents the height of the foot lift.

[0051] An agricultural bipedal robot includes a controller and a body and two legs connected to the controller. The controller is used to execute executable instructions, which contain program code for implementing the pose control algorithm of the agricultural bipedal robot as described above when executed.

[0052] In the technical solution of this invention, by optimizing control algorithms such as motion model, spatial state discrete model, and posture planning, the control efficiency of agricultural bipedal robots is improved, robot perception confusion is avoided, the complexity of agricultural production environment is adapted, motion control capability is improved, and agricultural production needs are better met. This makes bipedal robots more widely used in agricultural production, reduces manual labor, improves production efficiency, and alleviates the problem of labor shortage. It also adapts to the diversity and complexity of different terrains and environments, accurately executes agricultural production tasks, and improves the environmental friendliness of production and the quality and safety of agricultural products. Attached Figure Description

[0053] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the structures shown in these drawings without creative effort.

[0054] Figure 1 This is a flowchart illustrating the pose control algorithm for an agricultural bipedal robot according to the present invention.

[0055] Figure 2 This is a schematic diagram of a bipedal agricultural robot according to the present invention.

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

[0057] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present invention.

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

[0059] Furthermore, in this invention, descriptions involving "first," "second," etc., are for descriptive purposes only and should not be construed as indicating or implying their relative importance or implicitly specifying the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of that feature. In the description of this invention, "a plurality of" means at least two, such as two, three, etc., unless otherwise explicitly specified.

[0060] In this invention, unless otherwise explicitly specified and limited, the terms "connection," "fixed," etc., should be interpreted broadly. For example, "fixed" can mean a fixed connection, a detachable connection, or an integral part; it can mean a mechanical connection or an electrical connection; it can mean a direct connection or an indirect connection through an intermediate medium; it can mean the internal communication of two components or the interaction between two components, unless otherwise explicitly limited. Those skilled in the art can understand the specific meaning of the above terms in this invention according to the specific circumstances.

[0061] Furthermore, the technical solutions of the various embodiments of the present invention can be combined with each other, but only if they are feasible for those skilled in the art. If the combination of technical solutions is contradictory or cannot be implemented, it should be considered that such combination of technical solutions does not exist and is not within the scope of protection claimed by the present invention.

[0062] Please refer to Figure 1 This invention proposes a pose control algorithm for an agricultural bipedal robot, comprising the following steps:

[0063] S100. Initialize the basic parameters of the model predictive control algorithm;

[0064] S200. Establish a velocity model, an angular velocity model, an angle change model, and a position change model; establish a spatial state discrete model using 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. Construct a model predictive control algorithm based on the spatial state discrete model. The model predictive control algorithm includes spatial state equations, cost functions, and constraints.

[0066] S400. Perform posture planning using the model predictive control algorithm. The posture planning algorithm includes degrees of freedom, standing, squatting, stepping, walking posture, pausing, translation, and turning.

[0067] S500: The robot's motion trajectory and state are calculated through the model predictive control algorithm and the posture planning algorithm, thereby achieving smooth and stable output position and posture changes, and realizing stable control of the robot's posture.

[0068] In the technical solution of this invention, by optimizing control algorithms such as motion model, spatial state discrete model, and posture planning, the control efficiency of agricultural bipedal robots is improved, robot perception confusion is avoided, the complexity of agricultural production environment is adapted, motion control capability is improved, and agricultural production needs are better met. This makes bipedal robots more widely used in agricultural production, reduces manual labor, improves production efficiency, and alleviates the problem of labor shortage. It also adapts to the diversity and complexity of different terrains and environments, accurately executes agricultural production tasks, and improves the environmental friendliness of production and the quality and safety of agricultural products.

[0069] Specifically:

[0070] 1. Solving continuous models is complex and computationally intensive. Discretization introduces errors that can lead to control system instability. Computers can only handle discrete models. While continuous models can be converted into discrete models through exponential transformations, directly using a discrete model is more convenient and avoids wasting computing power on matrix transformations. This invention uses a discrete model, which has a relatively simple solution process and lower computational load. It effectively avoids instability problems caused by excessively long control periods and can be easily applied to practical control systems.

[0071] 2. Using the world coordinate system for real-time motion modeling and control can easily lead to confusion in the robot's perception:

[0072] Existing robot modeling methods often use a world coordinate system as the reference when moving forward. Frequent turns easily lead to perceptual confusion for the robot, making it difficult to determine cardinal directions. This invention chooses a forward coordinate system that aligns with the robot's planned orientation, with the current planned point also serving as the origin. This not only simplifies modeling but also eliminates drift interference from IMU sensors. This method effectively simplifies robot modeling, improves control efficiency, and avoids the perceptual confusion problems inherent in traditional methods.

[0073] 3. Error state modeling is complex, making program implementation difficult and prone to errors.

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

[0075] 4. The program's code structure is redundant, matrix operations are inefficient, and it is deeply dependent on third-party libraries such as ROS.

[0076] This invention preprocesses the motion control model, improves related algorithms such as filters and attitude planning, and abandons data file formats such as XML, YAML, and JSON, selecting lightweight, encapsulated third-party libraries. This improves computational efficiency, eliminates repeated jumps in code hierarchy, and facilitates maintenance and expansion.

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

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

[0079] S120, Initialization of sampling period and prediction period;

[0080] S130. Control input initialization, wherein the control input includes the force and torque at the left and right feet;

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

[0082] Specifically, in order to facilitate real-time reflection of the robot's current motion posture, three coordinate systems are established and can be converted between 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 robot's absolute position, where the three coordinates x, y, and z are all 0;

[0085] Body coordinate system (B-frame): Fixed at the robot's center of mass, the x-axis pointing in front of the robot is positive, and the angle of rotation around the x-axis is the roll angle; the y-axis pointing to the left of the robot is positive, and the angle of rotation around the y-axis is the pitch angle; the z-axis pointing directly above the robot is positive, and the angle of rotation around the z-axis is the yaw angle.

[0086] Forward coordinate system (F-frame): The coordinate system for the robot's forward movement. The coordinates are calculated from 0 for each calculation, and the model control prediction is based on the F-frame.

[0087] 1.2 Prediction Period

[0088] The robot's control process is not continuous. This invention directly adopts a discrete model, and the discrete time interval is the sampling period of the control signal (dt). Generally speaking, the smaller dt is, the faster the controller responds, but the computational load will increase, and the prediction accuracy will be limited. Therefore, the value of dt is taken as 1ms in this invention.

[0089] During each control process, the controller must solve for multiple control input parameters (u) in real time to determine the robot's future pose. The time span of these multiple u values ​​is called the prediction period (pt). If pt is too large, the prediction error will gradually accumulate, leading to a decrease in controller performance. Here, pt = 10 is chosen, meaning that each prediction will yield a u sequence of length 10.

[0090] 1.3 Control Input

[0091] As mentioned earlier, u, based on practical experience, is taken as the force and torque at the left and right feet of the robot, both of which are three-dimensional spatial vectors, with a total dimension of "2*2*3=12". u is a control input sequence {u} of length 10 in the prediction cycle. Due to external disturbances and model uncertainties, only the first value obtained is applied to the system.

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

[0093] In the formula: a is the rotational acceleration; L1 represents the length of the foot; L2 represents the width of the foot; I represents the moment of inertia. For ease of description, the torque of the left leg is abbreviated as m0, and the torque of the right leg is abbreviated as m1.

[0094] Force: The three-way supporting force on the soles of the left and right feet, F = m * g;

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

[0096] 1.4 Target Status

[0097] In model predictive control, there are three states that are prone to confusion:

[0098] Current actual state (x): can be obtained by actual measurement by the fuselage sensors, such as the velocity ν, angular velocity ω, angle θ, and position p at the center of mass;

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

[0100] Future target state or desired state (xd): For the future target position, it is (current target position + ν*Δt); where the velocity v can be changed by remote control during the motion.

[0101] The target state is the output parameter calculated by the Model Predictive Control (MPC) algorithm, including the future target state and the current target state. For simplicity, the MPC calculations mentioned above are all based on the F-framework, so the current target state is 0. The following four types of xd are commonly used:

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

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

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

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

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

[0107]

[0108] 1.5 Current state

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

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

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

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

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

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

[0115]

[0116] In another embodiment of the present invention, the spatial state discrete model expression 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 three - dimensional body angular velocity of the robot at time point i; v i represents the three - dimensional body movement speed of the robot at time point i; x i represents the state of the robot at time point i; Δθ iΔθ represents the change in angle of the robot from time point i-1 to time point i. i =ω*Δt; Δp i The value represents the change in position of the robot from time point i-1 to time point i; I represents the moment of inertia; u i The input variables at time point i are: l0 represents the length of the left leg, l1 represents the length of the right leg, E represents the identity matrix, A represents the matrix above the letter A in the second formula, and B represents the input variables at time point i. i Used to represent the second formula B i The matrix above, c, is used to represent the matrix above the second formula c.

[0120] Specifically, 2.1, velocity model:

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

[0122] y-velocity ν y : The translational speed of the robot in the y-direction.

[0123] z-velocity ν y Vertical velocity is 0 for bipedal robots walking.

[0124]

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

[0126]

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

[0128] 2.2 Angular velocity model:

[0129] angular velocity ω in the x direction x : The roll angular velocity of the robot rotating around the x-axis, which is set to 0 for simplified calculation.

[0130] Angular velocity ωy in the y-axis: The pitch angular velocity of the robot rotating around the y-axis, which is set to 0 for the sake of simplifying the calculation.

[0131] z-axis angular velocity ωz: The yaw angular velocity of the robot rotating around the z-axis, the initial value of which is obtained by real-time measurement by the sensor.

[0132]

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

[0134]

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

[0136] 2.3 Angle Change Model

[0137] The change in x-axis angle Δθx: the angle by which the robot rotates around the x-axis, i.e., roll.

[0138] y-axis angle change Δθy: the angle of rotation of the robot around the y-axis, i.e., pitch.

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

[0140]

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

[0142] 2.4 Position Change Model

[0143] x-axis positional change Δpx:

[0144] The change in position in the y-direction Δpy:

[0145] z-axis positional change Δpz:

[0146]

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

[0148] 2.5 Space State Equation

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

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

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

[0152]

[0153] In the formula, x represents the recursive sequence of states. The symbol above 'x' in the middle represents a stacked matrix expression; A This represents the stacked matrix at the top in the fourth formula. BThis represents the stacked matrix above the fourth formula; x n Let u represent the state variable at time n. n-1 This represents the input variable at time n-1, where n represents the prediction period. A 0 = Ax0 + g; A i =A· A i-1 +g.

[0154] Specifically, expand the aforementioned 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] Here, x is the state recursive sequence, followed by the stacked matrix expression. n Let u be the state variable at time n. n-1 Let n be the input variable at time n-1, and n be the prediction 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 contacting leg is called the standing leg, and the state at this time is the support phase; the other leg is called the swinging leg, and the state at this time is the swinging phase. The inequality constraints include: the robot's foot has an upward supporting force, a horizontal friction force, and a rotational friction force, and the point of equivalent force application does not exceed the polygon of the foot.

[0167] In yet another embodiment of the invention,

[0168] The equality constraints include:

[0169]

[0170] The inequality constraints include:

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

[0172]

[0173] In the formula: L in the sixth formula i This indicates that L in the seventh formula i The matrix above; the sixth formula M i The middle represents M in the seventh formula. i The matrix above; u in the sixth formula i In the seventh formula, u i The matrix above; m x Indicates the torque in the x-direction; m y Indicates the torque in the y-direction; m z f represents the torque in the z-direction; z f represents the force in the z-direction; x f represents the force in the x-direction. y This represents the force in the y-direction; max(f) z ) represents the maximum force in the z-direction; l y Indicates the lever arm in the y-direction; l x Indicates the lever 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] Where C is a constant; Q is a diagonal matrix, and each element in Q is an error weight coefficient; R is also a diagonal matrix, and each element in R is an input weight coefficient; It is the gravity feedforward given to the corresponding leg according to the future support phase;

[0180]

[0181] Specifically, the next step is to 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 is taken as the sum of error-weighted and input-weighted values, discarding the terminal error.

[0182]

[0183] Substitute into the above formula:

[0184] x = A + B ·u;

[0185]

[0186] The result is the new cost function, where:

[0187] H = B T QB +R;

[0188]

[0189] Where C is a constant; Q is a diagonal matrix, and each element of the matrix is ​​an error weight coefficient; R is also a diagonal matrix, and each element of the matrix is ​​an input weight coefficient; This involves feeding forward the gravity to the corresponding leg based on the future support phase. In bipedal MPC analysis, the cost term of u causes the predicted trajectory to decrease with gravity. The introduction of x can counteract this trend; d x represents the future target state or desired state; x represents the actual state detected by the sensor.

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

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

[0192] S420. Optimize the joint angles during the process by using the periodic variation of the cosine function;

[0193]

[0194] In the formula: j represents the real-time posture, j1 represents the state of each degree of freedom joint when squatting, j0 represents the state of each degree of freedom joint when standing, and t represents time, t∈[0,1).

[0195] S430. Optimize the speed of joint angles during the process using a sine function;

[0196]

[0197] In the formula: ω represents the rotational speed of the joint angle, and T represents the length of the attitude cycle.

[0198] In another embodiment of the present invention, formulas for foot position, velocity, and angular velocity are established.

[0199] Among them, the formula for foot position includes:

[0200]

[0201] In the formula: p0 and p1 represent the starting point and the ending 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 endpoint on the z-axis; p 1x Indicates the position of the endpoint on the x-axis; p 1y Indicates the position of the endpoint on the y-axis; p 0z Indicates the starting point's position on the z-axis; p 0x Indicates the position of the starting point on the x-axis; p 0y The starting point is located on the y-axis; s represents the phase of the motion, with a value of [0,1]; n represents the sampling period; T represents the stepping period; R represents the proportion of the period spent at rest; and h represents the height of the foot lift.

[0202] Specifically, attitude planning includes:

[0203] 4.1 Degrees of freedom:

[0204] To simplify the model, the bipedal robot initially only considers bipedal movement, with a total of 12 degrees of freedom for the whole body:

[0205] There are 6 joints in the left and right thighs around the x, y, and z axes, respectively.

[0206] There are two joints around the y-axis in the left and right lower legs.

[0207] The left and right ankles have four joints around the y and z axes, respectively.

[0208] 4.2 Standing:

[0209] When designing a robot, the standing posture is its initial posture, so it is only necessary to set the joint angles of each degree of freedom to 0. Note that the unit here is radians, 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 Squatting:

[0212] To squat naturally while standing, the following steps should be taken:

[0213] Both the left and right thighs rotate upwards by 0.5 arc degrees around the y-axis, which is 0.5 * 180 / π = 28.6 degrees, taking the negative value upwards;

[0214] Both the left and right lower legs must rotate downwards by 1 arc around the y-axis, meaning the lower legs must rotate in the opposite direction by twice the angle of the thigh to keep the point of force on the ankle joint unchanged.

[0215] Rotate both ankles upwards by 0.5 arcs around the y-axis to keep the soles of both feet level with the ground;

[0216] The final angular positions of the 12 joints are assigned as follows: j1 = {0,0,-0.5,1,-0.5,0,0,0,-0.5,1,-0.5,0};

[0217] During the squatting process, to ensure a smooth gait, the joint motors must smoothly change from the initial joint angle j0 to the final joint angle j1. This can be optimized through the periodic variation of the cosine function.

[0218]

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

[0220] Similarly, the joint velocity is processed using a sine function, as shown in the following formula, where T is the length of the attitude period:

[0221]

[0222] 4.4 Stepping:

[0223] A complete cycle of stationary marching consists of two phases: single-leg support and single-leg swing. The position of the single-leg swing is calculated using a one-dimensional cubic curve.

[0224]

[0225] Where: p0 and p1 represent the starting and ending points of the foot, px p represents the position of the foot on the x-axis. y Indicates the position of the foot on the y-axis, p z p represents the position of the foot on the z-axis. 0x p represents the starting position of the foot on the x-axis. 1x p represents the endpoint position of the foot on the x-axis. 0y p represents the starting position of the foot on the y-axis. 1y p represents the endpoint position of the foot on the y-axis. 0z p represents the starting position of the foot on the z-axis. 1z The z-axis represents the final position of the foot on the z-axis, s represents the phase of the movement, with a value of [0,1], n represents the sampling period, T represents the stepping period, R represents the proportion of the period when the foot is stationary, and h represents the height of the foot lift.

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

[0227] s is the phase during motion, with values ​​[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 takes a value of 0.001 seconds;

[0232] T represents the stepping period, with a value of 0.4s;

[0233] R represents the proportion of the period that is at rest, which is generally half for oscillation and half for support, and is 0.5.

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

[0235] Velocity formula:

[0236]

[0237] in:

[0238] When s = 0 (starting position), the velocity of the foot on the X-axis is Vx = 0; the velocity of the foot on the Y-axis is Vy = 0; the velocity of the foot on the Z-axis is 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's 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 robot's single-leg motion control during walking involves calculating the foot's end-effector velocity based on the current stage of operation. This velocity is then transformed to the target reference frame using a rotation matrix, and the influence of the target linear velocity and the foot's end-effector angular velocity in the target reference frame are subtracted to convert the velocity control input into a velocity relative to the target reference frame's forward direction. Finally, using a toothed matrix, the angles, angular velocities, and torques of the other joints are calculated. These joint values ​​are then assigned to the motors, enabling steady walking.

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

[0252] Step width: determined by the width of the robot's hips, 0.1 on each side at the center of mass, so the width remains constant at 0.2.

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

[0254] The controller issues a walking command:

[0255] When the W key is pressed, the device moves forward and increases cmdVx by a small value.

[0256] When the S key is pressed, the user moves backward, which reduces 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 Pause:

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

[0262] 4.7 Translation:

[0263] The controller issues a translation command:

[0264] When the A key is pressed, the cursor shifts to the left, increasing cmdVy by a small value.

[0265] When the D key is pressed, the slider moves to the right, decreasing 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 you press the J key, rotate to the left and increase cmdWz by a small value.

[0269] When the L key is pressed, the rotation to the right will decrease cmdWz by a small value.

[0270] An agricultural bipedal robot includes a controller and a body and two legs connected to the controller. The controller is used to execute executable instructions, which contain program code for implementing the pose control algorithm of the agricultural bipedal robot as described above when executed.

[0271] In one embodiment, the main program logic and computational framework are as follows:

[0272] The code structure strives for simplicity. The cost function optimization problem utilizes third-party open-source libraries such as eiquadprog, integrated with Eigen3, licensed under the LGPL, offering a concise and user-friendly solution. The RBDL library is used for kinematic and dynamic solutions. The robot's motion balance employs the MPC framework, utilizing three coordinate systems: world coordinates, body coordinates, and forward coordinates. Simulation programs can be used with Mujoco, Webots, Coppelia, etc., with Webots recommended for its direct real-time simulation control based on C++.

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

[0274]

[0275]

[0276] In another embodiment of the invention, the model simulation shows a bipedal robot designed to be life-size, with a torso of a 200mm*400mm*500mm cuboid, its center of mass located at the center of the cuboid. The thigh is 0.5m long, the lower leg is 0.4m long, and the foot is 0.1m high. The thigh root has three degrees of freedom, allowing rotation around the x, y, and z axes respectively. The lower leg connecting to the thigh has one degree of freedom, allowing rotation around the y-axis. The lower leg connecting to the foot has two degrees of freedom, allowing rotation 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 as follows:

[0278] S610: Assign gravity values ​​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 landing point based on the period (pcT) and the initial state (stT). Then, calculate the B0 matrix and the B matrix based on the state and landing point.

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

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

[0283] S660. Save the control input to 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(type[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, Webots (an open-source, multi-platform robot simulation software) is used to provide a development environment for modeling, programming, and simulation. A node is added to the Webots canvas, specifying the local coordinate system for the Transform; boundingObject, physics, and shape are added to the children node; and the hingejoint node is selected in the solid node to define the rotational joint parameters.

[0318] Specify the motor under device; add a gyroscope, IMU, position and acceleration sensor at the torso center of mass; 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), you can specify the controller in the model to perform simulation tests.

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

[0321] The method of this application involves the first four steps: initializing the basic parameters of model predictive control, establishing a discrete spatial state model, and constructing model predictive control and attitude planning.

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

[0323] The basic idea behind the 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. Begin processing multiple postures such as stepping in place, walking, translating, and turning, allowing the two legs to swing alternately, setting the speed at the end of the foot, and calculating the movement trajectory of the leg and ankle joints;

[0327] 3.4 The MPC thread calculates the end-effector velocity and force based on the ankle joint's trajectory;

[0328] 3.5. Using the actual feedback from the robot on the joint angles and target positions, solve for the target angles, target angular velocities, and target torques of each joint using inverse kinematics;

[0329] 3.6. The target parameters are transmitted to the controller of the simulation model. The controller can use any one of the three closed loops of position, velocity, and torque to achieve smooth control of the robot's posture.

[0330] Step 4: Body Manufacturing and Integration Testing

[0331] Based on the dimensions and materials in the simulation model, a bill of materials (BOM) is created. Then, a series of processes are performed, including hardware assembly and testing, software installation and debugging, power-on hardware and software integration testing, defect repair and improvement, etc.

[0332] The Bill of Materials (BOM) is as follows:

[0333] 4.1 Motor: Specified reducer, drive, and encoder for the joint motor of the brushless servo robot;

[0334] 4.2 Control Board: Select an STM32 (STM32 represents STMicroelectronics' 32-bit microcontroller based on the ARM Cortex-M series core) development board, AI vision 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 threads, wiring, power supplies, etc.;

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

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

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

[0339] The above are merely preferred embodiments of the present invention and do not limit the patent scope of the present invention. Any equivalent structural transformations made under the concept of the present invention using the description and drawings of the present invention, or direct / indirect applications in other related technical fields, are included within the patent protection scope of the present invention.

Claims

1. A pose control algorithm for an agricultural bipedal robot, characterized in that, Includes the following steps: Initialize the basic parameters of the model predictive control algorithm; Establish a velocity model, an angular velocity model, an angle change model, and a position change model; and establish a spatial state discrete model using 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. The spatial state discrete model expression includes: (one); (two); In the formula: Indicates the torque of the left leg; The torque represents the right leg torque; m represents the robot's mass. Indicates the strength of the left leg; The force represents the force in the right leg; g represents the acceleration due to gravity. Represents the three-dimensional angular velocity of the robot's body at time i; This represents the three-dimensional motion velocity of the robot's body at time point i; This represents the state of robot at time point i; This represents the change in the robot's angle from time point i-1 to time point i. ; I represents the change in position of the robot from time point i-1 to time point i; I represents the moment of inertia. Indicates the input variables at time point i; Indicates the length of the left leg. The length of the right leg is represented by E, which is used to denote the identity matrix. Based on the spatial state discrete model, a model predictive control algorithm is constructed, which includes spatial state equations, cost functions, and constraints. The model predictive control algorithm is used for attitude planning, which includes degrees of freedom, standing, squatting, stepping, walking posture, pausing, translation, and turning. The model predictive control algorithm and the attitude planning algorithm are used to calculate the robot's motion trajectory and state, thereby achieving smooth and stable output position and attitude changes, and realizing stable control of the robot's posture.

2. The pose control algorithm for an agricultural bipedal robot as described in claim 1, characterized in that, The step of initializing the basic parameters of the model predictive control algorithm includes the following steps: Coordinate system initialization, the coordinate system including world coordinate system, body coordinate system and forward coordinate system; Initialize the sampling period and prediction period; Control input initialization, the control input including the force and torque at the left and right feet; Target state initialization; the target state includes the current state measured by the sensor.

3. The pose control algorithm for an agricultural bipedal robot as described in claim 2, characterized in that, The space state equation includes: (three); (Four); In the formula, Represents a recursive sequence of states; Represents the state variable at time n. This represents the input variable at time n-1. This represents the input matrix at time n-1. .

4. The pose control algorithm for an agricultural bipedal robot as described in claim 1, characterized in that, The constraints include: 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 contacting leg is called the standing leg, and the state is the support phase; the other leg is called the swinging leg, and the state is the swinging phase. The inequality constraints include: the robot's foot has an upward supporting force, a horizontal friction force, and a rotational friction force, and the point of equivalent force application does not exceed the polygon of the foot.

5. The pose control algorithm for an agricultural bipedal robot as described in claim 4, characterized in that, The equality constraints include: (five); The inequality constraints include: (six); (seven); In the formula: This represents the torque in the x-direction; This represents the torque in the y-direction; This represents the torque in the z-direction; Represents the force in the z-direction; Represents the force in the x-direction; Represents the force in the y-direction; This represents the maximum force in the z-direction; Indicates the lever arm in the y-direction; Indicates the lever arm in the x-direction; Indicates the coefficient of friction; This represents the coefficient of friction in the z-direction.

6. The pose control algorithm for an agricultural bipedal robot as described in claim 3, characterized in that, The construction of the model predictive control algorithm based on the spatial state discrete model includes the following steps: spatial state equation, cost function, and constraint steps. The cost function includes: ； in: ； ； Where C is a constant; Q is a diagonal matrix, and each element in Q is an error weight coefficient; R is also a diagonal matrix, and each element in R is an input weight coefficient; It is the gravity feedforward given to the corresponding leg according to the future support phase; This represents the minimum value of the cost function; This represents the transpose of the input parameter matrix; Represents the input parameter matrix; Indicates the desired state; ； ； 。 7. The pose control algorithm for an agricultural bipedal robot as described in claim 1, characterized in that, The posture planning process includes steps such as degrees of freedom, standing, squatting, stepping, walking posture, pausing, translation, and turning. The squatting posture planning includes the following steps: Joint angles for each degree of freedom in the initial and squatting postures; The joint angles during the process are optimized by periodic changes in the cosine function; ； In the formula: j represents the real-time attitude. This indicates the state of each joint in the squatting posture. This represents the state of each joint in a standing posture, where t represents time. ; The speed of joint angles during the process is optimized using a sine function; ； Where: the The rotational speed represents the joint angle, and T represents the attitude cycle length.

8. The pose control algorithm for an agricultural bipedal robot as described in any one of claims 1-7, characterized in that, The posture planning includes steps such as degrees of freedom, standing, squatting, stepping, walking posture, pausing, translation, and turning, with stepping posture planning including the following steps; Establish the formulas for foot position, velocity, and angular velocity. Among them, the formula for foot position includes: ； ； In the formula: p0 and p1 represent the starting and ending points of the foot; Indicates the position of the foot on the x-axis; Indicates the position of the foot on the y-axis; The z-axis represents the position of the foot; s represents the phase of the motion, with a value of [0, 1]; n represents the sampling period; T represents the stepping period; J represents the proportion of the period spent at rest; h represents the height of the foot lift. Indicates the position of the endpoint on the z-axis; Indicates the position of the endpoint on the x-axis; Indicates the position of the endpoint on the y-axis; Indicates the position of the starting point on the z-axis; Indicates the position of the starting point on the x-axis; This indicates the starting point's position on the y-axis.

9. An agricultural bipedal robot, characterized in that, The device includes a controller and a body and two legs connected to the controller. The controller is used to execute executable instructions, which contain program code for implementing the pose control algorithm of an agricultural bipedal robot as described in any one of claims 1-8 when executed.

Images