JPH0276010A - Digital controller - Google Patents

Abstract

PURPOSE:To improve the dynamic characteristic and robust ability of a servo system by constituting a digital controller of two microprocessors and one host computer, executing servo control by one microprocessor and executing power control by the other microprocessor. CONSTITUTION:A microprocessor 11 executes servo arithmetic and a microprocessor 12 executes power control arithmetic. The microprocessors 11 and 12 are synchronously operated by the same sampling clock SCK. Accordingly, since a sampling period can be shortened and the arithmetic time delay of the servo can be extremely decreased, the characteristic of a whole control system can be improved. A prescribed command or a parameter can be asynchronously transmitted from the host computer independently from the internal processing of the respective microprocessors.

Description

[Detailed Description of the Invention] [Summary] This type of digital control device is concerned with a digital control device that simultaneously controls position and force by feeding back force detected by a force sensor attached to the end of an arm of an articulated robot or automatic machine. With the aim of improving the dynamic characteristics and robustness of the servo system and, as a result, improving the characteristics of the entire force control system, the digital control device is equipped with two microprocessors and one host computer. One microprocessor performs servo control, and the other microprocessor performs force control.

[Industrial application field]

The present invention feeds back the force detected by a force sensor attached to the arm tip of an articulated robot or automatic machine,
This invention relates to a digital control device that simultaneously controls position and force. In operations where the arm and the target object come into contact, such as fitting, tracing, and pressing operations in assembly operations, it is necessary to control both the position and the force at the same time.

[Conventional technology]

FIG. 15 is a diagram showing an example of a six-joint robot.

In the figure, Jl to J are joints, and I is a hand (
(wrist), Σ represents the reference coordinate system, and Σ represents the hand coordinate system, respectively.

When controlling an articulated robot, due to the complexity of the mechanism, a hand coordinate system Σ□ is established for the force sensor, a joint coordinate system is established for joint angles and motor drive torque, and a reference coordinate system Σ8 is established for control calculations. , the coordinate transformation is determined according to the characteristics of each mechanism.

FIG. 13 is a diagram showing an example of a force control system for an articulated robot. In the figure, 1 is a force compensation section, 2 and 3 are coordinate transformation sections, 4 is a function generator, 5 is a servo compensation section, 6 is a mechanism section,
7 is the force sensor, 11', R is the force setting value, FR is the current force value, F, TI is the force deviation, F' is the observed force value, V4
is the instructed speed, V is also the instructed speed, r is the target position, θ is the current position, e is the deviation, and U is the manipulated variable. Note that the subscript R indicates a variable expressed in the reference coordinate system, the subscript H indicates a variable expressed in the hand coordinate system, and the subscript without a subscript indicates a variable expressed in the joint coordinate system.

[Problem to be solved by the invention]

FIG. 14 is a diagram showing an example of the hardware configuration of a conventional force control system. In the figure, 8 indicates a microprocessor.

If the force control system shown in Fig. 13 is to be realized by digital control, a hardware configuration as shown in Fig. 14 can be considered, but if all the processing shown in Fig. 13 is sequentially processed by the microprocessor 8, Time will increase. As a result, the sampling period increases, the dynamic characteristics and robustness of the positioning servo system of the inner loop deteriorate, and the characteristics of the force control system also deteriorate.

The present invention was created in view of this point, and an object of the present invention is to provide a digital control device that can improve the dynamic characteristics and robustness of a servo system and, as a result, improve the characteristics of the entire force control system. It is said that

[Means to solve the problem]

The digital control device of the present invention controls an articulated robot or automatic machine having a hand attached to the tip of an arm and a force sensor that detects the force acting on the hand.

FIG. 1 shows the hardware configuration of a digital control device of the present invention. In the figure, 11 and 12 are microprocessors, 13 and 14 are external memories, and 15 and 16 are microprocessors.
is the I10 device, 17 and 18 are local buses, 19 and 2
0 is a bus, 21 is a synchronization clock source, 22 is a DMA sequence control section, 23 is a host interface control section,
24 each indicate a host computer.

As the microprocessor 11.12, a digital signal processor (for example, Fujitsu MB8764)
can be used. This digital signal
The processor has internal memory and DMAI capability. The microprocessors 11 and 12 take in the synchronization clock (sampling clock 5CK) from the synchronization clock source 21 and perform sampling synchronization. Microprocessor 11.12 also connects external memory 13.14 and I/O via local bus 17.18.
10 devices 15, 16 can be freely accessed without interfering with each other. I10 device 15 includes an encoder & counter, and I10 device 16 includes a force sensor. Communication between the microprocessors 11.12 must be performed at high speed for each sampling, and is performed via the bus 19.20 by DMA transfer asynchronously with the internal operations of the microprocessors 11.12. It is necessary to transfer data from the host computer 24 to each microprocessor 11.12 asynchronously with the processing of the microprocessor 11.12. Therefore, the host interface control unit 23 connects the bus on the host computer side and the DMA interface on the microprocessor side, so that the host computer 23 can access the microprocessors 11 and 12 by DMA. Furthermore, microprocessors 11 and 12
DMA transfer, host computer and microprocessor 11
DMA transfer between host computer and microprocessor 1
A DMA sequence control section 22 is provided with a function to avoid conflict when the timings of DMA transfer between two devices overlap.

FIG. 2 is a diagram showing the processing procedure of the microprocessor in the present invention. The microprocessor 11 performs servo calculations, and the microprocessor 12 performs force control calculations.

The microprocessor 11 and the microprocessor 12 are
It is assumed that they operate synchronously using the same sampling clock SCK.

First, the processing procedure of the microprocessor 11 will be explained.

- Check the trigger generation of the sampling clock SCK, and if it is the sampling timing, proceed to process [2].

■ Input the joint instruction speed V one step before by DMA transfer.

■ Manually calculate joint angle θ.

(2) Execute servo calculation according to e=r-0 and update morph driving force U.

(2) Calculate the target position r according to the joint instruction speed V input one step before in (2).

(2) The joint angle θ input in (2) is sent to the microprocessor 12 for coordinate transformation during the next sampling.

■ Analyze commands from the host computer.

■ Execute each command as necessary.

Next, the processing procedure of the microprocessor 12 will be explained.

- Check the trigger generation of the sampling clock SCK, and if it is the sampling timing, proceed to process [2].

■ Input sensor signal FH.

(2) Convert the sensor signal F1 to a value FR in the reference coordinate system based on the attitude θ one step before.

■ Compare the PR with the FiR set from the host computer,
Perform appropriate compensation calculation according to FiR=FiR−Fi,
This result is designated as the commanded speed vR.

(2) Convert the commanded velocity vll to joint velocity V, and pass it to the microprocessor 11 at the next sampling time.

■, ■ Performs host interface processing in the same way as the microprocessor 11.

Note that the coordinate transformations of (1) and (2) are determined by the structure of the mechanism, and the calculation method is described in, for example, Robofrobot Plater by Paul Paul.

FIG. 3 is a diagram showing a design example of a host interface buffer. When changing force control or servo control parameters (feedback gain, etc.) while moving the robot, all related parameters must be changed within one sampling period. For that purpose, −
A buffer capacity corresponding to the maximum number of parameters to be changed at a time and a procedure (software) for changing the parameters are required.

This is a particular problem because the speeds of the host computer and DSP are dozens of times different. To achieve this, it is necessary to transfer all parameters within the same sampling when receiving a transfer command. Therefore, it is necessary to reserve a host interface buffer area for the maximum amount of data to be transferred at one time between the host and the microprocessor, and form it as shown in FIG. In FIG. 3(a), the data to be transferred every time is limited to consecutive addresses, and only the specified number of data are transferred from the address specified by the host computer. In addition, commands can also be specified to cause the microprocessor to execute predetermined processing (servo on/off).
off, etc.). In this case, the microprocessor's memory
In the map, all data related to the same command processing must be placed at consecutive addresses. FIG. 3(g)) is a method of indicating the transfer destination to the microprocessor by specifying the addresses of transfer data one by one. In this case, the command may be fixedly assigned to the corresponding address.

In FIG. 3(C), a bread buffer having exactly the same size as the memory area used by the microprocessor in real time is prepared. An appropriate one may be selected depending on the size of the memory space of the microprocessor, processing speed, etc.

[Effect]

FIG. 4 is a diagram showing a comparison between the arithmetic processing of the present invention and the conventional example. Comparing the arithmetic processing of the present invention and conventional arithmetic processing,
It will look like Figure 4. The time from inputting the force signal FH to updating the manipulated variable u is almost the same in both the present invention and the conventional example, but according to the present invention, the sampling period of the servo system (microprocessor sensor #1) is It can be reduced by half, and
Computation time delay can be significantly reduced. As a result, the characteristics of not only the servo system but the entire force control system can be improved. Although θ used in the force control coordinate transformation is the value one step before, an error of this degree does not have a large effect on the force control calculation (the sampling period is about IKHz).

[Embodiment] FIG. 5 is a block diagram showing an embodiment of hybrid control of position and force. In the figure, 30 is a robot eyebrow break, 31 is a motor, 33 is an encoder & counter, 34 is a power amplifier, 35 is a D/A converter, 36 is a servo calculation/function generator, 37 is a force sensor, and 38 is a 39 is also a coordinate transformation section, 40 is an inverse Jacobian matrix calculation section, 41 is a force control section, 42 is a position control section, and 43 is a speed servo system.

While FIG. 13 shows a simple force control system, FIG. 5 shows a combination of force control and position control called high print control. For hybrid control, for example, M, l+, Ra1bert, Hybrid Po5
ition/Force Control of Ma
nipulators, ASME, 1981. In Figure 5, the right half (43, 30, 3
7, 38, 4L 40) correspond to the force control section shown in FIG. 13, whereas the left half of FIG. 5 represents the position control system. Hybrid control attempts to realize motion under constraint conditions by performing position control and force control in orthogonal directions. FIG. 5 shows a control system when a multi-jointed robot manipulator is used to copy and process the surface of an object. The speed servo system 43 includes a motor 31, an encoder & counter 33, and a per amplifier 34.
, a D/A converter 35, and a servo calculation/function generation section 36. There are as many speed servo systems 43 as there are degrees of freedom. At the tip of the robot manipulator 10 is a hand (
(not shown) is attached, and the force acting on one point of the hand is detected by a force sensor 37 attached to the wrist of the eyebrow break. Hand coordinate system (Xゎ, Yh
, Zb) is converted to the robot reference coordinate system (X, .

Y,,Z,) is converted into a value F, and input to the force control section 41. Each joint angle θ detected by the encoder & counter 33 is transformed into the position/posture Xr of the hand by the coordinate conversion unit 39.
is converted to The manipulator operates according to the sum v=vp+vp+vr of the speed command v0, the speed command vp generated by the position control section 42, and the speed command ■ generated by the force control section 41 based on the force control parameter. The velocity i of each joint of the manipulator for generating the velocity ■ in the reference coordinate system is calculated by the inverse Jacobian matrix calculation unit 40 and is given by θ-J-1·■. J-1 is an inverse Jacobian matrix. The manipulator operates when the speed servo system 43 realizes δ. By the way, the details of Figure 5 can be found in Japanese Patent Application No. 62-3046.
It is described in No. 06.

Figure 6 shows the control system in Figure 5 using two microprocessors (
FIG. 3 is a diagram showing an example of a processing procedure of a microprocessor when realized using a DSP. In the case of the control system shown in Figure 5, it is necessary to change position control parameters and force control parameters in real time according to the situation.
Buffer areas capable of collectively transferring these parameters are provided in the memory space of each DSP as shown in FIG. 3(a).

This type was chosen because the DSP's memory space is small (256 words of internal RAM). In the case of DSP,
Since a library of trigonometric functions is not prepared, approximation calculations are first performed, and this is used to perform calculations for the position control section, force control section, etc.

As shown in FIG. 6, the microprocessor 11 on the first side performs the following processing.

- Check the trigger generation of the sampling clock SCK, and if it is the sampling timing, proceed to process [2].

(2) Read the joint command speed δ from the microprocessor on the second side using DMA.

■ Input the angle θ from the encoder/counter.

■ Perform servo calculation and output the manipulated variable U.

■ Perform function generation.

■ Write the angle θ to the microprocessor on the second side using DMA.

■ Perform command analysis.

■ Execute the command.

The two microprocessors 12 perform the following processing.

- Check the trigger generation of the sampling clock SCK, and if it is the sampling timing, proceed to process [2].

■ Perform trigonometric function operations.

■ Obtain the current position X from the coordinate transformation 39.

(2) A velocity vector v in the position control direction is calculated by the calculation of the position control unit 42.

■ Input sensor signal F5 from the force sensor.

■ Coordinate transformation of F, to F.

■ Force deviation vectors F, -F in the force control unit 41.

Based on this, the velocity vector Vr in the force control direction is determined.

(2) Apply J-1 to the sum of speed commands v0 and V, and V to find joint command speed i.

■°Parse the command.

[Phase] Execute the command.

Even in the case of hybrid control as described above, by applying the principles of the present invention, a control system with better characteristics can be realized compared to a method that performs normal sequential processing.

Next, the hardware configuration of the digital control device of the present invention will be explained in detail. FIG. 7 is a diagram showing interfaces between multipaths and DSPs, and between DSPs. In the figure, 51 and 52 are DMA controllers, 53 to 60 are bus buffers, 61 and 62 are boards, *ECE
is extended memory select, to book〇EN is address input enable, *DTEN is data input enable, *O
C indicates data output enable.

The board 61 includes a microprocessor 11, an external memory 13, a DMA controller 51, a bus buffer 53,
A bus buffer 55, a bus buffer 57, a bus buffer 59, etc. are mounted. board 62 is board 6
It has the same configuration as 1. The DMA controller 51 is connected to the microprocessor 11 via a DSP control bus, and is also connected to a DMA controller 52 via an inter-DSP control bus.

DMA controllers 51,52 are comprised of programmable logic arrays. Bus buffer 53 is connected to bus buffer 54 via an inter-DSP data bus. The bus buffer 55 takes the address on the multipath onto the external data bus of the board 61, and the bus buffer 57 takes the data on the multipath onto the external data bus of the board 61.
The bus buffer 59 outputs the data on the external data bus of the board 61 to a multipath. Multipath is, for example, In
This is multipath from te1 company. *ECE is a signal for distinguishing between DMA transfer and external memory use, and is used to chip select the external memory. When a write command is issued from the host computer to each microprocessor 11.12, the upper address is decoded and the A mode sequence is executed. The memory address of each microprocessor 11.12 is fetched at *ADEN timing, and the data is fetched at *DTEN timing. Conversely, when a read command is issued from the host computer to each microprocessor 11.12, the A mode sequence is followed by the I mode sequence, and data is output to the multipath at the *OC timing. . Data transfer between the DSPs is started by outputting *A signal output request).

FIG. 8 is a diagram showing input/output signals of the DMA controller. Go to book〇EN is address manual enable, *DTEN
is data manual enable, *OC is data output enable, this RD is multipath read command, *WT
is multipath write command F, mAOF is output request, *BCT is output permission, *WCK is output synchronization clock,
This Atp indicates an input request, *ACT indicates input permission, and this RC indicates an input clock. Also, *oAop
A signal to which D is added, such as , is a signal exclusively for use between DSPs, and its meaning corresponds to that described above.

FIG. 9 is a diagram showing the state transition of the DMA controller in A mode. In state SO *RD=1. mWT・
1 and *D^IF=1, the state remains SO. When it is *RD.0 or umbrella-T.0 in state SO and becomes *DA I F.1, the state transitions to SIO.

Even if CT=1 in state S10, the state is SL
It remains O, and when *ACT=0, the state transitions to Sll. After transitioning to Sll, in synchronization with clock CKI, S12. S13. S14. S15. The process transitions to S16. In state S6, the state does not change even if *ACT.0 is reached, and the state transitions to 317 when *ACT.1 is reached.

Even if RD=0 or *WT=O in state S17, the state does not change and is the actual RD.

1 and becomes -T.1, the state transitions to SO.

When *DAIF・0 occurs in state SO, the state becomes S2
Transition to 0. At state B520, the state does not change even if it becomes *ACT・1, and when the pass CT becomes 0, the state changes to S2.
Transition to 1. Even if *ACT=O in state S21, the state does not change, and when this ACT becomes 1, the state becomes SO
Transition to.

The signal output from the DMA controller in each state of the Δ mode is as follows.

So: *＾IF=1. *RCK=1. *ADE
N=1. *DTEN=1. *ACK=1. *DACT=
1 S10: Umbrella A IF=0. *RCK=1. *ADE
N=*ACT, DTEN=1-^cg=t.

*DACT=1 Sll: *AIF=Sail*RCK=O, *ADEN=
O, *DTEN=1, *ACK=1. Book DACT=1 S12: Umbrella AIF・o, *pcK・l, *ADEN=
O, this DTIEN=1. *ACK=1. *D8CT・l 513: Nehe IP=1. *RCK=1. Umbrella ADEN=
1. Ne[]TIEN=0. Umbrella 8 CK = 1. neD8CT=
1 S14: *＾IF・11 RCK=O, *ADEN
=1. *DTEN=O, *ACK=1. This D^CT=1 S15: *AriF=1. neRCK=1. *ADEN
=1. Main DTEN=O, Main ACK=1, *[]ACT
=1 S162 AIF=1. Book R(J=1.*ADEN=
1. This DTEN=1. Car eight CK=1. Book 8 CT, l 5172 Book AIF=1. Book RCK=1. *ADEN engineering 1. *DTEN=1. mACK=*enT.

*DACT=1 S20: *AIF-this DAIF, this DACT=1.
*RCK=1. *^DEN=L, *DTEN=11 pieces A
CK=1 S21: This AIF=NEDAIF, *DACT=*AC
T, *RCK-*DRCK, car ADEN=1, *DTE
N=1. *DTEN・II*To 8C, 1 FIG. 10 is a timing chart of A mode. First, a case will be described in which data is transferred from multipath to DSPI (or DSP2).

When *WT=O in state SO, the state transitions to SIO. Note that the state transition is based on the clock CK 1 (16
KHz). In state SIO,
Book AIF=O.

Umbrella RCK=1. *ADEN nine ACT, DTI'N=1
．． *AIJ=1. This DACT becomes 1. In addition, mA(
J is a response (acknowledge) signal to the command. When the rate ACT=0 in state SIO, the state is S
Transition to ll. In the state BSII, *AIF=
O, *RCK=O, *8DEN=O, *DTEN=1, this ACK=1. *DACT=1. When the clock (, Kl rises in state S11, the state changes to Sll.
The process then transitions to S12.

In state S12, this AIF/01 RCK = 1
, this ADEN・01 this DTEN=1, 5ACK・
1. This is the umbrella DACT-1. When the clock CKI rises in state S12, the state transitions from S12 to 513. In state 313', *AIF=1. *R
CK211 pieces ADEN=1. *DTEN=O, *AC
K=l, *DACT=1. When the clock CKI rises in state S13, the state changes from S13 to S.
Transition to 14. In state S14, NingAIF=1
．． Book RCK=O, *^DEN=1. *DTEN=O,*
^CK=1, *DACT・1. Status JIQS1
When the clock CKI rises at 4, the state is S1
Transition from 4 to 315. In state S15, *A
tp=t, umbrella RCK=1. *ADEN・1, *DT
EN・Q, *ACK=1. This DACT=1. When the clock CKI rises in state B515, the state transitions to 316. In state S16, *AIF
=1. *RCK=1. *ADEN=1, car DTEN=1
．． To this AC = 1. '*DACT・1. condition jlJ
When CT=1 in S16, the state is S16
to 517. For MS17, *A
IF=1. *RCK・11 to 8 EN=1. Book DTEN
=1. Book ^ CK = book - T1 book DACT = 1. When the state becomes 1 in state S17, the state transitions from S17 to SO.

Next, from DSPI (or DSP2) to DSP2 (DSP
The case of transferring data to I) will be explained. Status S
When *DA IF, O at O, the state transitions from SO to 320. In case B520, this AIF=
Umbrella DAIF, book DACT=1, *RCK=1. *AD
EN engineering l1 main mouth TEN=1. This is ACK 1. When *ACT=O in state S20, the state transitions from S20 to 321. In state S21, the rate ^I
F=*DAIF, *DACT=*^CT, RCK=*
DRCK, *ADEN・l, *DTIl! N・19
Honguchi TEN, 15 ACKs, 1. Main AC in state S21? =1, the state transitions from S21 to SO.

FIG. 11 is a diagram showing the state B transition of the DMA controller in the ■mode. Even if *Aop・1 or CT=O or *DBCT=1 in state BSO, the state is SO
It remains as it is. In state SO, book Aop・Q, car A
When CT=1 and umbrella DBCT=O, the state transitions from SO to Sl.

In state S1, even if *AOF=1 or real WCK=1, the state does not change, and *AOF・0. *False CK=0 and *U·7, the state transitions from Sl to S2.

In addition, it is U-(014, Di3.012) and represents a machine. Note that machine number 1 is set for DSPI, machine number 2 is set for DSP2, and machine number 7 is virtually set for multipath mask. In state S2, the pass op
The state does not change even if =o or the actual WCK is 0, and if it is 4 leaves and 1 and the pass CK is 0, the state transitions from S2 to 83. When the clock CK2 rises in state S3, the state transitions to S4. In state S4, Zai R
The state does not change even if it becomes D.0, and if *Ro=i, the state changes from S4 to SO. In state S1, this AOF=0. When *WCK=O and *U≠7, the state transitions from Sl to 311.

In state 511, the state does not change even if *DBCT=O, and if this DBCT is 1, the state changes from Sll to So.

■The signal output from the DMA controller in each mode is as follows.

SO: Book BCT=1. Umbrella 0C=1. *ACK=1.
Book0AOF-BookAOForNOTBookACT.

*DWCK=Book-CM Sl: Umbrella BCT=0. Book 0C=1. *ACK=1.
Car DAOF=*AOForNOT*ACT, *DWCK
=Book-CK S2: BookBCT=O, Book0C=1. Real ACK=1.
Umbrella DAOF-Book AOForNOT*ACT.

*DWCK=*WCK S3: JCT=O, book0C=O, *ACK=1. *
DAOF=*80ForNOT*ACT.

*DWCK=*WCK S4: JCT=Book DBCT, *OC=Book RD, Umbrella A
CK=This RD, *0AOF=*AOForNOT*8C
T, *DWCK-Book WCKSll: *[1CT=Book DBCT, *0C=l, *^CK=1. *DAOF=*
AOForNOT*ACT, main DWCK, car-CK FIG. 12 is a diagram showing a timing chart of mode ■. First, data transfer from DSPI (or DSP2) to multipath will be explained.

In the state BSO, *BCT=1. *0C=1. Book A
CK=1. Book DAOF=*AOForNOT book^CT,
*DWCK=real WCK. *A in state SO
OF・0. *When it comes to ACT・1 and this DBCT・0,
The state changes from SO to 31. Note that the state transition is performed in synchronization with clock CK2. In condition BS1 *
BCT=0. *0C=1, *CK=1. *DAOF
-*AOForNOT*ACT, this DWCK=*W
It is CK. In state BS1, *AOF=O, *WC
If K=0 and U=7, the state transitions from Sl to 32. In state S2, *BCT=O, *0C=1
, *ACK=1. *DAOF=*AOForNO
T*ACT, *DWCK=*WCK. State S2
If the state is OF.1 and dcK.0, the state transitions from S2 to 83. *BCT in US3
・0. *0C=O, *^CK=1, book 0^OF=book AO
ForNOT*ACT, umbrella DWCK=*WCK. When clock CK2 rises in state S3,
The state transitions from S3 to 84. In state S4,
*BCT/this DBCT, *OC=*RD, *ACK
=*RD, *D^OF=*^0ForNOT*ACT
, *DWCK-Umbrella-CK. In state S4*
When RD=1, the state transitions from S4 to SO.

Next, from DSPI (or DSP2) to DSP2 (DSP
The case of transferring data to I) will be explained. Status S
1, the state transitions from Sl to Sll when car AOF=0-匈CK, O and U≠7. In state Sll, real BCT=real DBCT, *0C=1. *AC
K=1. *[1AOF-Ning A Kano orNOT book AcT,
This DWCK=*WCK. In state Sll
When the BCT becomes 1, the state changes from Sll to SO.

〔Effect of the invention〕

As is clear from the above description, according to the present invention, in a force control system that simultaneously controls force and position, the sampling period can be shortened and the servo calculation time delay can be significantly reduced. The overall characteristics can be improved, and predetermined commands and parameters can be asynchronously transmitted from the host computer independently of the internal processing of each microprocessor.

[Brief explanation of the drawing]

FIG. 1 is a diagram showing the hardware configuration of a digital control device according to the present invention, FIG. 2 is a diagram showing a processing procedure of a microprocessor according to the present invention, and FIG. 3 is a diagram showing an example of a host interface buffer according to the present invention. Figure 4 is a diagram showing a comparison of the arithmetic processing of the present invention and the conventional example, Figure 5 is a diagram showing an example of hybrid control of position and force, and Figure 6 is a diagram showing two control systems of Figure 5. Figure 7 shows an example of the microprocessor processing procedure when realized using a microprocessor.
FIG. 8 is a diagram showing the input/output signals of the DMA controller, FIG. 9 is a diagram showing the state transition of the DMA controller in A mode, and FIG. 10 is a diagram showing the D mode.
FIG. 11 is a diagram showing a timing chart of the MA controller in A mode. FIG. 11 is a diagram showing state transition of the DMA controller in I mode. FIG. 12 is a diagram showing a timing chart of the DMA controller in I mode. Fig. 13 is a diagram showing an example of a force control system for an articulated robot, Fig. 14 is a diagram showing an example of the hardware configuration of a conventional force control system, and Fig. 15 is a diagram showing an example of the hardware configuration of a conventional force control system.
The figure shows an example of a six-joint robot. 11 and 12... microprocessor, 13 and 14...
・External memory, 15 and 16... Bow 10 device, 17 and 18
...Local bus, 19 and 20...Bus, 21.
... Synchronization clock source, 22... DMA sequence control section, 23... Host interface control section, 24
...Host computer, 30...Robot eyebrow break, 31...Motor, 33...Encoder & counter, 34...Power amplifier, 35...D/A variable tA
2N, 36... Servo calculation/function generation section, 37...
- Force sensor, 38... Coordinate transformation section, three... Coordinate transformation section, 40... Inverse Jacobian matrix calculation section, 41... Force control section, 42... Position control section, 43. ...Speed servo system, 51 and 52...DMA controller, 53 or 60...Has buffer, 61 and 62...board. Patent Applicant: Fujitsu Limited Representative Patent Attorney Kyotani Shibu (a) (b) (c) Host Ingtuni Hisashi Badufa's staff is enough A Figure 3 If LL and power hyphenate disaster I? JP's
Figure 5 Markeno (Human Hi DSPM and DSP Xiangmano Ingface Figure 7 Figure 8 Input and output of the DMA controller Figure 8 State of the DMA controller in A mode, Transition M9 Diagram ■@■O■O @ ■ [phase] ■■■ (2) O ■■ - KyoAOF=1 or *ACT=O or * DBCT = l DMA condolence I mode Gin's bowed bladder l God M11 diagram ■■@■[Yes]■[ phase] [phase]

Claims (2)

[Claims] (1) An articulated robot with a hand attached to the tip of the arm and a force sensor that detects the force acting on the hand, or a digital control device that controls an automatic machine, which has internal memory and a DMA function. a first microprocessor (11) with internal memory and a DMA function; a second microprocessor (12) with internal memory and a DMA function;
external memory (13) and input/output devices (15) connected via a local bus (17) and a second microprocessor (12) via a local bus (
18), an external memory (14) and an input/output device (16), a DMA sequence control unit (22), and a DMA sequence control unit (22) and the first microprocessor (11) connected via a bus (19), a DMA sequence control unit (22) and a second microprocessor (
12), and a bus (20) connecting the first microprocessor (11) and the second microprocessor (1
2) a synchronous clock source (21) that supplies a sampling clock SCK, a host computer (24), and a host interface control unit that connects the bus of the host computer (24) and the DMA interface on the microprocessor side. (23); and, the DMA sequence control unit (22) performs DMA transfer between the first microprocessor (11) and the second microprocessor (12), the host computer (24) and the first DMA transfer between microprocessors (11), host computer (2)
[1] Trigger of sampling clock SCK The occurrence is checked, and if it is the sampling timing, the process proceeds to [2]. [2] Joint instruction speed v one step before by DMA transfer
Enter. [3] Input the joint angle θ. [4] Execute servo calculation according to e=r−θ and update motor driving force u. [5] Joint instruction speed v one step before input in [2]
The target position r is calculated accordingly. [6] Send the joint angle θ input in [3] to the microprocessor (12) for coordinate transformation during the next sampling. [7] Analyze commands from the host computer. [8] Execute each command as necessary. The second microprocessor (12) is configured to perform the following processes: [1] Checks the occurrence of a trigger of the sampling clock SCK, and if it is the sampling timing, proceeds to the process of [2]. [2] Input force sensor signal F^H. [3] Convert the force sensor signal F^H to a value F^R in the reference coordinate system based on the posture θ one step before. [4] Compare F^R with F_i^R set from the host computer, perform appropriate compensation calculation according to F_■^R = F_i^R - F^R, and use this result as the command speed v^R. do. [5] Convert the commanded velocity v^R to joint velocity v, and pass it to the microprocessor (11) at the next sampling time. [6] Analyze commands from the host computer. [7] Execute each command as necessary. A digital control device characterized in that it is configured to perform processing such as, where F_i^R is a force setting value in a reference coordinate system, F^
R is the current value of the force in the reference coordinate system, F_■^R is the deviation of the force in the reference coordinate system, F^H is the observed value of the force in the hand coordinate system, v^R is the instruction in the reference coordinate system The velocity, v is the commanded velocity in the joint coordinate system, r is the target position in the joint coordinate system, θ is the current position in the joint coordinate system, e is the deviation in the joint coordinate system, and u is the manipulated amount. (2) The first microprocessor (11) and the second microprocessor (12) each have a host interface buffer for storing data sent from the host computer (24). The digital control device according to item (1).