CN102328315A - Control method and device for action simulation of robot - Google Patents

Abstract

The invention relates to a control method and a control device for action simulation of a robot. The control method comprises the following steps of: A, programming the action of the robot in Maya to generate attitude data of a movable joint of the robot in different time, wherein the attitude data are spatial six-degree-of-freedom data; B, converting the attitude data of different time into displacement data of different time, and generating a curve of displacement and time relationship; and C, controlling the displacement of the movable joint according to the curve of the displacement and time relationship. According to the technical scheme, the attitude data of the movable joint of the robot in different time is converted into the displacement data of the movable joint in different time, and the action of the robot is not related with the attitude data and machinery any more and is only related with the displacement value of different time, so that the control method is simpler.

Description

A kind of control method of robot motion emulation and device

Technical field

The present invention relates to automatic control technology, more particularly, relate to a kind of control method and device of robot motion emulation.

Background technology

A kind of big machine people is arranged in the future world project of theme park, and this big machine people can make perform their routines on request, cooperates stunts such as other acousto-optic electricity again, gives a kind of new experience of visitor.

The perform their routines that robot makes is in Maya, to have carried out action planning in advance; Robot model's among the Maya action is a continuous action animation; And the robot model among the Maya is consistent with actual robot, so can require robot to make the robot model's among the full Maya of symbol action.Action planning among the Maya is to be made by special Maya cartoon technique personnel, and it is the attitude data about the turning joint of robot.The continuous variation of action is exactly the continuous variation of attitude data in fact.Attitude data is space six degree of freedom data: X coordinate, Y coordinate, Z coordinate, the rotation of X axle, the rotation of Y axle, the rotation of Z axle.But when the action of control robot; If with the six degree of freedom attitude data as control target; It is unusual complicated that the system that controls so will become, and control not only will solve the problem of control itself, also will spend great effort and be used to handle the gesture data in the 3d space.And handle this attitude data, also need know relevant frame for movement and mechanical dimension.

Summary of the invention

The technical problem that the present invention will solve is, to the complicated defective of the above-mentioned robot motion simulation control method of prior art, a kind of control method of simple robot motion emulation is provided.

The technical solution adopted for the present invention to solve the technical problems is: construct a kind of control method of robot motion emulation, comprising:

A. planning robot's action in Maya, with the turning joint that the produces robot attitude data at different time, said attitude data is space six degree of freedom data;

B. the attitude data of different time is converted into the displacement data of different time, and generate the curve of displacement and time relationship;

C. according to the displacement of the curve controlled turning joint of said displacement and time relationship.

In the control method of robot motion emulation of the present invention, between said step B and said step C, also comprise:

D. the curve to said displacement and time relationship carries out preliminary treatment.

In the control method of robot motion emulation of the present invention, said step D comprises:

Single order is level and smooth: set a maximum speed value, make the speed of the curve of displacement and time relationship after the processing be lower than maximum speed value;

Second order is level and smooth: set a maximum acceleration value, make the acceleration of the curve of displacement and time relationship after the processing be lower than maximum acceleration value;

Three rank are level and smooth: set a peak acceleration rate of change, make the acceleration of curve of displacement and time relationship after the processing continuous, and rate of acceleration change is lower than the peak acceleration rate of change.

In the control method of robot motion emulation of the present invention, said step C comprises:

Feedback element: the displacement of feedback turning joint;

Controlling unit: according to the curve of said displacement and time relationship, the Displacement Feedback that reaches turning joint is carried out proportion differential calculating, to generate controlled quentity controlled variable;

Output element: convert said controlled quentity controlled variable into voltage control signal, said voltage control signal is through the displacement of driving mechanisms control turning joint.

In the control method of robot motion emulation of the present invention, before said controlling unit, also comprise:

The parameter tuning of proportion differential: the parameter of the sine curve Comparative Examples differential that changes through given one-period is carried out on-line tuning, the shift motion of the corresponding turning joint of said sinusoidal amplitude.

In the control method of robot motion emulation of the present invention, said feedback element is:

Measurement is installed in the voltage of the electronic ruler on the said driving mechanism, and feeds back measured voltage, and measured voltage is relevant with the displacement of said driving mechanism.

In the control method of robot motion emulation of the present invention, said feedback element also comprises:

Whether the displacement of judging driving mechanism according to measured voltage is in protection domain.

In the control method of robot motion emulation of the present invention, in said output element, said voltage control signal is carried out following at least a processing:

The acceleration constraint: the variable quantity of control voltage control signal is lower than preset value;

Filtering Processing;

Spacing: the control voltage control signal is in preset range;

The terminal soft protection of driving mechanism is handled: voltage control signal is handled said driving mechanism is moved in travel range.

In the control method of robot motion emulation of the present invention; Said driving mechanism comprises electro-hydraulic proportional servo valve corresponding with said turning joint and oil cylinder; Said voltage control signal is flexible through electro-hydraulic proportional servo valve control oil cylinder, and then control turning joint is mobile.

The present invention also constructs a kind of control device of robot motion emulation, comprising:

The action planning module is used for the action the Maya planning robot, and with the turning joint that the produces robot attitude data at different time, said attitude data is space six degree of freedom data;

The displacement curve generation module is used for the attitude data of different time is converted into the displacement data of different time, and generates the curve of displacement and time relationship;

Control module is used for the displacement according to the curve controlled turning joint of said displacement and time relationship.

The technical scheme of embodiment of the present invention; Owing to convert the turning joint of robot the displacement data of turning joint at the attitude data of different time at different time; So the control robot motion is no longer relevant with attitude data and mechanical aspects; And become only relevantly with the shift value of different time, and then make control method simpler.

Description of drawings

Below in conjunction with accompanying drawing and embodiment the present invention is described further, in the accompanying drawing:

Figure 1A is the flow chart of the control method of the embodiment of the invention one robot motion emulation;

Figure 1B is the logic diagram of the control device of the embodiment of the invention one robot motion emulation;

Fig. 2 A is the flow chart of the control method of the embodiment of the invention three robot motion emulation;

Fig. 2 B is the logic diagram of the control device of the embodiment of the invention three robot motion emulation;

Fig. 3 is the logic diagram of the control module in the control device of the embodiment of the invention two robot motion emulation;

Fig. 4 A is the flow chart of feedback element in the control method of the embodiment of the invention four robot motion emulation;

Fig. 4 B is the flow chart of controlling unit in the control method of the embodiment of the invention four robot motion emulation;

Fig. 4 C is the flow chart of output element in the control method of the embodiment of the invention four robot motion emulation.

The specific embodiment

In order to make the object of the invention, technical scheme and advantage clearer,, the present invention is further specified below in conjunction with accompanying drawing and embodiment.Should be appreciated that specific embodiment described herein only in order to explanation the present invention, and be not used in qualification the present invention.

Shown in Figure 1A, in the flow chart of the control method embodiment one of robot motion emulation of the present invention, this control method comprises:

S100. planning robot's action in Maya, with the turning joint that the produces robot attitude data at different time, said attitude data is space six degree of freedom data;

S300. the attitude data of different time is converted into the displacement data of different time, and generate the curve of displacement and time relationship;

S500. according to the displacement of the curve controlled turning joint of said displacement and time relationship.

Shown in Figure 1B, in the logic diagram of the control device embodiment one of robot motion emulation of the present invention, this control device comprises action planning module 100, displacement curve generation module 300 and the control module 500 that links to each other successively.Wherein, action planning module 100 is used for the action the Maya planning robot, and with the turning joint that the produces robot attitude data at different time, said attitude data is space six degree of freedom data; Displacement curve generation module 300 is used for the attitude data of different time is converted into the displacement data of different time, and generates the curve of displacement and time relationship; Control module 500 is used for the displacement according to the curve controlled turning joint of said displacement and time relationship.

The technical scheme of embodiment of the present invention embodiment one; Owing to convert the turning joint of robot the displacement data of turning joint at the attitude data of different time at different time; So the control robot motion is no longer relevant with attitude data and mechanical aspects; And become only relevantly with the shift value of different time, and then make control method simpler.

Shown in Fig. 2 A, in the flow chart of the control method of the embodiment of the invention two robot motion emulation, this control method comprises step S100～step S500.Wherein, Step S100, step S300 and step S500 are identical with step S500 with step S100, the step S300 of the control method of the robot motion emulation of Shi Liyi; Do not do at this and to give unnecessary details; Different portions below only is described, between step S300 and step S500, is also comprised: step S400. carries out preliminary treatment to the curve of said displacement and time relationship.

With Fig. 2 A correspondingly; Fig. 2 B is the logic diagram of the control device of the embodiment of the invention two robot motion emulation, and this control device comprises action planning module 100, displacement curve generation module 300, displacement curve pre-processing module 400 and the control module 500 that links to each other successively.Wherein, Action planning module 100, displacement curve generation module 300 and control module 500 are identical with control module 500 with action planning module 100, the displacement curve generation module 300 of the control device of the robot motion emulation of Shi Liyi; Do not do at this and to give unnecessary details, below displacement curve pre-processing module 400 only is described.Displacement curve pre-processing module 400 is used for the curve of said displacement and time relationship is carried out preliminary treatment.

Because the displacement of the turning joint that is generated among the embodiment one and the curve of time relationship are not considered the problem of mechanical aspects; Might cause some action of robot maybe be too fast; The fierceness mechanical aspects does not reach to such an extent as to perhaps move too, thus be necessary displacement curve is carried out preliminary treatment, through the technical scheme of embodiment of the present invention embodiment two; The curve of displacement and time relationship can obtain level and smooth effect, makes the displacement curve after handling more can satisfy the requirement of machinery.

Preferably, in the embodiment of the invention two, the displacement curve preliminary treatment can comprise:

Single order is level and smooth: set a maximum speed value SpeedMax, make the speed of the curve of displacement and time relationship after the processing be lower than maximum speed value SpeedMax, this step can guarantee that turning joint can be not too fast;

Second order is level and smooth: set a maximum acceleration value AccelerateMax, make the acceleration of the curve of displacement and time relationship after the processing be lower than maximum acceleration value AccelerateMax, this step can guarantee that the action of turning joint is stressed can be not excessive;

Three rank are level and smooth: set a peak acceleration rate of change AkMax; Make the acceleration of curve of displacement and time relationship after the processing continuous; And rate of acceleration change is lower than peak acceleration rate of change AkMax; This step can guarantee that the stressed of turning joint is continuous, but and the flatness of safety action.

Explain that with an example contraposition moves the preprocess method of curve below, the data of establishing certain displacement curve are { S ₀, S ₁, S ₂, S ₃, S ₄S _N-2, S _N-1, S _n, each data time is spaced apart Δ t.

Speed is V, and acceleration is a.

One, single order is level and smooth

$V=\left|\frac{\mathrm{DS}}{\mathrm{Dt}}\right|\≤\mathrm{SpeedMax},$ Discrete processes does $V_k=\frac{|S_{k+1}-S_k|}{\mathrm{\Δ\; t}}\≤\mathrm{SpeedMax}$

$\mathrm{\Δt}+S_k\≤S_{k+1}\≤\mathrm{SpeedMax}*\mathrm{\Δt}+S_k$

If: V _k＞SpeedMax revises S _K+1=SpeedMax* Δ t+S _k

If: V _k＜-SpeedMax revises S _K+1=-SpeedMax* Δ t+S _k

Two, second order is level and smooth

$a=\left|\frac{\mathrm{DV}}{\mathrm{Dt}}\right|=\left|\frac{d^{2}S}{{\mathrm{Dt}}^2}\right|\≤\mathrm{AccelerateMax},$ Discrete processes does

$a_k=\frac{|V_k-V_{k-1}|}{\mathrm{\Δt}}=\frac{|\frac{S_{k+1}-S_k}{\mathrm{\Δt}}-\frac{S_k-S_{k-1}}{\mathrm{\Δt}}|}{\mathrm{\Δt}}=\frac{|S_{k+1}-{2S}_k+S_{k-1}|}{{\mathrm{\Δt}}^2}\≤\mathrm{AccelerateMax}$

$(-\mathrm{AccelerateMax}*{\mathrm{\Δt}}^2+S_{k+1}+S_{k-1})*\frac{1}{2}\≤S_k\≤(\mathrm{AccelerateMax}*{\mathrm{\Δt}}^2+S_{k+1}+S_{k-1})*\frac{1}{2}$

If: a _k＞AccelerateMax revises $S_k=(\mathrm{AccelerateMax}*{\mathrm{\Δ\; t}}^2+S_{k+1}+S_{k-1})*\frac{1}{2};$

If: a _k＜-AccelerateMax revises $S_k=(-\mathrm{AccelerateMax}*{\mathrm{\Δ\; t}}^2+S_{k+1}+S_{k-1})*\frac{1}{2};$

Three, three rank are level and smooth

$\mathrm{Jerk}=\left|\frac{\mathrm{Da}}{\mathrm{Dt}}\right|=\left|\frac{d^{3}S}{{\mathrm{Dt}}^3}\right|\≤\mathrm{AkMax},$ Discrete processes does

${\mathrm{Jerk}}_k=\frac{|a_k-a_{k-1}|}{\mathrm{\Δt}}=\frac{|S_{k+1}-{3S}_k+{3S}_{k-1}-S_{k-2}|}{{\mathrm{\Δt}}^3}\≤\mathrm{AkMax}$

$(-\mathrm{AkMax}*{\mathrm{\Δt}}^3+S_{k+1}+3*S_{k-1}-S_{k-2})*\frac{1}{3}\≤S_k\≤(\mathrm{AkMax}*{\mathrm{\Δt}}^3+S_{k+1}+3*S_{k-1}-S_{k-2})*\frac{1}{3}$

If: Jerk _k＞AkMax revises $S_k=(\mathrm{AkMax}*{\mathrm{\Δ\; t}}^3+S_{k+1}+3*S_{k-1}-S_{k-2})*\frac{1}{3};$

If: Jerk _k＜-AkMax revises $S_k=(-\mathrm{AkMax}*{\mathrm{\Δ\; t}}^3+S_{k+1}+3*S_{k-1}-S_{k-2})*\frac{1}{3}.$

The pretreated program of explanation displacement curve realizes below:

One, single order is level and smooth

Maximal rate is handled, and maximal rate is handled and only related to two points, when speed during greater than maximal rate, as long as revise the value that second people is ordered.Treatment step is following:

1, gets initial k=0;

2, calculate the speed of curve current point $V_k=\frac{S_{k+1}-S_k}{\mathrm{\Δ\; t}};$

If 3 V _k＞SpeedMax revises S _K+1=SpeedMax* Δ t+S _k

If 4 V _k＜-SpeedMax revises S _K+1=-SpeedMax* Δ t+S _k

5、k＝k+1；

If 6 k＜n forward step 2 to; If k==n finishes.

Two, second order is level and smooth

Peak acceleration is handled, and peak acceleration is handled and related to three points, when acceleration during greater than peak acceleration, revises the value of second point.Because the value of second point is modified, and causes the value of previous acceleration also to be affected, the value of previous acceleration is by the 0th, 1, and 2 three points determine.So, the value of having repaiied second point if current acceleration is excessive, next acceleration processing will return back to previous acceleration and handle again.Acceleration is because by three some decisions, three points have concavity and convexity in addition, and recessed is to quicken, and protruding is to slow down.Might change the concavity and convexity of virgin curve after the processing, might change the acceleration and deceleration of virgin curve after acceleration is handled in other words, making is to have become deceleration after the place processing of quickening originally, is to have become acceleration after the place processing of slowing down originally.Curve and the concavity and convexity of virgin curve after the processing are consistent.In processing procedure; In order to make processing procedure quicker; Reduce the number of times of handling; When running into when revising value a little above peak acceleration, be modified as the accekeration more a little bit smaller again to the value of point than peak acceleration, revise like this that current point might still meet the demands to the influence of the acceleration of previous point.Treatment step is following:

Virgin curve is calculated the concavity and convexity of each point, promptly calculate each point and be and quicken or slow down.If at the uniform velocity, just decide its concavity and convexity based on the distance that at the uniform velocity leaves former and later two nearest speed change points, if near from preceding speed change point, just consistent with the concavity and convexity of preceding speed change point, if it is speed change point is closely from the back, just consistent with the concavity and convexity of back speed change point.After handling like this, the concavity and convexity of virgin curve has just been known, in processing, will make the concavity and convexity of curve processing front and back constant.

Get initial k=1;

Calculate the curve current acceleration $a_k=\frac{S_{k+1}-2S_k+S_{k-1}}{{\mathrm{\Δ\; t}}^2};$

If a _k＞AccelerateMax, and the virgin curve current point is acceleration point (concavity),

Then revise

k=k-1, forward step 3 to;

If a _k＞AccelerateMax, and the virgin curve current point is deceleration point (convexity),

Then revise

k=k-1, forward step 3 to;

If a _k＜-AccelerateMax, and the virgin curve current point is deceleration point (convexity),

Revise

k=k-1, forward step 3 to;

If a _k＜-AccelerateMax, and the virgin curve current point is acceleration point (concavity),

Revise

k=k-1, forward step 3 to;

If-AccelerateMax≤a _k≤0, and the virgin curve current point is acceleration point (concavity),

Then revise $S_k=\frac{S_{k+1}+S_{k-1}}{2};$

If 0＜a _k≤AccelerateMax, and the virgin curve current point is deceleration point (convexity),

Then revise

7, k=k+1; If k＜n forwards step 3 to; If k==n finishes.

Three, three rank are level and smooth

The peak acceleration rate of change is handled and to be related to four points, when rate of acceleration change during greater than the peak acceleration rate of change, revises the value of the 3rd point.Because the value of the 3rd point has been revised, and causes the value of previous rate of acceleration change also to be affected, the value of previous rate of acceleration change is by the 0th, 1, and 2,3 four points determine.So, the value of having repaiied the 3rd point if current rate of acceleration change is excessive, next rate of acceleration change processing will return back to previous rate of acceleration change and handle again.In processing procedure; In order to make processing procedure quicker; Reduce the number of times of handling; When running into when revising value a little above the peak acceleration rate of change, be modified as the value more a little bit smaller again to the value of point than peak acceleration rate of change, revise like this that current point might still meet the demands to the influence of the rate of acceleration change of previous point.Treatment step is following:

Get initial value k=2;

Calculate the rate of acceleration change of current point ${\mathrm{Jerk}}_k=\frac{S_{k+1}-3S_k+{3S}_{k-1}-S_{k-2}}{{\mathrm{\Δ\; t}}^3};$

If Jerk _k＞AkMax then revises $S_k=(0.618*\mathrm{AkMax}*{\mathrm{\Δ\; t}}^3+S_{k+1}+3*S_{k-1}-S_{k-2})*\frac{1}{3},$

K=k-1 changes step 2;

If Jerk _k＜-AkMax then revises $S_k=(-0.618*\mathrm{AkMax}*{\mathrm{\Δ\; t}}^3+S_{k+1}+3*S_{k-1}-S_{k-2})*\frac{1}{3},$

K=k-1 changes step 2;

k＝k+1；

If k＜n-1 changes step 2; If k==n-1 finishes.

The displacement curve preliminary treatment can be carried out the maximal rate processing earlier, carries out peak acceleration again and handles, and carries out the peak acceleration rate of change at last and handles.In an example, maximal rate is provided by the mechanician, and peak acceleration is set at maximal rate * 4, and the peak acceleration rate of change is set at maximal rate * 50.

Fig. 3 is the logic diagram of the control module in the control device of the embodiment of the invention three robot motion emulation; Should be noted that; Embodiment three compares the control device of the robot motion emulation of embodiment one or embodiment two; Therefore action planning module, displacement curve generation module are identical, do not do at this and give unnecessary details, and below control module 500 only are described: this control module 500 comprises control module 510, output unit 520 and feedback unit 530.Wherein, feedback unit 530 is used to feed back the displacement of turning joint; Control module 510 is according to the curve of said displacement and time relationship, and the Displacement Feedback that reaches turning joint is carried out PID calculating, to generate controlled quentity controlled variable; Output ring element 520 is used for converting said controlled quentity controlled variable into voltage control signal, and said voltage control signal is through the displacement of driving mechanisms control turning joint.

Correspondingly, the step S500 in the control method of the embodiment of the invention three robot motion emulation comprises following link:

Feedback element: the displacement of feedback turning joint;

Controlling unit: according to the displacement of said turning joint and the curve of time relationship, the Displacement Feedback that reaches turning joint is carried out PID calculating, to generate controlled quentity controlled variable;

Output element: convert said controlled quentity controlled variable into voltage control signal, said voltage control signal is through the displacement of driving mechanisms control turning joint.

Preferably, above-mentioned driving mechanism can comprise and corresponding electro-hydraulic proportional servo valve and the oil cylinder of each turning joint of robot that said voltage control signal is flexible through electro-hydraulic proportional servo valve control oil cylinder, and then control turning joint is mobile.Certainly the driving mechanism among the present invention is not limited only to this, also can be motor, cylinder etc.

Preferably, feedback element is: measure and be installed in the voltage of the electronic ruler on the said driving mechanism, and feed back measured voltage, measured voltage is directly proportional with the displacement of said driving mechanism.

Preferably, feedback element also comprises:

Whether the displacement of judging driving mechanism according to measured voltage is in protection domain.

Preferably, in output element, said voltage control signal is carried out following at least a processing:

The acceleration constraint: the variable quantity of control voltage control signal is lower than preset value;

Filtering Processing;

Spacing: the control voltage control signal is in preset range;

The terminal soft protection of driving mechanism is handled: voltage control signal is handled said driving mechanism is moved in travel range.

Specify the operation principle and the flow process of explanation feedback element, controlling unit and output element below respectively:

One, feedback element

The displacement detecting of feedback element is to obtain through the voltage that measurement is installed in the electronic ruler on the oil cylinder, and electronic ruler is based on the principle of slide rheostat, two termination 5V voltages, and the voltage signal of tap is received on the AD data collecting card on the computer.The electronic ruler two ends are fixed on the cylinder wall, and tap terminals links to each other with the oil cylinder piston bar, and along with piston moves together, the length of electronic ruler is bigger than oil cylinder stroke, and the electronic ruler installation site should make electronic ruler that surplus is all arranged when oil cylinder is short and the longest.The voltage signal of feedback can be connected into positive logic, and in other words, during the oil cylinder elongation, feedback signal voltage increases; Oil cylinder shortens, and feedback signal voltage reduces.Through the positive and negative two ends of exchange electronic ruler 5V voltage, can change the positive and negative logic of feedback signal.

The detection principle of Displacement Feedback is described below: because Displacement Feedback is that (0～5V), the AD data collecting card is that the gain of setting is 0～10V (corresponding digital quantity is 0～4095) to voltage signal.Digital quantity to be converted into the long measure rice of actual physical meaning.Record oil cylinder feedback digital amount FeedbackRawMin the most in short-term, the feedback digital amount FeedbackRawMax when oil cylinder is the longest, oil cylinder stroke CylinderLength and current feedback digital amount FeedbackRaw.Then oil cylinder Displacement Feedback length (rice) is:

$\mathrm{FeedbackMeter}=\frac{\mathrm{FeedbackRaw}-\mathrm{FeedbackMin}}{\mathrm{FeedbackMax}-\mathrm{FeedbackMin}}*\mathrm{CylinderLength}$

After obtaining Displacement Feedback,, judge whether to be in the protection domain according to the terminal soft protection length BufferLength of the oil cylinder of setting.

Determination methods is following:

If FeedbackMeter＜BufferLength then is in the soft protection domain of oil cylinder near-end.

If FeedbackMeter＞CylinderLength-BufferLength then is in the soft protection domain of oil cylinder far-end.

The responsible AD data collecting card of the realization of program, the AD capture card of using in this instance is 32 passages, the frequency acquisition of maximum 100KS/s, 12 resolution ratio, plate carries 4KS FIFO, supports the interrupt mode programming.Closed-loop control will utilize feedback to control, the accuracy of feedback signal and the stable quality that will directly have influence on final control effect.The reliability of feedback signal mainly shows on the real-time to the accuracy of feedback voltage measurement and Measuring Time.Owing to used special AD capture card, so voltage measurement itself is very accurate, but feedback voltage is the voltage signal that contains noise, is only accurately through the feedback voltage signal behind the software filtering.Because measuring has time delay, be that it is few more, accurate more to delay time before how long so the real-time of Measuring Time mainly refers to the current data that measure.Data if only sample, time-delay is very short so, can ignore, real-time is very high, but accuracy will can not get assurance, because data can't be carried out Filtering Processing.So should want one group of data of continuous acquisition, then these group data are carried out filtering, real-time can decrease like this, but accuracy can be greatly improved.

The flow chart of the feedback element in the robot motion simulation control method of the embodiment of the invention four shown in Fig. 4 A; In this feedback element; First deposit data that collects of AD data collecting card is in the FIFO of integrated circuit board that controls oneself; When FIFO fills up half, will be sent to the data among the FIFO in the internal memory of computer, the user just can handle the data in the internal memory then.FIFO one will have 2K Samples, 2048 image data in the time of half-full.If the speed by prestissimo 100KS/s is gathered, to fill up half the required time be 2048/100000=0.02048 second=20.48 millisecond to FIFO so.These 2048 data that data are whole 32 passages, each passage has 2048/32=64 data, and these 64 data are the data that contain noise.Filtering method is following: these 64 data are carried out ascending the ordering; Data of size are as the current gained value of feedback of measuring in the middle of getting then; After converting the real displacement value to, be placed on this Displacement Feedback value in the internal memory, and per 20.48 milliseconds of renewals once.When other modules of program will be got value of feedback, can't come into contacts with, but directly get these the per 20.48 milliseconds of renewals Displacement Feedback value once in the internal memory with the AD card.Like this, feedback measured real-time property has 20.48 milliseconds time-delay, but data will be very steady, the interference phenomenon that can not occur fluctuating up and down.And after handling like this, module can be accomplished separate, when changing the AD card of other types into, only need write the program of feedback element module again, it goes without doing any modification of other modules.

Be the program that realizes feedback element below:

public：

Virtual double GetFeedback (); // get value of feedback, unit: rice

USHORT GetFeedbackRaw (); // get value of feedback, original figure amount 0～4096

Int GetPosState (); // fetch bit configuration state; Return 0: in the scope of normal position; 1: in near-end protective position scope; 2: in the distal embolic protection position range

Two, controlling unit

At first, the control algolithm of PID (PID) is following:

$u\left(t\right)=\mathrm{Kp}[e\left(t\right)+\frac{1}{\mathrm{Ti}}+e_0^t\left(t\right)\mathrm{dt}+\mathrm{Td}\frac{\mathrm{de}\left(t\right)}{\mathrm{dt}}]$

Discrete form is:

$u\left(k\right)=\mathrm{Kp}[e\left(k\right)+\frac{T}{\mathrm{Ti}}\underset{j=0}{\overset{K}{\mathrm{\Σ}}}e\left(j\right)+\frac{\mathrm{Td}}{T}[e\left(k\right)-e(k-1)\left]\right]$

$u\left(k\right)=\mathrm{Kpe}\left(k\right)+\mathrm{Kp}\frac{T}{\mathrm{Ti}}\underset{j=0}{\overset{K}{\mathrm{\Σ}}}e\left(j\right)+\mathrm{Kp}\frac{\mathrm{Td}}{T}[e\left(k\right)-e(k-1)]$

Order $\mathrm{Kp}=P,\mathrm{Kp}\frac{T}{\mathrm{Ti}}=I,\mathrm{Kp}\frac{\mathrm{Td}}{T}=D.$

Then $u\left(k\right)=\mathrm{Pe}\left(k\right)+I\underset{j=0}{\overset{K}{\mathrm{\Σ}}}e\left(j\right)+D[e\left(k\right)-e(k-1)]$

P is a proportionality coefficient, and I is an integral coefficient, and P is a differential coefficient.

Because primary requirement of action emulation is the stationarity of action, next is only accuracy.In this controlling unit, integral element can be eliminated static difference on principle, but this is as far as fixed set point control system.And action emulation is not a fixed set point control system, is servo-actuated control, and the control target is ceaselessly to change size, in servo-actuated control, adds integral element and not only can not eliminate static difference, can make system produce overshoot on the contrary, makes system be easy to generate concussion.So need not add integral element in the control method of robot motion emulation of the present invention, only control with PD (proportion differential).

In the flow chart of the feedback element in the robot motion simulation control method of the embodiment of the invention shown in Fig. 4 B four; Control cycle is in this example got 25 milliseconds; Cycle to be controlled is when arriving; Control module reads value of feedback FeedbackMeter, gets the displacement data SetValue of next action then; At last according to following formula calculation control amount u:

eNow＝SetValue-FeedbackMeter；

u＝eNow*P+(eNow-ePrev)*D；

ePrev＝eNow。

Be the program that realizes controlling unit below:

public：

Must call in following five functions in // each control cycle,

// input control position, unit: rice

// return the given controlling value in position of current control cycle.Rice

Double OnRunCurve (double posSet); // operation curve,

Double OnStandbyPos (double posSet); // running to ready position, ready position is exactly the position of first group of data of operation curve data, and operation curve must run to ready position and then beginning earlier.

Double OnStop (); // out of service

Double OnZeroPos (double posSet); // run to 0 position

Void OnPointCtrl (double dVoltage); // manually control, direct voltage output.

Three, output element

The main task of output element is to calculate control module to such an extent that controlled quentity controlled variable is converted into final voltage output, outputs on the electrohydraulic servo valve through the DA output card then.Method for transformation is: voltage output=controlled quentity controlled variable * K, and K is for transmitting proportionality coefficient, and its concrete meaning is: when giving electrohydraulic servo valve 1V voltage; The speed of service K mm/second of oil cylinder, this K value need not be very accurate, and estimation just can; Deviation greatly also has no relations very much, and the P parameter can be revised from action.In an example, K desirable 200.

The acceleration constraint is meant that acceleration can not be too fast, and just velocity variations can not be excessive.Because electro-hydraulic proportional servo valve voltage and flow are proportional.Voltage is big more, and flow is big more, and oil cylinder speed is fast more so.So as long as the variation of control magnitude of voltage just is equivalent to control the variation of the speed of oil cylinder.Acceleration constraint is exactly to control output voltage to make its variation can not be too greatly.The acceleration constraint has the constrained procedure of multiple different purposes.Two kinds of the way of restraint of nonlinear restriction and linear restriction only are described below:

1. nonlinear restriction:

Constraint function is got

wherein; F is the constraint factor constant, between the value 0～1.In fact, constrained procedure 1 is by changing constraint like minor function, V _k=V _K-1* F=V _K-2* F*F=......=V ₀* F ^k

If preceding 1 time is output as

, this output can only be at V so _kWith

Between change.Can know according to constraint function:

V _k＝V _k-1*F

$V_{k-1}=V_{k-2}*F\⇒V_{k-2}=\frac{V_{k-1}}{F}$

So,

The characteristics of this constrained procedure are that output voltage is high more, retrains more little; Output voltage is low more, retrains big more.Constraint factor is big more, retrains big more.This constrained procedure is applicable to that to require low speed steady, and the low speed velocity variations wants slow and the high speed velocity variations is wanted fast occasion.Speed is fast more, and the about speed of acceleration is more little; Speed is slow more, and the acceleration constraint is big more.If the constraint function by method 1 changes, drop to 0V to 10V, establish constraint factor and get 0.8, then changing function is V _k=10*0.8 ^k, when k will get infinity so, V _kJust become 0, all fall forever in other words less than 0V.So also will set a small voltage scope, make in the small voltage scope no longer to be tied, establish this small voltage scope for-LimV～+ LimV.So complete constrained procedure 1 can be expressed as as follows:

If: V _J-1＞+LimV or V _K-1＜-LimV, so $V_{k-1}*F\≤V_k\≤\frac{V_{k-1}}{F}$

If :-LimV≤V _K-1≤+LimV, so $\mathrm{Min}\{V_{k-1}*F,-\mathrm{LimV}\}\≤V_k\≤\mathrm{Max}\{\frac{V_{k-1}}{F},+\mathrm{LimV}\}$

2. linear restriction:

Constraint function is got V _k=V _K-1+ Δ V, Δ V is a confinement voltage, just gets.Actual this method is by changing constraint like minor function,

V _k＝V _k-1+ΔV＝V _k-2+2ΔV＝......＝V ₀+kΔV

If preceding 1 time is output as V _K-1, this output can only be at V so _kAnd V _K-2Between change.Can know according to constraint function

V _k＝V _k-1+ΔV

$V_{k-1}=V_{k-2}+\mathrm{\ΔV}\⇒V_{k-2}=V_{k-1}-\mathrm{\ΔV}$

So, V _K-1-Δ V≤this output voltage≤V _K-1+ Δ V

The characteristics of this constrained procedure are that the constraint size of whole variation process is the same, and the confinement voltage Δ V of setting is big more, and the acceleration constraint is more little, and velocity variations is fast more; V is more little for the confinement voltage Δ, and the acceleration constraint is big more, and velocity variations is slow more.

Should be noted that and above two kinds of constrained procedures only have been described, but the present invention is not restricted to this two kinds of the way of restraint.Can use the suitable acceleration constraint according to concrete control requirement.If do not require, also can not add the acceleration constraint.Acceleration constraint employing method 2 in this example, and Δ V gets 0.5V.

The adding of output filtering is to examine the appearance of filtering following situation: when oil cylinder moves under than higher speed; The change in displacement of oil cylinder is very fast; The minor variations of control voltage all can be easy to speed is changed to some extent with extraneous interference; Fluctuation appears in Displacement Feedback easily as a result, because PD control is based on the control of deviation, small up and down fluctuation has appearred in final output control voltage to be changed.This fluctuation is to be accompanied by whole action emulation control procedure, and output voltage is high more, and this fluctuation is big more.Be equivalent to control the component except containing in the output voltage, also contain the oscillator signal of fluctuation up and down, this oscillator signal amplitude is smaller, and the concussion frequency is higher than the control component, can get rid of it through exporting filtering.The method of output filtering is following: adopt smothing filtering, this output voltage values=(this calculate output voltage values+preceding 1 output voltage values+preceding 2 output voltage values+...+preceding n output voltage values)/(n+1).This calculates to such an extent that output voltage is big more, and n gets big more.Following the example of as follows of n in this example:

This calculate output voltage (volt)	n
		-2＜V＜＝2	0
2＜V＜=5 or-5＜V＜=-2	1
		5＜V＜=7 or-7＜V＜=-5	2
7＜V＜=10 or-10＜V＜=-7	3

Spacing is to instigate output voltage can not surpass certain limit.In this example, spacing voltage range is-10V～+ 10V, this also is the control voltage range of the electron hydraulic proportional servo used in this example.The following method of spacing employing:

If this calculate output voltage＞+10V, this output voltage=+ 10V;

If this calculate output voltage＜-10V, this output voltage=-10V.

The terminal protection of oil cylinder is an end of avoiding piston to knock oil cylinder in its running in order to protect oil cylinder, to make, and the bump of excessive velocities causes damage to oil cylinder and machinery easily.If oil cylinder is in proximal end, and oil cylinder also will then should make oil cylinder stop to proximal motion; If but oil cylinder will then need not stop oil cylinder to distal movement this moment, by normally moving.In like manner, oil cylinder is in distal tip, but also will then stop oil cylinder to distal movement, if to proximal motion, then by normally moving.The terminal defence program of oil cylinder adopts following method:

If be in oil cylinder near-end protection domain, and this calculate output voltage＜0, output voltage=0 then;

If be in oil cylinder distal embolic protection scope, and this calculate output voltage＞0, output voltage=0 then;

If be not in the terminal protection domain of oil cylinder, output voltage=this calculate output voltage.

In the flow chart of the output element in the robot motion simulation control method of the embodiment of the invention shown in Fig. 4 C four; At first; Convert said controlled quentity controlled variable into voltage control signal; Successively said voltage control signal is carried out acceleration constraint, output filtering and spacing processing then, the voltage control signal after will handling is at last sent into the DA output card.

Be the program that realizes output element below:

public：

Virtual void Output (double outputValue); // output, outputValue is that PID calculates gained output controlled quentity controlled variable

VoidVoltageOut (float voltage); // output, direct voltage output

protected：

Virtual float TransferFun (double outputValue); // transfer function according to the input controlled quentity controlled variable, obtains corresponding D A card output voltage.Return output voltage values.

The parameter tuning method of proportion differential is described below: the method for adjustment of general PD is through given step signal, adjusts based on the output response then.But in the present invention, and improper.If give step signal of electrohydraulic servo valve, will produce a very big impulsive force to system so, easily system is caused damage.So the PD parameter regulation means of using in this instance is not given step signal, but the sinusoidal signal of variation of given one-period.Through observing given and feedback, adjust the PD parameter.Can avoid the generation impacted so on the one hand, on the other hand, just because of given be the sinusoidal signal that changes, this also is the characteristics of servomechanism, through observing the follow-up capability of offset of sinusoidal signal, can reflect the running status and the performance of system largely.And this method of adjustment is online adjustment, while that is to say to move and adjust, adjusted parameter is applied in the control at once, so require the parameter of adjustment process slowly to change, the value of twice adjustment can not change too big.The program circuit of adjustment PD parameter is following:

The bigger sinusoidal signal (such as 20 seconds) of the given one-period of elder generation, amplitude is the full stroke of oil cylinder.In running, slowly transfer P greatly from little,, observe trace performance, up to satisfaction through the monitoring pattern interface of program.And then a little bit smaller sinusoidal signal (such as 10 seconds) of given one-period, adjust the P parameter again, up to satisfaction.So diminishing repeatedly of given sinusoidal signal cycle, till the fastest limit.Basically can meet the demands through adjusted P parameter so repeatedly.After the P parameter adjustment is good, controlled, and then the situation during according to actual motion with regard to carrying out servo-actuated, suitable adjustment D parameter, in general the adding of D parameter can make movement response rapider, if but D crosses conference is easy to generate impact.

The above is merely the preferred embodiments of the present invention, is not limited to the present invention, and for a person skilled in the art, the present invention can have various changes and variation.All within spirit of the present invention and principle, any modification of being done, be equal to replacement, improvement etc., all should be included within the claim scope of the present invention.

Claims (10)

1. the control method of a robot motion emulation is characterized in that, comprising:

A. planning robot's action in Maya, with the turning joint that the produces robot attitude data at different time, said attitude data is space six degree of freedom data;

B. the attitude data of different time is converted into the displacement data of different time, and generate the curve of displacement and time relationship;

C. according to the displacement of the curve controlled turning joint of said displacement and time relationship.

2. the control method of robot motion emulation according to claim 1 is characterized in that, between said step B and said step C, also comprises:

D. the curve to said displacement and time relationship carries out preliminary treatment.

3. the control method of robot motion emulation according to claim 2 is characterized in that, said step D comprises:

Single order is level and smooth: set a maximum speed value, make the speed of the curve of displacement and time relationship after the processing be lower than maximum speed value;

Second order is level and smooth: set a maximum acceleration value, make the acceleration of the curve of displacement and time relationship after the processing be lower than maximum acceleration value;

Three rank are level and smooth: set a peak acceleration rate of change, make the acceleration of curve of displacement and time relationship after the processing continuous, and rate of acceleration change is lower than the peak acceleration rate of change.

4. according to the control method of each described robot motion emulation of claim 1 to 3, it is characterized in that said step C comprises:

Feedback element: the displacement of feedback turning joint;

Controlling unit: according to the curve of said displacement and time relationship, the Displacement Feedback that reaches turning joint is carried out proportion differential calculating, to generate controlled quentity controlled variable;

Output element: convert said controlled quentity controlled variable into voltage control signal, said voltage control signal is through the displacement of driving mechanisms control turning joint.

5. the control method of robot motion emulation according to claim 4 is characterized in that, before said controlling unit, also comprises:

The parameter tuning of proportion differential: the parameter of the sine curve Comparative Examples differential that changes through given one-period is carried out on-line tuning, the shift motion of the corresponding turning joint of said sinusoidal amplitude.

6. the control method of robot motion emulation according to claim 4 is characterized in that, said feedback element is:

Measurement is installed in the voltage of the electronic ruler on the said driving mechanism, and feeds back measured voltage, and measured voltage is relevant with the displacement of said driving mechanism.

7. the control method of robot motion emulation according to claim 6 is characterized in that, said feedback element also comprises:

Whether the displacement of judging driving mechanism according to measured voltage is in protection domain.

8. the control method of robot motion emulation according to claim 7 is characterized in that, in said output element, said voltage control signal is carried out following at least a processing:

Acceleration Constraints Processing: voltage control signal handled making its variable quantity be lower than preset value;

Filtering Processing;

Spacing processing: voltage control signal handled making it in the predeterminated voltage scope;

The terminal soft protection of driving mechanism is handled: voltage control signal is handled said driving mechanism is moved in preset travel range.

9. the control method of robot motion emulation according to claim 1; It is characterized in that; Said driving mechanism comprises electro-hydraulic proportional servo valve corresponding with said turning joint and oil cylinder; Said voltage control signal is flexible through electro-hydraulic proportional servo valve control oil cylinder, and then control turning joint is mobile.

10. the control device of a robot motion emulation is characterized in that, comprising:

The action planning module is used for the action the Maya planning robot, and with the turning joint that the produces robot attitude data at different time, said attitude data is space six degree of freedom data;

The displacement curve generation module is used for the attitude data of different time is converted into the displacement data of different time, and generates the curve of displacement and time relationship;

Control module is used for the displacement according to the curve controlled turning joint of said displacement and time relationship.

Images