CN101745916A - Parallel robot device with functions of fault tolerance and fault correction and fault tolerating and correcting method thereof - Google Patents

Abstract

The invention belongs to a parallel robot device with the functions of fault tolerance and fault correction and a fault tolerating and correcting method thereof. The device comprises a movable platform, a fixed platform and a driving leg thereof, a flexible rope connected with a displacement sensor and a tensioning spring in series, a transmission system and a control system containing a field programmable gate array (FPGA). The fault tolerating and correcting method comprises the following steps of state monitoring, fault diagnosis, fault positioning and fault tolerance and correction of a fault sensor. Because the flexible rope with the displacement sensor and the tensioning spring is additionally arranged between the centers of the movable platform and the fixed platform and an FPGA controller is adopted in the control system, the parallel robot device has the characteristics that on-line reconstruction and self maintenance can be carried out for the faults of a driving leg sensor, the parallel robot device has short reconstructing time, high speed and high reconstructing and self-maintaining efficiency of a system, and can ensure that a robot can normally work when the driving leg sensor generates the faults so that the working efficiency and the working and operating safety and reliability are effectively enhanced and the heavy losses caused by the sudden faults of the driving leg sensor can be avoided, and the like.

Description

With fault-tolerant and parallel robot device and fault-tolerant and error correction method thereof error correction

Technical field

The invention belongs to robot manufacturing technology field, particularly a kind of four-freedom parallel device people device with fault-tolerant and error correction reaches the fault-tolerant and error correction method at this robot sensor input fault.

Background technology

Realize accurate control to parallel robot position, speed, just must detect in real time these physical quantitys by high-precision sensor, and convert testing result to digital quantity, feed back to controller, by controller these data are handled, the result who handles controls servomotor as controlled quentity controlled variable, and then the motion of robot etc. is realized accurate closed-loop control.So detect and add that with sensor feedback element is the important component part of servomotor closed-loop control system.In application number is 00105935.1 patent documentation, " a kind of four-freedom parallel connection robot mechanism " disclosed, the moving platform of this parallel robot is connected with fixed platform by four branches (driving leg), form parallel closed loop structure, wherein two single dof mobility pairs are respectively contained in two branches, a spherical hinge; A single dof mobility pair is respectively contained in two other branch, two spherical hinges or a spherical hinge and a Hooke's hinge; The position of this type of parallel robot, speed (motion) rely on the sensor that contains drive unit (servomotor), band photoelectric encoder be located in each branch (bar leg) to cooperatively interact at interior transmission system and PID (PID) controller and realize.PID (PID) controller forms closed-loop control to position, speed etc. according to the detection information of position, velocity sensor.Though this invention has positive and negative easy, high rigidity, high accuracy, harmonic motion quality, high dynamic performance, the advantages of simple structure and simple of separating of motion; But because PID controller itself does not possess functions such as fault-tolerant, data reconstruction; Thereby existence lacks the ability of online self-regeneration, in case sensor in motion process breaks down, just will cause the sensor no-output or export the result incorrect when robot, light then cause the precision of position, speed control to reduce, heavy then cause parallel robot out of control, cause serious defectives such as security incident.

Summary of the invention

The objective of the invention is defective at the background technology existence, research and design is a kind of with fault-tolerant and parallel robot device and fault-tolerant and error correction method thereof error correction, so that when catastrophic discontinuityfailure appears in the driving leg sensor, realize fault-tolerant and automatic error correction, carry out online maintenance certainly by intrasystem switching and reconstruct, guarantee its operate as normal and operation, thereby reach the security and the reliability of effective raising parallel robot operation, effectively avoid causing purposes such as heavy losses because of the sensor catastrophic discontinuityfailure.

Solution of the present invention is on the basis of background technology four-degree-of-freedom 2RPS+2TPS type parallel robot device (mechanism), between the center of parallel robot motion platform center and fixed platform, set up a gentle rope, one end of gentle rope is connected in series a displacement transducer and makes gentle rope keep the spring of tensioning state, in the course of the work, to send into fault detect and isolated location by the measurement value sensor of four driving legs and the measured value of gentle rope sensor, and be used for the running status of parallel robot is monitored; In four driving leg sensors, there is a sensor fault to occur; after judging fault sensor; then utilize the spatial closed-chain mechanism constraint; again calculate the measured value of answering of this fault sensor by the detected value of its excess-three bar driving leg and gentle rope upper sensor, by the data reconstruction unit in field programmable gate array (FPGA) logic controller; answer measured value to substitute the improper value of fault sensor output this; and former detection (unusually) value of fault sensor is given up; thereby realize fault-tolerant and automatic error correction, guarantee the robot operate as normal sensor fault.Therefore, parallel robot device of the present invention comprises moving platform, fixed platform (frame) and two platforms are flexibly connected to form four driving legs (branch) of parallel closed loop structure, wherein two driving legs respectively contain two single dof mobility pairs, a spherical hinge, two other driving leg respectively contains single dof mobility secondary and two spherical hinges or a spherical hinge and a Hooke's hinge, be located at the sensor that contains on each driving leg, servo-driver, servomotor is in interior transmission system, and be connected control system with each transmission system with the control robot motion, key is also to be provided with a gentle rope between the center of moving platform center and fixed platform, with the fixed platform tie point outside should gentle rope on, also be serially connected with the displacement transducer that is used to monitor variable in distance between two centers and make gentle rope be the spring of tensioning state, control system is then for containing the control system of field programmable gate array (FPGA) logic controller; One end and the moving platform center fixation of gentle rope, the other end then can relative slidingtype connects and is being anchored on the fixed platform behind serial connection displacement transducer and the tensioning spring on the gentle rope in the tie point outside again with the center of fixed platform, this displacement transducer also by holding wire be connected with controller, with the transmission monitoring information.

The above contains the control system of field programmable gate array (FPGA) logic controller, its control system comprises flash memories (FLASH), random-access memory (ram) and be attached thereto contain centralized control unit, be used for the communication unit that is connected with PC, be used for the interpolation operation unit that interpolation is calculated, PID control module as PID control, be used to handle the counting unit of feedback signal, be used for fault detect and isolated location that the operational factor that feedback parameter is corresponding with the input flash memories compares, this fault detect value is carried out the buffer cell of isolation processing, and be used to calculate fault sensor and answer the data reconstruction unit of measured value at interior field programmable gate array (FPGA) logic controller.And described gentle rope is at the elastic deformation amount of length direction enough little metal or the gentle rope of non-metal kind; Then colluding work by latch closure or ring with the connection at fixed platform center can connect by relative slidingtype.

And the fault-tolerant and error correction method in the parallel robot device course of work of the present invention comprises:

A. condition monitoring and fault diagnosis: at first according to the motion requirement of parallel robot, pass through trajectory planning, in the flash memories (FLASH) with its each bar driving leg length, velocity varies with time operational factor input control system, as the underlying parameter of driving leg length, speed, after this parameter value while interpolation operation cell processing in logic controller the result is sent into the PID control module and be used for the servomotor in each driving leg is controlled; Article four, the online detected value one tunnel of driving leg sensor and gentle rope sensor carries out PID control through buffer cell input PID control module to servomotor, and another road is imported random-access memory (ram), carried out On-line Control and monitoring with the operation conditions to parallel robot as detected value under the control of centralized control unit; Can calculate the calculated value of middle gentle rope sensor according to the restriction relation of space parallel mechanism by four driving leg sensor detected values, and relatively with the actual detected value of the calculated value of this gentle rope sensor and gentle rope sensor, if deviate has between the two exceeded setting range, then showing has the sensor of a driving leg fault to occur;

B. fault location: fault detect and shielding system compare the underlying parameter of driving leg sensor this moment and the real output value of driving leg sensor, if a certain driving leg deviate has between the two exceeded setting range, judge that then this driving leg sensor is a fault sensor;

C. the fault-tolerant and error correction of fault sensor: after determining fault sensor by the B step, promptly by the detected value of the detected value of other three driving leg sensors and middle gentle rope sensor by the data reconstruction unit in the FPGA motion controller, calculate the measured value of answering of fault sensor, and under the control of detection and isolated location, the fault detect value is abandoned by buffer, answer measured value to replace with this, be used for the pairing servomotor of fault sensor is controlled, thereby realization is to the fault-tolerant and error correction of fault sensor.

Described fault sensor comprises that the fault of driving leg sensor own or its circuit transmission fault or sensor probe are because of output unusually due to the physical obstacle.

The parallel robot device of the fault-tolerant and error correction of band of the present invention is owing to be connected a gentle rope between the center of the motion platform center of traditional four-degree-of-freedom 2RPS+2TPS type parallel robot device and fixed platform, gentle rope one end is connected in series a displacement transducer and makes gentle rope keep the spring of tensioning state, gentle rope is not interfered for the motion of parallel robot, and the redundancy that sensor provided that connects on it is used to fault detect and isolation when the parallel robot operate as normal; But it is fault-tolerant then to be to adopt field programmable gate array logic (FPGA) controller with computation capability and dynamic restructuring characteristics in control system with error correction method, to parallel robot operation carrying out dynamic monitoring; When parallel robot driving leg sensor breaks down, then utilize all the other operate as normal sensors measured value, carry out online data reconstruct automatically by reconfiguring of its internal logic resource, to guarantee the normal operation of system and to repair.Thereby; robot device of the present invention is in the course of the work when driving leg sensor generation catastrophic discontinuityfailure; can carry out online from maintenance; to the fault-tolerant and automatic error correction of sensor fault; and reconstitution time is short; speed is fast; the reconstruct efficient height of system; guarantee robot operate as normal and reparation in time when the sensor of driving leg breaks down; thereby effectively improved the operating efficiency of parallel robot and security, the reliability of work and operation, can effectively avoid causing characteristics such as heavy losses because of the sensor catastrophic discontinuityfailure.If having overcome the sensor of background technology existence in a single day breaks down, to cause sensor no-output or output result incorrect, light then cause the precision of position, speed control to reduce, heavy then cause the out of control of parallel robot even cause the disadvantages such as generation of major accident.

Description of drawings

Fig. 1. be parallel robot device structural representation of the present invention;

Fig. 2. for parallel robot device control system of the present invention reaches and PC and each driving leg annexation schematic diagram;

Fig. 3. be A among embodiment of the present invention and Fig. 1 ₁B ₁B ₃A ₃The plane parallel mechanism schematic diagram of the motion equivalence of loop.

Among the figure: 1. moving platform, 2. fixed platform, 2-1. latch closure, 3. driving leg, 3-1 (l ₁), 3-2 (l ₂), 3-3 (l ₃), 3-4 (l ₄): (being respectively the 1st, 2,3,4 driving legs), 4 (l ₅). gentle rope, 4-1. displacement transducer, 4-2. tensioning spring, 5 (O ₁P). pointer, 6. flash memories (FLASH), 7. random-access memory (ram), 8. field programmable gate array (FPGA) logic controller, 9.PC machine, I. digital to analog converter, II. servo-driver, III. servomotor, IV. driving leg sensor; A ₁, A ₃: revolute pair, A ₂, A ₄: Hooke's hinge or ball pivot, B ₁, B ₂, B ₃, B ₄: ball pivot.

The specific embodiment

In the control system of present embodiment: it is the fpga logic device of SPARTAN-3 and by the reconfiguring of internal logic resource that fpga logic controller 8 adopts models, forms and comprises centralized control unit, communication unit, interpolation operation unit, PID control module, data reconstruction unit, fault detect and isolated location, counting unit, buffer cell at interior controller; Dodging reservoir (FLASH) 6 models is Am29LV160D, it is the Synchronous Dynamic Random Access Memory of HY57V641620HG that random-access memory (ram) 7 adopts model, digital to analog converter I model is AD7564, servomotor III model is that MSMA042A1G, supporting with it servo-driver II model are MSDA043A1A, and it is the photoelectric encoder of CN65M121759 that driving leg sensor IV adopts model.

Present embodiment is to the fault-tolerant and automatic error correction method of sensor fault, being example in the parallel robot simulation run: the parallel robot structural parameters are as follows: fixed platform 2 and moving platform 1 all are square layout, its external radius of a circle is respectively R=175mm, r=100mm, moving platform 1 center O ₁Point is to fixed platform (A ₁, A ₂, A ₃, A ₄) distance h=350mm, in the center fixation of moving platform 1 pointer 5 (O are arranged ₁P), pointer is perpendicular to moving platform 1, pointer 5 (O ₁P) long is 100mm; Gentle rope 4 (l ₅) what select for use is steel wire rope, steel wire rope has changed trend by annulus 2-1, and it is grating chi sensor and the spring 4-2 of CN63M/GXC that the steel wire rope other end connects model, and spring 4-2 makes gentle rope 4 (l ₅) be in tensioning state all the time; Planning pointer 5 (O ₁P) do uniform circular motion, O around the Z of fixed platform 2 axle ₁The angle theta of P and Z axle=45 °, the period of motion is 360 seconds.

Its method is as follows:

A. at first according to above-mentioned motion requirement, by PC 9 by the flash memories 6 of trajectory planning with the time dependent operational factor input control system of each bar driving leg length, send into the PID control module after transferring to the interpolation operation cell processing simultaneously as the underlying parameter of driving leg length, its output control signal is controlled the operation of servomotor III by servo-driver II through Digital To Analog Convert I conversion back; Detect the value of feedback of gained by each sensor IV, after input counting unit is handled, send into the PID control module and be used for FEEDBACK CONTROL to servomotor III, simultaneously, this feedback data is deposited in the random access memory 7;

B. during second, introduce sudden error (output valve as sensor suddenly disappear) at sensor IV at t=115, fault detect and isolated location calculate gentle rope 4 (l by the detected value of four driving leg length and according to the restriction relation of space parallel mechanism ₅) length calculation value between fixed platform 2 and moving platform 1 center, the actual detected value of this section gentle rope length calculation value and the gentle Suo Changdu of this section is compared, if deviate has exceeded setting range, then showing has the sensor of a driving leg fault to occur; Fault detect and shielding system with this constantly the real output value of underlying parameter and the driving leg sensor of the feedback in the random access memory 7 of the driving leg length in flash memories 6 compare, this moment is owing to driving leg 3-4 (l ₄) between the two deviation exceeded setting range, so judge driving leg 3-4 (l ₄) sensor is fault sensor;

C. detecting and orienting driving leg 3-4 (l by the B step ₄) when sensor breaks down, promptly send isolated instructions to buffer unit by fault detect and isolated location, simultaneously, centralized control unit log-on data reconfiguration unit carries out data reconstruction to be handled, and present embodiment data reconstruction unit carries out data reconstruction as follows automatically:

Set up coordinate system as shown in Figure 1, coordinate system is set as follows: base coordinate is the center O point that the origin of coordinates of R is positioned at fixed platform, and X-axis is chosen for OA ₃Direction, the Z axle is chosen for perpendicular to plane A ₁A ₂A ₃A ₄Direction; The origin of coordinates of moving coordinate system R ' is positioned at the center O of moving platform ₁Point, X ' axle is chosen for O ₁B ₃Direction, Z ' axle is chosen for perpendicular to plane B ₁B ₂B ₃B ₄Direction.

Parallel manipulator philtrum A shown in Figure 1 ₁B ₁B ₃A ₃The motion equivalence of the motion of loop and plane parallel mechanism shown in Figure 3, moving platform upper link point B ₁, B ₃At base coordinate is that coordinate among the R can be expressed as:

B ₁＝(l ₅cosθ ₁-rcosθ ₂，0，l ₅sinθ ₁-rsinθ ₂) ^T(1)

B ₃＝(l ₅cosθ ₁+rcosθ ₂，0，l ₅sinθ ₁+rsinθ ₂) ^T

Fixed platform upper link point A ₁, A ₃At base coordinate is that coordinate among the R can be expressed as:

A ₁＝(-R，0，0) ^T，A ₃＝(R，0，0) ^T (2)

The length l of driving leg _iCan use hinge point A _i, B _iCoordinate show, that is:

$\left\{\begin{array}{c}{(l_5{c}_1-{\mathrm{<figure-callout\; id="2"\; label="rc"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">rc</figure-callout>}}_2+R)}^2+{(l_5{s}_1-{\mathrm{rs}}_2)}^2=l_1^2\\ {(l_5{c}_1+{\mathrm{rc}}_2-R)}^2+{(l_5{s}_1+{\mathrm{rs}}_2)}^2={\mathrm{<figure-callout\; id="3"\; label="l"\; filenames="H2009102634316E00021.png"\; state="\{\{state\}\}">l</figure-callout>}}_3^2\end{array}\right.---\left(3\right)$

$\left\{\begin{array}{c}{\mathrm{<figure-callout\; id="5"\; label="l"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">l</figure-callout>}}_5^2+{\mathrm{<figure-callout\; id="2"\; label="r"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">r</figure-callout>}}^2+R^2-l_1^2-2{\mathrm{<figure-callout\; id="5"\; label="l"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">l</figure-callout>}}_5{\mathrm{rc}}_1{c}_2-2{\mathrm{<figure-callout\; id="5"\; label="l"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">l</figure-callout>}}_5{\mathrm{rs}}_1{\mathrm{<figure-callout\; id="2"\; label="s"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">s</figure-callout>}}_2+2{\mathrm{<figure-callout\; id="5"\; label="l"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">l</figure-callout>}}_5{\mathrm{Rc}}_1-2\mathrm{<figure-callout\; id="2"\; label="r\; Rc"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">r</figure-callout>}{\mathrm{Rc}}_{}{}_2=0\\ {\mathrm{<figure-callout\; id="5"\; label="l"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">l</figure-callout>}}_5^2+{\mathrm{<figure-callout\; id="2"\; label="r"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">r</figure-callout>}}^2+R^2-{\mathrm{<figure-callout\; id="3"\; label="l"\; filenames="H2009102634316E00021.png"\; state="\{\{state\}\}">l</figure-callout>}}_3^2+2{\mathrm{<figure-callout\; id="5"\; label="l"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">l</figure-callout>}}_5{\mathrm{rc}}_1{\mathrm{<figure-callout\; id="2"\; label="c"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">c</figure-callout>}}_2+2{\mathrm{<figure-callout\; id="5"\; label="l"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">l</figure-callout>}}_5{\mathrm{rs}}_1{s}_2-2{\mathrm{<figure-callout\; id="5"\; label="l"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">l</figure-callout>}}_5{\mathrm{Rc}}_1-2\mathrm{<figure-callout\; id="2"\; label="r\; Rc"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">r</figure-callout>}{\mathrm{Rc}}_{}{}_2=0\end{array}\right.---\left(4\right)$

(4a)+(4b), arrangement can get:

${\mathrm{<figure-callout\; id="2"\; label="c"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">c</figure-callout>}}_2=({2\mathrm{<figure-callout\; id="5"\; label="l"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">l</figure-callout>}}_5^2+2{\mathrm{<figure-callout\; id="2"\; label="r"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">r</figure-callout>}}^2+2R^2-l_1^2-{\mathrm{<figure-callout\; id="3"\; label="l"\; filenames="H2009102634316E00021.png"\; state="\{\{state\}\}">l</figure-callout>}}_3^2)/4\mathrm{rR}---\left(5\right)$

Can solve θ by following formula ₂Value, it should be noted that θ ₂There are two to separate.With θ ₂Value substitution formula (4a), can get:

$(-2{\mathrm{<figure-callout\; id="5"\; label="l"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">l</figure-callout>}}_5{\mathrm{<figure-callout\; id="2"\; label="rc"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">rc</figure-callout>}}_2+2l_{5}R)c_1+-2{\mathrm{<figure-callout\; id="5"\; label="l"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">l</figure-callout>}}_5{\mathrm{rs}}_2{s}_1+(-2\mathrm{<figure-callout\; id="2"\; label="r\; Rc"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">r</figure-callout>}{\mathrm{Rc}}_{}{}_2+{\mathrm{<figure-callout\; id="5"\; label="l"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">l</figure-callout>}}_5^2+{\mathrm{<figure-callout\; id="2"\; label="r"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">r</figure-callout>}}^2+R^2-l_1^2)=0---\left(6\right)$

Can solve θ by following formula ₁Value, θ ₁There are two to separate.

Moving platform and its coordinate system R ' that is connected mutually are that the attitude of R can become behaviour by twice following rotation and obtains with respect to base coordinate: (1) is around Y-axis anglec of rotation θ ₂, (2) are around X ' axle (being obtained by rotation transformation 1) anglec of rotation θ ₃So rotation transformation matrix R can be expressed as:

$R=R(y,{\mathrm{\&theta;}}_2)R(x^{\&prime;},{\mathrm{\&theta;}}_3)=\left[\begin{array}{ccc}{\mathrm{<figure-callout\; id="2"\; label="c"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">c</figure-callout>}}_2& 0& {\mathrm{<figure-callout\; id="2"\; label="s"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">s</figure-callout>}}_2\\ 0& 1& 0\\ -{\mathrm{<figure-callout\; id="2"\; label="s"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">s</figure-callout>}}_2& 0& {\mathrm{<figure-callout\; id="2"\; label="c"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">c</figure-callout>}}_2\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& c_3& -{\mathrm{<figure-callout\; id="3"\; label="s"\; filenames="H2009102634316E00021.png"\; state="\{\{state\}\}">s</figure-callout>}}_3\\ 0& s_3& {\mathrm{<figure-callout\; id="3"\; label="c"\; filenames="H2009102634316E00021.png"\; state="\{\{state\}\}">c</figure-callout>}}_3\end{array}\right]$

$=\left[\begin{array}{ccc}{c}_2& s_2{s}_3& s_2{\mathrm{<figure-callout\; id="3"\; label="c"\; filenames="H2009102634316E00021.png"\; state="\{\{state\}\}">c</figure-callout>}}_3\\ 0& c_3& -s_3\\ -s_2& c_2{s}_3& c_2{\mathrm{<figure-callout\; id="3"\; label="c"\; filenames="H2009102634316E00021.png"\; state="\{\{state\}\}">c</figure-callout>}}_3\end{array}\right]$

Moving platform upper link point B ₂At base coordinate is that coordinate among the R can be expressed as:

${\mathrm{<figure-callout\; id="2"\; label="B"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">B</figure-callout>}}_2=\left[\begin{array}{c}{l}_5{c}_1\\ 0\\ l_5{s}_1\end{array}\right]+\left[\begin{array}{ccc}{c}_2& s_2{s}_3& s_2{\mathrm{<figure-callout\; id="3"\; label="c"\; filenames="H2009102634316E00021.png"\; state="\{\{state\}\}">c</figure-callout>}}_3\\ 0& c_3& -s_3\\ -s_2& c_2{s}_3& c_3{\mathrm{<figure-callout\; id="3"\; label="c"\; filenames="H2009102634316E00021.png"\; state="\{\{state\}\}">c</figure-callout>}}_3\end{array}\right]\left[\begin{array}{c}0\\ -\mathrm{<figure-callout\; id="0"\; label="r"\; filenames="H2009102634316E00011.png,H2009102634316E00022.png"\; state="\{\{state\}\}">r</figure-callout>}\\ 0\end{array}\right]=\left[\begin{array}{c}{l}_5{c}_1-{\mathrm{rs}}_2{s}_3\\ -{\mathrm{rc}}_3\\ l_5{s}_1-{\mathrm{rc}}_2{s}_3\end{array}\right]---\left(7\right)$

The length l of driving leg ₂Can use hinge point A ₂, B ₂Coordinate show the constraint equation of Here it is mechanism:

${(l_5{c}_1-{\mathrm{rs}}_2{s}_3)}^2+{(-{\mathrm{<figure-callout\; id="3"\; label="rc"\; filenames="H2009102634316E00021.png"\; state="\{\{state\}\}">rc</figure-callout>}}_3+R)}^2+{(l_5{s}_1-{\mathrm{rc}}_2{s}_3)}^2={\mathrm{<figure-callout\; id="2"\; label="l"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">l</figure-callout>}}_2^2$

(8)

Can get after the following formula arrangement:

$(-2{\mathrm{<figure-callout\; id="5"\; label="l"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">l</figure-callout>}}_5{\mathrm{rc}}_1{s}_2-2{\mathrm{<figure-callout\; id="5"\; label="l"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">l</figure-callout>}}_5{\mathrm{rs}}_1{c}_2)s_3-2{\mathrm{<figure-callout\; id="3"\; label="rRc"\; filenames="H2009102634316E00021.png"\; state="\{\{state\}\}">rRc</figure-callout>}}_3+({\mathrm{<figure-callout\; id="5"\; label="l"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">l</figure-callout>}}_5^2+{\mathrm{<figure-callout\; id="2"\; label="r"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">r</figure-callout>}}^2+R^2-{\mathrm{<figure-callout\; id="2"\; label="l"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">l</figure-callout>}}_2^2)=0---\left(9\right)$

Can solve θ by following formula ₃Value, θ ₃There are two to separate.

Moving platform upper link point B ₄At base coordinate is that coordinates table among the R is shown:

${\mathrm{<figure-callout\; id="4"\; label="B"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">B</figure-callout>}}_4=\left[\begin{array}{c}{l}_5{c}_1\\ 0\\ l_5{s}_1\end{array}\right]+\left[\begin{array}{ccc}{c}_2& s_2{s}_3& s_2{\mathrm{<figure-callout\; id="3"\; label="c"\; filenames="H2009102634316E00021.png"\; state="\{\{state\}\}">c</figure-callout>}}_3\\ 0& c_3& -s_3\\ -s_2& c_2{s}_3& c_2{\mathrm{<figure-callout\; id="3"\; label="c"\; filenames="H2009102634316E00021.png"\; state="\{\{state\}\}">c</figure-callout>}}_3\end{array}\right]\left[\begin{array}{c}0\\ \mathrm{<figure-callout\; id="0"\; label="r"\; filenames="H2009102634316E00011.png,H2009102634316E00022.png"\; state="\{\{state\}\}">r</figure-callout>}\\ 0\end{array}\right]=\left[\begin{array}{c}{l}_5{c}_1+{\mathrm{rs}}_2{\mathrm{<figure-callout\; id="3"\; label="s"\; filenames="H2009102634316E00021.png"\; state="\{\{state\}\}">s</figure-callout>}}_3\\ {\mathrm{rc}}_3\\ l_5{s}_1+{\mathrm{rc}}_2{s}_3\end{array}\right]---\left(10\right)$

And: A ₄=(0, R, 0) ^T

The length l of driving leg ₄Can use hinge point A ₄, B ₄Coordinate show:

${\mathrm{<figure-callout\; id="4"\; label="l"\; filenames="H2009102634316E00011.png"\; state="\{\{state\}\}">l</figure-callout>}}_4=\sqrt{{(l_5{c}_1+{\mathrm{rs}}_2{s}_3)}^2+{({\mathrm{rc}}_3-R)}^2+{(l_5{s}_1+{\mathrm{rc}}_2{s}_2)}^2}---\left(11\right)$

l ₄Be fault sensor current answer measured value, the data reconstruction unit answers measured value to send into the PID control module this with the detected value that replaces fault sensor the servomotor of driving leg four to be carried out PID control, thereby realize fault-tolerant and automatic error correction, guaranteed the normal operation of parallel robot driving leg four-sensor fault.

Claims (5)

1. one kind with fault-tolerant and parallel robot device error correction, comprise moving platform, fixed platform and two platforms are flexibly connected to form four driving legs of parallel closed loop structure, wherein two driving legs respectively contain two single dof mobility pairs, a spherical hinge, two other driving leg respectively contains single dof mobility secondary and two spherical hinges or a spherical hinge and a Hooke's hinge, be located at the sensor that contains on each driving leg, servo-driver, servomotor is in interior transmission system, and be connected control system with each transmission system with the control robot motion, key is also to be provided with a gentle rope between the center of moving platform center and fixed platform, with the fixed platform tie point outside should gentle rope on, also be serially connected with the displacement transducer that is used to monitor variable in distance between two centers and make gentle rope be the spring of tensioning state, control system is then for containing the control system of field programmable gate array logic controller; One end and the moving platform center fixation of gentle rope, the other end then can relative slidingtype connects and is being anchored on the fixed platform behind serial connection displacement transducer and the tensioning spring on the gentle rope in the tie point outside again with the center of fixed platform, this displacement transducer also by holding wire be connected with controller, with the transmission monitoring information.

2. described with fault-tolerant and parallel robot device error correction by claim 1, it is characterized in that the described control system that contains the field programmable gate array logic controller, its control system comprises flash memories, random access memory and be attached thereto contain centralized control unit, be used for the communication unit that is connected with PC, be used for the interpolation operation unit that interpolation is calculated, PID control module as PID control, be used to handle the counting unit of feedback signal, be used for fault detect and isolated location that the operational factor that feedback parameter is corresponding with the input flash memories compares, this fault detect value is carried out the buffer cell of isolation processing, and be used to calculate fault sensor and answer the data reconstruction unit of measured value at interior field programmable gate array logic controller.

3. described with fault-tolerant and parallel robot device error correction by claim 1, it is characterized in that described gentle rope is at the elastic deformation amount of length direction enough little metal or the gentle rope of non-metal kind; Then colluding work by latch closure or ring with the connection at fixed platform center can connect by relative slidingtype.

4. by the fault-tolerant and error correction method of the described parallel robot device of claim 1, comprising:

A. condition monitoring and fault diagnosis: at first according to the motion requirement of parallel robot, pass through trajectory planning, in the flash memories (FLASH) with its each bar driving leg length, velocity varies with time operational factor input control system, as the underlying parameter of driving leg length, speed, after this parameter value while interpolation operation cell processing in logic controller the result is sent into the PID control module and be used for the servomotor in each driving leg is controlled; Article four, the online detected value one tunnel of driving leg sensor and gentle rope sensor carries out PID control through buffer cell input PID control module to servomotor, and another road is imported random-access memory (ram), carried out On-line Control and monitoring with the operation conditions to parallel robot as detected value under the control of centralized control unit; Can calculate the calculated value of middle gentle rope sensor according to the restriction relation of space parallel mechanism by four driving leg sensor detected values, and relatively with the actual detected value of the calculated value of this gentle rope sensor and gentle rope sensor, if deviate has between the two exceeded setting range, then showing has the sensor of a driving leg fault to occur;

B. fault location: fault detect and shielding system compare the underlying parameter of driving leg sensor this moment and the real output value of driving leg sensor, if a certain driving leg deviate has between the two exceeded setting range, judge that then this driving leg sensor is a fault sensor;

C. the fault-tolerant and error correction of fault sensor: after determining fault sensor by the B step, promptly by the detected value of the detected value of other three driving leg sensors and middle gentle rope sensor by the data reconstruction unit in the FPGA motion controller, calculate the measured value of answering of fault sensor, and under the control of detection and isolated location, the fault detect value is abandoned by buffer, answer measured value to replace with this, be used for the pairing servomotor of fault sensor is controlled, thereby realization is to the fault-tolerant and error correction of fault sensor.

5. by the fault-tolerant and error correction method of the described parallel robot device of claim 4, it is characterized in that described fault sensor comprises that the fault of driving leg sensor own or its circuit transmission fault or sensor probe are because of output unusually due to the physical obstacle.

Images