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 quadruped robot control method of motion trace based on PSO PD neutral nets, including：(1) target drop point of the robot on movement locus is asked for；(2) the trunk position of centre of gravity of target drop point, feedback is input into the input layer of PSO PD neutral nets；After linear transformation enter the first hidden layer, scale operation with differentiate；Enter the second hidden layer after linear transformation, obtain instructing displacement on x directions and y directions；Into the 3rd hidden layer, try to achieve and instruct shift length, instruct trunk direction；Into layer 5, to sufficient end TRAJECTORY CONTROL, the course changing control of quadruped robot；Into layer 6, quadruped robot attitude is adjusted when disturbing；Into layer 7, trunk direction, the trunk position of centre of gravity of quadruped robot are asked for, towards layer 5 is fed back to, trunk position of centre of gravity feeds back to input layer to trunk.With more preferable Nonlinear Decoupling control ability, control is accurate, and stability is strong, with preferable antijamming capability.

Description

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

Technical field

The present invention relates to a kind of robot motion's method for controlling trajectory, and in particular to a kind of based on PSO-PD neutral nets Quadruped robot control method of motion trace.

Background technology

With developing rapidly for science and technology, robot technology is also increasingly being applied among complex environment.Wherein Legged type robot is moved with certain superiority relative to wheeled or caterpillar type robot in non-structure environment.And four-footed machine Device people with simple structure, the advantage such as motion stabilization, therefore anticipates compared to other Multifeet walking robots with research higher Justice and application value.When complex environment works, robot runs along pre-set movement locus.Can be with most fast Speed, minimum energy consumption passes through each operating point, and at the same time can also avoid various obstacles etc. influences stable factor.Cause Be applied to research of the movement locus control of this research quadruped robot to quadruped robot is significant.

Quadruped robot is a nonlinear system for complexity, and trunk direction has coupled relation with position of centre of gravity, because The control of this quadruped robot movement locus is a sufficiently complex Nonlinear Decoupling control problem.Existing method can only be to four-footed machine Device people realizes Linear track control, or circular trace control；Or on this basis by the gait simple non-linear rail of switching realization The control of mark.But continuous accurately nonlinear motion TRAJECTORY CONTROL cannot be realized to robot all the time.

General existing PD neutral nets are four layers of feedforward neural network, and ground floor is input layer, receive control signal with it is anti- Feedback signal；The second layer is proportion differential computation layer, and ratio calculating and differential calculation are carried out to error by action function；Third layer It is control rate output layer, the result differentiated by comparative example is carried out linear operation and obtains control rate；4th layer right for control As.

The content of the invention

In view of the shortcomings of the prior art, the invention provides a kind of quadruped robot motion based on PSO-PD neutral nets Method for controlling trajectory.

A kind of special PD neutral nets towards the control of quadruped robot movement locus are devised in the present invention, but due to this Existence feedback cannot realize god with the action function that can discontinuously lead by BP learning algorithms in PD neural network networks It is then of the invention to use the learning algorithm for improving PSO algorithms as PD neutral nets through the adaptive learning of network.Improve PSO Algorithmic statement is fast, high precision and should not be absorbed in local optimum, can accurately realize the adaptive learning of PD neutral nets.Always It, the present invention can realize that quadruped robot movement locus is controlled fast accurate, be expected to be obtained in motion planning and robot control Extensive use.

The technical scheme is that：

A kind of quadruped robot control method of motion trace based on PSO-PD neutral nets, including：

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

(2) target drop point, the body of Real-time Feedback of the quadruped robot center of gravity for asking for step (1) on the movement locus Dry position of centre of gravity (y_x,y_y) it is input into the input layer of PD neural network models；Into the first of PD neural network models after linear transformation Hidden layer, carries out scale operation and differentiates in the first hidden layer；Again into PD neural network models after linear transformation Second 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 is transported The coordinate system of the horizontal plane where dynamic, origin is quadruped robot original center of gravity position；Into the of PD neural network models Three hidden layers, try to achieve in the 3rd hidden layer and instruct shift length dr, instruct steering angle d θ, and it refers to four-footed to instruct shift length dr The distance that robot is stepped from current this step to next step, instruct steering angle refer to quadruped robot from current this step to The trunk steering angle of next step；Into the layer 5 of PD neural network models, the foot to quadruped robot is realized in layer 5 End TRAJECTORY CONTROL, course changing control；Into the layer 6 of PD neural network models, when quadruped robot foot end is slided, adjust Whole quadruped robot attitude, it is ensured that quadruped robot is stable；But the process can make robot center of gravity change, and can be considered The random disturbance of size that random times occur, in addition with the disturbance that many other reasonses are caused, adds in layer 6 and disturbs It is simulated, and the step-length after disturbance will be added to feed back to layer 5 as actual step size；Into the of PD neural network models Seven layers, ask for the trunk of quadruped robot towards y_θ, trunk position of centre of gravity (y_x,y_y), trunk is towards y_θIt refer to current end of taking a step Afterwards, the angle of the actual direction of trunk, i.e. trunk and X-axis；Trunk position of centre of gravity (y_x,y_y) refer to quadruped robot center of gravity in institute State the coordinate in coordinate system；The trunk of quadruped robot is towards y_θFeed back to the layer 5 of PD neural network models, trunk center of gravity Position (y_x,y_y) feed back to the input layer of PD neural network models.

According to currently preferred, using multilayer nerve in improved particle swarm optimization algorithm adjustment PD neural network models Connection weight between unit, 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 mesh of optimization problem Scalar functions are 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, defeated Outgoing vector is quadruped robot actual center gravity position；

In formula (I), Error is the object function of optimization problem, and xd (k), yd (k) are respectively quadruped robot kth step Center of gravity target drop point abscissa, ordinate, y_x(k)、y_yK () is the reality of the center of gravity target drop point of quadruped robot kth step Abscissa, 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 Initial velocity, with these three index expression particle characteristicses of position, speed and fitness, institute in positional representation PD neural network models Some connection weight values, speed represents the direction of each particle evolution, and fitness value is tried to achieve by fitness function, i.e. each grain The corresponding object function of son；And each particle represents a potential optimal solution of above-mentioned optimization problem；

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

In formula (II), id is the numbering of particle in population,It is the speed of the i-th generation particle,For the i-th generation particle exists I-th instead of preceding history optimum position,It is the position of optimal particle in the i-th generation population；ω (i) is the used of the i-th generation particle Property weight, its size determines that speed to what extent inherits the movement velocity of previous generation particles；%₁, %₂It is acceleration factor, Value 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 ω_startIt 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 is excellent First speed of searching optimization, the later stage focuses on low optimization accuracy, shown in the more new formula such as formula (III) of inertia weight ω：

In formula (III), maxg；N is maximum iteration；

Present invention adds decreases in non-linear inertia weight ω (i), inertia weight ω (i) determines speed to what extent The movement velocity of previous generation is inherited, the bigger speed of searching optimization of ω (i) is faster in an iterative process, but precision is lower；Otherwise then precision Higher, speed is lower.To reach the balance of speed and precision, inertia weight ω (i) is larger at the iteration initial stage, as iteration is carried out Inertia weight progressively reduces.

The speed of the particle after being updatedAfterwards, the position of the particle, the position of particle are updatedRenewal it is public Shown in formula such as formula (IV)：

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

In this way, carrying out successive ignition obtains approximate optimal solution, even if PD neural network models control error minimum is near Like best initial weights.

After adding the thought of the TSP question in genetic algorithm, i.e. particle position to update, there is certain probability to occur Variation in span.TSP question has expanded in iteration the population search space for constantly reducing, can be Bigger space development search, makes it jump out local optimum, and increase obtains the possibility of global optimum.Each particle in position more After new, a random number can be generated, if greater than 0.9 random initializtion particle again in Search Range.If less than Doed nothing equal to 0.9.

Improved particle swarm optimization algorithm takes into account convergence rate and low optimization accuracy while being difficult to be absorbed in local optimum.With It substitutes the adaptive learning that traditional PS O particle cluster algorithms realize PSO-PD neutral nets.Improved particle swarm optimization algorithm, learns The habit time is shorter, and control accuracy is higher, therefore can better meet the demand for control of quadruped robot movement locus control.

According to currently preferred, the step (1), according to quadruped robot movement locus, ask for quadruped robot and exist Target drop point on the movement locus, the movement locus for setting quadruped robot is sine curve, during along sinusoidal motion, machine The radius of turn of device people, angle of turn can be used to prove whether the control strategy is generally applicable all constantly changing In most nonlinear motion tracks.The span for setting default step-length L is 0.38-0.42m, when no external disturbance When, the center of gravity of robot will not change；Ask for the formula such as formula of target drop point of the quadruped robot on the movement locus (V) shown in：

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 When the abscissa and ordinate of back 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, iterates after 200 times, obtains Target drop point of the quadruped robot on the movement locus.

It is further preferred that the value of L is 0.4m.

According to currently preferred, 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：

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

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

According to currently preferred, realize the sufficient end TRAJECTORY CONTROL to quadruped robot, quadruped robot include trunk with And four legs being connected with the trunk, in a cycle of taking a step of quadruped robot, the two legs on same diagonal Swung in same movement mode, support trunk and promote the two legs referred to as support phase that trunk advances, and according to rail is previously set Two legs referred to as swing phase on another diagonal that mark is swung, including step is as follows：

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

Every leg includes thigh, shank；One of quadruped robot includes support phase, a swing in cycle of taking a step Phase, in trunk 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 Subpoint on the ground, under trunk coordinate system shown in sufficient end position model such as formula (VIII)：

In formula (VIII), p_x(θ₁,θ₂) it is sufficient end horizontal displacement and θ₁、θ₂Function, p_z(θ₁,θ₂) it is sufficient end height and θ₁、θ₂ Function, 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, Span is 0 ° -180 °；θ₂It is knee joint folding angle, span is 0 ° -180 °；H is robot trunk bottom to ground The distance in face；

B, using a kind of New Sinusoidal diagonal gait, ask for movement locus, pendulum of the support phase foot end under trunk coordinate system Mutually movement locus of the sufficient end under trunk coordinate system is moved, the New Sinusoidal diagonal gait refers to：Four legs of quadruped robot Be divided into two groups with diagonal, front left-leg and right rear leg are one group, front right-leg and rear left-leg are one group, two groups alternately as support phase with Swing phase；Support phase promotes trunk to move ahead, and swing phase is taken a step forward leaping over obstacles, and these processes can be carried out by sufficient end track Explain.Especially swing phase foot end track, directly determines that swing phase is taken a step in which way.Therefore, sufficient end track Selection has highly important meaning to motion planning and robot control.

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

In formula (Ⅸ), p1_xT () is support phase foot function of the end horizontal displacement on time t, p1_zT () support phase foot end is high Function of the degree on time t, S is current cycle step length of taking a step, and S_1 is upper one to take a step cycle actual step size, and t is to take a step in the cycle Moment, T is to take a step the cycle；

Support phase supports trunk and promotes it to move ahead, therefore, to make robot even running, support phase foot end is sat in trunk The lower motion of mark system should try one's best and remain a constant speed, and keep in the z-axis direction constant.In x0y planes, support phase foot end is carried out at the uniform velocity Distance do exercises for d=0.5* (S+S_1).

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

In formula (Ⅹ), p2_xT () is swing phase foot function of the end horizontal displacement on time t, p2_zT () swing phase foot end is high Function of the degree on 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 swing phase foot end is under trunk coordinate system Movement locus have 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 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 swing phase Control rate；

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 condition sieve Choosing obtains meeting the hip joint longitudinal direction folding angle and knee joint folding angle of joint constraint condition, is 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 controls of 2*200 each address Matrix processed, 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 for having support phase joint control rate is obtained from database Control 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.

According to currently preferred, 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 the cycle At the end of trunk towards θ；

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

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

H, in robot kinematics, the steering of trunk is turned in the opposite direction under trunk coordinate system by support phase Move identical angle and realize.Therefore, on the premise of known target steering angle, so that it may the phase that is supported hip joint transverse direction The control rate at folding angle, the control rate θ 1 at the transversely opened and closed angle of support phase hip joint is tried to achieve by formula (Ⅻ)₄：

I, the transversely opened and closed angle in hip joint of swing phase will be returned just, during SBR, y_ θ _ 1=y_ θ _ 2=0, to formula (XIII) it is Δ θ _ 1 to be iterated computing and try to achieve the positive-angle of returning of swing phase, and size is current trunk towards y_ θ _ 1 and the last week Difference of the phase initial time 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)₄：

K, by control rate distribute, by signal transmission give corresponding leg group, make its according to control rate change, realize four-footed machine The course changing control of device people.

Compared to sufficient end TRAJECTORY CONTROL, course changing control is relatively simple, and control rate can be tried to achieve directly by equation, it is only necessary to will be controlled Rate processed accurately realizes the rotary transform tensor of course changing control and swing phase by distributing to corresponding leg group.

According to currently preferred, when quadruped robot is disturbed, quadruped robot attitude is adjusted, it is ensured that four-footed machine Device people is stable, including step is as follows：

When robot is moved in complicated ground, at the end of taking a step, swing phase foot end may occur at random with ground It is relative to slide.This can influence the stability of robot motion.In T/10 after end cycle of taking a step, sufficient end TRAJECTORY CONTROL unit with Turning control cell cooperates and adjusts robot pose, 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, is asked for by formula (XV)：

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

By above-mentioned adjustment, although the direction and position of centre of gravity of robot trunk may all change.But left and right leg hip Joint line is maintained within the middle of support phase foot end drop point and swing phase foot end drop point, and the leg attitude in same leg group It is identical.Motion process can be so set to possess high stability.

According to currently preferred, the trunk of quadruped robot towards y is asked for_θ, trunk position of centre of gravity (y_x,y_y), ask for public affairs Shown in formula such as formula (XVII)：

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

Beneficial effects of the present invention are：

1st, the present invention carries out robot motion's TRAJECTORY CONTROL using PD neutral nets.Compared to traditional control method, PSO- PD neutral nets have more preferable Nonlinear Decoupling control ability, and control is accurate, and stability is strong, and with preferably anti-interference Ability.

2nd, the present invention substitutes traditional BP learning algorithm using PSO particle swarm optimization algorithms are improved, due to proposed by the present invention Existence feedback and the action function that can discontinuously lead towards in the PD neutral nets of quadruped robot movement locus control, because This BP learning algorithm can not realize the adaptive learning of neutral net, therefore using improvement PSO particle swarm optimization algorithms as god Through the adaptive learning algorithm of network.The algorithm the convergence speed is fast, high precision, and is difficult to be absorbed in local optimum, can be preferable The adaptive learning of the PD neutral nets is realized, in sum, proposed by the present invention based on improvement PSO-PD neutral nets four Biped robot control method of motion trace high speed, accurate, stabilization.

Brief description of the drawings

Fig. 1 is the movement locus and target drop point schematic diagram of quadruped robot；

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

Fig. 3 is swing phase foot end track schematic diagram；

Fig. 4 is the schematic process flow diagram of sufficient end TRAJECTORY CONTROL；

Fig. 5 is the schematic process flow diagram of course changing control；

Fig. 6 is the schematic network structure of PD neutral nets；

Fig. 7 is the control effect figure of PD neutral nets；

Fig. 8 is control effect figure when there is random perturbation.

Specific embodiment

The present invention is further qualified with reference to Figure of description and embodiment, but not limited to this.

Embodiment

A kind of quadruped robot control method of motion trace based on PSO-PD neutral nets, including：

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

(2) target drop point, the body of Real-time Feedback of the quadruped robot center of gravity for asking for step (1) on the movement locus Dry position of centre of gravity (y_x,y_y) it is input into the input layer of PD neural network models；Into the first of PD neural network models after linear transformation Hidden layer, carries out scale operation and differentiates in the first hidden layer；Again into PD neural network models after linear transformation Second 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 is transported The coordinate system of the horizontal plane where dynamic, origin is quadruped robot original center of gravity position；Into the of PD neural network models Three hidden layers, try to achieve in the 3rd hidden layer and instruct shift length dr, instruct steering angle d θ, and it refers to four-footed to instruct shift length dr The distance that robot is stepped from current this step to next step, instruct steering angle refer to quadruped robot from current this step to The trunk steering angle of next step；Into the layer 5 of PD neural network models, layer 5 is respectively sufficient end TRAJECTORY CONTROL unit With turning control cell.Sufficient end TRAJECTORY CONTROL unit is received and instructed distance and the actual displacement distance in a upper cycle, realizes four-footed The sufficient end TRAJECTORY CONTROL of robot；Turning control cell is received and instructs trunk direction and current trunk direction, realizes four-footed machine The course changing control of people, sufficient end TRAJECTORY CONTROL, the course changing control to quadruped robot are realized in layer 5；Into PD neutral net moulds The layer 6 of type, when quadruped robot foot end is slided, adjusts quadruped robot attitude, it is ensured that quadruped robot operation is steady It is fixed；But the process can make robot center of gravity change, the random disturbance of the size that random times occur is can be considered, in addition with The disturbance that many other reasonses are caused, adds disturbance to be simulated and will add the step-length after disturbing as actual step in layer 6 Length feeds back to layer 5；Into the layer 7 of PD neural network models, the trunk of quadruped robot towards y is asked for_θ, trunk center of gravity Position (y_x,y_y), trunk is towards y_θIt refer to the angle of the actual direction of trunk, i.e. trunk and X-axis after currently taking a step to terminate；Trunk Position of centre of gravity (y_x,y_y) refer to coordinate of the quadruped robot center of gravity in the coordinate system；The trunk of quadruped robot is towards y_θInstead Feed the layer 5 of PD neural network models, trunk position of centre of gravity (y_x,y_y) feed back to the input layer of PD neural network models.Should The structure of PD neural network models is as shown in Figure 6；

Using the connection weight between multilayer neuron in improved particle swarm optimization algorithm adjustment PD neural network models, The adaptive learning of the PD neutral nets of object manipulator movement locus control is realized, including：

A, the On The Choice of the connection weight between multilayer neuron is converted into optimization problem, the mesh of optimization problem Scalar functions are 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, defeated Outgoing vector is quadruped robot actual center gravity position；

In formula (I), Error is the object function of optimization problem, and xd (k), yd (k) are respectively quadruped robot kth step Center of gravity target drop point abscissa, ordinate, y_x(k)、y_yK () is the reality of the center of gravity target drop point of quadruped robot kth step Abscissa, 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 Initial velocity, with these three index expression particle characteristicses of position, speed and fitness, institute in positional representation PD neural network models Some connection weight values, speed represents the direction of each particle evolution, and fitness value is tried to achieve by fitness function, i.e. each grain The corresponding object function of son；And each particle represents a potential optimal solution of above-mentioned optimization problem；

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

In formula (II), id is the numbering of particle in population,It is the speed of the i-th generation particle,For the i-th generation particle exists I-th instead of preceding history optimum position,It is the position of optimal particle in the i-th generation population；ω (i) is the used of the i-th generation particle Property weight, its size determines that speed to what extent inherits the movement velocity of previous generation particles；%₁, %₂It is acceleration factor, Value 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 ω_startIt 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 is excellent First speed of searching optimization, the later stage focuses on low optimization accuracy, shown in the more new formula such as formula (III) of inertia weight ω：

In formula (III), maxg；N is maximum iteration；

Present invention adds decreases in non-linear inertia weight ω (i), inertia weight ω (i) determines speed to what extent The movement velocity of previous generation is inherited, the bigger speed of searching optimization of ω (i) is faster in an iterative process, but precision is lower；Otherwise then precision Higher, speed is lower.To reach the balance of speed and precision, inertia weight ω (i) is larger at the iteration initial stage, as iteration is carried out Inertia weight progressively reduces.

The speed of the particle after being updatedAfterwards, the position of the particle, the position of particle are updatedRenewal it is public Shown in formula such as formula (IV)：

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

In this way, carrying out successive ignition obtains approximate optimal solution, even if PD neural network models control error minimum is near Like best initial weights.

After adding the thought of the TSP question in genetic algorithm, i.e. particle position to update, there is certain probability to occur Variation in span.TSP question has expanded in iteration the population search space for constantly reducing, can be Bigger space development search, makes it jump out local optimum, and increase obtains the possibility of global optimum.Each particle in position more After new, a random number can be generated, if greater than 0.9 random initializtion particle again in Search Range.If less than Doed nothing equal to 0.9.

Improved particle swarm optimization algorithm takes into account convergence rate and low optimization accuracy while being difficult to be absorbed in local optimum.With It substitutes the adaptive learning that traditional PS O particle cluster algorithms realize PSO-PD neutral nets.Improved particle swarm optimization algorithm, learns The habit time is shorter, and control accuracy is higher, therefore can better meet the demand for control of quadruped robot movement locus control.

Step (1), according to quadruped robot movement locus, asks for target of the quadruped robot on the movement locus and falls Point, the movement locus of quadruped robot is sine curve in the present embodiment, during along sinusoidal motion, the turning half of robot Footpath, angle of turn can be used to prove whether the control strategy is widely used in most non-all constantly changing Linear motion trajectory.It is 0.4m to set default step-length L, and when not having external disturbance, the center of gravity of robot will not change, machine The forward travel distance of device people is step-length；Ask for formula such as formula (V) institute of target drop point of the quadruped robot on the movement locus Show：

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 When the abscissa and ordinate of back 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, iterates after 200 times, constitutes One matrix of 200*2, obtains target drop point of the quadruped robot on the movement locus.As shown in Figure 1.

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：

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

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

Realize to the sufficient end TRAJECTORY CONTROL of quadruped robot, quadruped robot includes trunk and is connected with the trunk Four legs, in a cycle of taking a step of quadruped robot, the two legs on same diagonal are swung in same movement mode, Support trunk simultaneously promotes the two legs referred to as support phase that trunk advances, and another diagonal according to be previously set that track swung Two legs on line 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 Robot foot end position model：

Every leg includes thigh, shank；One of quadruped robot includes support phase, a swing in cycle of taking a step Phase, in trunk 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 Subpoint on the ground, under trunk coordinate system shown in sufficient end position model such as formula (VIII)：

In formula (VIII), p_x(θ₁,θ₂) it is sufficient end horizontal displacement and θ₁、θ₂Function, p_z(θ₁,θ₂) it is sufficient end height and θ₁、θ₂ Function, the trunk of quadruped robot is with leg structure schematic diagram as shown in Fig. 2 L₁It is the length of robot thigh, L₁= 300mm, L₂It is the length of robot shank, L₂=300mm, θ₁It is robot hip joint longitudinal direction folding angle, span is 0°-180°；θ₂It is knee joint folding angle, span is 0 ° -180 °；H is distance of the robot trunk bottom to ground；

B, using a kind of New Sinusoidal diagonal gait, ask for movement locus, pendulum of the support phase foot end under trunk coordinate system Mutually movement locus of the sufficient end under trunk coordinate system is moved, the New Sinusoidal diagonal gait refers to：Four legs of quadruped robot Be divided into two groups with diagonal, front left-leg and right rear leg are one group, front right-leg and rear left-leg are one group, two groups alternately as support phase with Swing phase；Support phase promotes trunk to move ahead, and swing phase is taken a step forward leaping over obstacles, and these processes can be carried out by sufficient end track Explain.Especially swing phase foot end track, directly determines that swing phase is taken a step in which way.Therefore, sufficient end track Selection has highly important meaning to motion planning and robot control.

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

In formula (Ⅸ), p1_xT () is support phase foot function of the end horizontal displacement on time t, p1_zT () support phase foot end is high Function of the degree on time t, S is current cycle step length of taking a step, and S_1 is upper one to take a step cycle actual step size, and t is to take a step in the cycle Moment, T is to take a step the cycle；

Support phase supports trunk and promotes it to move ahead, therefore, to make robot even running, support phase foot end is sat in trunk The lower motion of mark system should try one's best and remain a constant speed, and keep in the z-axis direction constant.In x0y planes, support phase foot end is carried out at the uniform velocity Distance do exercises for d=0.5* (S+S_1).

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

In formula (Ⅹ), p2_xT () is swing phase foot function of the end horizontal displacement on time t, p2_zT () swing phase foot end is high Function of the degree on time t, h is high for step, is the peak reached in swing phase uphill process；Swing phase foot end track schematic diagram As shown in Figure 3.

The solution of C, control rate

The span for setting step-length is 0.38m~0.42m, and precision is 1mm, then swing phase foot end is under trunk coordinate system Movement locus have 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(V)、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 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 swing phase Control rate；

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 condition sieve Choosing obtains meeting the hip joint longitudinal direction folding angle and knee joint folding angle of joint constraint condition, is 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 controls of 2*200 each address Matrix processed, 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, as shown in Figure 4：

A, ask for S and S_1；

B, according to S_1 and S, carry out fast zoom table, the support phase for having support phase joint control rate is obtained from database Control 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.

The course changing control to quadruped robot is realized, as shown in figure 5, as follows including step：

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

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

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

H, in robot kinematics, the steering of trunk is turned in the opposite direction under trunk coordinate system by support phase Move identical angle and realize.Therefore, on the premise of known target steering angle, so that it may the phase that is supported hip joint transverse direction The control rate at folding angle, the control rate θ 1 at the transversely opened and closed angle of support phase hip joint is tried to achieve by formula (Ⅻ)₄：

I, the transversely opened and closed angle in hip joint of swing phase will be returned just, during SBR, y_ θ _ 1=y_ θ _ 2=0, to formula (XIII) it is Δ θ _ 1 to be iterated computing and try to achieve the positive-angle of returning of swing phase, and size is current trunk towards y_ θ _ 1 and the last week Difference of the phase initial time 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)₄：

K, by control rate distribute, by signal transmission give corresponding leg group, make its according to control rate change, realize four-footed machine The course changing control of device people.

Compared to sufficient end TRAJECTORY CONTROL, course changing control is relatively simple, and control rate can be tried to achieve directly by equation, it is only necessary to will be controlled Rate processed accurately realizes the rotary transform tensor of course changing control and swing phase by distributing to corresponding leg group.

When quadruped robot is disturbed, quadruped robot attitude is adjusted, it is ensured that quadruped robot is stable, including Step is as follows：

When robot is moved in complicated ground, at the end of taking a step, swing phase foot end may occur at random with ground It is relative to slide.This can influence the stability of robot motion.In T/10 after end cycle of taking a step, sufficient end TRAJECTORY CONTROL unit with Turning control cell cooperates and adjusts robot pose, 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, is asked for by formula (XV)：

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

By above-mentioned adjustment, although the direction and position of centre of gravity of robot trunk may all change.But left and right leg hip Joint line is maintained within the middle of support phase foot end drop point and swing phase foot end drop point, and the leg attitude in same leg group It is identical.Motion process can be so set to possess high stability.

Ask for the trunk of quadruped robot towards y_θ, trunk position of centre of gravity (y_x,y_y), ask for shown in formula such as formula (XVII)：

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

PD neutral nets (are added without disturbing in the case where sufficient end does not occur deviation and no other disturbances in simulation process In the case of dynamic) control effect figure as shown in fig. 7, the accumulated error of 200 steps is 0.1360m, actual center gravity after operation program Drop point is essentially coincided with target barycentric drop point, control effect fast accurate.

Swing phase foot end the random slip of size can occur after unstructuredness ground is touched, due to step-length restriction most Big step-length is 0.42m, and minimum step is 0.38m, therefore, the maximum sliding distance in x-axis, y-axis is 0.04m, to machine Maximum perturbation of people's center of gravity in x-axis, y-axis is 0.02m, it is contemplated that the disturbance that other reasonses cause, by robot center of gravity in x Maximum perturbation in axle, y-axis is set to 0.08m, and the random generation of disturbance, and the probability of generation is 10%, with the anti-of test system Interference performance.By program verification, the accumulation disturbance of 200 steps is 1.5544m, and the accumulated error of control system is 1.8694m, As shown in figure 8, after generation is disturbed, control system can immediately be reduced or eliminated error.It can be seen that based on PSO-PD neutral nets Quadruped robot control is accurate, strong antijamming capability, with 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.