CN102328315B - 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 action simulation of robot and device

Technical field

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

Background technology

A kind of large robot is arranged in the future world project of theme park, and this large robot can make perform their routines on request, then coordinates the stunt such as other acousto-optic-electrics, gives a kind of brand-new experience of visitor.

The perform their routines that robot makes is to have carried out in advance action planning in Maya, the action of robot model in Maya is a continuous action animation, and the robot model in Maya is consistent with actual robot, so can require robot to make the action of the robot model in the full Maya of symbol.Action planning in 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, X-axis rotation, Y-axis rotation, Z axis rotation.But when the action of control, if using the six-degree-of-freedom posture data as controlling target, the control system complex that will become, control and not only will solve the problem of control itself so, also will spend great effort for the treatment of the gesture data in 3d space.And process this attitude data, also need to know relevant frame for movement and mechanical dimension.

Summary of the invention

The technical problem to be solved in the present invention is, for the defect of the above-mentioned action simulation of robot control method complexity of prior art, provides a kind of control method of simple action simulation of robot.

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

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

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

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

In the control method of action simulation of robot of the present invention, between described step B and described step C, also comprise:

D. the curve of described displacement and time relationship carried out to pretreatment.

In the control method of action simulation of robot of the present invention, described step D comprises:

Single order is level and smooth: set a maximum speed value, the speed of the displacement after making to process and the curve of time relationship is lower than maximum speed value;

Second order is level and smooth: set a maximum acceleration value, the acceleration of the displacement after making to process and the curve of time relationship is lower than maximum acceleration value;

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

In the control method of action simulation of robot of the present invention, described step C comprises:

Feedback element: the displacement of feedback turning joint;

Controlling unit: according to the curve of described 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: described controlled quentity controlled variable is converted to voltage control signal, and described voltage control signal is by the displacement of driving mechanisms control turning joint.

In the control method of action simulation of robot of the present invention, before described controlling unit, also comprise:

The parameter tuning of proportion differential: the parameter of the sine curve Comparative Examples differential changed by a given cycle is carried out on-line tuning, the shift motion of the corresponding turning joint of described sinusoidal amplitude.

In the control method of action simulation of robot of the present invention, described feedback element is:

Measurement is arranged on the voltage of the electronic ruler on described driving mechanism, and feeds back measured voltage, and measured voltage is relevant to the displacement of described driving mechanism.

In the control method of action simulation of robot of the present invention, described feedback element also comprises:

Judge that according to measured voltage the displacement of driving mechanism is whether in protection domain.

In the control method of action simulation of robot of the present invention, in described output element, described voltage control signal is carried out to following at least one processing:

Acceleration constraint: control the variable quantity of voltage control signal lower than preset value;

Filtering is processed;

Spacing: as to control voltage control signal in preset range;

The soft conservation treatment of driving mechanism end: voltage control signal is processed described driving mechanism is moved in stroke range.

In the control method of action simulation of robot of the present invention, described driving mechanism comprises the electro-hydraulic proportional servo valve corresponding with described turning joint and oil cylinder, described voltage control signal is controlled the flexible of oil cylinder by electro-hydraulic proportional servo valve, and then controls the movement of turning joint.

The present invention also constructs a kind of control device of action simulation of robot, comprising:

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

The displacement curve generation module, be converted to the displacement data of different time for the attitude data by different time, and generate the curve of displacement and time relationship;

Control module, for the displacement of the curve controlled turning joint according to described displacement and time relationship.

Implement technical scheme of the present invention, be converted to the displacement data of turning joint at different time due to the turning joint by robot at the attitude data of different time, so the control action 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.

The accompanying drawing explanation

Below in conjunction with drawings and Examples, the invention will be further described, in accompanying drawing:

Figure 1A is the flow chart of the control method of the embodiment of the present invention one action simulation of robot;

Figure 1B is the logic diagram of the control device of the embodiment of the present invention one action simulation of robot;

Fig. 2 A is the flow chart of the control method of the embodiment of the present invention three action simulation of robots;

Fig. 2 B is the logic diagram of the control device of the embodiment of the present invention three action simulation of robots;

Fig. 3 is the logic diagram of the control module in the control device of the embodiment of the present invention two action simulation of robots;

Fig. 4 A is the flow chart of feedback element in the control method of the embodiment of the present invention four action simulation of robots;

Fig. 4 B is the flow chart of controlling unit in the control method of the embodiment of the present invention four action simulation of robots;

Fig. 4 C is the flow chart of output element in the control method of the embodiment of the present invention four action simulation of robots.

The specific embodiment

In order to make purpose of the present invention, technical scheme and advantage clearer, below in conjunction with drawings and Examples, the present invention is described in further detail.Should be appreciated that specific embodiment described herein, only in order to explain the present invention, is not intended to limit the present invention.

As shown in Figure 1A, in the flow chart of the control method embodiment mono-of action simulation of robot of the present invention, this control method comprises:

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

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

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

As shown in Figure 1B, in the logic diagram of the control device embodiment mono-of action simulation of robot of the present invention, this control device comprises successively connected action planning module 100, displacement curve generation module 300 and control module 500.Wherein, action planning module 100 is for the action the Maya planning robot, the attitude data with the turning joint that produces robot at different time, and described attitude data is space six degree of freedom data; Displacement curve generation module 300 is converted to the displacement data of different time for the attitude data by different time, and generates the curve of displacement and time relationship; Control module 500 is for the displacement of the curve controlled turning joint according to described displacement and time relationship.

Implement the technical scheme of the embodiment of the present invention one, be converted to the displacement data of turning joint at different time due to the turning joint by robot at the attitude data of different time, so the control action 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.

As shown in Figure 2 A, in the flow chart of the control method of the embodiment of the present invention two action simulation of robots, this control method comprises step S100～step S500.Wherein, step S100, step S300 are identical with step S500 with step S100, the step S300 of the control method of the action simulation of robot of Shi Liyi with step S500, do not do and repeat at this, different parts below only is described, also comprises between step S300 and step S500: step S400. carries out pretreatment to the curve of described 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 present invention two action simulation of robots, and this control device comprises successively connected action planning module 100, displacement curve generation module 300, displacement curve pretreatment module 400 and control module 500.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 action simulation of robot of Shi Liyi, do not do and repeat at this, below displacement curve pretreatment module 400 only is described.Displacement curve pretreatment module 400 is carried out pretreatment for the curve to described displacement and time relationship.

Because the curve of the displacement of the turning joint generated in embodiment mono-and time relationship is not considered the problem of mechanical aspects, likely can cause some action of robot may be too fast, to such an extent as to the too fierceness mechanical aspects of perhaps moving does not reach, so be necessary displacement curve is carried out to pretreatment, by implementing the technical scheme of the embodiment of the present invention two, the curve of displacement and time relationship can obtain level and smooth effect, makes the displacement curve after processing more can meet mechanical requirement.

Preferably, in the embodiment of the present invention two, the displacement curve pretreatment can comprise:

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

Second order is level and smooth: set a maximum acceleration value AccelerateMax, the acceleration of the displacement after making to process and the curve of time relationship is lower than maximum acceleration value AccelerateMax, and 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, the acceleration of the displacement after making to process and the curve of time relationship is 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.

Below with an example, illustrate that contraposition moves the preprocess method of curve, 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 is $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, revise S _k+1=SpeedMax* Δ t+S _k;

If: V _k<-SpeedMax, revise 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|\&le;\mathrm{AccelerateMax},$ Discrete processes is

$a_k=\frac{|V_k-V_{k-1}|}{\mathrm{\&Delta;t}}=\frac{|\frac{S_{k+1}-S_k}{\mathrm{\&Delta;t}}-\frac{S_k-S_{k-1}}{\mathrm{\&Delta;t}}|}{\mathrm{\&Delta;t}}=\frac{|S_{k+1}-{2S}_k+S_{k-1}|}{{\mathrm{\&Delta;t}}^2}\&le;\mathrm{AccelerateMax}$

$(-\mathrm{AccelerateMax}*{\mathrm{\&Delta;t}}^2+S_{k+1}+S_{k-1})*\frac{1}{2}\&le;S_k\&le;(\mathrm{AccelerateMax}*{\mathrm{\&Delta;t}}^2+S_{k+1}+S_{k-1})*\frac{1}{2}$

If: a _k>AccelerateMax, revise $S_k=(\mathrm{AccelerateMax}*{\mathrm{\&Delta;t}}^2+S_{k+1}+S_{k-1})*\frac{1}{2};$

If: a _k<-AccelerateMax, revise $S_k=(-\mathrm{AccelerateMax}*{\mathrm{\&Delta;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|\&le;\mathrm{AkMax},$ Discrete processes is

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

$(-\mathrm{AkMax}*{\mathrm{\&Delta;t}}^3+S_{k+1}+3*S_{k-1}-S_{k-2})*\frac{1}{3}\&le;S_k\&le;(\mathrm{AkMax}*{\mathrm{\&Delta;t}}^3+S_{k+1}+3*S_{k-1}-S_{k-2})*\frac{1}{3}$

If: Jerk _k>AkMax, revise $S_k=(\mathrm{AkMax}*{\mathrm{\&Delta;t}}^3+S_{k+1}+3*S_{k-1}-S_{k-2})*\frac{1}{3};$

If: Jerk _k<-AkMax, revise $S_k=(-\mathrm{AkMax}*{\mathrm{\&Delta;t}}^3+S_{k+1}+3*S_{k-1}-S_{k-2})*\frac{1}{3}.$

The following describes the pretreated program of displacement curve realizes:

One, single order is level and smooth

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

1, get initial k=0;

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

If 3 V _k>SpeedMax, revise S _k+1=SpeedMax* Δ t+S _k;

If 4 V _k<-SpeedMax, revise S _k+1=-SpeedMax* Δ t+S _k;

5、k＝k+1；

If 6 k<n, forward step 2 to; If k==n, finish.

Two, second order is level and smooth

Peak acceleration is processed, and peak acceleration is processed and related to three points, when acceleration is greater than peak acceleration, revises the value of second point.Because the value of second point is modified, cause the value of previous acceleration also to be affected, the value of previous acceleration is by the 0th, and 1,2 three point determines.So, the value of if current acceleration is excessive, having repaiied second point, next acceleration processing will return back to previous acceleration and again process.Acceleration is because determined by three points in addition, and three points have concavity and convexity, and recessed is to accelerate, and protruding is to slow down.Likely changed the concavity and convexity of virgin curve after processing, acceleration likely can change the acceleration and deceleration of virgin curve after processing in other words, and making is to have become deceleration after the place processing of accelerating originally, is to have become acceleration after the place processing of slowing down originally.Curve after making so also will take measures to process and the concavity and convexity of virgin curve are consistent.In processing procedure, in order to make processing procedure quicker, reduce the number of times of processing, when running into over peak acceleration when the value of modifying point, to be modified as the value of point than peak acceleration a little bit smaller accekeration again, revise like this that the impact of the current acceleration on previous point likely still meets the demands.Treatment step is as follows:

Virgin curve is calculated to the concavity and convexity of each point, calculating each point is accelerate or slow down.If at the uniform velocity, just according to the distance of at the uniform velocity putting the speed change point nearest from former and later two, decide its concavity and convexity, if close to front speed change point, just consistent with the concavity and convexity of front speed change point, if close to rear speed change point, just consistent with the concavity and convexity of rear speed change point.After such processing, 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{\&Delta;t}}^2};$

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

Revise

k=k-1, forward step 3 to;

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

Revise

k=k-1, forward step 3 to;

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

Revise

k=k-1, forward step 3 to;

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

Revise

k=k-1, forward step 3 to;

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

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

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

Revise

7, k=k+1; If k<n, forward step 3 to; If k==n, finish.

Three, three rank are level and smooth

The peak acceleration rate of change is processed and is related to four points, when rate of acceleration change is 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, cause the value of previous rate of acceleration change also to be affected, the value of previous rate of acceleration change is by the 0th, and 1,2,3 four points determine.So, the value of if current rate of acceleration change is excessive, having repaiied the 3rd point, next rate of acceleration change processing will return back to previous rate of acceleration change and again process.In processing procedure, in order to make processing procedure quicker, reduce the number of times of processing, when running into over the peak acceleration rate of change when the value of modifying point, to be modified as the value of point than peak acceleration rate of change a little bit smaller value again, revise like this that the impact of the current rate of acceleration change on previous point likely still meets the demands.Treatment step is as follows:

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{\&Delta;t}}^3};$

If Jerk _k>AkMax, revise $S_k=(0.618*\mathrm{AkMax}*{\mathrm{\&Delta;t}}^3+S_{k+1}+3*S_{k-1}-S_{k-2})*\frac{1}{3},$

K=k-1, go to step 2;

If Jerk _k<-AkMax, revise $S_k=(-0.618*\mathrm{AkMax}*{\mathrm{\&Delta;t}}^3+S_{k+1}+3*S_{k-1}-S_{k-2})*\frac{1}{3},$

K=k-1, go to step 2;

k＝k+1；

If k<n-1, go to step 2; If k==n-1, finish.

The maximal rate processing can be first carried out in the displacement curve pretreatment, then carries out the peak acceleration processing, finally carries out the processing of peak acceleration rate of change.In an example, maximal rate is provided by the mechanician, and peak acceleration is set as maximal rate * 4, and the peak acceleration rate of change is set as maximal rate * 50.

Fig. 3 is the logic diagram of the control module in the control device of the embodiment of the present invention three action simulation of robots, should be noted that, embodiment tri-compares the control device of the action simulation of robot of embodiment mono-or embodiment bis-, action planning module, displacement curve generation module are identical, therefore do not do and repeat at this, below control module 500 only is described: this control module 500 comprises control module 510, output unit 520 and feedback unit 530.Wherein, feedback unit 530 is for feeding back the displacement of turning joint; Control module 510 is according to the curve of described 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 unit 520 is for described controlled quentity controlled variable is converted to voltage control signal, and described voltage control signal is by the displacement of driving mechanisms control turning joint.

Correspondingly, the step S500 in the control method of the embodiment of the present invention three action simulation of robots comprises following link:

Feedback element: the displacement of feedback turning joint;

Controlling unit: according to the displacement of described 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: described controlled quentity controlled variable is converted to voltage control signal, and described voltage control signal is by the displacement of driving mechanisms control turning joint.

Preferably, above-mentioned driving mechanism can comprise electro-hydraulic proportional servo valve and the oil cylinder corresponding with robot each turning joint, and described voltage control signal is controlled the flexible of oil cylinder by electro-hydraulic proportional servo valve, and then controls the movement of turning joint.Certainly the driving mechanism in the present invention is not limited only to this, also can be motor, cylinder etc.

Preferably, feedback element is: measure and be arranged on the voltage of the electronic ruler on described driving mechanism, and feed back measured voltage, measured voltage is directly proportional to the displacement of described driving mechanism.

Preferably, feedback element also comprises:

Judge that according to measured voltage the displacement of driving mechanism is whether in protection domain.

Preferably, in output element, described voltage control signal is carried out to following at least one processing:

Acceleration constraint: control the variable quantity of voltage control signal lower than preset value;

Filtering is processed;

Spacing: as to control voltage control signal in preset range;

The soft conservation treatment of driving mechanism end: voltage control signal is processed described driving mechanism is moved in stroke range.

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

One, feedback element

The displacement detecting of feedback element is that the voltage that is arranged on the electronic ruler on oil cylinder by measurement obtains, 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 computer.The electronic ruler two ends are fixed on cylinder wall, and tap terminals is connected with cylinder piston rod, and along with piston moves together, the length of electronic ruler is larger than oil cylinder stroke, and the electronic ruler installation site should make electronic ruler that surplus is arranged when oil cylinder is the shortest 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.By the positive and negative two ends of exchang electron chi 5V voltage, can change the positive and negative logic of feedback signal.

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

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

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

Determination methods is as follows:

If FeedbackMeter<BufferLength, in the soft protection domain of oil cylinder near-end.

If FeedbackMeter>CylinderLength-BufferLength, in the soft protection domain of oil cylinder far-end.

Program realize responsible AD data collecting card, the AD capture card of using in this example is 32 passages, the frequency acquisition of maximum 100KS/s, the resolution ratio of 12, plate carries 4KS FIFO, supports the interrupt mode programming.Closed-loop control will utilize feedback to be controlled, and the Stability and veracity of feedback signal will directly have influence on the quality of final control effect.The reliability of feedback signal is mainly manifested on the real-time to the accuracy of feedback voltage measurement and Measuring Time.Owing to having used special AD capture card, so voltage measurement itself is very accurate, but feedback voltage is containing noisy voltage signal, by the feedback voltage signal after software filtering, is only accurately.Because measurement has time delay, so the real-time of Measuring Time mainly refers to the current data that measure, be that time delay is fewer, more accurate before how long.The data if only sample, time delay is very short so, can ignore, real-time is very high, but accuracy will can not be guaranteed, because data can't be carried out the filtering processing.So should want one group of data of continuous acquisition, then these group data are carried out to filtering, real-time can decrease like this, but accuracy can be greatly improved.

The flow chart of the feedback element in the action simulation of robot control method of the embodiment of the present invention four as shown in Figure 4 A, in this feedback element, the first deposit data collecting of AD data collecting card is in the FIFO of board that controls oneself, when FIFO fills up half, will be sent to the data in FIFO in the internal memory of computer, then the user just can be processed the data in internal memory.To have 2K Samples when FIFO mono-is half-full, 2048 image data.If the speed by prestissimo 100KS/s is gathered, to fill up half 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 containing noisy data.Filtering method is as follows: these 64 data are carried out to ascending the sequence, then in the middle of getting, data of size are as current measurement gained value of feedback, after converting the real displacement value to, this Displacement Feedback value is placed in internal memory, and every 20.48 milliseconds of renewals once.When other modules of program will be got value of feedback, can't come into contacts with the AD card, but directly get these the every 20.48 milliseconds of renewals Displacement Feedback value once in internal memory.Like this, the real-time that feedback is measured has the time delay of 20.48 milliseconds, but data will be very steady, not there will be the interference phenomenon of fluctuation up and down.And, after processing like this, module can be accomplished separate, when changing the AD card of other types into, only need to again write the program of feedback element module, it goes without doing any modification of other modules.

Below the program that realizes feedback element:

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 to 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 as follows:

$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{\&Sigma;}}}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{\&Sigma;}}}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.$

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

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

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

In the flow chart of the feedback element in the action simulation of robot control method of the embodiment of the present invention four shown in Fig. 4 B, control cycle is in this example got 25 milliseconds, cycle to be controlled is while arriving, control module reads value of feedback FeedbackMeter, then gets the displacement data SetValue of next action; Finally according to following formula, calculate controlled quentity controlled variable u:

eNow＝SetValue-FeedbackMeter；

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

ePrev＝eNow。

Below the program that realizes controlling unit:

public：

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

// input control position, unit: rice

// return to 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 first run to ready position and then beginning.

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, then by the DA output card, outputs on electrohydraulic servo valve.Method for transformation is: Voltage-output=controlled quentity controlled variable * K, and K is for transmitting proportionality coefficient, and its concrete meaning is: while 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 refers to that acceleration can not be too fast, and namely velocity variations can not be excessive.Due to electro-hydraulic proportional servo valve voltage and flow proportional.Voltage is larger, and flow is larger, and oil cylinder speed is faster so.So, as long as control the variation of magnitude of voltage, just be 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 below only are described:

1. nonlinear restriction:

Constraint function is got

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

If first 1 time is output as

, this output can only be at V so _kwith between change.Known according to constraint function:

V _k＝V _k-1*F

$V_{k-1}=V_{k-2}*F\&DoubleRightArrow;V_{k-2}=\frac{V_{k-1}}{F}$

So,

The characteristics of this constrained procedure are that output voltage is higher, retrains less; Output voltage is lower, retrains larger.Constraint factor is larger, retrains larger.This constrained procedure is applicable 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 faster, and acceleration approximately speed is less; Speed is slower, and the acceleration constraint is larger.If the constraint function by method 1 changes, drop to 0V to 10V, establish constraint factor and get 0.8, changing function is V _k=10*0.8 ^k, when k will get infinity so, V _kjust become 0, forever all fall less than 0V in other words.So also will set a small voltage scope, make no longer to be tied in the small voltage scope, establish this small voltage scope for-LimV～+ LimV.So complete constrained procedure 1 can be expressed as follows:

If: V _j-1>+LimV or V _k-1<-LimV, so $V_{k-1}*F\&le;V_k\&le;\frac{V_{k-1}}{F}$

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

2. linear restriction:

Constraint function is got V _k=V _k-1+ Δ V, Δ V is confinement voltage, just gets.Actual this method is by change constraint as minor function,

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

If first 1 time is output as V _k-1, this output can only be at V so _kand V _k-2between change.Known according to constraint function

V _k＝V _k-1+ΔV

$V_{k-1}=V_{k-2}+\mathrm{\&Delta;V}\&DoubleRightArrow;V_{k-2}=V_{k-1}-\mathrm{\&Delta;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 change procedure is the same, and the confinement voltage Δ V of setting is larger, and the acceleration constraint is less, and velocity variations is faster; V is less for the confinement voltage Δ, and the acceleration constraint is larger, and velocity variations is slower.

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

Adding of output filtering is to examine the appearance of filter to following situation: when oil cylinder moves under higher speed, the change in displacement of oil cylinder is very fast, minor variations and the extraneous interference of controlling voltage all can be easy to speed is changed to some extent, fluctuation easily appears in Displacement Feedback as a result, because PD controls the control that is based on deviation, final output is controlled voltage and has been occurred that upper and lower small fluctuation changes.This fluctuation is to be accompanied by whole action emulation control procedure, and output voltage is higher, and this fluctuation is larger.Be equivalent to control component except containing in 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 controlling component, by output filtering, can get rid of it.The method of output filtering is as follows: adopt smothing filtering, this output voltage values=(this calculate output voltage values+front 1 output voltage values+front 2 output voltage values+...+front n output voltage values)/(n+1).This calculates to such an extent that output voltage is larger, and n gets larger.In this example n follow the example of as follows:

This calculates to obtain 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 is also 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 protection of oil cylinder end is in order to protect oil cylinder, makes in its running to avoid piston to knock the end of oil cylinder, and the shock of excessive velocities easily causes damage to oil cylinder and machinery.If oil cylinder is in proximal end, and oil cylinder also will move to near-end, should make oil cylinder stop; But if oil cylinder now will need not stop oil cylinder to distal movement, by normally moving.In like manner, oil cylinder is in distal tip, but also will stop oil cylinder to distal movement, if to the near-end motion, by normally moving.Oil cylinder end defence program adopts following method:

If in oil cylinder near-end protection domain, and this calculates to obtain output voltage<0, output voltage=0;

If in oil cylinder distal embolic protection scope, and this calculates to obtain output voltage>0, output voltage=0;

If not in oil cylinder end protection domain, output voltage=this calculates to obtain output voltage.

In the flow chart of the output element in the action simulation of robot control method of the embodiment of the present invention four shown in Fig. 4 C, at first, described controlled quentity controlled variable is converted to voltage control signal, then successively described voltage control signal is carried out to acceleration constraint, output filtering and spacing processing, finally the voltage control signal after processing is sent into to the DA output card.

Below the program that realizes output element:

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 control amount, obtain corresponding DA card output voltage.Return to output voltage values.

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

The larger sinusoidal signal (such as 20 seconds) of first given one-period, the full stroke that amplitude is oil cylinder.In running, P is slowly tuned up from little, by the monitoring pattern interface of program, observe trace performance, until satisfied.And then a little bit smaller sinusoidal signal (such as 10 seconds) of given one-period, then adjust the P parameter, until satisfied.So diminishing repeatedly of given sinusoidal signal cycle, until the fastest limit.P parameter after so repeatedly adjusting can meet the demands basically.After the P parameter adjustment is good, just can carry out servo-actuated control, and then the situation during according to actual motion, suitable adjustment D parameter, in general adding of D parameter can make movement response rapider, if but D crosses conference easily produces impact.

The foregoing is only the preferred embodiments of the present invention, be not limited to the present invention, for a person skilled in the art, the present invention can have various modifications and variations.Within the spirit and principles in the present invention all, any modification of doing, be equal to replacement, improvement etc., within all should being included in claim scope of the present invention.

Claims (5)

1. the control method of an action simulation of robot, is characterized in that, comprising:

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

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

C. the curve of described displacement and time relationship carried out to pretreatment, comprising:

Single order is level and smooth: set a maximum speed value, the speed of the displacement after making to process and the curve of time relationship is lower than maximum speed value;

Second order is level and smooth: set a maximum acceleration value, the acceleration of the displacement after making to process and the curve of time relationship is lower than maximum acceleration value;

Three rank are level and smooth: set a peak acceleration rate of change, the acceleration of the displacement after making to process and the curve of time relationship is continuous, and rate of acceleration change is lower than the peak acceleration rate of change;

D. according to the displacement of the curve controlled turning joint of described displacement and time relationship, comprising:

Feedback element: adopt the measurement of AD data collecting card to be arranged on the voltage of the electronic ruler on driving mechanism, and feed back measured voltage, measured voltage is relevant to the displacement of described driving mechanism;

Controlling unit: according to the curve of described 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: described controlled quentity controlled variable is converted to voltage control signal, and described voltage control signal is by the displacement of driving mechanisms control turning joint;

Wherein, before described controlling unit, also comprise the parameter tuning of proportion differential: the parameter of the sine curve Comparative Examples differential changed by a given cycle is carried out on-line tuning, the shift motion of the corresponding turning joint of described sinusoidal amplitude.

2. the control method of action simulation of robot according to claim 1, is characterized in that, described feedback element also comprises:

Judge that according to measured voltage the displacement of driving mechanism is whether in protection domain.

3. the control method of action simulation of robot according to claim 1, is characterized in that, in described output element, described voltage control signal carried out to following at least one processing:

The acceleration constraint is processed: voltage control signal is processed and made its variable quantity lower than preset value;

Filtering is processed;

Spacing processing: voltage control signal is processed and made it in the predeterminated voltage scope;

The soft conservation treatment of driving mechanism end: voltage control signal is processed described driving mechanism is moved in default stroke range.

4. the control method of action simulation of robot according to claim 1, it is characterized in that, described driving mechanism comprises the electro-hydraulic proportional servo valve corresponding with described turning joint and oil cylinder, described voltage control signal is controlled the flexible of oil cylinder by electro-hydraulic proportional servo valve, and then controls the movement of turning joint.

5. the control device of an action simulation of robot, is characterized in that, comprising:

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

The displacement curve generation module, be converted to the displacement data of different time for the attitude data by different time, and generate the curve of displacement and time relationship;

The displacement curve pretreatment module, carry out pretreatment for the curve to described displacement and time relationship, comprises the single order smoothing processing: set a maximum speed value, the speed of the displacement after making to process and the curve of time relationship is lower than maximum speed value; The second order smoothing processing: set a maximum acceleration value, the acceleration of the displacement after making to process and the curve of time relationship is lower than maximum acceleration value; Three rank smoothing processing: set a peak acceleration rate of change, the acceleration of the displacement after making to process and the curve of time relationship is continuous, and rate of acceleration change is lower than the peak acceleration rate of change;

Control module, for the displacement of the curve controlled turning joint according to described displacement and time relationship, described control module comprises control module, output unit and feedback unit; Wherein, described feedback unit is for adopting the measurement of AD data collecting card to be arranged on the voltage of the electronic ruler on driving mechanism, and feed back measured voltage, measured voltage is relevant to the displacement of described driving mechanism, described control module is according to the curve of described displacement and time relationship, and the Displacement Feedback of turning joint is carried out proportion differential calculating, to generate controlled quentity controlled variable, described output unit is for described controlled quentity controlled variable is converted to voltage control signal, and described voltage control signal is by the displacement of driving mechanisms control turning joint.

Images