Environment map generation program and three-dimensional sensor control device

Abstract

To provide an environment map generation program capable of generating an environment map accurately and at high speed even in an environment where moving objects are present.SOLUTION: An environment map generation program 10 causes a computer to function as: a data acquisition unit 21 that acquires orthogonal-coordinate point cloud data of multiple imaging frames captured by a three-dimensional sensor with a fixed imaging range; a first coordinate conversion unit 23 that converts the orthogonal-coordinate point cloud data into polar-coordinate point cloud data on the basis of a first conversion formula from orthogonal coordinates to polar coordinates; a farthest-point search unit 24 that, for each small region of the imaging range, searches for polar-coordinate point cloud data farthest from the three-dimensional sensor as farthest-point data; a second coordinate conversion unit 25 that converts the farthest-point data into orthogonal-coordinate point cloud data on the basis of a second conversion formula from polar coordinates to orthogonal coordinates; and an environment map output unit 26 that outputs environment map data of the imaging range on the basis of the orthogonal-coordinate point cloud data obtained by the second coordinate conversion unit 25.SELECTED DRAWING: Figure 2

Description

[Technical Field]

[0001] The present invention relates to an environmental map generation program for generating an environmental map. [Background technology]

[0002] Autonomous driving of vehicles, autonomous mobile robots (AGVs: Automatic Guided Vehicles), collaborative robots, food delivery robots, robot vacuum cleaners, drone control, and Augmented Reality (AR) technology require a map of the surrounding environment. Conventionally, a technique called SLAM (Simultaneous Localization and Mapping: simultaneous execution of self-localization and environmental map creation) has been used to create environmental maps. SLAM can perform self-localization and environmental map creation under certain conditions. For example, Patent Document 1 describes the collection of spatial data, etc., by LiDAR SLAM using a LiDAR (Light Detection and Ranging) sensor. Generally, a three-dimensional sensor such as a LiDAR sensor acquires point cloud data expressed in three-dimensional coordinates (x, y, z). [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Japanese Patent Publication No. 2022-134119 Summary of the Invention [Problem to be solved by the invention]

[0004] However, conventional SLAM has three issues. First, the density of point cloud data is coarse compared to the resolution of typical still images, so matching between point cloud data (point cloud matching) may not be sufficient. For example, point cloud matching cannot be performed sufficiently in places with few structural features, such as plains, and an environmental map cannot be generated properly. Second, point cloud matching places a heavy processing load, requiring a high-spec computer or some kind of speed-up. Third, because SLAM assumes that the environment is static, in environments where moving objects such as people are present, moving objects may be recognized as landmarks, resulting in the creation of an incorrect environmental map.

[0005] The present invention has been made in consideration of the above-mentioned problems, and its purpose is to provide an environmental map generation program and a three-dimensional sensor control device that can generate an environmental map accurately and quickly, even in an environment where moving objects are present. [Means for solving the problem]

[0006] To achieve the above-mentioned object, a first invention provides an environmental map generation program that causes a computer to function as a data acquisition unit that acquires Cartesian coordinate point cloud data of multiple shooting frames captured by a three-dimensional sensor with a fixed shooting range; a first coordinate conversion unit that converts the Cartesian coordinate point cloud data into polar coordinate point cloud data based on a first conversion formula from Cartesian coordinates to polar coordinates; a farthest point search unit that searches for the polar coordinate point cloud data that is farthest from the three-dimensional sensor for each small region of the shooting range as farthest point data; a second coordinate conversion unit that converts the farthest point data into Cartesian coordinate point cloud data based on a second conversion formula from polar coordinates to Cartesian coordinates; and an environmental map output unit that outputs environmental map data of the shooting range based on the Cartesian coordinate point cloud data obtained by the second coordinate conversion unit.

[0007] The polar coordinates may be spherical coordinates having components of a radius, a polar angle, and an azimuth angle, and the small regions may be divided by the polar angle and the azimuth angle, and the farthest point search unit may search for the polar coordinate point cloud data having the largest value of the radius for each small region as the farthest point data.

[0008] In addition, the first coordinate conversion unit may assign identification information that identifies the small area based on the polar angle and azimuth angle values of the polar coordinate point cloud data, and the farthest point search unit may search for the polar coordinate point cloud data that is farthest from the three-dimensional sensor as the farthest point data for each of the identification information.

[0009] A second invention is a three-dimensional sensor control device in which the environmental map generation program of the first invention is installed and which controls the operation of the three-dimensional sensor. [Effects of the Invention]

[0010] The present invention can provide an environmental map generation program and a three-dimensional sensor control device that can generate an environmental map accurately and quickly even in an environment where moving objects exist. [Brief explanation of the drawings]

[0011] [Figure 1] A block diagram showing an example of computer hardware constituting the three-dimensional sensor control device of the present invention. [Figure 2] Functional block diagram showing the functions of the three-dimensional sensor control device of FIG. [Figure 3] FIG. 2 is a diagram for schematically explaining the environment map data generated by the environment map generation program of the present invention. [Figure 4] A flowchart showing an example of the flow of an environment map generation process realized by the environment map generation program of FIG. [Figure 5] A diagram illustrating an example of polar coordinates [Figure 6] A diagram for explaining the process by the small area and farthest point search unit. DETAILED DESCRIPTION OF THE INVENTION

[0012] The present embodiment will be described in detail below with reference to the drawings. Fig. 1 is a block diagram showing an example of computer hardware constituting a three-dimensional sensor control device of the present invention. The three-dimensional sensor control device 1 is a computer that controls the operation of a three-dimensional sensor 2 (performs control related to the acquisition of depth information by the three-dimensional sensor), and hardware such as a CPU (Central Processing Unit) 11, memory 12, auxiliary storage device 13, and input / output interface 14 are connected via a bus 15.

[0013] The CPU 11 reads out a program stored in advance in an auxiliary storage device 13 or the like into the memory 12 and executes it to perform the processing described below. The auxiliary storage device 13 is a hard disk drive, a solid state drive, or the like, and stores data used in the processing described below. The input / output interface 14 inputs signals from the three-dimensional sensor 2, a mouse, a keyboard, a microphone, or the like, and outputs signals to the three-dimensional sensor 2, a display, a speaker, or the like. The input / output interface 14 also includes a communication interface for transmitting and receiving data to an external computer. The input / output interface 14 may be, for example, a standard compatible with the USB (Universal Serial Bus) standard, the HDMI (registered trademark) standard, or the LAN (Local Area Network). The input / output interface 14 may be a port conforming to the IEEE 802.11 Area Network standard or the like, or a communication device conforming to the Bluetooth (registered trademark) standard, wireless LAN standard, etc. The input / output interface 14 may be wired or wireless and is not limited to these examples.

[0014] The computer constituting the three-dimensional sensor control device 1 is a general-purpose PC (Personal Computer) The three-dimensional sensor control device 1 may be a computer (or a device incorporating dedicated hardware). In addition, some or all of the functions of the three-dimensional sensor control device 1 described below may be realized by a dedicated integrated circuit constructed for a specific application, such as a hardware circuit, an ASIC (Application Specific Integrated Circuit), or an FPGA (Field Programmable Gate Array). In addition, the number of computers that execute the programs in this embodiment is not particularly limited. For example, the three-dimensional sensor control device 1 may execute processing in cooperation with an external computer.

[0015] The three-dimensional sensor 2 is a sensor capable of measuring the three-dimensional position of an object, and acquires point cloud data, which is a collection of points indicating the positions of detected points on the surface of the object. The three-dimensional sensor 2 may be, for example, an active stereo camera, a passive stereo camera, a three-dimensional LiDAR (Light Detection And Ranging) sensor, or a ToF (Time of Flight) sensor. Examples of point cloud data include, but are not limited to, a three-dimensional flight camera. The point cloud data acquired by the three-dimensional sensor 2 is Cartesian coordinate point cloud data expressed in three-dimensional Cartesian coordinates (x, y, z). The range that each component of the three-dimensional coordinates (x, y, z) can take depends on the angle of view and detection distance range of the three-dimensional sensor 2. The file format of the point cloud data is, for example, PCD (Point Cloud Data), which was developed by the open source library PCL (Point Cloud Library). Examples of such files include, but are not limited to, data files.

[0016] Figure 2 is a functional block diagram showing the functions of the three-dimensional sensor control device of Figure 1. The functions of the three-dimensional sensor control device 1 shown in Figure 2 are realized by installing an environment map generation program 10 of the present invention. The environment map generation program 10 may be distributed in a state where it is stored on a computer-readable storage medium, or may be downloaded from a network such as the Internet.

[0017] The environmental map generation program 10 is a program for causing a computer (=3D sensor control device 1) to function as a data acquisition unit 21, a data synthesis unit 22, a first coordinate conversion unit 23, a farthest point search unit 24, a second coordinate conversion unit 25, and an environmental map output unit 26.

[0018] The data acquisition unit 21 acquires Cartesian coordinate point cloud data of multiple shooting frames captured by the 3D sensor 2, which has a fixed shooting range. The data synthesis unit 22 synthesizes the Cartesian coordinate point cloud data of the multiple shooting frames acquired by the data acquisition unit 21 into a single array. The first coordinate conversion unit 23 converts the Cartesian coordinate point cloud data into polar coordinate point cloud data based on a first conversion formula from Cartesian coordinates to polar coordinates. The farthest point search unit 24 searches for the polar coordinate point cloud data that is farthest from the 3D sensor 2 for each small region of the shooting range as the farthest point data. The second coordinate conversion unit 25 converts the farthest point data into Cartesian coordinate point cloud data based on a second conversion formula from polar coordinates to Cartesian coordinates. The environmental map output unit 26 outputs environmental map data of the shooting range based on the Cartesian coordinate point cloud data obtained by the second coordinate conversion unit 25. Details of each function will be described later with reference to FIG. 4.

[0019] In addition to the functions shown in FIG. 2, the three-dimensional sensor control device 1 also has a function to control the operation of the three-dimensional sensor 2 and a function to execute predetermined processing using environmental map data, but details thereof will be omitted.

[0020] FIG. 3 is a diagram illustrating the environment map data generated by the environment map generation program of the present invention. For simplicity, FIG. 3 is a schematic diagram of a two-dimensional XZ plane, but in reality, the three-dimensional sensor control device 1 targets a three-dimensional XYZ space. In the example shown in FIG. 3, the number of shooting frames captured by the three-dimensional sensor 2 is N, from the first shooting frame to the Nth shooting frame in chronological order. The shooting range 3 of the first shooting frame includes a moving object MB, such as a person, and a stationary object SB, such as a device, table, or floor. The moving object MB moves before the Nth shooting frame is captured, and the shooting range 3 of the Nth shooting frame includes only the stationary object SB.

[0021] 3, the imaging direction of the 3D sensor 2 is from above vertically downward, but this is not limited to this. The imaging direction of the 3D sensor 2 may be from below vertically upward, horizontally, or any other direction. Furthermore, the 3D sensor 2 may be installed on a ceiling, floor, wall, tripod, or on a vehicle, robot, drone, computer, etc. that uses the environmental map data 4.

[0022] The environmental map generation process realized by the environmental map generation program 10 of the present invention generates environmental map data 4 accurately and quickly even in an environment where moving objects MB exist. More specifically, the environmental map generation process of this embodiment quickly generates environmental map data 4 in which only static objects SB are used as landmarks, assuming that no moving objects MB exist, even in an environment where moving objects MB and static objects SB exist together.

[0023] Fig. 4 is a flowchart showing an example of the flow of the environment map generation process realized by the environment map generation program of Fig. 2. As shown in Fig. 4, the data acquisition unit 21 of the three-dimensional sensor control device 1 compares a predetermined designated number of times N with the number of times i of images taken by the three-dimensional sensor 2 for the environment map generation process, and executes a repeat process described later while the number of times i of images taken is equal to or less than the designated number of times N (step S1).

[0024] In the repetitive process, the data acquisition unit 21 acquires orthogonal coordinate point cloud data from the 3D sensor 2 via the input / output interface 14 (step S2), counts up the number of times of shooting i, i.e., substitutes i+1 for i (step S3), and executes the determination of step S1. Through this process, the data acquisition unit 21 acquires orthogonal coordinate point cloud data for N shooting frames.

[0025] Next, the data synthesis unit 22 of the three-dimensional sensor control device 1 synthesizes the Cartesian coordinate point cloud data of the N photographic frames acquired by the data acquisition unit 21 into a single array (step S4). For example, if there are D pieces of Cartesian coordinate point cloud data per photographic frame, the data synthesis unit 22 synthesizes each element into a single piece of Cartesian coordinate point cloud data into an array of N×D elements. Because the Cartesian coordinate point cloud data is expressed in three-dimensional Cartesian coordinates (x, y, z), the array synthesized in step S4 is a set of N×D Cartesian coordinates.

[0026] Next, the first coordinate conversion unit 23 of the three-dimensional sensor control device 1 converts the Cartesian coordinate point cloud data, which are each element of the array synthesized in step S4, into polar coordinate point cloud data based on the first conversion formula from Cartesian coordinates to polar coordinates (step S5).

[0027] FIG. 5 is a diagram illustrating a schematic example of polar coordinates. Possible polar coordinates include spherical coordinates in a three-dimensional space, circular coordinates in a two-dimensional space, and cylindrical coordinates that add the Z axis to circular coordinates. As shown in FIG. 5, spherical coordinates (r, θ, φ) have components of radius r, polar angle (also called zenith angle) θ, and azimuth angle φ. Circular coordinates (r, θ) have components of radius r and deflection angle θ. Cylindrical coordinates (ρ, φ, z) have components of axial distance (also called radial distance) ρ, azimuth angle φ, and axial coordinate (also called height) z. In the following, polar coordinates will be described as spherical coordinates.

[0028] The range that each component of the spherical coordinates (r, θ, φ) can take depends on the angle of view and detection distance range of the three-dimensional sensor 2. The conversion formulas from the three-dimensional Cartesian coordinates (x, y, z) to the spherical coordinates (r, θ, φ) are as shown in equations (1) to (3).

[0029]

number

[0030] Returning to the explanation of Fig. 4, next, first coordinate conversion unit 23 of three-dimensional sensor control device 1 assigns identification information for identifying small regions of shooting range 3 to each element of the array synthesized in step S4, based on the values of polar angle θ and azimuth angle φ of the polar coordinate point cloud data (step S6).

[0031] FIG. 6 is a diagram for explaining the process performed by the small area and farthest point search unit. The small area 5 is divided by the polar angle θ and the azimuth angle φ. More specifically, as shown in FIG. 6, the small area 5 is divided by the polar angle θ (θ j+1 -θ j ) (where θ j+1 >θ j ) and the division interval of the azimuth angle φ (φ j+1 -φ j ) (where φ j+1 >φ j ) is a three-dimensional region in the shape of a quadrangular pyramid defined by the polar angle θ and the azimuthal angle φ. For example, if the division intervals for both the polar angle θ and the azimuthal angle φ are 1 degree, the small regions 5 are divided as follows: (first region) 0°<θ≦1° and 0°<φ≦1°, (second region) 0°<θ≦1° and 1°<φ≦2°, etc. The identification information may be any information that uniquely identifies these regions, and may be a sequential number or a combination of two numbers such as (1,1), (1,2), etc.

[0032] The array synthesized in step S4 includes point cloud data of shooting frames captured at different times in the same shooting range 3, so there are elements with the same polar angle θ and azimuthal angle φ. That is, the same identification information may be assigned to multiple elements of the array. Furthermore, elements of the array to which the same identification information is assigned may have the same or different values of the radial coordinate r.

[0033] Return to the description of FIG. 4. The farthest point search unit 24 of the three-dimensional sensor control device 1 checks the value of the radial distance r of each element for each piece of identification information (= small area 5) given in step S6, and searches for the element (= polar coordinate point group data) with the largest value of the radial distance r as the farthest point data (step S7). In the description of FIG. 2, the farthest point data is defined as the polar coordinate point group data with the farthest distance from the three-dimensional sensor 2. Therefore, if the position of the three-dimensional sensor 2 is set as the origin of the coordinates, the polar coordinate point group data with the largest value of the radial distance r is the farthest point data.

[0034] In the example shown in FIG. 6, θ j <θ ≤ θ j+1 and φ j <φ ≤ φ j+1 The small area 5 defined by contains three pieces of polar coordinate point group data P1(r1, θ1, φ1), P2(r2, θ2, φ2), and P3(r3, θ3, φ3). For example, if r1 < r2 < r3, the farthest point search unit 24 sets P3 with the largest value of the radial distance as the farthest point data.

[0035] If even one photographed frame without the moving body MB at each position in the photographing range 3 can be acquired, the farthest point data searched by the farthest point search unit 24 does not include the position information of the moving body MB. This is because the radial distance r of the polar coordinate point group data regarding the position of the moving body MB such as a person is smaller than the radial distance r of the polar coordinate point group data regarding the position of the static object SB such as the floor. Therefore, if only the farthest point data is used, the environmental map data 4 can be generated assuming that the moving body MB does not exist.

[0036] Next, the data synthesis unit 22 of the three-dimensional sensor control device 1 synthesizes the farthest point data searched by the farthest point search unit 24 as a single array (step S8). The array synthesized in step S8 is a set of spherical coordinates with an uncertain number of elements (approximately D).

[0037] Next, the second coordinate conversion unit 25 of the three-dimensional sensor control device 1 converts the farthest point data (=polar coordinate point cloud data), which are each element of the array synthesized in step S8, into Cartesian coordinate point cloud data based on the second conversion formula from polar coordinates to Cartesian coordinates (step S9). The conversion formula from spherical coordinates (r, θ, φ) to three-dimensional Cartesian coordinates (x, y, z) is as shown in formulas (4) to (6).

[0038]

number

[0039] Next, the environment map output unit 26 of the three-dimensional sensor control device 1 generates environment map data 4 using only the Cartesian coordinate point cloud data converted from the farthest point data in step S9. Then, the environment map output unit 26 outputs the environment map data 4 to an output device such as a display or an external computer via the input / output interface 14 (step S10). The output environment map data 4 does not include position information of moving objects MB, but only position information of static objects SB.

[0040] As described above, the three-dimensional sensor control device 1 converts the Cartesian coordinate point cloud data into polar coordinate point cloud data, and searches for the polar coordinate point cloud data that is farthest from the three-dimensional sensor 2 for each small region 5 as the farthest point data. This makes it possible to generate environmental map data 4 in which only the static objects SB are landmarks, assuming that no moving objects MB exist, even in an environment where both moving objects MB and static objects SB exist.

[0041] In the case of a typical still image, it is possible to eliminate moving objects MB using background subtraction. However, in the case of point cloud data acquired by a 3D sensor 2, variations occur even when images are taken in the same environment, making it impossible to simply extract differences as with background subtraction. Therefore, the 3D sensor control device 1 distinguishes between moving objects MB and static objects SB based on distance information in polar coordinates. This method does not require matching between point cloud data (= point cloud matching), enabling high-speed processing. Furthermore, moving objects MB are recognized as landmarks, and erroneous environmental map data 4 is not generated.

[0042] In this embodiment, the polar coordinates are spherical coordinates with components of radius r, polar angle θ, and azimuth angle φ. Furthermore, for each small region 5 divided by polar angle θ and azimuth angle φ, the three-dimensional sensor control device 1 searches for the polar coordinate point cloud data with the largest value of radius r as the farthest point data. The detectable distance, vertical angle of view, and horizontal angle of view of the three-dimensional sensor 2 correspond to the possible ranges of radius r, polar angle θ, and azimuth angle φ of the spherical coordinates, respectively. In other words, since the possible ranges of each component of the spherical coordinates (r, θ, φ) can be predicted from the performance of the three-dimensional sensor 2, it is reasonable to use spherical coordinates. Furthermore, when using spherical coordinates, it is easy to intuitively understand why the present invention can eliminate moving objects MB.

[0043] The three-dimensional sensor control device 1 also assigns identification information for identifying small regions 5 based on the polar angle θ and azimuth angle φ of the polar coordinate point cloud data, and searches for the farthest point data for each identification information, thereby enabling high-speed search processing for the farthest point data.

[0044] In the above description, the Cartesian coordinate point cloud data is represented by three-dimensional Cartesian coordinates (x, y, z). However, it may be represented by two-dimensional Cartesian coordinates (x, y). In this case, the three-dimensional sensor control device 1 uses polar coordinates as circular coordinates (r, θ) shown in FIG. 5 and a known conversion formula from two-dimensional Cartesian coordinates to circular coordinates or a known conversion formula from circular coordinates to two-dimensional Cartesian coordinates. The three-dimensional sensor control device 1 then searches for the polar coordinate point cloud data with the largest value of radius vector r for each triangular small region 5 divided by the argument θ as the farthest point data. For example, when the three-dimensional sensor 2 is installed on a robot vacuum cleaner that has a small vertical dimension and moves only horizontally, and environmental map data 4 is generated, two-dimensional information may be sufficient.

[0045] Alternatively, the polar coordinates may be expressed as cylindrical coordinates (ρ, φ, z) as shown in FIG. 5. In this case, the three-dimensional sensor control device 1 uses a known conversion formula from three-dimensional Cartesian coordinates to cylindrical coordinates, or a known conversion formula from cylindrical coordinates to three-dimensional Cartesian coordinates. The three-dimensional sensor control device 1 then searches for the polar coordinate point cloud data with the largest value of the axial distance ρ for each triangular prism-shaped small region 5 divided by the axis coordinate z and the azimuth angle φ, as the farthest point data. For example, when the three-dimensional sensor 2 is installed on a cylindrical coordinate robot equipped with at least one joint for rotational motion and one joint for translational motion, and environmental map data 4 is generated, it may be reasonable to use cylindrical coordinates.

[0046] While preferred embodiments of the environmental map generation program and the 3D sensor control device according to the present invention have been described above with reference to the accompanying drawings, the present invention is not limited to these examples. It is clear that a person skilled in the art can conceive of various modifications and alterations within the scope of the technical ideas disclosed herein, and it is understood that these modifications and alterations also fall within the technical scope of the present invention. [Explanation of symbols]

[0047] 1...3D sensor control device 2...3D sensor 3. Shooting range 4. Environmental map data 5……Small area 10...Environmental map generation program 21...Data acquisition section 22...Data synthesis section 23...First coordinate conversion unit 24……Farthest point search section 25...Second coordinate conversion unit 26...Environment map output section

Claims

1. Computer, a data acquisition unit that acquires orthogonal coordinate point cloud data of a plurality of photographed frames photographed by a three-dimensional sensor having a fixed photographing range; a first coordinate conversion unit that converts the orthogonal coordinate point cloud data into polar coordinate point cloud data based on a first conversion formula from orthogonal coordinates to polar coordinates; a farthest point search unit that searches for the polar coordinate point cloud data that is farthest from the three-dimensional sensor for each small region of the imaging range as farthest point data; a second coordinate conversion unit that converts the farthest point data into the orthogonal coordinate point cloud data based on a second conversion formula from the polar coordinates to the orthogonal coordinates; an environmental map output unit that outputs environmental map data of the shooting range based on the orthogonal coordinate point cloud data obtained by the second coordinate transformation unit; An environmental map generating program characterized by functioning as follows.

2. the polar coordinates are spherical coordinates having a radius, a polar angle, and an azimuth angle as components, and the small regions are divided by the polar angle and the azimuth angle; The farthest point search unit searches for the polar coordinate point cloud data with the largest radius vector value for each small region as the farthest point data.

2. The environmental map generating program according to claim 1, wherein:

3. the first coordinate transformation unit assigns identification information for identifying the small region based on the polar angle and the azimuth angle of the polar coordinate point cloud data; The farthest point search unit searches for the polar coordinate point cloud data that is farthest from the three-dimensional sensor for each of the identification information as the farthest point data.

3. The environmental map generating program according to claim 2.

4. A three-dimensional sensor control device having the environmental map generation program according to claim 1 installed therein, for controlling the operation of the three-dimensional sensor.

Images