Simultaneous position estimation of lidar and camera base differing in view angle, and map creation method and system

Abstract

To provide a simultaneous position estimation and a map creation method and system that make it possible to maintain the feature point detected with a camera the view angle of which is narrow in conformity with the wide view angle of a lidar and secure robustness.SOLUTION: The present invention includes: a lidar for detecting the feature point ahead of a mobility; a camera for acquiring the front image of the mobility; and a controller for receiving transmission of data from the lidar regarding the feature point ahead of the mobility, receiving the front image of the mobility from the camera and searching for a feature point from the front image, setting the feature point transmitted from the lidar and the feature point searched from the front image as a point cloud (PCL), and performing simultaneous position estimation and map creation on the basis of the feature points in the PCL. The controller deletes a feature point that satisfies a deletion condition in response to the arrival of an object search cycle from the PCL and adds a feature point newly searched after a preceding object search cycle to the PCL.SELECTED DRAWING: Figure 2

Description

Technical Field

[0001] The present invention relates to a method and system for Simultaneous Localization And Mapping (SLAM). More specifically, the present invention relates to a method and system for simultaneous localization and mapping with enhanced robustness by maintaining feature points detected by a camera with a relatively narrow field of view in accordance with the relatively wide field of view of a lidar.

Background Art

[0002] Simultaneous Localization And Mapping (SLAM) refers to a technology that measures the current self-position while exploring an environment with unknown mobility and simultaneously creates a map of the surrounding environment. Such SLAM is performed based on a 2D lidar or a 3D lidar. However, SLAM based on a 3D lidar is expensive and has a large amount of computation, and is hardly applicable to small mobility that does not transport people. SLAM based on a 2D lidar is mainly applied to small mobility. When performing SLAM based on a 2D lidar, areas that are not at the height where the laser is irradiated cannot be recognized, and an incorrect map may be constructed. In order to solve the problems of SLAM performed based on such a 2D lidar, a method of performing SLAM by fusing data detected by a 2D lidar and data detected using a camera has been studied.

[0003] However, although feature points that have deviated from the camera's field of view due to the relatively narrow field of view angle of the camera and the movement of the mobility are detected by a 2D lidar with a relatively wide field of view angle, they may be recognized as non-existent feature points and not reflected in SLAM. As a result, an incorrect map may be created, which may compromise driving safety. The matters described in this background art section are created to enhance the understanding of the background of the invention and may include matters that are not prior art already known to those with ordinary knowledge in the field to which this technology belongs.

Prior Art Documents

Patent Documents

[0004]

Patent Document 1

Summary of the Invention

Problems to be Solved by the Invention

[0005] An object of the present invention is to provide a simultaneous localization and mapping method and system with ensured robustness by maintaining feature points detected by a camera with a relatively narrow field of view angle in accordance with the relatively wide field of view angle of a lidar.

Means for Solving the Problems

[0006] The simultaneous localization and mapping (SLAM) system according to an embodiment of the present invention includes a lidar configured to detect feature points in front of the mobility, a camera configured to acquire an image in front of the mobility, a controller that receives transmission of data regarding feature points in front of the mobility from the lidar, receives an image in front of the mobility from the camera, searches for feature points from the image in front of the mobility, sets the feature points transmitted from the lidar and the feature points searched from the image in front of the mobility as a point cloud (PCL), and is configured to perform simultaneous localization and mapping (SLAM) based on the feature points in the PCL. The controller is further configured to remove, from the PCL, feature points that satisfy a removal condition in response to the arrival of an object search period, and add, to the PCL, feature points newly searched after the previous object search period. The feature points that satisfy the removal condition are feature points whose distance to the mobility is greater than a first set distance.

[0007] The controller can maintain, in the PCL, feature points that do not satisfy the removal condition.

[0008] The controller can be configured to first remove, from the PCL, feature points that satisfy the removal condition, and then add, to the PCL, feature points newly searched.

[0009] The controller can be further configured to set, as a new point cloud (nPCL), feature points newly searched after the previous object search period, determine whether the feature points in the nPCL are the same as the feature points in the PCL, and add, to the PCL, the feature points in the nPCL in response to a determination that the feature points in the nPCL are not the same as the feature points in the PCL.

[0010] The controller can be configured to determine that the feature points in the nPCL are not the same as the feature points in the PCL in response to a determination that a minimum value of distances between the feature points in the nPCL and the feature points in the PCL is greater than or equal to a second set distance.

[0011] In response to determining that the minimum value of the distance between the feature points in the nPCL and the feature points in the PCL is less than a second set distance, the controller determines whether the positional relationship between the feature points in the nPCL and the feature points in the PCL excluding the said feature point in the PCL matches the positional relationship between the said feature point in the PCL and the feature points in the PCL excluding the said feature point in the PCL, and in response to determining that the positional relationship between the feature points in the nPCL and the feature points in the PCL excluding the said feature point in the PCL does not match the positional relationship between the said feature point in the PCL and the feature points in the PCL excluding the said feature point in the PCL, the controller can be configured to determine that the feature points in the nPCL are not the same as the feature points in the PCL.

[0012] In response to determining that the positional relationship between the feature points in the nPCL and the feature points in the PCL excluding the said feature point in the PCL matches the positional relationship between the said feature point in the PCL and the feature points in the PCL excluding the said feature point in the PCL, the controller can be configured to determine that the said feature point in the nPCL is the same as the said feature point in the PCL.

[0013] The controller may be further configured to remove noise in the nPCL through data clustering.

[0014] According to another embodiment of the present invention, a simultaneous localization and mapping (SLAM) method includes, in response to triggering simultaneous localization and mapping (SLAM), a step of receiving, by a controller of mobility, data regarding feature points in front of the mobility from a lidar, a step of receiving, by the controller, a front image of the mobility from a camera and searching for feature points from the front image, a step of setting, by the controller, the feature points transmitted from the lidar and the feature points searched from the front image as a point cloud (PCL), a step of removing, by the controller, feature points that satisfy a removal condition from the PCL in response to the arrival of an object search period, and a step of adding, by the controller, feature points newly searched after the previous object search period to the PCL.

[0015] The feature points that satisfy the removal condition may be feature points whose distance to the mobility is greater than a first set distance.

[0016] The SLAM method can further include a step of maintaining, by a controller, feature points that do not satisfy the removal condition in the PCL.

[0017] The step of adding feature points newly explored after the previous object exploration cycle to the PCL can include setting the feature points newly explored after the previous object exploration cycle as a new point cloud (nPCL), determining whether the feature points in the nPCL are the same as the feature points in the PCL, and adding the feature points in the nPCL to the PCL in response to the determination that the feature points in the nPCL are not the same as the feature points in the PCL.

[0018] The step of determining whether the feature points in the nPCL are the same as the feature points in the PCL can include comparing the minimum value of the distances between the feature points in the nPCL and the feature points in the PCL with a second set distance.

[0019] The step of determining whether the feature points in the nPCL are the same as the feature points in the PCL can further include determining that the feature points in the nPCL are not the same as the feature points in the PCL in response to determining that the minimum value of the distances between the feature points in the nPCL and the feature points in the PCL is greater than or equal to the second set distance.

[0020] The step of determining whether the feature points in the nPCL are the same as the feature points in the PCL can further include determining whether the positional relationship between the feature points in the nPCL and the feature points excluding the corresponding feature points in the PCL matches the positional relationship between the corresponding feature points in the PCL and the feature points excluding the corresponding feature points in the PCL in response to determining that the minimum value of the distances between the feature points in the nPCL and the feature points in the PCL is less than the second set distance.

[0021] The step of determining whether the feature points in the nPCL are the same as the feature points in the PCL may further include a step of determining that the feature points in the nPCL are not the same as the feature points in the PCL in response to a determination that the positional relationship between the feature points in the nPCL and the feature points excluding the corresponding feature points in the PCL does not match the positional relationship between the corresponding feature points in the PCL and the feature points excluding the corresponding feature points in the PCL.

[0022] The step of determining whether the feature points in the nPCL are the same as the feature points in the PCL may further include a step of determining that the corresponding feature points in the nPCL are the same as the corresponding feature points in the PCL in response to a determination that the positional relationship between the feature points in the nPCL and the feature points excluding the corresponding feature points in the PCL matches the positional relationship between the corresponding feature points in the PCL and the feature points excluding the corresponding feature points in the PCL.

[0023] The step of adding the feature points newly explored after the previous object exploration cycle to the PCL may further include a step of removing the noise in the nPCL through data clustering.

Advantages of the Invention

[0024] According to the present invention, even if the feature points detected by the camera deviate from the field of view of the camera, as long as they are within the field of view angle of the lidar, the robustness of SLAM can be ensured by maintaining them as feature points. The memory usage can be reduced while ensuring the robustness of SLAM by deleting the feature points only when the distance between any feature point and the mobility is greater than a preset first set distance. Since new feature points are added after deleting the feature points that meet the deletion conditions, the memory usage and the computation amount can be further reduced. In addition, the effects obtained or predicted by the embodiments of the present invention will be directly or implicitly disclosed in the detailed description of the embodiments of the present invention. That is, various effects predicted by the embodiments of the present invention will be disclosed in the following detailed description.

Brief Description of the Drawings

[0025]

Figure 1

Figure 2

Figure 3

Figure 4

Figure 5

Figure 6

Figure 7

Figure 8

Figure 9

Figure 10a

Figure 10b

Figure 10c

[0026] The drawings are not necessarily shown to scale and present somewhat simplified representations of various preferred features that illustrate the basic principles of the present disclosure. For example, certain design features of the present disclosure, including specific dimensions, directions, positions, and shapes, are determined in part by the particular intended application and the use environment.

Embodiments for Carrying Out the Invention

[0027] The terms are for describing specific embodiments and are not intended to limit the present invention. The singular forms also include the plural forms unless explicitly stated otherwise in the context. The terms "comprise" and / or "comprising", as used herein, specify the presence of the recited features, integers, steps, operations, components and / or elements, but do not preclude the presence or addition of one or more of other features, integers, steps, operations, components, elements and / or groups thereof. The term "and / or" includes any and all combinations of one or more of the associated listed items.

[0028] As used herein, the term "mobility" or "mobility of" or other similar terms includes general land mobility including passenger vehicles such as sports utility vehicles (SUVs), buses, trucks, various commercial vehicles, etc., marine mobility including various boats and ships, and air mobility including aircraft, drones, etc., and includes all objects that can move powered by a power source.

[0029] Also, as used herein, the term "mobility" or "mobility of" or other similar terms includes hybrid mobility, electric mobility, plug-in hybrid mobility, hydrogen-powered mobility and other alternative fuel (e.g., fuels derived from resources other than petroleum) mobility. As referred to herein, hybrid mobility includes mobility having two or more power sources, such as gasoline-powered and electric-powered mobility. Mobility according to embodiments of the present invention includes not only manually driven mobility but also somewhat autonomous and / or automatically driven mobility.

[0030] Additionally, one or more of the following methods or aspects thereof can be performed by at least one controller. The term "controller" refers to a hardware device that includes a memory and a processor. The memory is configured to store program instructions, and the processor is specially programmed to execute the program instructions to perform one or more processes described in more detail below. The controller controls the operation of units, modules, components, devices, or the like as described herein. Also, the following methods can be performed by a device that includes a controller along with one or more other components, as would be recognized by one of ordinary skill in the art.

[0031] Also, the controller of the present disclosure can be implemented as a non-transitory computer-readable recording medium including executable program instructions executed by a processor. Examples of computer-readable recording media include, but are not limited to, ROM (Read-Only Memory), RAM (Random Access Memory), CD-ROM (Compact Disc Read-Only Memory), magnetic tape, floppy disk, flash drive, smart card, and optical data storage devices. The computer-readable recording medium can also be one in which program instructions are stored and executed in a distributed manner across an entire computer network, for example, in a distributed manner such as a telematics server or a Controller Area Network (CAN).

[0032] Hereinafter, embodiments of the present invention will be described in detail with reference to the accompanying drawings.

[0033] FIG. 1 is a block diagram of a simultaneous position estimation and mapping system according to an embodiment of the present invention. As shown in FIG. 1, a mobility object tracking system according to an embodiment of the present invention includes a lidar 10, an encoder 20, an inertial sensor 30, a camera 40, a controller 50, and a mobility 60.

[0034] Rider 10 is mounted on mobility 60. After irradiating a laser pulse in front of mobility 60, it measures the time when the laser pulse reflected from an object within the visual field 64 of rider 10 (see FIG. 5) returns, and detects information about the object such as the distance from rider 10 to the object, the direction of the object, speed, temperature, substance distribution, and concentration characteristics. Here, the object may be another mobility, a person, a thing, etc. existing outside the mobility 60 on which the rider sensor 10 is mounted, but the present invention is not particularly limited to the type of the object. Rider 10 is connected to controller 50 to detect object data (for example, a plurality of feature points included in the object) within the visual field 64 of rider 10 and transmit the object data to controller 50. Here, a set of feature points is referred to as a point cloud (PCL).

[0035] Encoder 20 measures information regarding the rotation of a drive motor or wheels provided in mobility 60. Encoder 20 is connected to controller 50 and transmits the measured information regarding the rotation of the drive motor or wheels to controller 50. Controller 50 calculates mobility movement data such as the movement speed and / or movement distance of mobility 60 based on the information regarding the rotation of the drive motor or wheels.

[0036] Inertial sensor 30 measures information regarding the movement state of mobility 60 including the speed and direction of mobility 60, gravity, and acceleration. Inertial sensor 30 is connected to controller 50 and transmits the measured information regarding the movement state of mobility 60 to controller 50. Controller 50 detects or supplements mobility movement data based on the information regarding the movement state of mobility 60.

[0037] Here, an example is given in which both encoder 20 and inertial sensor 30 are used as movement data sensors for detecting the movement data of mobility 60, but only one of encoder 20 and inertial sensor 30 can also be used as a movement data sensor. Further, the movement data sensor is not limited to encoder 20 and inertial sensor 30, and includes various sensors for detecting the movement data of mobility 60.

[0038] The camera 40 is mounted on the mobility 60 and acquires a front image of the mobility 60 within the field of view 62 (see FIG. 5) of the camera 40. The camera 40 is connected to the controller 50 and transmits the acquired image to the controller 50. The controller 50 receives object data from the rider 10, receives information regarding the rotation of the drive motor or wheels from the encoder 20, receives information regarding the movement status of the mobility 60 from the inertial sensor 30, and receives a front image of the mobility 60 from the camera 40.

[0039] The controller 50 is configured to search for objects (e.g., feature points) within the image through an object search algorithm such as an artificial neural network based on the received front image. Here, the set of feature points searched from the front image is also referred to as a point cloud (PCL).

[0040] The controller 50 is configured to remove feature points that satisfy the removal conditions from the point cloud (PCL) at each set object search period. Thereafter, the controller 50 is configured to update the point cloud (PCL) by adding newly searched feature points by the camera 40 to the point cloud (PCL).

[0041] The controller 50 detects mobility 60 movement data based on the information regarding the rotation of the drive motor or wheels received from the encoder 20 or the information regarding the movement status of the mobility 60 received from the inertial sensor 30, estimates the position of the mobility 60 based on the mobility movement data, and measures the absolute position of the mobility 60 through a known position measurement method. The controller 50 is configured to perform SLAM based on the absolute position of the mobility 60 and the updated PCL.

[0042] For such purposes, the controller 50 is provided with one or more microprocessors, and the one or more microprocessors may be programmed to perform each stage of SLAM according to an embodiment of the present invention. The controller 50 is connected to the mobility 60 and generates the path of the mobility 60 or controls the movement of the mobility 60 using the map produced through the SLAM method according to an embodiment of the present invention. For example, the controller 50 controls the mobility 60 to go towards the object or controls the mobility 60 to avoid the object.

[0043] FIG. 2 is a flowchart of a simultaneous localization and mapping method according to another embodiment of the present invention, FIG. 3 is a flowchart of the S120 stage of FIG. 2, and FIG. 4 is a flowchart of the S130 stage of FIG. 2.

[0044] As shown in FIG. 2, the simultaneous localization and mapping method according to another embodiment of the present invention starts when the engine of the mobility 60 is started. For example, the user presses the engine start button of the mobility 60 or starts the engine of the mobility 60 through the user interface.

[0045] Once the mobility 60 has the engine started, the user presses the SLAM button provided on the mobility 60 or triggers SLAM through the user interface (S100). Once SLAM is triggered, the lidar 10 detects the feature points in front of the mobility 60, the camera 40 detects the front image of the mobility 60, the controller 50 receives the transmission of data regarding the feature points in front of the mobility 60 from the lidar 10, receives the front image of the mobility 60 from the camera 40 and searches for feature points from the front image, and sets the feature points transmitted from the lidar 10 and the feature points searched from the front image as a point cloud (PCL). The controller 50 removes noise from the PCL through data clustering or the like.

[0046] After that, the controller 50 determines whether a preset object search period has arrived (S110). Here, the object search period may be 30 FPS, but is not limited thereto.

[0047] If the object search period has not arrived at step S110, the controller 50 waits until the object search period arrives. In contrast, if the object search period has arrived at step S110, the controller 50 removes feature points that satisfy the removal condition from the point cloud (PCL) (S120). As described above, the point cloud (PCL) means a set of feature points detected by the lidar 10 and the camera 40, and can be updated in a previous object search period and stored in the memory of the controller 50 or the like. Further, the removal condition is that the distance between any feature point in the point cloud (PCL) and the mobility 60 is greater than a first set distance (D1). When the removal condition is satisfied, the feature point is removed from the PCL.

[0048] When updating the PCL, the memory usage can be reduced by removing the feature points first, and the amount of computation for performing the method according to the embodiment of the present invention is reduced. Referring to FIG. 3, step S120 will be described in more detail. As shown in FIG. 3, the controller 50 reads the coordinates of the feature points included in the PCL stored in the memory (S200). For example, if the PCL contains n feature points, the coordinates of each feature point from PCL[1] to PCL[n] are read. Usually, the coordinates of the feature points, including both the absolute coordinates and the relative coordinates with respect to the mobility 60, are all stored in the memory. However, if only the absolute coordinates of the feature points are stored in the memory, at step S200, the controller 50 detects the relative coordinates of the feature points.

[0049] If the coordinates of the feature points are read in the S200 stage, the controller 50 determines whether the distance between the feature points included in the PCL and the mobility 60 is greater than the first set distance (D1) (S210). Since the relative coordinates of the feature points with respect to the mobility 60 are read in the S200 stage, the controller 50 calculates the distance between the feature points and the mobility 60 using the relative coordinates, and determines whether the calculated distance is greater than the first set distance. Here, the first set distance (D1) is a distance that hardly affects the path of the mobility 60 and can be appropriately set by those skilled in the art.

[0050] If it is determined in the S210 stage that the distance between the feature points and the mobility 60 is less than or equal to the first set distance (D1), the method proceeds to the S230 stage, and the controller 50 updates the PCL. In contrast, if it is determined in the S210 stage that the distance between the feature points and the mobility 60 is greater than the first set distance (D1), the controller 50 deletes the corresponding feature point (for example, PCL[i]) (S220) and updates the PCL (S230).

[0051] The stages from S210 to S230 are repeated for all the feature points in the PCL. That is, if there are n feature points in the PCL, the stages from S210 to S230 are repeated n times. Referring to FIG. 2 again, when the feature points that satisfy the removal condition are removed from the PCL in the S120 stage, the controller 50 adds a new point cloud (nPCL) to the point cloud (PCL) (S130). Referring to FIG. 4, the S130 stage will be described in more detail.

[0052] As shown in FIG. 4, the controller 50 searches for new feature points (S300). Here, the set of new feature points is referred to as a new point cloud (nPCL). When new feature points are searched, the controller 50 removes noise from the nPCL (S310). For example, outliers in the nPCL are removed through data clustering or the like. Such data clustering is well known to those skilled in the art, so further detailed description is omitted.

[0053] After that, the controller 50 determines whether the feature points in the nPCL are the same as the feature points in the PCL. Although various methods can be used to determine whether the feature points in the nPCL are the same as the feature points in the PCL, here, a method using the distance between the feature points in the nPCL and the feature points in the PCL will be exemplarily described.

[0054] First, the controller 50 calculates the distance between an arbitrary feature point (nPCL[k]) in the nPCL and an arbitrary feature point (PCL[j]) in the PCL, and determines whether the minimum value of the distances between the arbitrary feature points in the nPCL and the feature points in the PCL (min(distance between PCL[j] and nPCL[k])) is less than the second set distance (D2) (S320). Here, the second set distance (D2) means the distance at which nPCL[k] can be regarded as the same feature point as PCL[j], and is appropriately set by those skilled in the art.

[0055] If it is determined that the minimum value of the distances between an arbitrary feature point (nPCL[k]) in the nPCL and the feature points in the PCL is greater than or equal to the second set distance (D2), the controller 50 adds the corresponding feature point (nPCL[k]) in the nPCL to the PCL (S340).

[0056] If it is determined that the minimum value of the distances between an arbitrary feature point (nPCL[k]) in the nPCL and the feature points in the PCL is less than the second set distance (D2), the controller 50 determines whether an arbitrary feature point (nPCL[k]) in the nPCL is matched with the remaining feature points in the PCL excluding the corresponding feature point (PCL[j]) in the PCL with the minimum distance between them (S330). Even if the distance between an arbitrary feature point (nPCL[k]) in the nPCL and a feature point (PCL[j]) in the PCL is small, an arbitrary feature point (nPCL[k]) in the nPCL may be a feature point that was not explored in the previous object search cycle.

[0057] The controller 50 determines whether an arbitrary feature point (nPCL[k]) in the nPCL is a feature point that was not explored in the previous object search cycle by determining whether the arbitrary feature point (nPCL[k]) in the nPCL matches the remaining feature points in the PCL excluding the feature point (PCL[j]) in the PCL with the minimum distance between them. That is, it determines whether the positional relationship between an arbitrary feature point (nPCL[k]) in the nPCL and the remaining feature points in the PCL excluding the said feature point in the PCL satisfies the positional relationship between the said feature point in the PCL and the remaining feature points.

[0058] If, in step S330, the positional relationship between an arbitrary feature point (nPCL[k]) in the nPCL and the remaining feature points in the PCL excluding the said feature point (PCL[j]) in the PCL satisfies the positional relationship between the said feature point (PCL[j]) in the PCL and the remaining feature points, the arbitrary feature point (nPCL[k]) in the nPCL is determined to be the same as the said feature point (PCL[j]) in the PCL. Therefore, the method proceeds to step S320, and the controller 50 determines whether other feature points in the nPCL are the same as the feature points in the PCL (steps S320 and S330 are repeated).

[0059] If, in step S330, the positional relationship between an arbitrary feature point (nPCL[k]) in the nPCL and the remaining feature points in the PCL excluding the said feature point (PCL[j]) in the PCL does not satisfy the positional relationship between the said feature point (PCL[j]) in the PCL and the remaining feature points, the arbitrary feature point (nPCL[k]) in the nPCL is determined not to be the same as the feature points in the PCL. Therefore, the method proceeds to step S340, and the controller 50 adds the said feature point (nPCL[k]) in the nPCL to the PCL. Thereafter, the controller 50 updates the PCL (S350).

[0060] Referring back to FIG. 2, when the PCL is updated by removing the feature points that satisfy the removal conditions from the PCL and adding the newly explored feature points to the PCL, the controller 50 performs SLAM with the updated PCL (S140). That is, positioning and mapping are created using the feature points in the updated PCL. Since SLAM using feature points is well known to those skilled in the art, further detailed description is omitted.

[0061] Thereafter, the controller 50 determines whether SLAM has ended (S150). If SLAM has not ended, the method returns to step S110, and the controller 50 determines whether the object exploration period has arrived. In contrast, if SLAM has ended in step S150, the method ends.

[0062] Hereinafter, with reference to FIGS. 5 to 9, the SLAM method according to an embodiment of the present invention will be described in more detail.

[0063] FIG. 5 exemplarily shows the feature points detected by the lidar and the camera. FIG. 6 exemplarily shows removing noise from the feature points detected in FIG. 5. FIG. 7 exemplarily shows deleting the stored feature points. FIG. 8 exemplarily shows maintaining the stored feature points and adding new feature points. FIG. 9 exemplarily shows the point cloud (PCL) in which the feature points stored in FIG. 8 are maintained and new feature points are added.

[0064] Normally, when the object exploration period arrives, the controller 50 explores the feature points 66, 68, and 70 using the lidar 10 and the camera 40. As shown in FIG. 5, since the field of view 64 of the lidar 10 is wider than the field of view 62 of the camera 40, some of the feature points 68 are detected only by the lidar 10, and the other feature points 66 are detected by both the lidar 10 and the camera 40.

[0065] After searching for an object in the previous object search cycle to update the PCL, the mobility 60 can move along the path until the current object search cycle. Therefore, when the current object search cycle arrives, the controller 50 deletes from the PCL the feature points 72 that satisfy the removal conditions among the feature points 66, 68, and 72 stored in the PCL. For example, as shown in FIG. 7, if the distance between the mobility 60 and the feature point 72 is greater than the first set distance, the feature point 72 is deleted from the PCL.

[0066] After deleting from the PCL the feature points 72 that satisfy the removal conditions, the controller 50 searches for new feature points 66, 68, and 70. For example, as shown in FIG. 5, the feature point 68 is searched for by the lidar 10, the feature points 66 and 70 are searched for by the camera 40, and the controller 70 sets the set of newly searched feature points 66, 68, and 70 as the nPCL.

[0067] The controller 50 removes the noise 70 among the searched feature points 66, 68, and 70. For example, as shown in FIG. 6, the noise 70 determined through data clustering or the like is removed from the nPCL. Then, the controller 50 determines whether the newly searched feature points are the same as the existing feature points. For example, as shown in FIG. 8, the new feature point 74 is compared with the existing feature point 66. First, the controller 50 determines whether there is a new feature point 74 whose minimum distance from the existing feature point is less than the second set distance (D2). In FIG. 8, the new feature point 74 on the left has a minimum distance from the existing feature point 66 that is less than the second set distance (D2), while the new feature point 74 on the right has a minimum distance from the existing feature point 66 that is greater than the second set distance and is added to the PCL as a new feature point (see FIG. 9).

[0068] After that, the controller 50 determines whether the positional relationship between the remaining existing feature points excluding the existing feature point with the minimum distance from the new feature point and the new feature point matches the positional relationship between the existing feature point with the minimum distance from the new feature point and the remaining existing feature points excluding the said feature point. For example, among the existing feature points 66 searched by the camera 40 in FIG. 8, the positional relationship between the rightmost feature point 66 and the existing feature points 66 other than the rightmost feature point 66 corresponds to the positional relationship between the new feature point 74 on the left side and the existing feature points 66 other than the rightmost feature point 66. Therefore, the new feature point 74 on the left side is determined as an existing feature point and not added to the PCL.

[0069] On the other hand, in FIG. 8, "76" indicates an existing feature point or a feature point not newly searched. Since such a feature point 76 does not satisfy the removal condition, it is not deleted and is maintained in the PCL.

[0070] As a result, the PCL is updated as shown in FIG. 9.

[0071] FIG. 10a shows an example of a map of an arbitrary location. FIG. 10b shows an example of a map created by conventional 2D SLAM of the location in FIG. 10a. FIG. 10c shows an example of a map created by SLAM according to an embodiment of the present invention of the location in FIG. 10a. As shown in FIG. 10a, there are two first objects 80 and two second objects 82 at the location. As shown in FIG. 10b, when creating a map by conventional 2D SLAM, the first object 80 is searched, but the second object 82 may not be searched due to the difference in the viewing angles of the lidar 10 and the camera 40.

[0072] Thus, if the mobility 60 travels on a map lacking the second object 82, there is a possibility that the mobility 60 will collide with the second object 82. In contrast, as shown in FIG. 10c, according to an embodiment of the present invention, even a feature point that deviates from the viewing field of the camera 40 with a relatively narrow viewing angle is maintained if it does not satisfy the removal condition, so that feature point omission due to the difference in the viewing angles of the two types of detectors does not occur. As a result, as shown in FIG. 10c, two first objects 80 and two second objects 82 are searched in the map created by SLAM according to an embodiment of the present invention.

[0073] Although the preferred embodiments of the present invention have been described above, the present invention is not limited to the embodiments, and includes all modifications that can be easily modified by those having ordinary knowledge in the technical field to which the present invention pertains and are recognized as equivalent within the scope thereof.

Explanation of Reference Numerals

[0074] 10 Rider 20 Encoder 30 Inertial Sensor 40 Camera 50 Controller 60 Mobility 62 Field of View of Camera 64 Field of View of Rider 66 Feature Point 68 Feature Point 70 Feature Point 72 Feature Point 76 Feature Point 80 First Object 82 Second Object

Claims

1. A lidar configured to detect feature points in front of the mobility, A camera configured to acquire a front image of the mobility, Receives the transmission of data regarding the feature points in front of the mobility from the lidar, receives the front image of the mobility from the camera, searches for feature points from the front image, sets the feature points transmitted from the lidar and the feature points searched from the front image as a point cloud (PCL), and is configured to perform simultaneous localization and mapping (SLAM) based on the feature points in the PCL, including, The controller is further configured to remove feature points that satisfy the removal condition from the PCL in response to the arrival of the object search period, and add feature points newly searched after the previous object search period to the PCL, The feature points that satisfy the removal condition are feature points whose distance to the mobility is greater than a first set distance, and the simultaneous localization and mapping (SLAM) system is characterized in that.

2. The SLAM system according to claim 1, wherein the controller maintains feature points that do not satisfy the removal condition in the PCL.

3. The SLAM system according to claim 1, wherein the controller is configured to first remove feature points that satisfy the removal condition from the PCL, and then add feature points newly searched to the PCL.

4. The controller sets the feature points newly searched after the previous object search period as a new point cloud (nPCL), determines whether the feature points in the nPCL are the same as the feature points in the PCL, and is further configured to add the feature points in the nPCL to the PCL in response to the determination that the feature points in the nPCL are not the same as the feature points in the PCL. The SLAM system according to claim 1 is characterized in that.

5. The SLAM system according to claim 4, wherein the controller is configured to determine that the feature points in the nPCL are not the same as the feature points in the PCL in response to determining that the minimum value of the distance between the feature points in the nPCL and the feature points in the PCL is greater than or equal to a second set distance.

6. In response to determining that the minimum value of the distance between the feature points in the nPCL and the feature points in the PCL is less than a second set distance, the controller determines whether the positional relationship between the feature points in the nPCL and the feature points in the PCL excluding the said feature point matches the positional relationship between the said feature point in the PCL and the feature points in the PCL excluding the said feature point, and in response to determining that the positional relationship between the feature points in the nPCL and the feature points in the PCL excluding the said feature point does not match the positional relationship between the said feature point in the PCL and the feature points in the PCL excluding the said feature point, the controller is configured to determine that the feature points in the nPCL are not the same as the feature points in the PCL. The SLAM system according to claim 4, characterized in that.

7. In response to determining that the positional relationship between the feature points in the nPCL and the feature points in the PCL excluding the said feature point matches the positional relationship between the said feature point in the PCL and the feature points in the PCL excluding the said feature point, the controller is configured to determine that the said feature point in the nPCL is the same as the said feature point in the PCL. The SLAM system according to claim 6, characterized in that.

8. The controller is further configured to remove noise in the nPCL through data clustering. The SLAM system according to claim 4, characterized in that.

9. In response to Simultaneous Localization and Mapping (SLAM) being triggered, by a mobility controller, receiving data regarding feature points in front of the mobility from a lidar, receiving, by the controller, a front image of the mobility from a camera and searching for feature points from the front image, setting, by the controller, the feature points transmitted from the lidar and the feature points searched from the front image as a Point Cloud (PCL), removing, by the controller, feature points that satisfy a removal condition from the PCL in response to the object search period arriving, adding, by the controller, feature points newly searched after the previous object search period to the PCL, A Simultaneous Localization and Mapping (SLAM) method, characterized by including.

10. The feature points that satisfy the removal condition are feature points whose distance to the mobility is greater than a first set distance. The SLAM method according to claim 9, characterized in that.

11. The SLAM method according to claim 9, further comprising a step of maintaining, by the controller, feature points that do not satisfy the removal condition within the PCL.

12. The step of adding feature points newly explored after the previous object exploration cycle to the PCL includes: setting the feature points newly explored after the previous object exploration cycle as a new point cloud (nPCL); determining whether the feature points in the nPCL are the same as the feature points in the PCL; responding to the determination that the feature points in the nPCL are not the same as the feature points in the PCL, and adding the feature points in the nPCL to the PCL; The SLAM method according to claim 9, characterized by including the above steps.

13. The step of determining whether the feature points in the nPCL are the same as the feature points in the PCL includes comparing the minimum value of the distance between the feature points in the nPCL and the feature points in the PCL with a second set distance. The SLAM method according to claim 12 is characterized by this.

14. The step of determining whether the feature points in the nPCL are the same as the feature points in the PCL further includes, in response to the determination that the minimum value of the distance between the feature points in the nPCL and the feature points in the PCL is greater than or equal to the second set distance, determining that the feature points in the nPCL are not the same as the feature points in the PCL. The SLAM method according to claim 13 is characterized by this.

15. The step of determining whether the feature points in the nPCL are the same as the feature points in the PCL further includes, in response to the determination that the minimum value of the distance between the feature points in the nPCL and the feature points in the PCL is less than the second set distance, determining whether the positional relationship between the feature points in the nPCL and the feature points excluding the corresponding feature points in the PCL matches the positional relationship between the corresponding feature points in the PCL and the feature points excluding the corresponding feature points in the PCL. The SLAM method according to claim 13 is characterized by this.

16. The step of determining whether the feature points in the nPCL are the same as the feature points in the PCL further includes, in response to the determination that the positional relationship between the feature points in the nPCL and the feature points excluding the corresponding feature points in the PCL does not match the positional relationship between the corresponding feature points in the PCL and the feature points excluding the corresponding feature points in the PCL, determining that the feature points in the nPCL are not the same as the feature points in the PCL. The SLAM method according to claim 15 is characterized by this.

17. The step of determining whether the feature points in the nPCL are the same as the feature points in the PCL further includes the step of determining that the feature point in the nPCL is the same as the feature point in the PCL in response to the determination that the positional relationship between the feature point in the nPCL and the feature points excluding the feature point in the PCL matches the positional relationship between the feature point in the PCL and the feature points excluding the feature point in the PCL. The SLAM method according to claim 15, characterized in that it further includes the above steps.

18. The step of adding the feature points newly explored after the previous object exploration cycle to the PCL further includes the step of removing noise in the nPCL through data clustering. The SLAM method according to claim 12, characterized in that it further includes the above steps.

Images