CN106886155A - A kind of quadruped robot control method of motion trace based on PSO PD neutral nets - Google Patents

Abstract

The present invention relates to a kind of motion track control method of quadruped robot based on PSO-PD neural network, comprising: (1) finding the target falling point of robot on the moving track; Enter the input layer of the PSO-PD neural network; enter the first hidden layer after linear transformation, proportional operation and differential operation; enter the second hidden layer after linear transformation, and get the guidance displacement in the x direction and y direction; enter the third In the hidden layer, obtain the guide displacement distance and guide the trunk orientation; enter the fifth layer, control the foot trajectory and steering of the quadruped robot; enter the sixth layer, adjust the posture of the quadruped robot when disturbance occurs; enter the seventh layer , to obtain the trunk orientation and the position of the center of gravity of the quadruped robot, the orientation of the trunk is fed back to the fifth layer, and the position of the center of gravity of the trunk is fed back to the input layer. It has better nonlinear decoupling control ability, precise control, strong stability, and better anti-interference ability.

Description

A Quadruped Robot Motion Trajectory Control Method Based on PSO-PD Neural Network

technical field

The invention relates to a method for controlling the trajectory of a robot, in particular to a method for controlling the trajectory of a quadruped robot based on a PSO-PD neural network.

Background technique

With the rapid development of science and technology, robotics is increasingly used in complex environments. Compared with wheeled or tracked robots, legged robots have certain advantages in moving in unstructured environments. Compared with other multi-legged walking robots, the quadruped robot has the advantages of simple structure and stable motion, so it has high research significance and application value. When working in a complex environment, the robot runs along a preset trajectory. It can pass through each working point with the fastest speed and the lowest energy consumption, and at the same time avoid various obstacles and other factors that affect the stable operation. Therefore, it is of great significance to study the trajectory control of quadruped robots for the application of quadruped robots.

The quadruped robot is a complex nonlinear system, and there is a coupling relationship between the orientation of the torso and the position of the center of gravity, so the trajectory control of the quadruped robot is a very complex nonlinear decoupling control problem. Existing methods can only realize linear trajectory control or circular trajectory control for quadruped robots; or realize simple nonlinear trajectory control through gait switching on this basis. However, it is still impossible to achieve continuous and precise nonlinear motion trajectory control for robots.

Generally, the existing PD neural network is a four-layer forward neural network. The first layer is the input layer, which receives control signals and feedback signals; the second layer is the proportional differential calculation layer, which performs proportional calculation and differential calculation on the error through the action function; The third layer is the control rate output layer, and the control rate is obtained by performing linear operations on the results of the proportional differential operation; the fourth layer is the control object.

Contents of the invention

Aiming at the deficiencies of the prior art, the present invention provides a motion track control method of a quadruped robot based on a PSO-PD neural network.

In the present invention, a kind of PD neural network specially oriented to quadruped robot motion trajectory control is designed, but because there is state feedback and discontinuous derivable action function in this PD neural network, it is impossible to realize the automatic control of neural network by BP learning algorithm. To adapt to learning, the present invention uses the improved PSO algorithm as the learning algorithm of the PD neural network. The improved PSO algorithm has fast convergence, high precision and is not suitable to fall into local optimum, and can accurately realize the adaptive learning of PD neural network. In a word, the present invention can quickly and accurately realize the motion trajectory control of a quadruped robot, and is expected to be widely used in robot motion control.

Technical scheme of the present invention is:

A method for controlling motion trajectory of a quadruped robot based on PSO-PD neural network, comprising:

(1) According to the trajectory of the quadruped robot and the preset step size, the target landing point of the center of gravity of the quadruped robot on the trajectory is obtained, that is, the coordinates of the center of gravity of the quadruped robot on the trajectory; it is relative to the actual result spoken.

(2) Input the target landing point of the quadruped robot's center of gravity on the motion trajectory obtained in step (1) and the real-time feedback position of the center of gravity (y _x , y _y ) of the torso into the input layer of the PD neural network model; after linear transformation Enter the first hidden layer of the PD neural network model, perform proportional operation and differential operation in the first hidden layer; enter the second hidden layer of the PD neural network model after linear transformation again, and obtain the x direction in the second hidden layer The coordinate system is the coordinate system of the horizontal plane where the quadruped robot moves, and the origin is the initial center of gravity of the quadruped robot; enter the third hidden layer of the PD neural network model, in the third hidden layer The guidance displacement distance dr and the guidance steering angle dθ are calculated in the containing layer. The guidance displacement distance dr refers to the distance that the quadruped robot takes from the current step to the next step, and the guidance steering angle refers to the distance from the current step to the next step of the quadruped robot. torso steering angle; enter the fifth layer of the PD neural network model, and realize the foot trajectory control and steering control of the quadruped robot on the fifth layer; enter the sixth layer of the PD neural network model, when the foot end of the quadruped robot When sliding, adjust the posture of the quadruped robot to ensure the stable operation of the quadruped robot; but this process will change the center of gravity of the robot, which can be regarded as random disturbances of random sizes at random times. In addition, there are many disturbances caused by other reasons. Add disturbance to the sixth layer for simulation, and feed back the step size after adding the disturbance as the actual step size to the fifth layer; enter the seventh layer of the PD neural network model to obtain the trunk orientation y _θ of the quadruped robot and the position of the trunk center of gravity ( y _x , y _y ), the orientation of the trunk y _θ refers to the actual orientation of the trunk after the current step is completed, that is, the angle between the trunk and the X axis; the position of the center of gravity of the trunk (y _x , y _y ) refers to the location The coordinates in the above coordinate system; the trunk orientation y _θ of the quadruped robot is fed back to the fifth layer of the PD neural network model, and the position of the center of gravity of the trunk (y _x , y _y ) is fed back to the input layer of the PD neural network model.

Preferably according to the present invention, an improved particle swarm optimization algorithm is used to adjust the connection weights between multi-layer neurons in the PD neural network model, so as to realize the adaptive learning of the PD neural network oriented to robot motion trajectory control, including:

a. Transform the selection of connection weights between multi-layer neurons into an optimization problem. The objective function of the optimization problem is the L2 norm of the output vector and the tutor signal vector. As shown in formula (I), the tutor signal That is, the coordinates of the target landing point, and the output vector is the actual center of gravity position of the quadruped robot;

In formula (I), Error is the objective function of the optimization problem, xd(k) and yd(k) are respectively the abscissa and ordinate of the center-of-gravity target landing point of the quadruped robot at step k, and y _x (k), y _y (k) is the actual abscissa and ordinate of the center of gravity target landing point of the kth step of the quadruped robot;

b. Use previous experience to determine the value range of each connection weight, that is, to determine the optimization range;

c. Randomly initialize a group of particles in the optimization range, that is, particle swarm, including the initial position and initial velocity of the initialized particles. The three indicators of position, speed and fitness are used to represent the particle characteristics, and the position represents all the parameters in the PD neural network model. The connection weight value, the speed represents the evolution direction of each particle, and the fitness value is obtained from the fitness function, that is, the objective function corresponding to each particle; and each particle represents a potential optimal solution of the above optimization problem ;

The speed of the particle is updated according to the current position of the particle, the current speed, the historical best position Pbest of the particle and the position Gbest of the optimal particle in the particle swarm, and the speed of the particle The update formula of is shown in formula (II):

In formula (II), id is the number of the particle in the particle swarm, is the velocity of the i-th generation particle, is the historical best position of the i-th generation particle before the i-th generation, is the position of the optimal particle in the i-th generation particle swarm; ω(i) is the inertia weight of the i-th generation particle, and its size determines how much the speed inherits the motion speed of the previous generation particle; % ₁ and % ₂ are the acceleration factors , the value is a non-negative constant; r ₁ and r ₂ are random numbers between 0 and 1; is the position of the i-th generation particle; the value ω _start of ω is 0.9 at the time of initialization, and the value ω _34d of ω at the end of the iteration is 0.01. During the iteration process, the inertia weight ω accelerates and decays. Excellent precision, the update formula of inertia weight ω is shown in formula (Ⅲ):

In formula (Ⅲ), maxg; n is the maximum number of iterations;

The present invention adds a non-linear decreasing inertia weight ω(i), and the inertia weight ω(i) determines how much the speed inherits the motion speed of the previous generation. In the iterative process, the greater the ω(i), the faster the optimization speed, but The lower the precision; otherwise, the higher the precision, the lower the speed. In order to achieve a balance between speed and accuracy, the inertia weight ω(i) is relatively large at the beginning of the iteration, and gradually decreases as the iteration progresses.

Get the updated particle velocity After that, update the particle's position, the particle's position The update formula of is shown in formula (Ⅳ):

If formula (Ⅳ) obtained The corresponding objective function is less than The corresponding fitness function, then on the contrary, At the same time, the position of the optimal particle in the population is updated to obtain

In this way, an approximate optimal solution can be obtained by performing multiple iterations, that is, an approximate optimal weight value with the smallest control error of the PD neural network model.

Add the idea of adaptive mutation in the genetic algorithm, that is, after the particle position is updated, there is a certain probability that the mutation will occur within the value range. Adaptive mutation expands the particle swarm search space that is continuously shrinking in iterations, enabling it to search in a larger space, making it jump out of the local optimum and increase the possibility of obtaining the global optimum. After the position of each particle is updated, a random number will be generated. If it is greater than 0.9, the particle will be re-initialized randomly within the optimization range. If less than or equal to 0.9 do nothing.

The improved particle swarm optimization algorithm takes into account the convergence speed and optimization accuracy while not easily falling into local optimum. It is used to replace the traditional PSO particle swarm algorithm to realize the adaptive learning of PSO-PD neural network. The improved particle swarm optimization algorithm has shorter learning time and higher control precision, so it can better meet the control requirements of quadruped robot motion trajectory control.

Preferably according to the present invention, said step (1), according to the trajectory of the quadruped robot, obtains the target landing point of the quadruped robot on the trajectory, sets the trajectory of the quadruped robot as a sinusoidal curve, and follows the sinusoidal curve. When moving, the turning radius and turning angle of the robot are constantly changing, so it can be used to prove whether the control strategy is widely applicable to most nonlinear motion trajectories. Set the value range of the preset step length L to 0.38-0.42m. When there is no external disturbance, the center of gravity of the robot will not change; ) as shown:

In formula (Ⅴ), x_1 and y_1 are respectively the abscissa and ordinate of the center of gravity of the quadruped robot in the previous step, and x and y are respectively the abscissa and ordinate of the center of gravity of the quadruped robot in the current step;

The initial values of x_1 and y_1 are both 0. After calculating x and y, they are assigned to x_1 and y_1. After repeated iterations for 200 times, the target landing point of the quadruped robot on the trajectory is obtained.

Further preferably, the value of L is 0.4m.

Preferably according to the present invention, the guidance displacement distance dr and guidance trunk orientation dθ are obtained in the third hidden layer, including:

Calculate the guiding displacement distance dr through formula (Ⅵ), and the formula (Ⅵ) is as follows:

In formula (VI), dΔx and dΔy are the guidance x increment and guidance y increment obtained by the second hidden layer, that is, the guidance displacement in the x direction and the y direction;

Calculate the guiding trunk orientation dθ through formula (VII), and the formula (VII) is as follows:

Preferably, according to the present invention, the foot trajectory control of the quadruped robot is realized. The quadruped robot includes a torso and four legs connected with the torso. One leg swings in the same way, the two legs that support the trunk and push the trunk forward are called the support phase, and the two legs on the other diagonal that swing according to the preset trajectory are called the swing phase, including the following steps :

A. By analyzing the leg structure of the quadruped robot, construct the mathematical model of the leg motion of the quadruped robot, including the position model of the foot end of the quadruped robot:

Each leg includes a thigh and a lower leg; a step cycle of a quadruped robot includes a support phase and a swing phase. In the torso coordinate system, the x-axis represents the displacement of the foot in the horizontal direction, the y-axis represents the height of the foot, and the origin o is the projection point of the leg-hip joint on the ground, and the position model of the foot in the torso coordinate system is shown in formula (Ⅷ):

In formula (Ⅷ), p _x (θ ₁ , θ ₂ ) is the function of the horizontal displacement of the foot end and θ ₁ , θ ₂ , and p _z (θ ₁ , θ ₂ ) is the function of the foot end height and θ ₁ , θ ₂ , L ₁ is the length of the thigh of the robot, L ₂ is the length of the lower leg of the robot, θ ₁ is the longitudinal opening and closing angle of the robot hip joint, the value range is 0°-180°; θ ₂ is the knee joint opening and closing angle, the value range is 0°-180°; H is the distance from the bottom of the robot torso to the ground;

B, adopt a kind of novel sinusoidal diagonal gait, obtain the motion trajectory of the support phase foot end under the trunk coordinate system, the motion trajectory of the swing phase foot end under the trunk coordinate system, and the new sinusoidal diagonal gait refers to : The four legs of the quadruped robot are divided into two groups diagonally, the front left leg and the right rear leg form a group, the front right leg and the rear left leg form a group, and the two groups alternately serve as the support phase and the swing phase; the support phase pushes The forward movement of the trunk and the swing phase forward step over obstacles can all be explained by foot trajectories. In particular, the trajectory of the foot end of the swing phase directly determines how the swing phase is carried out. Therefore, the choice of foot trajectory is very important for robot motion control.

The trajectory of the foot end of the supporting phase in the torso coordinate system is shown in formula (IX):

In formula (IX), p1 _x (t) is the function of the horizontal displacement of the foot end of the support phase with respect to time t, p1 _z (t) is the function of the height of the foot end of the support phase with respect to time t, S is the step length of the current step cycle, and S_1 is The actual step length of the previous step cycle, t is the moment in the step cycle, and T is the step cycle;

The support phase supports the torso and pushes it forward. Therefore, in order to make the robot run smoothly, the movement of the foot of the support phase in the coordinate system of the torso should be kept as uniform as possible and kept constant in the z-axis direction. On the x0y plane, the uniform motion distance of the foot end of the strut phase is d=0.5*(S+S_1).

The movement trajectory of the foot end of the swing phase in the torso coordinate system is shown in formula (Ⅹ):

In formula (Ⅹ), p2 _x (t) is the function of the horizontal displacement of the foot end of the swing phase with respect to time t, p2 _z (t) is the function of the height of the foot end of the swing phase with respect to time t, h is the step height, and is the rising process of the swing phase the highest point reached in

C. Solution of control rate

The value range of the set step length is 0.38m ~ 0.42m, and the precision is 1mm, then there are 41*41=1681 possibilities for the movement trajectory of the foot end of the swing phase in the torso coordinate system;

Put S_1 and S corresponding to 1681 possibilities into the formula (Ⅹ) to obtain the position p2 _x (t) and p2 _z (t) of the foot end of the swing phase at different times;

Substituting the obtained positions p2 _x (t) and p2 _z (t) of the foot end of the swing phase at different times into p _x (θ ₁ ,θ ₂ ) and p _z (θ ₁ ,θ ₂ ) in formula (Ⅷ), we get A nonlinear equation system, solve the nonlinear equation system by MATLAB, and obtain the longitudinal opening and closing angle of the hip joint and the opening and closing angle of the knee joint that meet the joint constraint conditions according to the joint constraint conditions, as the control rate for controlling the swing phase motion;

Put S_1 and S corresponding to the 1681 possibilities into the formula (Ⅹ) to obtain the positions p1 _x (t) and p1 _z (t) of the foot end of the supporting phase at different moments;

Substituting the obtained positions p1 _x (t) and p1 _z (t) of the foot end of the supporting phase at different times into p _x (θ ₁ ,θ ₂ ) and p _z (θ ₁ ,θ ₂ ) in formula (Ⅷ), Obtain a nonlinear equation system, solve the nonlinear equation system by MATLAB, and obtain the longitudinal opening and closing angle of the hip joint and the opening and closing angle of the knee joint that meet the joint constraint conditions according to the joint constraint conditions, as the control rate for controlling the movement of the support phase;

D. Establish a database according to the calculated control rate of the swing phase motion and the control rate of the support phase motion

The database adopts two-dimensional address pointers, the first dimension is S_1, the second dimension is S, and each address stores two 2*200 control matrices, which are respectively support-related joint control rate and swing-related joint control rate;

E. Realize the foot trajectory control of the quadruped robot through the fast look-up table method

a. Calculate S and S_1;

b. According to S_1 and S, perform a quick table lookup, and obtain the support phase control matrix with the support phase joint control rate and the swing phase control matrix with the swing phase joint control rate from the database;

c. Transfer the control matrix of the support phase to the support phase, and transfer the control matrix of the swing phase to the swing phase, and move according to the corresponding control rate.

Preferably according to the present invention, realize the steering control to quadruped robot, comprise steps as follows:

F. Obtain the torso orientation y _θ _1 at the end of the previous step cycle of the quadruped robot, ideally the orientation θ of the torso at the end of the step cycle;

G. Obtain the steering angle Δθ of the trunk through the formula (Ⅺ), and the formula (Ⅺ) is as follows:

y _θ =y _θ _1+Δθ(Ⅺ)

H. During the movement of the robot, the turning of the torso is realized by the support phase turning in the opposite direction by the same angle in the torso coordinate system. Therefore, on the premise that the target steering angle is known, the control rate of the lateral opening and closing angle of the hip joint in the support phase can be obtained, and the control rate θ1 ₄ of the lateral opening and closing angle of the hip joint in the support phase can be obtained by formula (Ⅻ):

I. The lateral opening and closing angles of the hip joints in the swing phase should be corrected. In the ready state, y_θ_1=y_θ_2=0. Iteratively calculate the formula (XIII) to get the corrected angle of the swing phase to be Δθ_1, which is the current trunk orientation y_θ_1 The difference from the torso orientation y_θ_2 at the initial moment of the previous cycle:

Δθ_1=y _θ _1-y _θ _2 (XIII)

J. Obtain the control rate θ2 ₄ of the lateral opening and closing angle of the hip joint in the swing phase by formula (XIV):

K. Through the control rate distribution, the signal is transmitted to the corresponding leg group, so that it changes according to the control rate, and the steering control of the quadruped robot is realized.

Compared with the foot track control, the steering control is relatively simple, and the control rate can be obtained directly through the equation, and the steering control and swing phase return control can be accurately realized only by assigning the control rate to the corresponding leg group.

Preferably according to the present invention, when the quadruped robot is disturbed, the posture of the quadruped robot is adjusted to ensure the stable operation of the quadruped robot, including the following steps:

When the robot is moving on a complex ground, at the end of the step, the foot end of the swing phase and the ground may randomly slide relative to each other. This affects the stability of the robot's motion. Within T/10 after the end of the step cycle, the foot trajectory control unit and the steering control unit work together to adjust the robot posture. Assume that the size of the random disturbance is (Δp _x , Δp _y ), (Δp _x , Δp _y ) refers to the phase The deviation from the target landing point, including:

As the swing phase, the legs move forward close to the ground. While adjusting the posture, the energy consumption is minimized. The step size is Δr, which is calculated by formula (XV):

At the same time, the support phase propels the robot torso forward At the same time, the lateral opening and closing angle of the hip joint changes by Δθ ₂ , which can be obtained by formula (XVI):

After the above adjustments, although the orientation of the robot torso and the position of the center of gravity may change. However, the connection line between the hip joints of the left and right legs can be kept in the middle of the landing point of the foot end of the support phase and the landing point of the foot end of the swing phase, and the posture of the legs in the same leg group is the same. This results in a very high degree of stability during the movement.

Preferably, according to the present invention, the torso orientation y _θ of the quadruped robot and the position of the center of gravity (y _x , y _y ) of the torso are calculated, and the calculation formula is shown in formula (XVII):

In formula (XVII), U _θ refers to the actual torso steering angle, and U _r refers to the displacement distance of the actual center of gravity of the quadruped robot.

The beneficial effects of the present invention are:

1. The present invention uses the PD neural network to control the trajectory of the robot. Compared with the traditional control method, the PSO-PD neural network has better nonlinear decoupling control ability, precise control, strong stability, and better anti-interference ability.

2. The present invention utilizes the improved PSO particle swarm optimization algorithm to replace the traditional BP learning algorithm. Because there are state feedback and discontinuous derivable action functions in the PD neural network facing the quadruped robot motion trajectory control proposed by the present invention, the BP learning algorithm The adaptive learning of the neural network cannot be realized, so the improved PSO particle swarm optimization algorithm is adopted as the adaptive learning algorithm of the neural network. The algorithm has fast convergence speed, high precision, and is not easy to fall into a local optimum, and can better realize the adaptive learning of the PD neural network. In summary, the quadruped robot based on the improved PSO-PD neural network proposed by the present invention The motion trajectory control method is high-speed, precise and stable.

Description of drawings

Fig. 1 is a schematic diagram of the trajectory of the quadruped robot and the target landing point;

Fig. 2 is a schematic diagram of the trunk and leg structure of the quadruped robot;

Fig. 3 is a schematic diagram of the trajectory of the foot end of the swing phase;

Fig. 4 is a schematic block diagram of the process of foot track control;

Fig. 5 is a flow schematic block diagram of steering control;

Fig. 6 is a schematic diagram of the network structure of the PD neural network;

Fig. 7 is a control effect diagram of the PD neural network;

Figure 8 is a diagram of the control effect when there is random disturbance.

detailed description

The present invention will be further limited below in conjunction with the accompanying drawings and embodiments, but not limited thereto.

Example

A method for controlling motion trajectory of a quadruped robot based on PSO-PD neural network, comprising:

(1) According to the trajectory of the quadruped robot and the preset step size, the target landing point of the center of gravity of the quadruped robot on the trajectory is obtained, that is, the coordinates of the center of gravity of the quadruped robot on the trajectory; it is relative to the actual result spoken.

(2) Input the target landing point of the quadruped robot's center of gravity on the motion trajectory obtained in step (1) and the real-time feedback position of the center of gravity (y _x , y _y ) of the torso into the input layer of the PD neural network model; after linear transformation Enter the first hidden layer of the PD neural network model, perform proportional operation and differential operation in the first hidden layer; enter the second hidden layer of the PD neural network model after linear transformation again, and obtain the x direction in the second hidden layer The coordinate system is the coordinate system of the horizontal plane where the quadruped robot moves, and the origin is the initial center of gravity of the quadruped robot; enter the third hidden layer of the PD neural network model, in the third hidden layer The guidance displacement distance dr and the guidance steering angle dθ are calculated in the containing layer. The guidance displacement distance dr refers to the distance that the quadruped robot takes from the current step to the next step, and the guidance steering angle refers to the distance from the current step to the next step of the quadruped robot. The steering angle of the torso; enter the fifth layer of the PD neural network model, the fifth layer is the foot track control unit and the steering control unit. The foot trajectory control unit receives the guidance distance and the actual displacement distance of the previous cycle to realize the foot trajectory control of the quadruped robot; the steering control unit receives the guidance trunk orientation and the current trunk orientation to realize the steering control of the quadruped robot. layer to realize the trajectory control and steering control of the quadruped robot; enter the sixth layer of the PD neural network model, when the foot of the quadruped robot slides, adjust the posture of the quadruped robot to ensure the stability of the quadruped robot; but the The process will change the center of gravity of the robot, which can be regarded as random disturbances of random size at random times. In addition, there are disturbances caused by many other reasons. The disturbance is added to the sixth layer for simulation and the step after adding the disturbance is used as the actual step. Feedback to the fifth layer; enter the seventh layer of the PD neural network model, and obtain the trunk orientation y _θ of the quadruped robot and the position of the trunk center of gravity (y _x , y _y ) _. The actual orientation of , that is, the angle between the torso and the X axis; the position of the center of gravity of the torso (y _x , y _y ) refers to the coordinates of the center of gravity of the quadruped robot in the coordinate system; the orientation of the torso of the quadruped robot y _{θ is} fed back to the PD nerve In the fifth layer of the network model, the position of the center of gravity of the trunk (y _x , y _y ) is fed back to the input layer of the PD neural network model. The structure of the PD neural network model is shown in Figure 6;

The improved particle swarm optimization algorithm is used to adjust the connection weights between multi-layer neurons in the PD neural network model to realize the adaptive learning of the PD neural network for robot trajectory control, including:

a. Transform the selection of connection weights between multi-layer neurons into an optimization problem. The objective function of the optimization problem is the L2 norm of the output vector and the tutor signal vector. As shown in formula (I), the tutor signal That is, the coordinates of the target landing point, and the output vector is the actual center of gravity position of the quadruped robot;

In formula (I), Error is the objective function of the optimization problem, xd(k) and yd(k) are respectively the abscissa and ordinate of the center-of-gravity target landing point of the quadruped robot at step k, and y _x (k), y _y (k) is the actual abscissa and ordinate of the center of gravity target landing point of the kth step of the quadruped robot;

b. Use previous experience to determine the value range of each connection weight, that is, to determine the optimization range;

c. Randomly initialize a group of particles in the optimization range, that is, particle swarm, including the initial position and initial velocity of the initialized particles. The three indicators of position, speed and fitness are used to represent the particle characteristics, and the position represents all the parameters in the PD neural network model. The connection weight value, the speed represents the evolution direction of each particle, and the fitness value is obtained from the fitness function, that is, the objective function corresponding to each particle; and each particle represents a potential optimal solution of the above optimization problem ;

The speed of the particle is updated according to the current position of the particle, the current speed, the historical best position Pbest of the particle and the position Gbest of the optimal particle in the particle swarm, and the speed of the particle The update formula of is shown in formula (II):

In formula (II), id is the number of the particle in the particle swarm, is the velocity of the i-th generation particle, is the historical best position of the i-th generation particle before the i-th generation, is the position of the optimal particle in the i-th generation particle swarm; ω(i) is the inertia weight of the i-th generation particle, and its size determines how much the speed inherits the motion speed of the previous generation particle; % ₁ and % ₂ are the acceleration factors , the value is a non-negative constant; r ₁ and r ₂ are random numbers between 0 and 1; is the position of the i-th generation particle; the value ω _start of ω is 0.9 at the time of initialization, and the value ω _34d of ω at the end of the iteration is 0.01. During the iteration process, the inertia weight ω accelerates and decays. Excellent precision, the update formula of inertia weight ω is shown in formula (Ⅲ):

In formula (Ⅲ), maxg; n is the maximum number of iterations;

The present invention adds a non-linear decreasing inertia weight ω(i), and the inertia weight ω(i) determines how much the speed inherits the motion speed of the previous generation. In the iterative process, the greater the ω(i), the faster the optimization speed, but The lower the precision; otherwise, the higher the precision, the lower the speed. In order to achieve a balance between speed and accuracy, the inertia weight ω(i) is relatively large at the beginning of the iteration, and gradually decreases as the iteration progresses.

Get the updated particle velocity After that, update the particle's position, the particle's position The update formula of is shown in formula (Ⅳ):

If formula (Ⅳ) obtained The corresponding objective function is less than The corresponding fitness function, then on the contrary, At the same time, the position of the optimal particle in the population is updated to obtain

In this way, an approximate optimal solution can be obtained by performing multiple iterations, that is, an approximate optimal weight value with the smallest control error of the PD neural network model.

Add the idea of adaptive mutation in the genetic algorithm, that is, after the particle position is updated, there is a certain probability that the mutation will occur within the value range. Adaptive mutation expands the particle swarm search space that is continuously shrinking in iterations, enabling it to search in a larger space, making it jump out of the local optimum and increase the possibility of obtaining the global optimum. After the position of each particle is updated, a random number will be generated. If it is greater than 0.9, the particle will be re-initialized randomly within the optimization range. If less than or equal to 0.9 do nothing.

The improved particle swarm optimization algorithm takes into account the convergence speed and optimization accuracy while not easily falling into local optimum. It is used to replace the traditional PSO particle swarm algorithm to realize the adaptive learning of PSO-PD neural network. The improved particle swarm optimization algorithm has shorter learning time and higher control precision, so it can better meet the control requirements of quadruped robot motion trajectory control.

Step (1), according to the trajectory of the quadruped robot, obtain the target landing point of the quadruped robot on the trajectory. In this embodiment, the trajectory of the quadruped robot is a sinusoidal curve. Radius, turning angle are constantly changing, so it can be used to prove whether the control strategy is widely applicable to most nonlinear motion trajectories. Set the preset step length L to 0.4m. When there is no external disturbance, the center of gravity of the robot will not change, and the forward distance of the robot is the step length; the formula for finding the target landing point of the quadruped robot on the trajectory is as follows Shown in formula (Ⅴ):

In formula (Ⅴ), x_1 and y_1 are respectively the abscissa and ordinate of the center of gravity of the quadruped robot in the previous step, and x and y are respectively the abscissa and ordinate of the center of gravity of the quadruped robot in the current step;

The initial values of x_1 and y_1 are both 0. After calculating x and y, assign them to x_1 and y_1. After repeated iterations for 200 times, a 200*2 matrix is formed to obtain the target of the quadruped robot on the trajectory. Drop point. As shown in Figure 1.

In the third hidden layer, the guidance displacement distance dr and the guidance trunk orientation dθ are obtained, including:

Calculate the guiding displacement distance dr through formula (Ⅵ), and the formula (Ⅵ) is as follows:

In formula (VI), dΔx and dΔy are the guidance x increment and guidance y increment obtained by the second hidden layer, that is, the guidance displacement in the x direction and the y direction;

Calculate the guiding trunk orientation dθ through formula (VII), and the formula (VII) is as follows:

Realize the foot trajectory control of the quadruped robot. The quadruped robot includes a torso and four legs connected to the torso. In one step cycle of the quadruped robot, the two legs on the same diagonal line move The two legs that swing, support the trunk and push the trunk forward are called the support phase, and the two legs on the other diagonal that swing according to the preset trajectory are called the swing phase, including the following steps:

A. By analyzing the leg structure of the quadruped robot, construct the mathematical model of the leg motion of the quadruped robot, including the position model of the foot end of the quadruped robot:

Each leg includes a thigh and a lower leg; a step cycle of a quadruped robot includes a support phase and a swing phase. In the torso coordinate system, the x-axis represents the displacement of the foot in the horizontal direction, the y-axis represents the height of the foot, and the origin o is the projection point of the leg-hip joint on the ground, and the position model of the foot in the torso coordinate system is shown in formula (Ⅷ):

In formula (Ⅷ), p _x (θ ₁ , θ ₂ ) is the function of the horizontal displacement of the foot end and θ ₁ , θ ₂ , and p _z (θ ₁ , θ ₂ ) is the function of the foot end height and θ ₁ , θ ₂ , the schematic diagram of the trunk and leg structure of the quadruped robot is shown in Figure 2, L ₁ is the length of the thigh of the robot, L ₁ =300mm, L ₂ is the length of the lower leg of the robot, L ₂ =300mm, θ ₁ is the longitudinal direction of the robot hip joint Opening and closing angle, the value range is 0°-180°; θ ₂ is the knee joint opening and closing angle, the value range is 0°-180°; H is the distance from the bottom of the robot torso to the ground;

B, adopt a kind of novel sinusoidal diagonal gait, obtain the motion trajectory of the support phase foot end under the trunk coordinate system, the motion trajectory of the swing phase foot end under the trunk coordinate system, and the new sinusoidal diagonal gait refers to : The four legs of the quadruped robot are divided into two groups diagonally, the front left leg and the right rear leg form a group, the front right leg and the rear left leg form a group, and the two groups alternately serve as the support phase and the swing phase; the support phase pushes The forward movement of the trunk and the swing phase forward step over obstacles can all be explained by foot trajectories. In particular, the trajectory of the foot end of the swing phase directly determines how the swing phase is carried out. Therefore, the choice of foot trajectory is very important for robot motion control.

The trajectory of the foot end of the supporting phase in the torso coordinate system is shown in formula (IX):

In formula (IX), p1 _x (t) is the function of the horizontal displacement of the foot end of the support phase with respect to time t, p1 _z (t) is the function of the height of the foot end of the support phase with respect to time t, S is the step length of the current step cycle, and S_1 is The actual step length of the last step cycle, t is the moment in the step cycle, and T is the step cycle;

The support phase supports the torso and pushes it forward. Therefore, in order to make the robot run smoothly, the movement of the foot of the support phase in the coordinate system of the torso should be kept as uniform as possible and kept constant in the z-axis direction. On the x0y plane, the uniform motion distance of the foot end of the strut phase is d=0.5*(S+S_1).

The movement trajectory of the foot end of the swing phase in the torso coordinate system is shown in formula (Ⅹ):

In formula (Ⅹ), p2 _x (t) is the function of the horizontal displacement of the foot end of the swing phase with respect to time t, p2 _z (t) is the function of the height of the foot end of the swing phase with respect to time t, h is the step height, and is the rising process of the swing phase The highest point reached in ; the schematic diagram of the foot end trajectory of the swing phase is shown in Figure 3.

C. Solution of control rate

The value range of the set step length is 0.38m ~ 0.42m, and the precision is 1mm, then there are 41*41=1681 possibilities for the movement trajectory of the foot end of the swing phase in the torso coordinate system;

Put S_1 and S corresponding to 1681 possibilities into the formula (Ⅹ) to obtain the position p2 _x (t) and p2 _z (t) of the foot end of the swing phase at different times;

Substituting the obtained positions p2 _x (V) and p2 _z (t) of the foot end of the swing phase at different times into p _x (θ ₁ ,θ ₂ ) and p _z (θ ₁ ,θ ₂ ) in formula (Ⅷ), we get A nonlinear equation system, solve the nonlinear equation system by MATLAB, and obtain the longitudinal opening and closing angle of the hip joint and the opening and closing angle of the knee joint that meet the joint constraint conditions according to the joint constraint conditions, as the control rate for controlling the swing phase motion;

Put S_1 and S corresponding to the 1681 possibilities into the formula (Ⅹ) to obtain the positions p1 _x (t) and p1 _z (t) of the foot end of the supporting phase at different moments;

Substituting the obtained positions p1 _x (t) and p1 _z (t) of the foot end of the supporting phase at different times into p _x (θ ₁ ,θ ₂ ) and p _z (θ ₁ ,θ ₂ ) in formula (Ⅷ), Obtain a nonlinear equation system, solve the nonlinear equation system by MATLAB, and obtain the longitudinal opening and closing angle of the hip joint and the opening and closing angle of the knee joint that meet the joint constraint conditions according to the joint constraint conditions, as the control rate for controlling the movement of the support phase;

D. Establish a database according to the calculated control rate of the swing phase motion and the control rate of the support phase motion

The database adopts two-dimensional address pointers, the first dimension is S_1, the second dimension is S, and each address stores two 2*200 control matrices, which are respectively support-related joint control rate and swing-related joint control rate;

E. Realize the foot track control of the quadruped robot through the fast look-up table method, as shown in Figure 4:

a. Calculate S and S_1;

b. According to S_1 and S, perform a quick table lookup, and obtain the support phase control matrix with the support phase joint control rate and the swing phase control matrix with the swing phase joint control rate from the database;

c. Transfer the control matrix of the support phase to the support phase, and transfer the control matrix of the swing phase to the swing phase, and move according to the corresponding control rate.

Realize the steering control of the quadruped robot, as shown in Figure 5, including the following steps:

F. Obtain the torso orientation y _θ _1 at the end of the previous step cycle of the quadruped robot, ideally the orientation θ of the torso at the end of the step cycle;

G. Obtain the steering angle Δθ of the trunk through the formula (Ⅺ), and the formula (Ⅺ) is as follows:

y _θ =y _θ _1+Δθ(Ⅺ)

H. During the movement of the robot, the turning of the torso is realized by the support phase turning in the opposite direction by the same angle in the torso coordinate system. Therefore, on the premise that the target steering angle is known, the control rate of the lateral opening and closing angle of the hip joint in the support phase can be obtained, and the control rate θ1 ₄ of the lateral opening and closing angle of the hip joint in the support phase can be obtained by formula (Ⅻ):

I. The lateral opening and closing angles of the hip joints in the swing phase should be corrected. In the ready state, y_θ_1=y_θ_2=0. Iteratively calculate the formula (XIII) to get the corrected angle of the swing phase to be Δθ_1, which is the current trunk orientation y_θ_1 The difference from the torso orientation y_θ_2 at the initial moment of the previous cycle:

Δθ_1=y _θ _1-y _θ _2 (XIII)

J. Obtain the control rate θ2 ₄ of the lateral opening and closing angle of the hip joint in the swing phase by formula (XIV):

K. Through the control rate distribution, the signal is transmitted to the corresponding leg group, so that it changes according to the control rate, and the steering control of the quadruped robot is realized.

Compared with the foot track control, the steering control is relatively simple, and the control rate can be obtained directly through the equation, and the steering control and swing phase return control can be accurately realized only by assigning the control rate to the corresponding leg group.

When the quadruped robot is disturbed, adjust the posture of the quadruped robot to ensure the stable operation of the quadruped robot, including the following steps:

When the robot is moving on a complex ground, at the end of the step, the foot end of the swing phase and the ground may randomly slide relative to each other. This affects the stability of the robot's motion. Within T/10 after the end of the step cycle, the foot trajectory control unit and the steering control unit work together to adjust the robot posture. Assume that the size of the random disturbance is (Δp _x , Δp _y ), (Δp _x , Δp _y ) refers to the phase The deviation from the target landing point, including:

As the swing phase, the legs move forward close to the ground. While adjusting the posture, the energy consumption is minimized. The step size is Δr, which is calculated by formula (XV):

At the same time, the support phase propels the robot torso forward At the same time, the lateral opening and closing angle of the hip joint changes by Δθ ₂ , which can be obtained by formula (XVI):

After the above adjustments, although the orientation of the robot torso and the position of the center of gravity may change. However, the connection line between the hip joints of the left and right legs can be kept in the middle of the landing point of the foot end of the support phase and the landing point of the foot end of the swing phase, and the posture of the legs in the same leg group is the same. This results in a very high degree of stability during the movement.

Find the trunk orientation y _θ of the quadruped robot and the position of the center of gravity (y _x , y _y ) of the trunk, and the formula is shown in formula (XVII):

In formula (XVII), U _θ refers to the actual torso steering angle, and U _r refers to the displacement distance of the actual center of gravity of the quadruped robot.

The control effect diagram of the PD neural network when there is no deviation at the foot end and no other disturbances (that is, when no disturbances are added during the simulation process) is shown in Figure 7. The cumulative error of 200 steps after running the program is 0.1360m, The actual center of gravity drop point basically coincides with the target center of gravity drop point, and the control effect is fast and accurate.

The foot end of the swing phase will slide randomly in size after touching the non-structural ground. Since the maximum step length is limited to 0.42m and the minimum step length is 0.38m, the maximum sliding on the x-axis and y-axis The distance is 0.04m, and the maximum disturbance of the robot's center of gravity on the x-axis and y-axis is 0.02m. Considering the disturbance caused by other reasons, the maximum disturbance of the robot's center of gravity on the x-axis and y-axis is set to 0.08m , and the disturbance occurs randomly with a probability of 10%, in order to test the anti-interference ability of the system. After program verification, the cumulative disturbance of 200 steps is 1.5544m, while the cumulative error of the control system is 1.8694m. As shown in Figure 8, the control system will immediately reduce or eliminate the error after the disturbance occurs. It can be seen that the quadruped robot based on the PSO-PD neural network has precise control, strong anti-interference ability, and has extremely high practical value.

Claims (9)

1. a kind of quadruped robot control method of motion trace based on PSO-PD neutral nets, it is characterised in that including：

(1) according to quadruped robot movement locus and default step-length, mesh of the quadruped robot center of gravity on the movement locus is asked for Village point, i.e. coordinate of the quadruped robot center of gravity on the movement locus；

(2) target drop point, the trunk weight of Real-time Feedback of the quadruped robot center of gravity for asking for step (1) on the movement locus Heart position (y_x,y_y) it is input into the input layer of PD neural network models；Implied into the first of PD neural network models after linear transformation Layer, carries out scale operation and differentiates in the first hidden layer；Again into the second of PD neural network models after linear transformation Hidden layer, the displacement of instructing on x directions and y directions is obtained in the second hidden layer, and residing coordinate system is that quadruped robot moves institute Horizontal plane coordinate system, origin be quadruped robot original center of gravity position；Into PD neural network models the 3rd is hidden Containing layer, tried to achieve in the 3rd hidden layer and instruct shift length dr, instruct steering angle d θ, it refers to four-footed machine to instruct shift length dr The distance that people steps from current this step to next step, it refers to quadruped robot from current this step to next to instruct steering angle The trunk steering angle of step；Into the layer 5 of PD neural network models, the sufficient end rail to quadruped robot is realized in layer 5 Mark control, course changing control；Into the layer 6 of PD neural network models, when quadruped robot foot end is slided, adjustment four Biped robot attitude, it is ensured that quadruped robot is stable；Layer 6 add disturbance be simulated, and will add disturbance after Step-length feeds back to layer 5 as actual step size；Into the layer 7 of PD neural network models, the trunk of quadruped robot is asked for Towards y_θ, trunk position of centre of gravity (y_x,y_y), trunk is towards y_θRefer to the actual direction of trunk, i.e. trunk after currently taking a step to terminate With the angle of X-axis；Trunk position of centre of gravity (y_x,y_y) refer to coordinate of the quadruped robot center of gravity in the coordinate system；Four-footed machine The trunk of people is towards y_θFeed back to the layer 5 of PD neural network models, trunk position of centre of gravity (y_x,y_y) feed back to PD neutral nets The input layer of model.

2. a kind of quadruped robot control method of motion trace based on PSO-PD neutral nets according to claim 1, Characterized in that, using the connection weight between multilayer neuron in improved particle swarm optimization algorithm adjustment PD neural network models Value, realizes the adaptive learning of the PD neutral nets of object manipulator movement locus control, including：

A, the On The Choice of the connection weight between multilayer neuron is converted into optimization problem, the target letter of optimization problem Number is the L2 norms of output vector and tutor's signal vector, and as shown in formula (I), tutor's signal is target landing point coordinates, export to Amount is quadruped robot actual center gravity position；

$\min Error={\mathrm{\Σ}}_{k=1}^{200}\sqrt{{\left(xd\right(k)-y_x(k\left)\right)}^2+{\left(yd\right(k)-y_y(k\left)\right)}^2}---\left(I\right)$

In formula (I), Error is the object function of optimization problem, and xd (k), yd (k) are respectively the weight of quadruped robot kth step The abscissa of target centroid drop point, ordinate, y_x(k)、y_yK () is the actual horizontal seat of the center of gravity target drop point of quadruped robot kth step Mark, ordinate；

B, the span that each connection weight is determined using previous experience, that is, determine Search Range；

C, random initializtion a group particle, i.e. population in Search Range, including initialization particle initial position with it is initial Speed, it is all of in positional representation PD neural network models with these three index expression particle characteristicses of position, speed and fitness Connection weight value, speed represents the direction of each particle evolution, and fitness value is tried to achieve by fitness function, i.e. each particle pair The object function answered；

The speed of particle according in the current location of particle, present speed, the history optimum position Pbest of particle and population most The position Gbest of excellent particle updates, the speed of particleMore new formula such as formula (II) shown in：

$V_{id}^{i+1}=\mathrm{\ω}\left(i\right)V_{id}^i+c_1{r}_1(P_{id}^i-X_{id}^i)+c_2{r}_2(P_{gd}^i-X_{id}^i)---\left(II\right)$

In formula (II), id is the numbering of particle in population,It is the speed of the i-th generation particle,It is the i-th generation particle in the i-th generation History optimum position before,It is the position of optimal particle in the i-th generation population；ω (i) is the inertia power of the i-th generation particle Weight, its size determines that speed to what extent inherits the movement velocity of previous generation particles；%₁, %₂It is acceleration factor, value It is nonnegative constant；r₁、r₂It is the random number between 0 to 1；It is the position of the i-th generation particle；The value ω of ω during initialization_start It is 0.9, the value ω of ω at the end of iteration_34dIt is 0.01, inertia weight ω accelerates decay in an iterative process, the initial stage preferentially seeks Excellent speed, the later stage focuses on low optimization accuracy, shown in the more new formula such as formula (III) of inertia weight ω：

$\mathrm{\ω}\left(i\right)={\mathrm{\ω}}_{start}-({\mathrm{\ω}}_{start}-{\mathrm{\ω}}_{end})*{\left(\frac{i}{\max gen}\right)}^2---\left(III\right)$

In formula (III), maxgen is maximum iteration；

The speed of the particle after being updatedAfterwards, the position of the particle, the position of particle are updatedMore new formula such as formula (IV) shown in：

$X_{id}^{i+1}=X_{id}^i+V_{id}^{i+1}---\left(IV\right)$

If formula (IV) ask forCorresponding object function is less thanCorresponding fitness function, thenConversely,Meanwhile, the position of optimal particle, obtains in Population Regeneration

In this way, carrying out successive ignition obtains approximate optimal solution, though PD neural network models control error it is minimum it is approximate most Excellent weights.

3. a kind of quadruped robot control method of motion trace based on PSO-PD neutral nets according to claim 1, Characterized in that, the step (1), according to quadruped robot movement locus, quadruped robot is asked on the movement locus Target drop point, the movement locus for setting quadruped robot is sine curve, and the span for setting default step-length L is 0.38- 0.42m, asks for shown in the formula such as formula (V) of target drop point of the quadruped robot on the movement locus：

$\left\{\begin{array}{c}y=4*(1-cos(0.25*x))\\ {(x-x\_1)}^2+{(y-y\_1)}^2=L^2\\ x-x\_1\≥0\end{array}\right.---\left(V\right)$

In formula (V), x_1, y_1 are respectively the abscissa and ordinate of back quadruped robot center of gravity, and x, y are respectively currently The abscissa and ordinate of one step quadruped robot center of gravity；

The initial value of x_1, y_1 is 0, after asking for x, y, then is assigned to x_1, y_1, after iterating, obtains quadruped robot Target drop point on the movement locus.

4. a kind of quadruped robot control method of motion trace based on PSO-PD neutral nets according to claim 3, Characterized in that, the value of L is 0.4m.

5. a kind of quadruped robot control method of motion trace based on PSO-PD neutral nets according to claim 1, Characterized in that, tried to achieve in the 3rd hidden layer and instruct shift length dr, instruct trunk towards d θ, including：

Ask for instructing shift length dr by formula (VI), formula (VI) is as follows：

$dr=\sqrt{{\left(d\mathrm{\Δ}x\right)}^2+{\left(d\mathrm{\Δ}y\right)}^2}---\left(VI\right)$

In formula (VI), d Δs x, d Δ y is instructed x increments and is instructed y increments, i.e. x directions and y directions by what the second hidden layer was tried to achieve On instruct displacement；

Ask for instructing trunk towards d θ by formula (VII), formula (VII) is as follows：

$d\mathrm{\θ}=arctan\frac{d\mathrm{\Δ}x}{d\mathrm{\Δ}y}---\left(VII\right).$

6. a kind of quadruped robot control method of motion trace based on PSO-PD neutral nets according to claim 1, Characterized in that, realizing the sufficient end TRAJECTORY CONTROL to quadruped robot, quadruped robot includes trunk and connects with the trunk Four legs for connecing, in a cycle of taking a step of quadruped robot, the two legs on same diagonal are in same movement mode Swing, support trunk and promote the two legs referred to as support phase that trunk advances, and it is another according to be previously set that track swung Two legs on a pair of linea angulatas are referred to as swing phase, including step is as follows：

A, by analyzing quadruped robot leg structure, build quadruped robot leg exercise Mathematical Modeling, including four-footed machine People's foot end position model：

Every leg includes thigh, shank；One of quadruped robot takes a step the cycle including support phase, a swing phase, body In dry coordinate system, x-axis represents the displacement in the horizontal direction of sufficient end, and y-axis represents sufficient end highly, and origin o is leg hip joint on ground Subpoint on face, under trunk coordinate system shown in sufficient end position model such as formula (VIII)：

$\left\{\begin{array}{c}{p}_x({\mathrm{\θ}}_1,{\mathrm{\θ}}_2)=L_1*{\mathrm{sin\θ}}_1-L_2*sin({\mathrm{\θ}}_1+{\mathrm{\θ}}_2)\\ p_z({\mathrm{\θ}}_1,{\mathrm{\θ}}_2)=H-L_1*cos+L_2*\cos ({\mathrm{\θ}}_1+{\mathrm{\θ}}_2)\end{array}\right.---\left(VIII\right)$

In formula (VIII), p_x(θ₁,θ₂) it is sufficient end horizontal displacement and θ₁、θ₂Function, p_z(θ₁,θ₂) it is sufficient end height and θ₁、θ₂Letter Number, L₁It is the length of robot thigh, L₂It is the length of robot shank, θ₁It is robot hip joint longitudinal direction folding angle, value Scope is 0 ° -180 °；θ₂It is knee joint folding angle, span is 0 ° -180 °；H is robot trunk bottom to ground Distance；

B, using a kind of New Sinusoidal diagonal gait, ask for movement locus, swing phase of the support phase foot end under trunk coordinate system Movement locus of the sufficient end under trunk coordinate system, the New Sinusoidal diagonal gait refers to：Four legs of quadruped robot are with right Angle is divided into two groups, and front left-leg and right rear leg are one group, and front right-leg and rear left-leg are one group, and two groups alternately as support phase and swing Phase；Support phase promotes trunk to move ahead, and swing phase is taken a step forward leaping over obstacles；

Shown in movement locus such as formula (Ⅸ) of the support phase foot end under trunk coordinate system：

$\left\{\begin{array}{c}p{1}_x\left(t\right)=0.5*S\_1-0.5*(S+S\_1)*\frac{t}{T}\\ p{1}_z\left(t\right)=0\end{array}\right.---\left(IX\right)$

In formula (Ⅸ), p1_xT () is support phase foot function of the end horizontal displacement on time t, p1_zHighly close at (t) support phase foot end In the function of time t, S is current cycle step length of taking a step, and S_1 takes a step cycle actual step size for upper one, t be in the cycle of taking a step when Carve, T is to take a step the cycle；

Shown in movement locus such as formula (Ⅹ) of the swing phase foot end under trunk coordinate system：

$\left\{\begin{array}{cc}p{2}_x\left(t\right)=-0.5*S\_1+0.5+(S\_1+S)*sin(0.5*\mathrm{\π}*\frac{t}{T}),& 0\≤t\≤T\\ p{2}_z\left(t\right)=h*sin(\mathrm{\π}*\frac{t}{T}),& 0\≤t\≤T\end{array}\right.---\left(X\right)$

In formula (Ⅹ), p2_xT () is swing phase foot function of the end horizontal displacement on time t, p2_zHighly close at (t) swing phase foot end In the function of time t, h is high for step, is the peak reached in swing phase uphill process；

The solution of C, control rate

The span for setting step-length is 0.38m~0.42m, and precision is 1mm, then fortune of the swing phase foot end under trunk coordinate system Dynamic rail mark has 41*41=1681 kind possibilities；

Bring 1681 kinds of possibilities corresponding S_1, S into formula (Ⅹ), try to achieve swing phase foot end position p2 not in the same time_x(t)、 p2_z(t)；

Swing phase foot end position p2 not in the same time will be tried to achieve_x(t)、p2_zT () substitutes into the p in formula (VIII)_x(θ₁,θ₂)、p_z(θ₁, θ₂), a Nonlinear System of Equations is obtained, the Nonlinear System of Equations is solved by MATLAB, and obtain according to joint constraint conditional filtering To the hip joint longitudinal direction folding angle and knee joint folding angle that meet joint constraint condition, as the control that control swing phase is moved Rate processed；

Bring 1681 kinds of possibilities corresponding S_1, S into formula (Ⅹ), try to achieve support phase foot end position p1 not in the same time_x(t)、 p1_z(t)；

The support phase foot end that will be tried to achieve position p1 not in the same time_x(t)、p1_zT () substitutes into the p in formula (VIII)_x(θ₁,θ₂)、p_z (θ₁,θ₂), a Nonlinear System of Equations is obtained, the Nonlinear System of Equations is solved by MATLAB, and according to joint constraint conditional filtering Obtain meeting the hip joint longitudinal direction folding angle and knee joint folding angle of joint constraint condition, moved as control support phase Control rate；

D, the control rate according to the swing phase motion asked for, the control rate of support phase motion, set up database

Database uses two-dimensional address pointer, and the first dimension is S_1, and the second dimension is S, and there are 2 control squares of 2*200 each address Battle array, respectively support phase joint control rate and swing phase joint control rate；

E, by quick look-up table, realize the sufficient end TRAJECTORY CONTROL to quadruped robot

A, ask for S and S_1；

B, according to S_1 and S, carry out fast zoom table, the support phase control for having support phase joint control rate is obtained from database Matrix and the swing phase control matrix for having swing phase joint control rate；

C, will support phase control matrix algebraic eqation to support phase, phase control matrix algebraic eqation will be swung to swing phase, according to corresponding control Rate motion processed.

7. a kind of quadruped robot control method of motion trace based on PSO-PD neutral nets according to claim 1, Characterized in that, the course changing control to quadruped robot is realized, including step is as follows：

F, the trunk at quadruped robot previous end cycle moment of taking a step is obtained towards y_θ_ 1, ideally take a step end cycle When trunk towards θ；

G, the steering angle Δ θ that trunk is tried to achieve by formula (Ⅺ), formula (Ⅺ) are as follows：

y_θ=y_θ_1+Δθ (Ⅺ)

H, the control rate θ 1 that the transversely opened and closed angle of support phase hip joint is tried to achieve by formula (Ⅻ)₄：

$\mathrm{\θ}{1}_4=-\mathrm{\Δ}\mathrm{\θ}*\frac{t}{T}---\left(XII\right)$

I, the transversely opened and closed angle in hip joint of swing phase will be returned just, and during SBR, y_ θ _ 1=y_ θ _ 2=0 enter to formula (XIII) The positive-angle of returning that swing phase is tried to achieve in row iteration computing is Δ θ _ 1, when size is that current trunk begins towards y_ θ _ 1 and the last week are initial Carve difference of the trunk towards y_ θ _ 2：

Δ θ _ 1=y_θ_1-y_θ_2(XIII)

J, the control rate θ 2 that the transversely opened and closed angle of swing phase hip joint is tried to achieve by formula (XIV)₄：

$\mathrm{\θ}{2}_4=\mathrm{\Δ}\mathrm{\θ}\_1*\frac{t}{T}---\left(XIV\right)$

K, by control rate distribute, by signal transmission give corresponding leg group, make its according to control rate change, realize quadruped robot Course changing control.

8. a kind of quadruped robot control method of motion trace based on PSO-PD neutral nets according to claim 1, Characterized in that, when quadruped robot is disturbed, adjust quadruped robot attitude, it is ensured that quadruped robot is stable, It is as follows including step：

In T/10 after end cycle of taking a step, sufficient end TRAJECTORY CONTROL unit cooperates with turning control cell and adjusts robot appearance State, it is assumed that the random disturbance size for occurring is (Δ p_x,Δp_y), (Δ p_x,Δp_y) refer to compared to the departure of target drop point, Including：

It is close to the ground as the leg of swing phase and move forward, while attitude is adjusted, energy consumption is preferably minimized, step-length is Δ r, Asked for by formula (XV)：

$\mathrm{\Δ}r=\sqrt{{\mathrm{\Δp}}_x^2+{\mathrm{\Δp}}_y^2}---\left(XV\right)$

At the same time, support phase pushes ahead robot trunkThe transversely opened and closed angle of hip joint changes Δ θ simultaneously₂, by formula (XVI) ask for：

${\mathrm{\Δ\θ}}_2=arctan\frac{p_y}{p_x}-arctan\frac{p_y+{\mathrm{\Δp}}_y}{p_x+{\mathrm{\Δp}}_x}---\left(XVI\right).$

9. a kind of quadruped robot control method of motion trace based on PSO-PD neutral nets according to claim 6, Characterized in that, asking for the trunk of quadruped robot towards y_θ, trunk position of centre of gravity (y_x,y_y), ask for formula such as formula (XVII) institute Show：

$\left\{\begin{array}{c}{y}_{\mathrm{\θ}}=y_{\mathrm{\θ}}\_1+U_{\mathrm{\θ}}\\ y_x=0.5*cos(y_{\mathrm{\θ}}\_1+U_{\mathrm{\θ}})*U_r\\ y_y=0.5*sin(y_{\mathrm{\θ}}\_1+U_{\mathrm{\θ}})*U_r\end{array}\right.---\left(XVII\right);$

In formula (XVII), U_θRefer to actual trunk steering angle, U_rIt refer to actual quadruped robot displacement of center of gravity distance.