Interference detection device and interference detection method

Abstract

To provide means for implementing interference detection, by giving a variety of shapes to an interference detection region, which detects the presence or absence of interference, using a small amount of data, that can easily and quickly obtain the presence or absence of interference between a surrounding object and the interference detection region, and the shortest distance between the object and the interference detection region.SOLUTION: An interference detection device according to the present embodiment is an interference detection device which detects the presence or absence of interference between a set three-dimensional region or two-dimensional region and a peripheral object based on a measured distance to the peripheral object, and which is characterized by comprising shape representation means for the interference detection region that detects the presence or absence of interference, the shape representation means retaining the interference detection region in a superquadratic functional form.SELECTED DRAWING: Figure 1

Description

[Technical Field]

[0001] The present invention relates to an interference detection device and an interference detection method relating to technology for detecting and avoiding interference between various mobile bodies, whether manned or autonomous, and surrounding objects, such as automobiles, construction and agricultural vehicles, mobile robots including drones and automated guided vehicles, and the movable manipulator parts of articulated robots.Furthermore, the present invention relates to an interference detection device and an interference detection method relating to technology for detecting intrusion of people or objects into an interference detection area set around a robot or sensor such as a safety scanner, and for detecting intrusion into the interference detection area of ​​the robot or sensor itself. [Background technology]

[0002] In the automotive field, cars equipped with cameras that recognize the surrounding environment and LiDAR (Light Detection and Ranging) are now being put into practical use, and they have autonomous or semi-autonomous driving functions that allow them to avoid collisions with people or other vehicles, park automatically, and even drive themselves.Similarly, in the fields of construction and agricultural vehicles, vehicles that can carry out construction and tilling work unmanned have also been realized.

[0003] Additionally, in the field of robotics, autonomous mobile robots are currently being put to practical use, such as robots and automated guided vehicles that transport workpieces, materials, and various products within factories and logistics facilities, self-propelled cleaning robots that operate within homes, and drones that fly indoors and outdoors to take pictures and transport supplies. Among these autonomous mobile robots are mobile manipulator robots that have a multi-joint manipulator mounted on top of their traveling and moving parts. Even in multi-joint manipulators whose bases are fixed to the floor or a pedestal on equipment, the link parts from the base, which correspond to arms, can be considered a type of mobile body.

[0004] Furthermore, in the field of sensors, there are currently safety sensors known as safety scanners, which are used in various automated facilities, including those using articulated manipulators, to detect the approach of people, logistics vehicles, and other such objects. Safety scanners are installed on fixed facilities, but are also sometimes mounted on autonomously moving robots and automated guided vehicles. Detecting and avoiding collisions and interference with obstacles while these various vehicles, robots, and other moving objects are moving, as well as using sensors in automated facilities to detect and avoid collisions and interference with approaching people or vehicles, are becoming important technologies.

[0005] For example, in recent years, automobiles have not only achieved autonomous driving on roads, but also automated parking in ever-narrower spaces. Furthermore, mobile robots and automated guided vehicles are increasingly using the same paths as people rather than dedicated lanes. As such, as mobile objects, including automobiles, mobile robots, agricultural and construction vehicles, whether manned or unmanned, move more precisely within tighter spaces and perform tasks that require movement without stopping, a rough collision avoidance strategy—such as stopping when a distance measurement device, such as LiDAR, mounted on the vehicle detects an obstacle within a certain distance in the direction of travel—is no longer sufficient. Instead, more precise collision avoidance strategies are required, such as slipping through obstacles perpendicular to the direction of travel without stopping, and approaching obstacles as close as possible to them.

[0006] Mobile robots equipped with manipulators on their running sections are also on the rise. Mobile robots equipped with such manipulators must be able to prevent the manipulator, which corresponds to a human arm, from colliding with surrounding objects other than the target object during operation, and must also be able to prevent the manipulator from colliding with the mobile robot's body itself. In the latter case, the manipulator's elbow may be separated from the waist, which has a smaller circumference, due to the narrowness of the body, but may be close to coming into contact with the chest, which has a larger circumference.

[0007] Furthermore, when a mobile robot uses a manipulator to grasp a door handle, open a door, and pass through the door, it will need to move back slightly to the rear right because the door it is opening is approaching from the front left side of the body.

[0008] For drones, which are included in mobile robots, it is expected that in the future information on no-approach airspace established by government agencies and businesses will be updated either manually by the drone administrator or automatically by the drone control device itself, and that even if a pilot intentionally or accidentally tries to approach such a no-approach airspace, the drone will be prevented from entering the no-approach airspace. In such cases, it would be desirable to be able to set no-approach airspace in a variety of shapes, such as roughly enveloping buildings of various shapes, cylindrical or rectangular shapes that cover a certain area around a steel tower or chimney, or hemispherical shapes centered on a certain point.

[0009] In this case, it would be desirable to be able to represent these various no-approach airspace shapes using a single framework and a small amount of data, since flight avoidance processing could also be performed using a single framework and with fewer computational resources. Furthermore, when manually spraying pesticides only on specific fields, for example, drones can be used in a similar way: by setting an interference detection area over the target field and flying the drone to spray. When the drone leaves the target field, the interference detection device's output changes from interference to non-interference, which is detected by a person or the drone's control device, and the drone then returns in the opposite direction to re-enter the target field. This is useful when fields are large and it is difficult to determine from the operating point on the ground whether the drone has left the target field at a point far from the operating point.

[0010] While unmanned operation and construction are becoming more common in agricultural and construction vehicles, in the future, unmanned operation will be required not only in large, simple plots of land or construction sites where surrounding trees have been cleared and maintained, but also in fields and construction sites where humans currently perform work and construction with skill and care. In such cases, it will be necessary not only to avoid collisions with other vehicles or people on the ground, but also to avoid contact or collision with high-hanging structures, such as tree branches that have not been removed. In this case, it is expected that the shortest distance and direction to the same branch will be different between the passenger compartment and the engine bonnet, and as a result, the risk of contact will be different between the two.

[0011] Safety sensors that prevent collisions and interference are now being put to practical use, not only for detecting objects on a two-dimensional plane, but also for detecting objects in three-dimensional space. Therefore, there is a demand for sensors that can easily set up interference detection areas of various shapes in three-dimensional space.

[0012] Conventionally, industrial articulated manipulators have often been equipped with a function that virtually defines a rectangular parallelepiped area in the space around the manipulator and outputs a signal from the control device to an external device when the hand enters that area. This function is used for safety purposes, such as prohibiting approach to the area to protect equipment within the area and interlocking with other cooperating robots and equipment, as well as for the purpose of actively using interference detection for control, such as issuing operation start commands to other cooperating robots and equipment. However, the shape of the area is limited to a rectangular parallelepiped, which is easy to set.

[0013] To summarize the above, as the operation of mobile bodies and sensors in various fields becomes more sophisticated, simply knowing the distance between the distance measurement means provided in the mobile body or sensor and the objects around the mobile body is not enough for the mobile body or sensor to detect and avoid collisions between the mobile body and surrounding objects, or to detect interference between an interference detection area set by the sensor and an object or person.Furthermore, it is necessary to be able to easily check whether or not interference occurs between a person or an object and an interference detection area of ​​various shapes, including the body of the mobile body, and to know the distance between each part of the interference detection area of ​​various shapes, including the body of the mobile body, and surrounding objects as accurately and as quickly as possible.

[0014] Regarding the shape of the collision detection area, the body shapes of moving objects are becoming increasingly complex, particularly for automobiles, agricultural machinery, construction machinery, humanoid mobile robots, etc. Therefore, there is a demand for a method of collision detection and distance calculation for the collision detection area that can handle such complex body shapes and that can handle as little data as possible.

[0015] Various methods have been proposed for detecting and avoiding collisions and interference between moving objects and sensors. However, when considering the background and requirements described above, these methods are not necessarily sufficient.

[0016] Patent Document 1 discloses a method of modeling a robot and its environment as a polyhedron composed of triangular or quadrilateral polygons, and determining interference based on whether the ridges that make up a polygon intersect with the polygons that make up the opposing polyhedron. However, this method is intended as a simulator to check the robot's operation in advance, and is not a technology for detecting or avoiding interference in real time while the robot is operating. The main reason it remains limited to simulation is that modeling the robot and its external environment as a polyhedron results in a huge amount of polyhedron data and data processing.

[0017] Looking specifically at the amount of computational processing required for this interference detection, when determining whether a polyhedron on the robot side interferes with a polyhedron on the environment side, it is necessary to perform m × n intersection checks between the edges and polygons for all m edges that make up the polygons on the robot side polyhedron and all n polyhedrons on the environment side.

[0018] Furthermore, because it is difficult to instantly reverse-calculate which polygons on one polyhedron are closest to which polygons on the other polyhedron, it is difficult to avoid the m × n combination exhaustive search and reduce the amount of calculations without special modifications to the processing program. Furthermore, even if only the robot moves, for either the robot or the environment polyhedron, calculations are required to convert the coordinate values ​​of all vertices of the polygons into the other's coordinate system according to the robot's movement. Another problem is that the accuracy of collision detection depends on the resolution of the polygons. Increasing the resolution to improve accuracy increases both the amount of data and the amount of calculation required for collision detection. Thus, although modeling the shape of the collision detection area using polygons is a common technique, it is not a good solution for real-time collision detection.

[0019] Patent Document 2 discloses a collision avoidance method for mobile robots that travel mainly along road-like paths, in which a control device for the mobile robot equipped with an obstacle sensor adjusts the movement speed relative to obstacles located in the robot's direction of travel. However, the collision avoidance process is performed according to the distance to the obstacle in a sensor coordinate system based on the sensor position, and the collision avoidance process does not take into account the distance between the obstacle and each part of the body, which is not necessarily a simple shape such as a rectangular parallelepiped or cylindrical. As a result, the movement path to avoid collision must be a large detour around the obstacle, making it difficult to move through narrow spaces.

[0020] Patent Document 3 discloses a technology for avoiding obstacles by calculating the shortest distance between the robot's body and an obstacle based on 3D models of both the robot's body and the obstacle. This allows the robot to navigate a collision-avoidance path with a small turn according to the shape of the robot's body and the obstacle, and even to move through narrow spaces. However, because the 3D model is converted into a convex polyhedron model and the shortest distance calculation is performed, a negative distance error occurs for concave portions of the body.

[0021] Therefore, if we imagine a mobile robot with a body shape similar to that of a human, equipped with manipulators as arms, and assuming that this mobile robot is to grasp an object at the back of a table, even though there is still some distance between the table top and the narrow waist, the shortest distance error associated with the convex polyhedron transformation may cause the waist and the table top to be judged to be in an interfering state, preventing the robot from bringing its hand close to the object at the back of the table.

[0022] Furthermore, although Patent Document 3 does not specify the specific format of the 3D model, obtaining the convex polyhedron model requires at least point cloud data on the constituent surfaces of the robot body and obstacles. This means that, as with Patent Document 1, the interference detection means must store a large amount of data. Furthermore, the GJK algorithm is used as an example of a method for calculating the shortest distance between two convex polyhedrons.

[0023] This method calculates the shortest distance between a point and a polygonal face that makes up a convex polyhedron, and unless special ingenuity is used, it requires an exhaustive search of combinations of vertices on one convex polyhedron and the polygons that make up the other polyhedron. Similar to Patent Document 1, the accuracy of the calculation of the shortest distance depends on the resolution of the polygons that make up the convex polyhedron, and coordinate transformation of the point cloud data of either the robot body or obstacles is required as the robot moves. For these reasons, the same problems as those in Patent Document 1 arise regarding the format of the 3D model.

[0024] Patent Document 4 discloses a technology relating to a method for setting detection conditions for a sensor to detect the approach of an object to a machine, but it is not a technology relating to the sensor itself. The interference detection area of ​​the sensor assumed in Patent Document 4 is two-dimensional, not three-dimensional. The interference detection area is set around the sensor, and it is not possible to set an interference detection area at a position distant from the sensor. [Prior art documents] [Patent documents]

[0025] [Patent Document 1] Japanese Patent Application Publication No. 8-278990 [Patent Document 2] Japanese Patent Application Publication No. 2019-21202 [Patent Document 3] Special Publication No. 2019-516146 [Patent Document 4] Patent Publication No. 2021-186946 Summary of the Invention [Problem to be solved by the invention]

[0026] In contrast to the above prior art documents, the requirements for interference detection that the present invention addresses are as follows: The area in which interference is detected may be a roughly cylindrical or roughly part-cylindrical area around the robot or sensor, a roughly rectangular or spherical no-approach area that is fixedly provided in a space away from the robot or sensor, the general shape of the car or robot body itself, or a shape offset a certain distance from the general shape of the body.

[0027] In this case, while using the same interference detection procedure, it is convenient if the shape of the interference detection region can be varied in a variety of ways, such as being roughly cylindrical as described above, roughly rectangular parallelepiped in some cases, or roughly the shape of an automobile or robot body in other cases.

[0028] It is also desirable to be able to represent such diverse shapes with a small amount of data.Furthermore, it is desirable to be able to easily check, with a small amount of calculation, whether there is interference between a surrounding object, the distance of which is measured in part or in whole by a distance measurement means, and the interference detection area, and to be able to easily and quickly obtain the shortest distance between a distance measurement point on a surrounding object and the interference detection area, in order to predict the possibility of interference after a certain period of time even when there is no interference, or to avoid surrounding objects by making excessively large detours.

[0029] The present invention has been made in view of the above, and has as its object to provide a means for implementing interference detection that can give an interference detection area that detects the presence or absence of interference a variety of shapes using a small amount of data, and that can easily and quickly determine the presence or absence of interference between a surrounding object and the interference detection area, as well as the shortest distance between the object and the interference detection area. [Means for solving the problem]

[0030] The interference detection device according to the present invention comprises: It is mounted on a moving object or a sensor, Based on the measured distance to the surrounding objects, a set 3D or 2D area is created. neighborhood An interference detection device that detects whether or not there is interference with an object, comprising: an interference detection area shape representation means that holds an interference detection area that detects whether or not there is interference in the form of a superquadric function; a setting switching means for switching the first to third interference detection area shapes; Equipped with. The first to third interference detection area shapes are a first area shape that should avoid active approach of a moving body or sensor equipped with an interference detection device, a second area shape that should avoid passive approach of the moving body or sensor to a surrounding object due to active movement of the surrounding object, and a third area shape that corresponds to the approximate shape of all or part of the moving body. [Effects of the Invention]

[0031] The interference detection device according to the present invention can provide an interference detection area that detects the presence or absence of interference with a variety of shapes using a small amount of data, and can easily and quickly determine the presence or absence of interference between a surrounding object and the interference detection area, as well as the shortest distance between the object and the interference detection area. [Brief explanation of the drawings]

[0032] [Figure 1] FIG. 1 is a block diagram showing the configuration and data transfer relationship when an interference detection device according to this embodiment is incorporated into a control device for a moving body. [Figure 2]FIG. 2 is a block diagram showing the configuration and data transfer relationship when the interference detection device of this embodiment is incorporated into a sensor. [Figure 3] FIG. 3 is a diagram for explaining an example of a sphere whose shape is expressed in the form of a superquadric function. [Figure 4] FIG. 4 is a diagram for explaining an example of a rectangular parallelepiped whose shape is expressed in the form of a superquadric function. [Figure 5] FIG. 5 is a diagram for explaining an example of a cylinder whose shape is expressed in the form of a superquadric function. [Figure 6] FIG. 6 is a diagram for explaining an example of an intermediate shape between a rectangular parallelepiped and an ellipsoid, the shape of which is expressed in the form of a superquadric function. [Figure 7] FIG. 7 is a diagram for explaining an example of a cone, the shape of which is expressed in the form of a superquadric function, and only the upper half of the cone is shown. [Figure 8] FIG. 8 shows a modification of the cylinder shown in FIG. [Figure 9] FIG. 9 shows a modification of the cylinder shown in FIG. [Figure 10] FIG. 10 is a diagram for explaining an example of a moving object whose shape is expressed in the form of a superquadric function. [Figure 11] FIG. 11 is a diagram for explaining an example of interference detection between the movement trajectory of a moving body and an interference detection area. [Figure 12] FIG. 12 is a diagram for explaining an example of interference detection between a manipulator and the body of a mobile robot equipped with a manipulator and a surrounding no-approach area. [Figure 13] FIG. 13 is a diagram for explaining an example of interference detection between a manipulator and a body in a mobile robot equipped with a manipulator. [Figure 14] FIG. 14 is a diagram for explaining an example of interference detection at distance measurement points by a sensor. [Figure 15] FIG. 15 is a diagram for explaining an example of a process for searching for the nearest point on the body of a moving object. [Figure 16] FIG. 16 is a diagram for explaining an example of a process for calculating the shortest distance between a point outside the body of a moving object and the body. DETAILED DESCRIPTION OF THE INVENTION

[0033] In this embodiment, the three-dimensional shape of the interference detection area is represented by a superquadric function, which is simple yet has a wide range of shape expressibility. This makes it possible to represent the shape of the interference detection area with a small amount of data, from a simple, roughly cylindrical shape to the more complex body shapes of an automobile or robot, and also makes it possible to change the shape easily and flexibly.

[0034] By substituting the coordinate values ​​of the point (x, y, z) into the superquadric function formula, it is possible to easily determine whether the superquadric function surface is inside or outside by checking whether the calculated value exceeds 1, making it possible to easily and quickly determine whether an object is interfering with the interference detection area.

[0035] Furthermore, even if it is determined that no interference has occurred through interference check using this inside / outside judgment of the superquadric function surface, it is possible to efficiently calculate the distance between an object and the boundary of the interference detection area, which is necessary to predict the possibility of a future collision or to generate a trajectory for a moving body equipped with this interference detection device to avoid collision with the object.

[0036] For this purpose, we utilize the property that, although the surface shape given by a superquadric function forms a solid in the orthogonal three-dimensional space of X, Y, and Z, any point on the surface can be sampled (= pointed to) using two variables: azimuth angle and elevation angle.

[0037] Hereinafter, an embodiment of an interference detection device and an interference detection method according to the present disclosure will be described in detail with reference to the drawings. Note that the present invention is not limited to these embodiments. Furthermore, the components in the following embodiments include those that are replaceable and easily conceivable by a person skilled in the art, or those that are substantially the same.

[0038] The interference detection device according to this embodiment is a device that detects the presence or absence of interference between a set three-dimensional or two-dimensional area and a surrounding object based on the measured distance to the surrounding object. The interference detection device functions by being incorporated into the control systems of mobile objects such as automobiles, mobile robots including drones and automated guided vehicles, agricultural machinery, and construction machinery. It also functions by being incorporated into safety sensors that are mounted on these mobile objects and form the control systems of the mobile objects. It also functions by being incorporated into the control systems of fixed equipment such as various types of automated equipment.

[0039] 1 is a block diagram showing the configuration and data exchange relationship when the interference detection device of this embodiment is incorporated into a mobile body control device of a mobile body. In the mobile body control device 3 of the mobile body 1, the interference detection device 4 of this embodiment detects whether or not there is interference with the interference detection target area, and calculates the shortest distance between the interference detection area and a distance measurement point. These results are passed to the execution control means constituting the mobile body control device 3, which stops the traveling unit drive means of the mobile body 1, or to the collision avoidance means, which uses these results to control the traveling unit drive means so that the mobile body 1 avoids obstacles.

[0040] 2 is a block diagram showing the configuration and data exchange relationship when the interference detection device of this embodiment is incorporated into a sensor. When incorporated into sensor 2, interference detection device 4 of this embodiment returns the interference detection result to an external device, that is, to mobile body control device 3 in FIG. 2, whether or not a distance measurement point in the surrounding environment, the distance of which is measured by the distance measurement means, is interfering with the interference detection area. When a separate command is received from the external mobile body control device 3, the shortest distance between the interference detection area and the distance measurement point is also returned to the mobile body control device 3.

[0041] 1 and 2, the components of the interference detection device 4, namely, interference detection area shape representation means 41, interference detection means 42, interference detection area shape setting switching means 43, and interference detection area shortest distance calculation means 44, will be described.

[0042] First, the collision detection area shape representation means 41 will be described. The collision detection area shape representation means 41 holds the collision detection area that detects the presence or absence of collision in the form of a superquadric function. There are three types of collision detection areas targeted by the collision detection device 4 of this embodiment, corresponding to the usage format of the device. The first type is an area that is set in advance at an arbitrary position and orientation in the coordinate system of the surrounding environment of the mobile object 1 or sensor 2, and is an area to which the mobile object 1 or sensor 2 itself should avoid active approach.

[0043] The second is a region that is set at an arbitrary position and orientation outside the mobile body 1 or sensor 2 and in a coordinate system on the mobile body 1 or sensor 2, and is a region where passive approach of targets such as other objects or people should be avoided. The third is an interference detection region that corresponds to the approximate shape of all or part of the body of the mobile body 1.

[0044] The second, passive approach of an object, includes the new detection of an object due to the active movement of the mobile object 1 or the sensor 2. The three-dimensional shape of the above interference detection area is defined by the superquadric function shown in the following equation (1).

[0045]

number

[0046] The superquadric functions referred to here are functions expressed as superquadrics in English. Superquadric functions form three-dimensional shapes, but if the parameters ε1 = ε2 = 1 in equation (1), and if a1, a2, and a3 are all set to the same value, the shape will be a sphere, and if a1, a2, and a3 are set to different values, the shape will be an ellipsoid.

[0047] As described above, a1, a2, and a3 are dimensional parameters that define the size or extent of the three-dimensional shape given by the superquadric function in the X, Y, and Z directions. On the other hand, ε1 and ε2 are both shape parameters that control the roundness of the shape. In this embodiment, a 3D model of a moving object 1 or an obstacle is created using a superquadric function. For example, for common shapes such as an approximately cylinder, an approximately rectangular parallelepiped, an approximately cone including a truncated cone, an approximately ellipsoid including a sphere, an approximately torus, and an approximately pyramid, a large number of shapes with pre-set parameters are prepared. The user can select a shape that is close to the actual shape and fine-tune the parameters to more closely approximate the actual shape. For complex shapes, the manufacturer of the moving object provides a mathematical formula that represents the shape, or the user of the sensor 2 can adjust the parameters in the formula.

[0048] Figures 3 to 7 show point cloud meshes of three-dimensional shapes given by the basic superquadric function shown in equation (1) along with the set values. Figure 3 is an example of a sphere. Figure 4 is an example of a rectangular parallelepiped. Figure 5 is an example of a cylinder. Figure 6 is an example of an intermediate shape between a rectangular parallelepiped and an ellipsoid. Figure 7 is an example of a cone, with only the upper half shown. To illustrate these, it is necessary to sample points on the superquadric surface. A superquadric coordinate system (X S ,Y S ,Z S ) at point P on the superquadric surface S =(x S ,y S ,z S ) T is sampled according to the following equation (2), which is an extension of the transformation equation between ellipsoidal coordinates and orthogonal three-dimensional coordinates.

[0049]

number

[0050] sgn() is the sign function. As shown in equation (2), ω is the superquadric function coordinate system (X S ,Y S ,Z S ) and point P S The vector connecting SThe angle formed with respect to the axis corresponds to longitude on the Earth, and is called the azimuth angle in this embodiment. S -Y S The angle formed with respect to the plane corresponds to latitude on the Earth, and in this embodiment is called an elevation angle.

[0051] The collision detection area can also be a two-dimensional area on a plane. A two-dimensional area is equivalent to setting the z term in the parentheses ( ) on the left side of equation (1) to 0, and in equation (2) the elevation angle η should be set to 0. As a two-dimensional area is thus encompassed in a three-dimensional area, the following explanation will continue assuming that the collision detection area is a three-dimensional area.

[0052] Next, we will show how to improve the shape representation of superquadric functions. There are several extensions of superquadric functions. One of them is called deformable superquadrics, which is a form in which x, y, and z in equation (1) are not just variables but functions called deformed functions, as shown in the following equation (3).

[0053]

number

[0054] In formula (3), f x -1 (y,z), f y -1 (x,z), f z -1 (x,y) are the transformation functions f x (y,z), f y (x,z), f z It is the inverse function of (x,y).

[0055] As for deformation functions, as described in Reference 1, tapering, bending, cavity deformation, etc. have been proposed, and composite transformations of these are also possible.

[0056] Reference 1: F. Solina, R. Bajcsy, Recovery of Parametric Models from Range Images: The Case for Superquadrics with Global Deformations, IEEE Transactions on Pattern Analysis and Machine Intelligence, pp.131-146, Vol.12, No.2, (1990).

[0057] Figure 8 shows a modified version of the cylinder shown in Figure 5, where tapering is applied to increase or decrease the diameter of the bottom and top surfaces by 30% relative to the center, and bending is applied to bend the cylinder at an elevation angle of 20°. Figure 9 shows an example of a cylinder that has been deformed into a torus shape by bending the cylinder at an elevation angle of 180°. Basically, any deformation function can be defined, but if it is necessary to determine whether a coordinate point is inside or outside by substituting coordinate values ​​into equation (3), an inverse transformation must be defined along with the forward transformation.

[0058] Figure 10 shows a shape that resembles a moving body (for example, the body of a minivan) that has been subjected to deformation using a sigmoid function with an inverse function in addition to tapering deformation. If it is not necessary to determine whether the object is inside or outside by substituting coordinate values ​​into equation (3), more complex shape deformation can be performed using various function approximation methods such as regression or curve interpolation without considering inverse transformation.

[0059] The samples of points on the superquadric function surface are also used by the interference detection means 42 and the means 44 for calculating the shortest distance to the interference detection area. In the former, to detect interference with the target shape without bias, and in the latter, to accurately find the point on the interference detection area that is closest to an external point, it is desirable to be able to sample points on the interference detection area as uniformly as possible, like the surface meshes shown in Figures 3 to 10, 12 and 13, and 15 and 16.

[0060] There are several methods for uniform sampling, and detailed information is provided in references 2 and 3. However, some adjustments may be necessary depending on the method to be applied, the shape of the deformation function, and the shape of the surface to be approximated.

[0061] Reference 2: E. Bardinet, LD Cohen, N. Ayache, A parametric deformable model to fit unstructured 3D data, Computer Vision and Image Understanding, 71(1), pp.39-54, 1998.

[0062] Reference 3: BC Vemuri, A. Radisavwevic, Multiresolution Stochastic Hybrid Shape Models with Fractal Priors, ACM Transactions on Graphics, Vol. 13, No. 2, pp.177-207, 1994.

[0063] The examples in FIGS. 3 to 10 are based on Reference 1, in which case Equation (2) is transformed into Equation (4) below.

[0064]

number

[0065]

number

[0066] The important thing about both equations (2) and (4) is that although the superquadric surface forms a solid, if you specify only two angles, the azimuth angle ω and the elevation angle η, you can find the three-dimensional coordinate value point (x S ,y S ,z S) are the points that can be sampled at any time. By varying the definition ranges of these two angles shown in equations (2) and (4) with an arbitrary resolution, it is possible to sample points on the entire surface.

[0067] This is because a superquadric is geometrically a two-dimensional manifold like a sphere, and just as a position on the Earth's surface is determined by longitude and latitude, a point on the superquadric surface is determined by azimuth and elevation angles. This allows for efficient interference detection and shortest distance calculation, as will be described later.

[0068] This characteristic is significantly different from that of conventional technologies, which require the point cloud data that constitutes the interference detection area and the three-dimensional coordinate values ​​(x, y, z) of the vertices of polyhedrons to be stored at a specific roughness resolution, and which do not have a means of directly accessing points in specific parts of the area without a search means.

[0069] In addition to the extended system in which x, y, and z in equation (1) are transformed functions described here, there are also other systems such as Extended Superquadrics in which the parameters ε1 and ε2 in equation (1) are functions, a further generalized form called Hyperquadrics, and a form in which a1, a2, and a3 in equation (1) are functions. In this embodiment, these extended systems are collectively referred to as superquadrics, and are included in the scope of the present invention.

[0070] In addition to the examples of 3D shapes shown in Figures 3 to 10, these extension systems can be used to represent a variety of 3D shapes with a small, finite number of parameters, such as a wedge, banana, hat, ashtray, shoe, etc. Conversely, it is also possible to fit a superquadric function to shape measurement data of various objects using optimization calculations, and there are examples where superquadric functions have been adapted to the shapes of beverage cans, candy packets, the ventricles of the heart, etc.

[0071] In this way, the superquadric function, which can express a variety of three-dimensional shapes using the five parameters of equation (1) that define the size and shape and a small, finite number of parameters that define various deformations, is suitable for the shape expression means 41 of the interference detection area.

[0072] Next, the interference detection means 42 will be described. In the coordinate system (X S , Y S , Z S ), the so-called inside / outside determination as to whether a certain point P(x, y, z) is located inside the surface represented by the superquadratic function can be easily determined by simply substituting the coordinate values of (x, y, z) into the left side of Equation (1) or Equation (3), which is called the inside-outside function.

[0073] That is, when the left side of Equation (1) is represented as F(x, y, z), if F(x, y, z) < 1, the point P is located inside the surface represented by the superquadratic function; if F(x, y, z) = 1, the point P is located on the superquadratic function surface; and if 1 < F(x, y, z), the point P is located outside the superquadratic function surface. The same applies to Equation (3). Using this characteristic, the interference detection means 42 in this embodiment detects interference for the following three types.

[0074] First, first, it is determined whether a point on the movement trajectory sampled from the movement trajectory of the moving body 1 interferes with the interference detection area represented by the superquadratic function. FIG. 11 is a diagram for explaining an example of interference detection between the movement trajectory of the moving body 1 and the interference detection area. FIG. 11 shows an interference detection area 111 and a movement trajectory 112. The interference detection area 111 shown in FIG. 11 may correspond to a prohibited approach area set around the outside of the moving body 1 or the moving body 1 itself.

[0075] Points on the movement trajectory 112 of the moving body 1 such as the manipulator tip or the drone are regarded as representing the moving body 1, and this is a process for detecting interference more roughly than the following second detailed interference detection, and is intended to be executed before the operation of the moving body 1 or to be executed in a form that pre-reads points on the trajectory during the operation and calculates them earlier than the actual operation.

[0076] In other words, the interference detection means 42 sets an interference detection area expressed in a superquadric form, which corresponds to a no-approach zone, within the space surrounding the interference detection device 4, and determines whether sample points of the movement trajectory of the mobile body 1 provided by the control means of the mobile body 1 interfere with the interference detection area by performing an inside / outside determination calculation for each trajectory sample point with respect to the superquadric surface representing the interference detection area. Note that the interference detection area 111 may be configured by combining and arranging multiple superquadric surfaces, and the multiple superquadric surfaces may each have a different shape. In addition, the trajectory data required for this processing is passed to the interference detection means 42 from the trajectory storage means or trajectory generation means of the mobile body control device 3.

[0077] Secondly, it is determined whether point cloud data approximating the overall shape of the moving body 1 or the shape of a portion of the moving body 1 where interference is to be detected or avoided interferes with the interference detection area expressed by a superquadric function. This shape approximation point cloud data may be stored as data created based on CAD (Computer Aided Design) data for the moving body 1, but from the perspective of reducing the amount of data storage, it is more appropriate to sample data as appropriate when interference is detected based on equations (2) and (4) from a superquadric function approximating the shape of the relevant portion.

[0078] In this case, the interference detection area may correspond to a no-approach area set around the mobile object 1, or may be the body of the mobile object 1 itself. FIG. 12 is a diagram illustrating an example of interference detection between a manipulator and the body of a mobile robot equipped with a manipulator and the surrounding no-approach area. FIG. 13 is a diagram illustrating an example of interference detection between a manipulator and the body of a mobile robot equipped with a manipulator. FIGS. 12 and 13 show a manipulator 121, a mobile robot 122, an interference detection area 123, and an interference occurrence point 124. Note that the point cloud meshes of the outer shapes of both the body 133 and the manipulator 121 shown in FIGS. 12 and 13 are generated by sampling a superquadric function.

[0079] Considering a mobile robot 122 equipped with a manipulator 121 (FIGS. 12 and 13), the manipulator portion is approximated by a hyperquadric function, and it is possible to determine whether a point cloud roughly sampled from the manipulator will interfere with the no-approach zone outside the mobile robot 122 using the method based on the interior and exterior functions described above (FIG. 12). Similarly, it is also possible to set the interference detection zone 123 as the body 133 of the mobile robot 122 and determine whether the manipulator 121 will interfere with the body 133 during operation (FIG. 13). Like the first detection process, this detection process is intended to be executed before the mobile body 1 starts to move, or to look ahead to points on the trajectory during operation.

[0080] In other words, the interference detection means 42 sets an interference detection area 123 expressed in a superquadric form, which corresponds to a no-approach area, within the space surrounding the interference detection device 4, and when a mobile body 1 (mobile robot 122) incorporating the interference detection device 4 as part of the mobile body control device 3 moves along a predetermined trajectory, it represents the shape of part or all of the constituent parts of the mobile body 1 as point cloud data, and determines whether part or all of the constituent parts will interfere with the interference detection area 123 by performing an inside / outside determination calculation for each point of the point cloud with respect to the superquadric function surface representing the interference detection area 123.

[0081] Third, it is determined whether a surrounding object or environmental distance measurement point corresponding to the distance measured by the distance measurement means provided on the mobile object 1 or sensor 2 interferes with the interference detection area expressed by the superquadric function. For example, an interference detection area equivalent to a no-approach area is set around the mobile object 1 or sensor 2, and the intrusion of a person or vehicle into a specific range in the direction of travel of the mobile object 1 and the approach of an obstacle are detected, or the intrusion of a person or vehicle into the monitoring area of ​​sensor 2 fixedly installed near a dangerous location is detected. FIG. 14 shows an example of this. FIG. 14 shows the interference detection of a distance measurement point by sensor 2. FIG. 14 shows an interference detection area 141 expressed by a superquadric function, a distance sensor 142 as an example of sensor 2, and a distance measurement point (distance detection point) 143.

[0082] By performing coordinate transformation, the superquadric function can be placed at any position in three-dimensional space in any orientation. This makes it possible to set an interference detection area equivalent to a no-approach area tilted in space in the first interference detection, or to approximate the link shape of the manipulator 121 with a superquadric function (FIGS. 12 and 13) in the second interference detection, and determine whether or not the links moving with six degrees of freedom will interfere with the interference detection area 123 equivalent to a no-approach area set in any coordinate system space (FIG. 12). This will be specifically shown using an example of the most basic coordinate transformation.

[0083] The superquadric function coordinate system (X) has its origin at the center of the three-dimensional shape of the superquadric function surface, which is the same as the center of a sphere or ellipsoid, which is the basic form of the superquadric function. S ,Y S ,Z S ) point p on S =(x S ,y S ,z S ) T and a distance measurement means coordinate system (X M ,Y M ,Z M ) point p on M =(x M ,y M ,z M ) T The relationship between these is expressed by the following equations (6) and (7):

[0084]

number

[0085]

number

[0086] These equations are general equations that represent coordinate transformation. Among the components of the coordinate transformation matrix T in equation (6), d is the coordinate system (X M ,Y M ,Z M) from the origin of the superquadric coordinate system (X S ,Y S ,Z S ) to the origin, R is a vector (3 × 1) that represents the translation of (X M ,Y M ,Z M ) to (X S ,Y S ,Z S ) represents a 3x3 rotation matrix.

[0087] The points on the hyperquadric surface sampled by equations (2) and (4) can be placed at any position in the distance measurement means coordinate system with any rotational orientation using equation (6). Similarly, it is naturally possible to transform the distance measurement means coordinate system into, for example, the mobile body coordinate system that holds the distance measurement means, or from the mobile body coordinate system into the world coordinate system in which the mobile body 1 travels.

[0088] Conversely, by converting the distance measurement point 143 on the distance measurement means coordinate system corresponding to the distance measured by the distance measurement means into the superquadric function coordinate system according to equation (7), it is possible to determine whether the distance measurement point is within the interference detection area 141 expressed by the superquadric function by an inside / outside determination using an inside / outside function in the interference detection means 42.

[0089] Using general knowledge of robotics, the position and attitude angle of each link that makes up the manipulator during manipulator movement can be calculated using the concept of kinematics, and the calculated position and attitude angle can be substituted into d and R in equation (6), respectively. This makes the second form of interference detection possible. There are various ways to express attitude angles, such as Euler angles and angular axis vectors, but either can be converted into the rotation matrix format of equation (6).

[0090] Next, the interference detection area shape setting and switching means 43 will be described. The interference detection area shape setting and switching means 43 stores a plurality of parameters related to the shape and location of the hyperquadric function surface that represents the interference detection area for detecting the presence or absence of interference. In either case, when the interference detection device 4 is incorporated into a mobile body (FIG. 1) or when the interference detection device 4 is incorporated into the sensor 2 (FIG. 2), these parameters can be set or switched by signals or communications from outside the interference detection device 4, or by manual setting. Furthermore, a user or manufacturer may add hyperquadric function parameters corresponding to a new interference detection area shape or set it as a processing target from outside the mobile body 1 or sensor 2 via an external interface.

[0091] For example, the means 43 for setting and switching the shape of the interference detection area receives an interference detection area switching command corresponding to the operation mode of the mobile body 1 from the execution control means of the mobile body control device 3. For example, the switching command may switch the parameters of the hyperquadric function with a specified number to an interference detection area outside the mobile body in order to detect interference between the mobile body 1 moving on a predetermined trajectory and a specific surrounding object, or may switch the interference detection area to the body shape of the mobile body with a specified number.

[0092] The interference detection area targeted by the interference detection device 4 of this embodiment is of three types corresponding to the usage format of the device.

[0093] The first type is a region that is set in advance at an arbitrary position and orientation in the coordinate system of the surrounding environment of the mobile body 1 or sensor 2, and is a region that the mobile body 1 or the sensor 2 mounted on the mobile body 1 should avoid actively approaching. The second type is a region that is set at an arbitrary position and orientation in the coordinate system outside the mobile body 1 or sensor 2 and on the mobile body 1 or sensor 2, and is a region that should avoid passive approach of targets such as other objects or people. Note that passive approach of targets includes the detection of new targets as the mobile body 1 or sensor 2 actively moves. The third type is an interference detection region that corresponds to the approximate shape of all or part of the body of the mobile body 1.

[0094] In response to the differences between the three types of interference detection areas, the interference detection area shape setting switching means 43 accepts manual setting of the interference detection area shape when changing the operation of the mobile body 1 or the sensor 2, and also receives a command from the execution control means of the mobile body control device 3 according to the activity status of the mobile body 1 and switches the interference detection area.

[0095] First, we will explain how the system works when supporting the first and second types. For typical general shapes such as an approximately cylinder or an approximately rectangular parallelepiped, the user can select the shape manually from an external interface or by external signals or communications, and the roundness parameters ε1 and ε2 in equation (1) are given in advance so that the superquadric surface will have the selected shape, and the dimensional parameters a1, a2, and a3 can be changed by the user.

[0096] Furthermore, the translation amount d and rotation matrix R that make up the coordinate transformation matrix T in equation (6) can be set and saved for the set position and orientation of the collision detection area given by the superquadric surface. As a result, it is possible to select a shape from among general representative shapes such as an approximately cylinder, an approximately rectangular parallelepiped, an approximately cone including a truncated cone, an approximately ellipsoid including a sphere, an approximately torus, and an approximately pyramid, as shown in Figure 3, and by storing in advance the parameters that will result in the superquadric surface becoming the selected shape and making it possible to manually change the dimensions and placement position, the user can select a variety of 3D shapes for the collision detection area while saving the effort of detailed settings.

[0097] Next, we will discuss the specification and switching of the body shape of the third type of moving body 1. Because the body shape of a moving body 1 is complex, it is reasonable to think that it is expressed by a combination of multiple superquadric functions or a transformation function of a superquadric function specialized for that body shape. Furthermore, regarding the body shape, the user who operates the moving body 1 does not usually set parameters of a superquadric function including a transformation function in the interference detection device 4. It is assumed that the normal operation mode is for the manufacturer of the moving body 1 that incorporates the interference detection device 4 to set parameters of a superquadric function related to the body of a specific model in the interference detection device 4.

[0098] In this case, considering that the body shape of the moving body 1 is generally complex and model-specific, and that it is desirable to shorten the processing time for interference detection, it is appropriate for the manufacturer of the moving body 1 to directly write a mathematical formula representing the body shape, including a combination of multiple superquadric functions and a deformation function that approximates the contour, into the processing execution program of the interference detection means 42. Then, the interference detection area shape setting switching means 43 is configured to call this function when it receives a designation from outside the interference detection device 4, or to notify the number so that the interference detection means 42 can switch the function that executes the processing.

[0099] Similarly, for transformation functions of superquadric functions defined by individual mathematical expressions, the transformation functions are implemented for each type in the processing execution program of the interference detection means 42. Then, when a transformation function of a format designated by the interference detection area shape setting switching means 43 is called in the processing execution program of the interference detection means 42, the interference detection area shape setting switching means 43 passes the stored parameters of the transformation function to the interference detection means 42, together with parameters that determine the shape and position of the superquadric function.

[0100] Next, the means 44 for calculating the shortest distance to the interference detection area will be described. The means 44 for calculating the shortest distance to the interference detection area samples points on the hyperquadric function surface that represents the interference detection area, thereby calculating the shortest distance between the interference detection area and distance measurement points on surrounding objects that exist around the interference detection device 4. The target points for which the shortest distance to the interference detection area is calculated are of the following three types, the same as the interference detection targets of the interference detection means.

[0101] The first type is distance measurement points on the objects around the moving body 1 or the sensor 2, which correspond to the distance measured by the distance detection means. The second type is each point of point cloud data that approximates the overall shape of the moving body 1 or the shape of a part where interference is to be detected or avoided. The third type is points on the trajectory of the moving body 1.

[0102] Even if the interference detection means 42 determines that there is no interference, it is very useful for the collision avoidance means 36 to predict the possibility of a collision after a certain period of time or to avoid surrounding objects by not making too large a detour, to know the shortest distance between a distance measurement point on a surrounding object or a point on the trajectory of the moving body 1 and the interference detection area.

[0103] Therefore, by repeatedly sampling points on the superquadric function surface while changing the azimuth angle and elevation angle according to formula (2) or formula (4), and exploratory calculation of the distance between each of these points and the target point, the shortest distance between the target point and the collision detection area is calculated. An example of this procedure is specifically shown below.

[0104] First, the azimuth and elevation angles at which to start the search calculation for the shortest distance are calculated from the position of the target point for which the shortest distance is to be calculated.The surface given by the superquadric functions shown in equations (1) and (3) can be regarded as a surface obtained by scaling the surface of a sphere or ellipsoid using parameters ε1 and ε2 or a deformation function, or as a surface obtained by distorting it.

[0105] Based on this understanding, several methods for inverse calculation using spheres or ellipsoids can be considered. Which method gives the angle closest to the true value may vary depending on the shape of the hyperquadric surface, but here we will show an example of a simple and widely applicable method.

[0106] This method focuses only on a1, a2, and a3 in equation (1), which are parameters that define the extent of the superquadric surface, and calculates the azimuth and elevation angles that give the point on the ellipsoid that is closest to the target point for the ellipsoid determined by these three parameters. When the target point (x, y, z) for which the shortest distance is to be calculated is given, the azimuth angle ω and elevation angle η of the point on the ellipsoid that corresponds to this point before transformation into a superquadric, i.e., ε1 = ε2 = 1, can be easily calculated using general geometric knowledge about ellipsoids using the following equation (8).

[0107]

number

[0108] Next, the search for the shortest distance begins using the two angle values ​​of azimuth and elevation obtained in this way as the initial search values. Below is a summary of the overall procedure for calculating the shortest distance and shortest-distance point between point A and the interference detection area given by a superquadric function when point A in three-dimensional space is given. Note that if the shape of the interference detection area is a sphere and the superquadric function represents it, where a1 = a2 = a3, then these azimuth and elevation angles are the angle values ​​that give the shortest-distance point, and it goes without saying that the search process described below is not necessary.

[0109] (Procedure for calculating the shortest distance point) Step 1: For example, according to equation (8), the azimuth angle and elevation angle of point A in the interference detection area are calculated. These azimuth angle and elevation angle are set as the initial angles for the search. Also, a large value is set as the provisional shortest distance value. Step 2: Search for the shortest distance within a certain angle range near the initial angle found in step 1 as follows: Step 2-1: The azimuth angle and elevation angle are each changed by a fixed angular increment around the initial search angle. Step 2-2: Substitute the changed azimuth and elevation angles into equation (4) to sample a point on the surface given by the superquadric function, and set this as point B. Step 2-3: The distance between point A and point B is calculated, and if the distance value is smaller than the temporary shortest distance value, the temporary shortest distance value is updated to that distance value. At the same time, the coordinates of point B are stored as the temporary shortest distance point. Step 3: When the search for the azimuth angle and elevation angle has been completed within the predetermined angle range, the provisional shortest distance and provisional shortest distance point at that time are set as the final provisional shortest distance and provisional shortest distance point, respectively.

[0110] The azimuth and elevation angles that gave the shortest distance point in step 3 can be used as the initial search angles, and the vicinity can be searched for at finer angle increments according to step 2, to calculate a more accurate shortest distance.

[0111] Figures 15 and 16 show the process of calculating the shortest distance to a point outside an interference detection area with a complex shape based on the above method. Figure 15 shows the process of searching for the nearest point on the body. Figure 16 shows an example of calculating the shortest distance between a point outside the body and the body. In this example, the object of the interference detection area is the body shape of the mobile robot CONOID-3 manufactured by Shibaura Machine, which is equipped with a manipulator. CONOID is a registered trademark. The transformation function f in equation (3) x , f y The approximate formula for the contour shape is set to, and the body shape is expressed by a super-quadratic function.

[0112] Point 151 in Figure 15 is the point for which the shortest distance is calculated. Point 152 indicates the initial search point on the body corresponding to the initial search angles of azimuth and elevation obtained based on equation (8). Line 153 indicates a vector corresponding to the distance calculated by changing the azimuth and elevation angles around the initial search angles during the search process.

[0113] From the above search process, the shortest distance and the corresponding shortest distance point are found. Here, although the hyperquadric function surface forms a solid in the orthogonal three-dimensional space of X, Y, and Z, based on equations (4) and (2), as shown in the above-mentioned interference detection means 42, an arbitrary point (x S ,y S ,z S ) can be sampled in two variables: azimuth angle ω and elevation angle η.

[0114] Therefore, without searching in the three directions of X, Y, and Z, by sampling points on the superquadric surface by changing two angles limited to the vicinity of the initial angle of the search narrowed down as shown in equation (8), the shortest distance point and the shortest distance can be calculated efficiently.

[0115] Figure 16 shows an example in which the shortest distance point and shortest distance are calculated using the above-mentioned search process for point 161 shown in Figure 16. Point 162 is the search initial point corresponding to the search initial angles of azimuth and elevation. Line 163 is a vector corresponding to the shortest distance calculated for point 161.

[0116] The data required to represent this body shape are the five parameters in equation (1) and f which approximates the contour shapes of the left and right sides. y and two f functions that approximate the contour shapes of the front and rear surfaces. x This is a simple function, which is less complicated than the shape. Figures 12 and 13 also show the same robot.

[0117] In this way, the body is set as the interference detection area, and the shortest distance between the body and a representative point on the manipulator or surrounding object and the shortest distance point on the body that gives the shortest distance are determined. Information on the shortest distance and the shortest distance point is transmitted to the collision avoidance means 36 shown in Figure 1, thereby making it possible to avoid collision between the body and the manipulator, or between the body and surrounding objects.

[0118] As described above, the interference detection device 4 of this embodiment detects the presence or absence of interference between a set three-dimensional or two-dimensional area and an object based on the measured distance to the surrounding object, and stores the interference detection area for detecting the presence or absence of interference in the form of a superquadric function.

[0119] For example, it is clear that it is preferable to use a superquadric function for the interference detection area that detects the presence or absence of interference with a surrounding object, even from the fact that the various three-dimensional shapes shown in Fig. 1 can be expressed by the simple formula (1). When the interference detection device 4 of this embodiment stores the shape of the interference detection area, it is not necessary to store all point cloud data or all polygon data of the boundary of the interference detection area as in existing technologies, and with regard to shape information other than the position and orientation, it is only necessary to store five parameters that define the size and shape of the superquadric function, and a small, finite number of parameters that give shape deformation.

[0120] Furthermore, the interference detection device 4 of this embodiment determines whether or not a sample point of the movement trajectory of the moving body 1 provided by the control means of the moving body 1 interferes with the interference detection area by performing an inside / outside determination calculation of each trajectory sample point with respect to the hyperquadric function surface representing the interference detection area.

[0121] For example, to determine whether or not there is interference with the interference detection area, all that is required is to input the (x, y, z) coordinate values ​​of a point on an object or a point on a trajectory for which you want to determine whether or not there is interference into a superquadric function formula, and then check whether or not the superquadric function formula exceeds 1. This means that interference can be detected in a very short time, for example, on an actual robot in real time while it is operating, or immediately before it starts operating.

[0122] Specifically, when there are n points in an interference detection area where interference must be determined, the number of times interference determination is performed in this embodiment is the simple inside / outside determination calculation described above, which is n times. This is because, for example, when an interference detection area formed by a polygonal polyhedron has m edges (of polygons) and interference between that polyhedron and another polyhedron formed by n polygons is to be determined, the number of processing calculations is significantly different from that of conventional methods which require m×n combination intersection determinations between edges and polygons.

[0123] Furthermore, the interference detection device 4 of this embodiment stores a small number of setting parameters of the hyperquadric function surface that represents the interference detection area for detecting the presence or absence of interference in a setting and switching means for the interference detection area shape, and sets or switches these parameters manually or by external signals or communication.

[0124] For example, in switching the settings of the collision detection area shape, in order to take advantage of the characteristics of the superquadric function and enable a variety of 3D shapes to be set as collision detection areas, it is now possible to switch or adjust a small number of parameters that define the superquadric function manually or via external communication or signals, etc. As a result, it is now possible to select general representative shapes such as an almost cylinder or an almost rectangular parallelepiped, and by providing parameters in advance that will result in the superquadric function surface becoming the selected shape and making it possible to manually change the dimensions and placement position, the user can select a variety of 3D shapes for the collision detection area while saving the effort of detailed settings.

[0125] On the other hand, taking a mobile robot equipped with a manipulator as an example, when traveling over a wide area without using the manipulator, a roughly rectangular parallelepiped interference detection area whose size automatically changes depending on the traveling speed is set up in front of the mobile robot, and obstacles on the traveling path are detected. Next, when performing a local task using the manipulator, the mobile robot's body is automatically switched to the interference detection area, and distance measurement points that measure the distance to surrounding objects and the multiple links of the manipulator, which are approximated by superquadrics like the body, are monitored to see if they interfere with the body. In addition, an interference detection area equivalent to a no-approach zone is automatically set outside the mobile robot, and a judgment is made to see if the trajectory of the manipulator's hand will interfere with this. The above-mentioned functions are possible.

[0126] Taking a sensor that detects the approach of people or objects as an example, it is possible to sometimes set an outer cylindrical interference detection area centered on the sensor in three-dimensional space by changing its diameter and height, and at other times manually switch to set an interference detection area equivalent to a roughly rectangular parallelepiped no-approach area in a space away from the sensor.

[0127] Furthermore, the interference detection device 4 of this embodiment samples points on the hyperquadric function surface that represents the interference detection area, thereby calculating the shortest distance between the interference detection area and distance measurement points on surrounding objects that exist around the interference detection device 4, which are required for collision avoidance processing.

[0128] For example, unlike conventional technology, which has difficulty in instantly calculating which polygon on one polyhedron is closest to which polygon on the other polyhedron when calculating the shortest distance to an interference detection area, this technology can instantly calculate the corresponding azimuth and elevation angles of the superquadric function for the target point for which the shortest distance is to be calculated, although this can be approximated depending on the interference detection area.

[0129] Furthermore, although the surface shape given by the superquadric function forms a solid in the orthogonal three-dimensional space of X, Y, and Z, at any point on the surface (x S ,y S ,z S) can be sampled using two variables, azimuth angle and elevation angle. Therefore, without searching in the three directions of X, Y, and Z, the shortest distance can be calculated efficiently by sampling points on the superquadric surface by varying the two angles limited to the vicinity of the inverse-calculated azimuth angle and elevation angle, and then exploratory calculation of the distance between that point and the target point.

[0130] In conventional technologies that approximate three-dimensional shapes with point clouds or polyhedra, data on the points that make up the point cloud and the vertices of the polygons that make up the polyhedron are stored in memory, but adjacent points or adjacent vertices are not necessarily adjacent in memory, making it difficult to narrow down the search for the shortest distance to a specific area.On the other hand, with superquadric functions, simply changing the azimuth and elevation angles near the inversely calculated shortest distance in 0.1-degree increments, for example, from 10° to +10°, makes it possible to continuously search around those angles, allowing for efficient shortest distance calculations.

[0131] Furthermore, even for three-dimensional shapes with concave portions, distance can be calculated with high accuracy because convex approximation is not performed as in conventional technologies. Furthermore, unlike conventional technologies, which require finer resolution for point clouds and polyhedrons to improve distance accuracy and consequently increase the amount of storage required for point clouds and polyhedrons, this technology allows the resolution of the changes in azimuth and elevation angles when searching for the shortest distance to be made coarse or fine without changing the amount of parameter storage required for the shape of the interference detection area. Therefore, by narrowing down the azimuth and elevation angles that give the point with the shortest distance in a coarse-resolution search and then performing a fine-resolution search, distance accuracy can be improved while shortening the distance calculation time.

[0132] The above-described embodiment can be modified as needed by changing part of the configuration or function of the interference detection device 4. Therefore, several modified examples of the above-described embodiment will be described below. The following mainly describes differences from the above-described embodiment, and detailed descriptions of commonalities with the content already described will be omitted. The modified examples described below may be implemented individually or in appropriate combination.

[0133] (Variation 1) For example, when it is difficult to approximate the three-dimensional shape of the interference detection area to a surface using a superquadric function, for example, when it is difficult to generate a group of points uniformly distributed on a superquadric function surface, or when approximating the contour using a deformation function is extremely difficult, the shape representation means 41 of the interference detection area may approximate the three-dimensional shape of the interference detection area to a shape in which cross sections at each height are stacked. Also, the contour shape of each cross section may be approximated by a two-dimensional superquadric function.

[0134] (Variation 2) For example, the interference detection means 42 may use one or more superquadric functions to represent the shape of the body of the mobile object 1 offset by a certain distance as the interference detection region, rather than the actual body shape of the mobile object 1. This makes it possible to detect early, by an amount equivalent to the offset, that there is a risk of interference with the body of the mobile object 1 by another object or the manipulator of the mobile robot.

[0135] (Variation 3) For example, in this embodiment, the means 44 for calculating the shortest distance to the interference detection area provides a method for efficiently calculating the shortest distance to an external point without using a gradient for a curved surface shape for which it is not easy to calculate the gradient of a superquadric function, as shown in Figures 15 and 16. Here, in order to specifically and directly explain the calculation method, a so-called grid search technique has been used. However, for step 2, instead of such a grid search, a nonlinear optimization method that does not use the gradient of the evaluation function, such as the Nelder-Mead method, may be used.

[0136] In the above-described embodiments, the program for executing the above-described processing has a modular configuration including the above-described functional units, and in actual hardware, for example, a CPU (Central Processing Unit) reads and executes the processing program from a ROM (Read Only Memory), an HDD (Hard Disk Drive), or an SSD (Solid State Drive), thereby loading the above-described functional units into a RAM (Random Access Memory) and generating the above-described functional units in the RAM. Note that some or all of the above-described functional units can also be realized using dedicated hardware such as an FPGA (Field-Programmable Gate Array).

[0137] Although the embodiments of the present invention have been described above, the above-described embodiments are presented as examples and are not intended to limit the scope of the present invention. This novel embodiment can be embodied in various other forms. Furthermore, various omissions, substitutions, and modifications can be made without departing from the spirit of the invention. Furthermore, this embodiment is included within the scope and spirit of the invention, and is also included in the inventions and their equivalents as defined in the claims. [Explanation of symbols]

[0138] 1. Mobile 2 sensors 3 Mobile control device 4. Interference detection device 41 Shape representation method for interference detection area 42 Interference detection means 43. Collision detection area shape setting switching means 44 Means for calculating the shortest distance to the interference detection area

Claims

1. An interference detection device that is mounted on a moving body or a sensor and detects whether or not there is interference between a set three-dimensional or two-dimensional area and a surrounding object based on the measured distance to the surrounding object, an interference detection area shape representation means for holding an interference detection area for detecting the presence or absence of interference in a superquadric function format; a setting switching means for switching the interference detection area shape from the first to third interference detection area shapes; Equipped with an interference detection device, characterized in that the first to third interference detection area shapes are a first area shape in which active approach of the moving body or the sensor on which the interference detection device is mounted should be avoided, a second area shape in which passive approach of the moving body or the sensor to the peripheral object due to active movement of the peripheral object should be avoided, and a third area shape corresponding to an approximate shape of all or part of the moving body.

2. a setting and switching means for setting and switching the shape of the interference detection area, which stores a small number of setting parameters of the hyperquadric function surface representing the interference detection area for detecting the presence or absence of interference, and which sets or switches the shape of the interference detection area manually or by an external signal or communication; 2. The interference detection device according to claim 1, further comprising:

3. a shortest distance calculation means for calculating the shortest distance between the interference detection area and a distance measurement point on a surrounding object existing around the interference detection device by sampling points on a hyperquadric function surface representing the interference detection area; 3. The interference detection device according to claim 1, further comprising:

4. an interference detection area expressed in the hyperquadric function format, which corresponds to a no-approach area, is set within a space surrounding the interference detection device; an interference detection means for determining whether a sample point of a moving trajectory of the moving body provided by a control means of the moving body interferes with the interference detection area by performing an inside / outside determination calculation of each trajectory sample point with respect to a hyperquadric function surface representing the interference detection area; The interference detection device according to claim 3, further comprising:

5. an interference detection area expressed in the hyperquadric function format, which corresponds to a no-approach area, is set within a space surrounding the interference detection device; an interference detection means for representing the shape of a part or the whole of a component part of a moving body, which incorporates the interference detection device as a part of a control device, as point cloud data when the moving body moves along a predetermined trajectory, and for determining whether a part or the whole of the component part interferes with the interference detection area by performing an inside / outside determination calculation for each point of the point cloud with respect to a hyperquadric function surface which represents the interference detection area; The interference detection device according to claim 3, further comprising:

6. The interference detection region is expressed by one or more hyperquadric functions, which represent the general shape of the body of a moving object incorporating the interference detection device as a part of a control device, or a shape obtained by offsetting the body shape by a certain distance. The interference detection device according to claim 3 .

7. An interference detection method executed by an interference detection device mounted on a moving body or a sensor, which detects whether or not there is interference between a set three-dimensional or two-dimensional area and a peripheral object based on a measured distance to the peripheral object, a step of retaining an interference detection region for detecting the presence or absence of interference in the form of a superquadric function; switching the shapes of the interference detection areas from first to third; Including, an interference detection method, characterized in that the first to third interference detection area shapes are a first area shape in which active approach of the moving body or the sensor on which the interference detection device is mounted should be avoided, a second area shape in which passive approach of the moving body or the sensor to the peripheral object due to active movement of the peripheral object should be avoided, and a third area shape corresponding to an approximate shape of all or part of the moving body.

Images