JP7375426B2 - Robot control arithmetic processing FPGA and data bus width determination method for arithmetic processing FPGA - Google Patents

Description

The present invention relates to an FPGA that performs arithmetic processing to control a robot having a movable part, and a method for determining a data bus width for the FPGA to access a storage part.

There is a technology that performs processing to determine whether or not a robot will collide with an obstacle such as equipment during actual operation, teaching, or motion simulation of the robot. Since the computation of this collision determination process is very heavy, it is necessary to use a CPU having high-performance arithmetic processing functions, especially in order to perform the collision determination process in real time.

Japanese Patent Application Publication No. 2013-184242

However, since a high-performance CPU is naturally expensive, from the viewpoint of suppressing an increase in product costs, it is desirable to be able to perform collision determination processing in real time even with an inexpensive CPU. To this end, a configuration is assumed in which an FPGA (Field Programmable Gate Array) is used in conjunction with the FPGA to execute the collision determination process, and the CPU acquires only the determination result.

In order for the FPGA to perform collision determination processing, it is necessary to read model data representing the position and shape of the object to be determined from memory, but in order to execute the determination processing at high speed, each model data must be read once. It is desirable to determine the data bus width so that it can be read by memory access. Conventionally, no specific index has been presented for determining the data bus width as described above.

The present invention has been made in view of the above-mentioned circumstances, and its purpose is to provide an FPGA for arithmetic processing for robot control, which reduces the arithmetic processing load on the CPU and makes it possible to perform real-time calculations for collision determination processing. Another object of the present invention is to provide a data bus width determination method for an FPGA for arithmetic processing.

According to the FPGA for arithmetic processing of robot control according to claim 1, when a request for collision determination processing between a movable part of the robot and an obstacle is input, the shape of at least a part of the movable part and the shape of at least one part of the obstacle are input. The data of both models is read out from the storage unit that stores the data of the models representing the shapes of the parts, etc., collision determination is made between both, and when the determination results are output, the calculations related to the collision determination are performed. Logic is configured to form a pipeline that processes at least in part in parallel. Note that "shape, etc." includes the position and size of an object in addition to its shape.

With this configuration, when the CPU inputs a collision determination processing request to the FPGA, the FPGA reads the data of both models from the storage unit, performs collision determination for both, and outputs the determination result to the CPU. It is possible to configure a system called . Therefore, by combining a relatively inexpensive CPU and FPGA, the CPU can process other controls of the robot while having the FPGA perform collision determination. In addition, collision determination calculations have a high processing load, but by configuring the FPGA to form a pipeline that processes at least part of the calculations in parallel, it is possible to perform the calculations efficiently. Collision determination can be executed within the time required by the CPU.

Then, the maximum link length of the robot is L [mm], the allowable judgment error is E [mm], the number of divisions of the model shape etc. into multiple objects is D, and the function that rounds up the decimal point of x to an integer is f _RU ( x), Assuming that the minimum required bit width of the data bus is DB _W 0, the bit width DB _W 0 is determined by the following equation (7),

記憶部に接続されるデータバスのビット幅ＤＢ_Ｗ１を、ビット幅ＤＢ_Ｗ０以上に設定する。

The bit width DB _W 1 of the data bus connected to the storage section is set to be greater than or equal to the bit width DB _W 0.

The bit width DB _W 0 includes the number of data bits required to represent the position of each model, and the number of data bits required in consideration of dividing the size of each model into multiple objects. Determined by consideration. Therefore, if the bit width DB _W 1 of the data bus connecting the FPGA and the storage section is set to the bit width DB _W 0 or more, the FPGA can reliably read model data from the storage section with one access. .

According to the FPGA for arithmetic processing of robot control according to claim 2, if the number of bits of data representing the position of the model is N _P and the bit width DB _W when using a sign bit and a quaternion is 2, then the bit width DB _W 2 is determined by the following formula (8),
DB _W 2 = DB _W 0 + 4 ( _NP + 1) ... (8)
The bit width DB _W 3 of the data bus connected to the storage section is set to the bit width DB _W 2 or more. In this way, by adding 4 (N _P +1) bits to the bit width DB _W 0, the code and quaternion can also be included in the model data, so the bit width DB _W 3 of the data bus can be reduced to the bit width DB _W If set to 2 or more, the FPGA can execute collision determination processing faster.

According to the robot control arithmetic processing FPGA according to the third aspect, both models are rectangular parallelepiped models, and collision determination is performed using separation axes in 15 directions. The rectangular parallelepiped model is a model commonly used in collision determination, and in that case, collision determination is performed using separation axes in 15 directions. By processing at least part of the calculations related to collision determination using the separation axes in 15 directions in parallel in a pipeline, collision determination between rectangular parallelepiped models can be efficiently performed.

According to the FPGA for arithmetic processing of robot control according to claim 4, the pipeline specifically includes a load stage for reading model data from a storage section, a coordinate conversion stage for converting both rectangular parallelepiped models into the same coordinate system, A direction calculation stage that determines the separation axis from a specific axial direction of both rectangular parallelepiped models or their cross product, and a separation axis determination stage that projects both rectangular parallelepiped models onto the separation axis and determines whether the images of each model overlap. , and a collision determination stage that performs collision determination based on the determination results for all separated axes in the separation axis determination stage. With this configuration, each process from reading model data to collision determination can be efficiently performed by each stage of the pipeline.

This is the first embodiment and is a diagram showing a pipeline structure composed of internal logic of an FPGA. Flowchart showing the processing procedure related to the memory load stage A diagram explaining the separation axis determination performed when the shapes of two objects are treated as rectangular parallelepiped models. Diagram showing an example of hardware configuration (Part 1) Diagram showing an example of hardware configuration (Part 2) Diagram showing an example of hardware configuration (Part 3) Diagram showing an overview of processing/calculation timing performed between the CPU and FPGA This is a second embodiment and is a diagram showing a pipeline structure composed of internal logic of an FPGA. This is a third embodiment, and is a diagram showing a pipeline structure composed of internal logic of an FPGA. This is the fourth embodiment and is a diagram showing a pipeline structure composed of internal logic of an FPGA. This is the fifth embodiment and is a diagram showing a pipeline structure composed of internal logic of an FPGA. This is the sixth embodiment and is a diagram showing a pipeline structure composed of internal logic of an FPGA. This is the seventh embodiment and is a diagram showing a pipeline structure composed of internal logic of an FPGA. This is the eighth embodiment and is a diagram showing a pipeline structure composed of internal logic of an FPGA. This is the ninth embodiment and is a diagram showing a pipeline structure composed of internal logic of an FPGA.

(First embodiment)
The first embodiment will be described below with reference to FIGS. 1 to 5. This embodiment is characterized by how the internal logic of the FPGA is defined and configured, and how the memory bus width for the FPGA to read model data is set. Therefore, the external hardware configuration is a general combination of CPU1 and FPGA2.

For example, as shown in FIG. 4A, the CPU 1 has its own control program transferred from the ROM, hard disk, etc., and accesses the RAM 3 used as a work area, and the FPGA 2 receives model data for collision determination from the hard disk, etc. The configuration is such that the RAM 4 to be transferred is accessed. FIG. 4B shows a configuration in which the RAM 4 is incorporated inside the FPGA 2. FIG. 4C shows a configuration in which the CPU 1, FPGA 2, and RAM 4 are configured as an SoC (System on Chip) 5, and the RAM 3 is externally attached to the SoC 5. Note that if the capacity of the RAM 4 in the SoC 5 can be sufficiently secured, a configuration may be adopted in which the CPU 1 also accesses the RAM 4, excluding the external RAM 3.

FIG. 5 shows a processing sequence performed between the CPU 1 and the FPGA 2 in this embodiment. When the CPU 1 is powered on and started, it instructs the FPGA 2 to configure. The configuration instruction may also be a release of reset for the FPGA 2. Then, the FPGA 2 reads the configuration program from, for example, a PROM or a flash ROM, executes the configuration, and upon completion, notifies the CPU 1 of the completion of the configuration. After that, when the CPU 2 inputs an instruction to start the collision determination process to the FPGA 2, the FPGA 2 performs a collision determination process calculation. When the collision determination processing calculation is completed, the CPU 1 is repeatedly notified of the completion and the determination result. During this time, the CPU 1 executes other arithmetic processing related to robot control.

As an example, the operating clock frequency of the CPU 1 is about 1 GHz to 2 GHz, the operating clock frequency of the FPGA 2 is about 500 MHz, and the execution interval of the collision determination process is, for example, about 1 ms.

Figure 3 explains the separation axis determination performed when the shape of the arm, which is the movable part of the robot, and the shape of the obstacle, for example, each piece of equipment in a factory, are treated as a rectangular parallelepiped model. It is something to do. Let two objects be objects A and B. Let the directional vectors of object A be XA, YA, and ZA shown in the figure, and let the directional vectors of object B be XB, YB, and ZB. Then, the determination axis V is set as the direction vector XA. Also,
LA=[LAX LAY LAZ], PA=[PAX PAY PAZ]
LB=[LBX LBY LBZ], PB=[PBX PBY PBZ]
shall be.

The following judgment formula (|LA・V|-|LB・V|)/2<|(PA-PB)・V|...(1)
If this holds true, the determination axis V is a separation axis, and objects A and B do not collide. On the other hand, if the above conditions are not met, the determination axis V is not the separation axis, and objects A and B will collide. In the case of V=XA, the determination formula (1) becomes as follows.
(LAX+LXB')/2<|PAX-PBX|...(2)

Separation axis determination for the rectangular parallelepiped model is performed using the following 15 axes as determination axes.
・Each direction vector of object A: XA, YA, ZA
・Each direction vector of object B: XB, YB, ZB
- Cross product of each direction vector of objects A and B: XA×XB, XA×YB, XA×ZB
YA×XB, YA×YB, YA×ZB, ZA×XB, ZA×YB, ZA×ZB
If there is no separation axis among these 15 judgment axes, it means that object A and object B have collided with each other.

As shown in FIG. 1, the logic of the FPGA 2 of this embodiment is configured to form a five-stage pipeline. Each stage of the pipeline is as follows: L: Memory load stage T: Coordinate transformation stage D: Direction calculation stage S15: Separation axis determination stage R: Collision determination stage

<Memory load stage>
The shape of each model data is read from the RAM 4 in units of objects divided into a plurality of parts.
<Coordinate transformation stage>
Convert both rectangular parallelepiped models to the same coordinate system.
<Direction calculation stage>
The separation axis is determined by specific axial directions of both rectangular parallelepiped models or their cross product.
<Separation axis judgment stage>
Both rectangular parallelepiped models are projected onto the separation axis, and it is determined whether the images of each model overlap. Here, parallel processing is performed on separation axes in 15 directions.
<Collision judgment stage>
Collision determination is performed based on the determination results for all the separated axes in the separated axis determination stage.

FIG. 2 is a flowchart showing the behavior when data of each model is read for each object in the memory load stage. First, the object number [1] is loaded from the memory M in which object information is stored, that is, the RAM 4, into the determination register MA (S1). A and b are pointers in which the determination target object numbers of objects A and B are stored, respectively, and "2" is stored in pointers a and b, respectively (S2, S3).

Next, it is determined whether the pointer b is less than or equal to the number of objects N (S4), and if it is less than or equal to N (YES), the contents of M[b] are loaded into the determination register MB (S5). Then, after incrementing the pointer b (S6), the process returns to step S4. In step S4, if the pointer b exceeds the number N of objects (NO), it is determined whether the pointer b is less than the number N of objects (S7). If it is less than N (YES), similarly to the A side, the contents of M[a] are loaded into the register MA (S8) and the pointer a is incremented (S9). Then, after storing the contents of pointer a into pointer b, the process returns to step S4. In step S7, when the pointer a reaches the number of objects N (NO), the process ends.

In the above process, the collision detection model for object B is updated first, and when it reaches the end, B is returned to the beginning and the other A is updated to the next collision detection model, but the relationship between A and B is The opposite may also be true.

For example, a determination made after A is returned to the beginning and before B is updated may be ignored as no collision, or no calculation may be performed. However, since the collision determination calculation for the same combination is only performed twice, the calculation may be performed as is. In that case, there is a possibility that objects in the previous positional relationship may remain at the time of the first collision determination in each cycle. However, since the influence of this over-detection can be ignored without any problem, the calculation may be performed as is. Furthermore, by saving the first collision determination model in another register, A can be returned to the beginning from the register while B is being updated to the next collision determination model.

"L1, 2", "L1, 3", ..., "L1, N", etc. of the memory load stage shown in FIG. 1 correspond to the processing of steps S1, S5, S8 in FIG. After executing "L(N-1), N", a "NO" determination is made in step S7. In this way, the FPGA 2 processes collision determination using a five-stage, five-stage pipeline, allowing efficient execution of calculations.

Here, as shown in FIG. 1, in order to smoothly execute pipeline processing, it is necessary to read data of a required size, that is, the number of bits, at one time in the memory load stage. Specifically, we will consider how much data bus width is required to transfer data between the FPGA 2 and the RAM 4 at one time.

Assume that the maximum link length among all the links of the robot is L [mm], the allowable judgment error is E [mm], and the number of model divisions is D. This number of divisions D is equal to the number of objects N described above. To represent the position of each model, at least

bits are required. However, the function f _RU (x) on the right side is a function that rounds up the decimal part of x to an integer. Also, considering that the size of each model is subdivided by division, the size of each model is at least

bits are required. In total, at least

Requires memory width of bits. Also, by combining round-up functions, we can approximate

There is no problem in evaluating that a memory width greater than the memory width of bits is required.

For example, if the link length is 500 [mm], the allowable judgment error is 1 [mm], and the number of divisions is 16, then N _P = 9, N _S = 8, and DB _W 0 = 3 x 9 + 3 x 8 = 51. , a memory with a bit width of DB _W 1 of 51 bits or more is required. For example, DB _W 1 is set to 52 bits. In other words, with the commonly used 32-bit bus width, loading the collision determination model cannot be completed with one memory load.

After the collision determination model is loaded into the FPGA 2 and until the collision determination result using the model is output, it is sufficient to load memory with a number of bits greater than or equal to that determined by equation (5) or (6). In addition, if the pipeline is composed of multiple calculation blocks that can be processed in an overlapping manner, the bits determined by equation (5) or (6) during the longest calculation time among each block. All you have to do is load more memory than that number.

Although the above formula does not include the sign bit in the model position, by appropriately setting the origin of the coordinate system of each model, for example, if the smallest point of each axis is set as the origin, the position can be corrected. This is because it can be expressed only by direction. Of course, the total number increases by 3 bits, but a sign bit may be added to each position.

In addition, the direction component of the collision detection model can be the same as the coordinate system to which it belongs, for example, the coordinate system of the first axis link if it is a collision detection model that constitutes the first axis link of the arm. The expression does not include the directional component.
Considering the inclusion of the directional component, for example, if we write the quaternion as q={w,(x,y,z)} and use the fact that it is a unit quaternion, we can write, for example, w=√{1-(x ² + y ² +z ² )}
By finding w as , one element can be found by calculation without loading it from memory. Therefore, it can be expressed with three elements.

Adding the sign bit to the number of bits equivalent to the model position, at least N _R =N _P +1 bits are required per element. Of course, the model may have all four elements of the quaternion, or the direction component may be provided as a 3×3 rotation matrix consisting of nine elements. By increasing the number of elements, the calculation processing required for coordinate transformation and direction calculation can be reduced. In particular, if there are only three elements, it is necessary to derive the remaining one element by calculation, which requires calculations such as square root calculations that require a relatively large circuit size, so it is better to widen the memory bandwidth. The overall circuit scale may also be reduced. Therefore, the most suitable configuration is one that has all four quaternion elements.

However, regarding the calculation time, by adding a rotating element interpolation calculation unit that calculates the remaining one element as a new pipeline stage, that is, a calculation block, the impact of the increase in calculation time on the collision detection process as a whole can be suppressed. be able to. In this case, the amount of calculation increases by only one time for all the determinations, not for the determination between each model.

In the case of the specific example presented earlier, since N _P =9, N _R =10, and if the rotation component is 4 elements, a memory width of DB _W 2 = 51 + 4 x 10 = 91 bits or more is required. In other words, the bit width DB _W 2 is
DB _W 2 = DB _W 0 + 4 ( _NP + 1) ... (8)
determined by In this case, even with a 64-bit memory width or a 72-bit memory width with an ECC (Error Check and Correct) function added, loading the collision detection model will not be completed in one memory load. . In this embodiment, a 96-bit data bus configuration, for example, is adopted in response to a memory width of 91 bits or more. This 96 bits is an example of the bit width DB _W 3.

Note that the discussion up to this point is based on the assumption that the collision determination calculation is performed using a fixed point. The advantage of using fixed-point numbers is that the required accuracy can be achieved with a minimum circuit scale. On the other hand, if it is required to obtain the same result as the content calculated by the CPU, the calculation may be performed using floating point numbers. In this case, if the direction component is not included, 32×6=192 bits are required, and if the direction component is included, 32×10=320 bits are required for four elements, and the required memory width becomes even larger.

As described above, according to the present embodiment, when the CPU 1 inputs a request for processing for determining a collision between the arm of the robot and an obstacle, the FPGA 2 determines the shape of at least a portion of the arm and the shape of at least a portion of the obstacle. The data of each model is read out from the RAM 4 in which data representing the shape, etc. of each model is stored, a collision determination is made between the two, and the determination result is output to the CPU 1. The logic of the FPGA 2 is configured to form a pipeline that processes at least a portion of calculations related to collision determination in parallel.

Thereby, when the CPU 1 inputs a collision determination processing request to the FPGA 2, a system can be constructed in which the FPGA 2 reads data of both models from the RAM 4, performs collision determination, and outputs the determination result to the CPU 1. Therefore, by combining the relatively inexpensive CPU 1 and FPGA 2, the CPU 1 can process other controls of the robot while having the FPGA 2 perform collision determination. In addition, collision determination calculations have a high processing load, but by configuring the FPGA 2 to form a pipeline that processes at least part of the calculations in parallel, it is possible to perform the calculations efficiently. Collision determination can be executed within the time required by the CPU 1.

In this case, both models are rectangular parallelepiped models, and collision determination is performed using separation axes in 15 directions. The rectangular parallelepiped model is a model commonly used in collision detection, so by processing at least a part of the calculations related to collision determination using separation axes in 15 directions in parallel in the pipeline, it is possible to improve the relationship between the rectangular parallelepiped models. Collision determination can be performed efficiently.

Specifically, the pipeline of FPGA2 includes a load stage that reads model data from RAM4, a coordinate conversion stage that transforms both rectangular parallelepiped models into the same coordinate system, a specific axis direction of both rectangular parallelepiped models, or their outer product. A direction calculation stage that calculates the separation axis by , a separation axis judgment stage that projects both rectangular parallelepiped models onto the separation axis and determines whether the images of each model overlap, and a judgment about all separation axes in the separation axis judgment stage. It consists of a collision determination stage that performs collision determination based on the results. This allows each stage of the pipeline to efficiently perform each process from reading model data to collision determination.

Then, assuming that the minimum required bit width of the data bus is DB _W 0, the bit width DB _W 0 is determined by equation (6), and the bit width DB _W 1 of the data bus connected to the RAM 4 is determined as the bit width DB. _W Set to 0 or higher. This bit width DB _W 0 also includes the number of data bits required to represent the position of each model, and the number of data bits required to take into account the size of each model to be divided into multiple objects. It has been decided with consideration. Therefore, by setting the bit width DB _W 1 of the data bus to be greater than or equal to the bit width DB _W 0, the FPGA 2 can reliably read model data from the RAM 4 with one access.

Furthermore, by determining the bit width DB _W 2 using equation (8) and setting the bit width DB _W 3 of the data bus connected to the RAM 4 to be greater than or equal to the bit width DB _W 2, the code and quaternion can also be included in the model data. Therefore, the FPGA 2 can execute collision determination processing at higher speed.

(Second embodiment)
Hereinafter, parts that are the same as those in the first embodiment are given the same reference numerals and explanations will be omitted, and different parts will be explained. As shown in FIG. 6, in the second embodiment, the logic is configured so that the pipeline of the FPGA 2 has a six-stage, three-stage configuration. The stages different from the first embodiment are as follows.
LA: 1st memory load stage LB: 2nd memory load stage D/S9A: Direction calculation stage (parallel processing of part of separation axis determination)
S9B: Separation axis judgment stage

<1st memory load stage>
The shape and the like of the model data of object A are read from the RAM 4 in units of objects divided into a plurality of parts.
<Second memory load stage>
The shape and the like of the model data of object B are read from the RAM 4 in units of objects divided into a plurality of parts. That is, in the second embodiment, model data of objects A and B are sequentially read in one stage of pipeline processing.
<Direction calculation stage>
In addition to finding the separation axes, the separation axis determination of each directional vector XA, YA, ZA of object A and each directional vector XB, YB, ZB of object B is processed six times in parallel. This corresponds to the first process.
<Separation axis judgment stage>
Parallel processing is performed on separation axis determination including cross product calculations in the remaining nine directions. This corresponds to the second process.

The reason why the 6-direction separation axis determination that is processed in parallel in the direction calculation stage is set to "S9A" is that the same calculation block as "S9B" used in the separation axis determination stage is used. Therefore, they cannot be executed in parallel.

As described above, according to the second embodiment, the memory load stage is composed of a first stage that reads data of a rectangular parallelepiped model of object A, and a second stage that reads data of a rectangular parallelepiped model of object B. This makes it possible to cope with the case where both model data cannot be read out simultaneously in one memory access due to data bus width constraints, etc., and the memory access must be performed twice.

Then, among the 15 separation axes, the process is divided into a first process in which the separation axis judgment consisting of only the direction vectors of objects A and B is processed in parallel, and a second process in which the remaining separation axis judgments are processed in parallel. That is, the first process is a separation axis determination without a cross product calculation, and the second process is a separation axis determination including a cross product calculation. This makes it possible to process the first process in parallel in the direction calculation stage, and only needs to process the second process in parallel in the separation axis determination stage.

With this configuration, if the memory load stage is divided into the first and second stages, and part of the parallel processing for separation axis determination is performed in the direction calculation stage, each calculation block in the FPGA 2 will be Operation rate improves. Therefore, there is a good balance between the amount of logic circuit resources used in the FPGA 2 and the calculation speed. Furthermore, since the FPGA 2 is configured to execute the first and second processing of separation axis determination using a common calculation block, logic circuit resources can be used more efficiently.

Other configurations that sequentially read the model data of objects A and B in one stage of pipeline processing include:
- Have multiple memories and store at least some of the same collision judgment model data in them. For example, if there are memories A and B, the robots and fences that are judged first are used only in memory A, so they do not need to be stored in memory B, and the robots and fences that are judged last are stored in memory B. There is no need to store it in memory A since it is only used. In other words, it is not necessary that the data stored in memories A and B be completely the same.
- Store each robot to be judged and obstacles such as fences in separate memories.
・Use dual-port memory to load both collision detection models from the same memory at the same time.
etc.

(Third to Ninth Embodiments)
The third to ninth embodiments show variations in pipeline configuration. Depending on the internal logic resources of the FPGA, it is assumed that there is a limit to the number of processes that can be processed in parallel in one stage. To accommodate such cases, variations are shown below.

The third embodiment shown in FIG. 7 is a case in which the separation axis determination stage of the first embodiment is divided into a first stage: S8A and a second stage: S8B, and configured into six stages and three stages. For example, "S8A" is 8-parallel processing, and "S8B" is 7-parallel processing, but they use the same calculation block.

In the fourth embodiment shown in FIG. 8, the separation axis determination stage of the first embodiment is divided into a first stage: S5A, a second stage: S5B, and a third stage: S5C, and has a seven-stage, three-stage configuration. This is the case. "S5A" to "S5C" each perform 5 parallel processing and use the same calculation block.

The fifth embodiment shown in FIG. 9 is a case where the separation axis determination first stage S5A of the fourth embodiment is incorporated into the direction calculation stage and processed in parallel, resulting in a six-stage, two-stage configuration. In this case, in "S5A", five of the directional vectors XA, YA, ZA of object A and the directional vectors XB, YB, ZB of object B are selected and processed in parallel, as in the second embodiment. do.

The sixth embodiment shown in FIG. 10 is a case where the separation axis determination stage of the first embodiment is divided into a first stage: S3A to a fifth stage: S3E, and configured into nine stages and two stages. "S3A" to "S3E" each perform three parallel processes and use the same calculation block.

The seventh embodiment shown in FIG. 11 is a case in which the first separated axis determination stage S3A of the fifth embodiment is incorporated into the direction calculation stage and processed in parallel, resulting in an eight-stage, two-stage configuration. In this case, in "S3A", three of the directional vectors XA, YA, ZA of object A and the directional vectors XB, YB, ZB of object B are selected and processed in parallel.

The eighth embodiment shown in FIG. 12 is a case where the separation axis determination stage of the first embodiment is divided into 12 stages, 2 stages, from the first stage: S2A to the eighth stage: S2H. "S2A" to "S2G" each perform two parallel processes, and "S2H" performs single processing. All of these use the same calculation block.

The ninth embodiment shown in FIG. 13 is a case where the first separated axis determination stage S2A of the eighth embodiment is incorporated into the direction calculation stage and processed in parallel, resulting in an 11-stage, two-stage configuration. In this case, in "S2A", two of the directional vectors XA, YA, ZA of object A and the directional vectors XB, YB, ZB of object B are selected and processed in parallel.

The present invention is not limited to the embodiments described above or illustrated in the drawings, but can be modified or expanded as described below.
The operating clock frequencies of the CPU and FPGA may be changed as appropriate depending on individual designs.
The specific numerical values of DB _W 0 to DB _W 3 may also be set as appropriate depending on the individual design.
The target model for collision determination is not limited to a rectangular parallelepiped model. Therefore, the number of directions in which separation axis determination is performed is not limited to "15".

In the drawings, 1 represents a CPU, 2 represents an FPGA, and 4 represents a RAM.

Claims (6)

When a request for collision determination processing between a movable part of the robot and an obstacle is input, data of a model representing the shape, etc. of at least a part of the movable part and the shape, etc. of at least a part of the obstacle is stored. In order to read the data of both models from the storage unit in which the data is stored, perform a collision determination for both, and output the determination result, a pipeline is configured to process at least a part of the calculations related to the collision determination in parallel. The logic is configured and
The maximum link length among all the links of the robot is L [mm], the allowable judgment error is E [mm], the number of divisions of the model shape etc. into multiple objects is D, and the function that rounds up the decimal point of x to an integer is f _RU (x) and the minimum required data bus bit width DB _W 0, the bit width DB _W 0 is determined by the following formula,

An FPGA for arithmetic processing for robot control, wherein a bit width DB _W 1 of a data bus connected to the storage section is set to be greater than or equal to the bit width DB _W 0. If the number of bits of data representing the position of the model is N _P , and the bit width when using sign bits and quaternions is DB _W 2, then the bit width DB _W 2 is expressed as the following formula: DB _W 2=DB _W 0+4(N _P +1)
determined by
2. The FPGA for robot control arithmetic processing according to claim 1, wherein a bit width DB _W 3 of a data bus connected to said storage section is set to be greater than said bit width DB _W 2. Both of the above models are rectangular parallelepiped models,
3. The FPGA for arithmetic processing of robot control according to claim 1, wherein said collision determination is performed using separation axes in 15 directions. The pipeline is
a load stage for reading the data from the storage unit;
a coordinate transformation stage that transforms both rectangular parallelepiped models into the same coordinate system;
a direction calculation stage for determining a separation axis by a specific axis direction of both of the rectangular parallelepiped models or their cross product;
a separation axis determination stage that projects both of the rectangular parallelepiped models onto the separation axis and determines whether images of each model overlap;
4. The FPGA for arithmetic processing of robot control according to claim 3, further comprising a collision determination stage that performs collision determination based on the determination results for all the separation axes in the separation axis determination stage. When a request for collision determination processing between a movable part of the robot and an obstacle is input, data of a model representing the shape, etc. of at least a part of the movable part and the shape, etc. of at least a part of the obstacle is stored. It reads the data of both models from the storage unit that is installed, performs a collision judgment for both, and outputs the judgment result.
A data bus width determination method applied to a robot control arithmetic processing FPGA in which logic is configured to form a pipeline that processes at least a part of the calculations related to collision determination in parallel, the method comprising:
The maximum link length among all the links of the robot is L [mm], the allowable judgment error is E [mm], the number of divisions of the model shape etc. into multiple objects is D, and the function that rounds up the decimal point of x to an integer is f _RU (x) and the minimum required data bus bit width DB _W 0, the bit width DB _W 0 is determined by the following formula,

A data bus width determining method for an FPGA for arithmetic processing in which a bit width DB _W 1 of a data bus connected to the storage unit is set to be equal to or greater than the bit width DB _W 0. If the number of bits of data representing the position of the model is N _P and the bit width when using sign bits and quaternions is DB _W 2, then the bit width DB _W 2 can be expressed as the following formula: DB _W 2=DB _W 0+4(N _P +1)
determined by
6. The data bus width determination method for an arithmetic processing FPGA according to claim 5, wherein the bit width DB _W 3 of the data bus connected to the storage section is set to be equal to or larger than the bit width DB _W 2.

Images