Sequential Mapping and Localization for Navigation (SMAL)

Abstract

The present application discloses a sequential mapping and localization (SMAL) method (i.e., SMAL method) for navigating a mobile target. The SMAL method includes generating an initial map of an unknown environment during mapping; determining the position of the mobile target in the initial map during localization; and guiding the mobile target in the unknown environment, for example, by creating controls or instructions for the mobile target. The present application also discloses a system using the SMAL method and a computer program product for implementing the SMAL method.

Description

[0001] This application claims as its priority date the filing date of Singapore Patent Application No. 10201913873Q filed with the Intellectual Property Office of Singapore (IPOS) on 30 December 2019 and having the same title “Sequential Mapping and Localization for Navigation (SMAL)”. All relevant contents and / or subject matter of the earlier priority patent application are hereby incorporated by reference, where appropriate.

[0002] The present application relates to a sequential mapping and localization (SMAL) method for navigation (e.g., navigation of a mobile object). The present application also discloses a system for navigation by sequential mapping and localization (SMAL) and a computer program product for implementing a sequential mapping and localization (SMAL) method for navigation.

[0003] Navigation, such as navigation for a mobile target, includes a mapping process and a positioning process. The positioning is the main function for the mobile target (such as an autonomous vehicle or robot). Before navigating itself to the destination and completing its task, the mobile target should know its position and direction. Traditionally, real-time kinematic (RTK), global navigation satellite system (GNSS) (including the United States' Global Positioning System (GPS), China's Beidou, Europe's Galileo and Russia's GLONASS) and inertial measurement unit (IMU) navigation systems (abbreviated as RTK-GNSS / IMU navigation systems) are used to navigate mobile targets. High-definition maps (HD maps) are also provided for positioning. The HD maps are collected in advance. Recently, a synchronous positioning and mapping (SLAM) solution is used for the positioning function of the mobile target (such as an autonomous vehicle or robot) by building or updating a map of an unknown environment and simultaneously tracking the position of the mobile target in the map. The SLAM solution works well when there are enough fixed surrounding features in the environment, such as buildings, trees, telephone poles on public roads, or walls, tables, and chairs in indoor environments.

[0004] However, in certain specific areas, such as container terminals, the RTK-GNSS / IMU navigation system does not work well because the GNSS results often deviate a lot when the satellite signals of the GNSS are easily blocked by gantry cranes or piles of containers in the container terminal. The HD map has multiple uses, such as structural targets for positioning, lane connections with markers for path planning, and detailed marker locations for good-looking graphical user interfaces (GUIs). Therefore, the HD map is generated independently and is still based on structural targets in the HD map, such as buildings, trees, utility poles, etc.

[0005] At the same time, the SLAM solution cannot achieve satisfactory results when there are almost no fixed surrounding targets in open environments such as container terminals and airport sites. For example, although there are many containers in the container terminal, they are not fixed and their positions vary greatly, which will adversely affect the positioning results of the SLAM solution. In the container terminal, no autonomous vehicles have been deployed, only automated guided vehicles (AGVs) that need to deploy a large number of beacons such as radio frequency identification (RFID) tags or ultra-wideband (UWB) stations in the environment.

[0006] Therefore, the present application discloses a sequential mapping and localization (SMAL) method for solving the localization problem of a mobile target (such as the autonomous vehicle or the robot) in an open environment (such as the airport site and the container terminal). The SMAL method uses one or more existing natural road features for sequential mapping and localization.

[0007] In the first aspect, the present application discloses a sequential mapping and localization (SMAL) method (i.e., SMAL method) for navigating a mobile target. The SMAL method includes generating an initial map of an unknown environment during a mapping process; determining the position of the mobile target in the initial map during a localization process; and guiding the mobile target in the unknown environment, for example, by creating controls or instructions for the mobile target. Compared with the SLAM solution, the SMAL method separates the mapping process in the generation step from the localization process in the calculation step. In addition, unless the unknown environment near the mobile target changes dramatically, a series of observations of the unknown environment will not be carried out in discrete time steps to update the initial map.

[0008] The unknown environment optionally includes an open area without any beacon (e.g., an airport or a container terminal). In contrast to existing technologies for autonomous vehicles in open areas (e.g., radio frequency identification tags or ultra-wideband base stations), the SMAL method does not require the construction or establishment of beacons to detect unknown environments and send signals to mobile targets. In addition, if beacons are established in unknown environments, the SMAL method and existing technologies can be used simultaneously. In addition, the SMAL method can also be combined with traditional technologies, including RTK-GNSS / IMU navigation systems, high-definition maps (HD maps) and / or SLAM solutions for unknown environments other than open areas (e.g., urban areas).

[0009] The mapping process is used to construct an initial map of an unknown environment. Optionally, the mapping process of the SMAL method includes: a step of collecting multiple first environmental data from a detection device; a step of merging multiple first environmental data into fused data (RGB-DD image or point cloud); a step of performing edge detection on the fused data to detect edge features; a step of extracting a first set of road features from the fused data; and a step of saving the first set of road features to the initial map. The first environmental data describes multiple features of the unknown environment, including road features and environmental features. In contrast to the prior art that uses environmental features (such as buildings, trees, and utility poles), the SMAL method is more suitable for open areas because road features are available in open areas.

[0010] Optionally, the detection device in the collecting step includes a range-based sensor, a vision-based sensor, or a combination thereof. A range-based sensor can provide accurate depth information, but the information does not have rich features. In contrast, a vision-based sensor can provide information that lacks depth estimation but is feature-rich. Therefore, a combination of a range-based sensor and a vision-based sensor can provide information that has both depth estimation and sufficient features.

[0011] Sensors based on ranging may include light detection and ranging (LIDAR), acoustic sensors, or a combination thereof. Acoustic sensors, also known as sound navigation and ranging (sonar) sensors, locate moving targets by receiving echo signals reflected by moving targets. The power consumption of acoustic sensors is typically 0.01 watt to 1 watt, and the depth is typically 2 meters to 5 meters. Acoustic sensors are generally not affected by color and transparency, and are therefore suitable for use in dark environments. In particular, acoustic sensors may include ultrasonic sensors with ultrasonic frequencies of 20 kHz or more. Ultrasonic sensors have more accurate depth information. In addition, acoustic sensors occupy only a few cubic inches and are compact. However, when there are many soft materials in an unknown environment, the acoustic sensor cannot achieve good results because the echo signals are easily absorbed by the soft materials. The performance of the acoustic sensor may also be affected by other factors in the unknown environment, such as temperature, humidity, and pressure. The detection results of the acoustic sensor can be corrected by compensating for its performance.

[0012] Light detection and ranging works similarly to acoustic sensors. Light detection and ranging uses electromagnetic waves (e.g., light) instead of sound waves as echo signals. Optionally, light detection and ranging emits up to 1 million pulses per second to generate a 360-degree field of view three-dimensional (3D) visualization of the unknown environment (i.e., point cloud). Compared with acoustic sensors, light detection and ranging consumes more power, typically 50 watts to 200 watts, and can measure a deeper depth range, typically 50 meters to 300 meters. In particular, light detection and ranging can have higher depth accuracy and higher depth accuracy than acoustic sensors, both of which have depth accuracy within a few centimeters (cm). In addition, light detection and ranging can also provide angular resolution accurate to 0.1 degrees to 1 degree. Therefore, light detection and ranging is more suitable for autonomous vehicles than acoustic sensors. For example, the Velodyne HDL-64E light detection and ranging is suitable for driverless cars. However, light detection and ranging is bulky and not suitable for use at low power.

[0013] Vision-based sensors include monocular cameras, omnidirectional cameras, event cameras, or a combination thereof. Monocular cameras may include one or more standard RGB cameras, including color televisions and video cameras, image scanners, and digital cameras. Monocular cameras optionally have simple hardware (e.g., GoPro Hero-4) and a small form factor ranging in volume from a few cubic inches; therefore, monocular cameras can be mounted on small mobile targets (e.g., mobile phones) without any additional hardware. Because standard RGB cameras are passive sensors, the power consumption of monocular cameras is also between 0.01 watts and 10 watts. However, because monocular cameras cannot directly infer depth information from static images, they need to work with complex algorithms. In addition, monocular cameras have the problem of scale drift.

[0014] Omnidirectional cameras can also include one or more standard RGB cameras with a 360-degree field of view, such as Samsung Gear 360, GoPro Fusion, Ricoh Theta V, Detu Twin, LGR105, and Yi 360VR with two fisheye lenses. Therefore, omnidirectional cameras can provide panoramic photography in real time without any post-processing. Omnidirectional cameras are compact and have power consumption ranging from 1 watt to 20 watts. However, omnidirectional cameras cannot provide depth information.

[0015] Event cameras (e.g., dynamic vision sensors) are biomimetic vision sensors that output pixel-level brightness changes instead of standard intensity frames. Therefore, event cameras have several advantages, such as high dynamic range, no motion blur, and only microsecond latency. Because event cameras do not capture any redundant information, they are very power efficient. The typical power consumption of event cameras is 0.15 watts to 1 watt. However, event cameras require special algorithms to detect high temporal resolution and asynchronous events. Traditional algorithms are not suitable for event cameras that output a series of asynchronous events instead of actual intensity images.

[0016] Alternatively, the detection device may optionally include an RGB-D sensor, a stereo camera, and the like. An RGB-D sensor or a stereo camera may provide depth information and feature-rich information. An RGB-D sensor (e.g., a Microsoft Kinect RGB-D sensor) provides a combination of a monocular camera, an infrared (IR) transmitter, and an infrared receiver. Therefore, an RGB-D sensor may provide scene details and an estimated depth of each pixel in the scene. In particular, the RGB-D sensor may optionally use two depth calculation techniques, namely, structured light (SL) technology or time of flight (TOF) technology. Structured light technology projects an infrared speckle pattern using an infrared transmitter, and the infrared speckle pattern is captured by an infrared receiver. The infrared speckle pattern is compared part by part with a reference pattern provided in advance and with a known depth. After matching the infrared speckle pattern with the reference pattern, the depth of each pixel is estimated by the RGB-D sensor. The working principle of the time of flight technology is similar to light detection and ranging. The power consumption of an RGB-D sensor is typically 2 watts to 5 watts; the depth range is typically 3 meters to 5 meters. In addition, due to the use of a monocular camera, the RGB-D sensor also has a scale drift problem.

[0017] Stereo cameras, such as the Bumblebee stereo camera, calculate depth information using the difference between two camera images viewing the same scene. Contrary to RGB-D sensors, stereo cameras are passive cameras; therefore, they do not suffer from scale drift. The power consumption of stereo cameras ranges from 2 watts to 15 watts; the depth range is 5 meters to 20 meters. In addition, the depth accuracy of stereo cameras is typically a few millimeters (mm) to a few centimeters (cm).

[0018] Optionally, the mobile target has a communication hub for communicating with the detection device and a driving mechanism (e.g., a motor) for moving the mobile target on the ground. The mobile target may also include a processing unit that receives observations of the unknown environment from the communication hub, processes the observations and generates instructions to the driving mechanism to guide the mobile target to move. Therefore, the detection device, the processing unit, and the driving mechanism form a closed loop that guides the mobile target to move in an unknown environment. In some embodiments, the detection device is installed on the mobile target; therefore, the detection device moves with the mobile target (i.e., a dynamic detection device). Optionally, the dynamic detection device is installed in a non-blocking position of the mobile target (e.g., the top or upper end of the mobile target) so as to detect the unknown environment with a 360-degree field of view. In some embodiments, the detection device adopts a static structure in an unknown environment, so it does not move with the mobile target (i.e., a static detection device). The static detection device sends observations to the mobile target passing through the static detection device. Therefore, if the unknown environment remains fairly stable, the static detection device does not need to obtain observations within a discrete time step. On the contrary, the static detection device will be activated and work only when the unknown environment changes dramatically.

[0019] Road features already exist in open areas (e.g., highways). Optionally, the first set of road features in the extraction step includes markings (e.g., white markings and yellow markings), curbs, grass, dedicated lines, points, road edges, different road surface edges, or any combination thereof. Road features are more accurate and easier to distinguish than environmental features (e.g., buildings, trees, and telephone poles), especially at night and rainy days. In some embodiments, the detection device includes light detection and ranging as range-based sensors and a camera as a vision-based sensor for real-time detection at night and rainy days. In some embodiments, because light detection and ranging can provide more accurate positioning than a camera at night and rainy days, the detection device only includes light detection and ranging.

[0020] In the collecting step, the detection device may collect noise and first environmental data from the unknown environment. The mapping process may also include a step of applying a probabilistic method to the first environmental data to remove noise from the first environmental data. The probabilistic method first distributes the noise and the first environmental data, then identifies the noise as abnormal information, and finally removes the abnormal information from the first environmental data, thereby separating the noise from the first environmental data (including road features and environmental features). The probabilistic method optionally includes a recursive Bayesian estimation (also called a Bayesian filter). Recursive Bayesian estimation is suitable for estimating the dynamic state of a system evolving over time given sequential observations or measurements about the system. Therefore, recursive Bayesian estimation is suitable for mapping processes performed when the unknown environment undergoes drastic changes.

[0021] Optionally, the mapping process is performed in an offline manner. Once the saving step is completed, the mapping process will end at the same time; and it will only be activated when the unknown environment changes drastically. In other words, unless the unknown environment changes drastically and is not suitable for guiding the mobile target, the initial map will remain unchanged and will not be updated.

[0022] Optionally, the acquisition step is performed frame by frame. In other words, the first environmental data is represented by collecting a plurality of frames. These frames are acquired by a detection device, such as a camera, light detection and ranging, or a combination of a camera and light detection and ranging.

[0023] Optionally, the frames are aligned with a world coordinate system, such as RTK-GNSS / IMU, to perform the merging step. In particular, each frame can be associated with a position in a world coordinate system (such as RTK-GNSS / IMU). Each frame should be associated with an accurate position in the RTK-GNSS / IMU. Optionally, the merging step also includes correcting bad frames whose positions are inaccurate in a world coordinate system (such as RTK-GNSS / IMU navigation system). The bad frame position in the RTK-GNSS / IMU is inaccurate and deviates from its actual position. In other words, if the GNSS positioning deviates from the actual position, the accurate position will not be associated with the bad frame. Optionally, the correction step is performed by referring to the original overlap of the bad frame with other frames (also called normal frames) with accurate positions. However, the correction step requires sufficient overlap to determine the exact position of the bad frame. If the overlap is not sufficient to achieve this purpose, the correction step may include collecting other normal frames near the bad frame with a detection device when the original overlap number is below a certain threshold to generate other overlap steps. In other words, other normal frames near the bad frame are collected to create sufficient overlap. Or discard the bad frame and collect a new good frame at the GNSS position of the bad frame.

[0024] Edge detection generally uses various mathematical methods to identify specific points in a single image or point cloud. These specific points have discontinuities, that is, some properties (such as brightness) of each specific point will change significantly. These specific points generally form a set of line segments called edges. Because road features have different characteristics from their respective surroundings, edge detection can be used to identify road features. Optionally, edge detection includes spot detection for extracting road features. Spots are defined as areas where the internal characteristics are basically constant or approximately constant. Therefore, these characteristics are expressed as functions of each point on a single image or point cloud. Edge detection can be performed by difference methods or methods based on local extrema of functions or deep learning. Difference methods are implemented based on the derivatives of functions at relative positions; while local extrema-based methods aim to find local maxima and minima of functions. Spot detection optionally includes convolution of a single image or point cloud. Spot detection can be performed using different methods, including Laplacian operators of different scales for finding the maximum response, K-means clustering (distance) and deep learning.

[0025] The mapping algorithm is shown below.

[0026] Pseudocode for the mapping algorithm in SMAL.

[0027] Input: images from camera {I} and / or point clouds from detection and ranging {C}

[0028] Output: Map M

[0029] algorithm:

[0030]

[0031]

[0032] The positioning process is used to determine the corresponding position of the mobile target in the unknown environment in the initial map. Optionally, the positioning process includes: the step of collecting a plurality of second environmental data near the mobile target; the step of identifying a second set of road features from the second environmental data; and the step of matching the second set of road features of the unknown environment with the first set of road features in the initial map. Afterwards, the mobile target is guided to move in the unknown environment based on the position of the mobile target in the initial map. In addition, optionally, the positioning process also includes the step of updating the position of the mobile target in the initial map after stopping the movement.

[0033] In some embodiments, the second environmental data is collected from a detection device (i.e., an existing detection device) that generates the first environmental data, including a dynamic detection device and a static detection device. In some embodiments, the second environmental data is collected from other detection devices. Other detection devices may also include range-based sensors, vision-based sensors, or a combination thereof. Other detection devices may work independently or in conjunction with existing detection devices.

[0034] In contrast to the mapping process, the localization process is optionally performed in an online manner, that is, the position of the mobile object in the initial map is updated in discrete time steps as the position of the mobile object in the unknown environment changes.

[0035] The unknown environment may include static features that do not move when the mobile target passes by; and dynamic features that actively move around the mobile target. For example, there may be multiple vehicles entering an airport to transport passengers and cargo. As another example, a small number of forklifts may work at a container terminal to lift and move containers. Therefore, the mobile target in the unknown environment should be guided to avoid any collision with the dynamic features. Optionally, the collecting step of the positioning process includes a step of separating a first part of the second environmental data of the static features near the mobile target and a second part of the second environmental data of the dynamic features near the mobile target.

[0036] The collecting step of the positioning process may also optionally include the step of tracking dynamic features in the vicinity of the moving target by combining the results of the dynamic features observed over time. In some embodiments, the tracking step includes the step of tracking a specific dynamic feature using a filtering method, wherein the filtering method evaluates its state based on the observations over time. In some embodiments, the tracking step includes the step of tracking multiple dynamic features using data association, wherein the data association is used to identify the observations of their respective dynamic features; and the step of tracking each of the multiple dynamic features using a filtering method, wherein the filtering method is used to evaluate its state based on the observations over time.

[0037] The SMAL method may also include the step of updating the initial map to a first update map when the unknown environment changes beyond a first predetermined threshold. The initial map is replaced by the first update map for guiding a mobile target in an unknown environment. Similarly, the SMAL method may also include the step of updating the first update map to a second update map when the unknown environment changes beyond a second predetermined threshold. The first update map is replaced by the second update map for guiding a mobile target in an unknown environment. Similarly, the SMAL method may also include the step of updating the second update map when the unknown environment changes beyond a third predetermined threshold and updating other subsequent corresponding maps when the unknown environment changes beyond other subsequent predetermined thresholds.

[0038] As a second aspect, the present application discloses a system for navigating a mobile target using sequential mapping and localization (SMAL). The system includes: a device for generating an initial map of an unknown environment using a mapping mechanism; a device for determining the position of the mobile target in the initial map by using a localization mechanism; a device for guiding the mobile target in the unknown environment by creating controls or instructions.

[0039] The mapping mechanism optionally includes means for collecting a plurality of first environmental data from a detection device; means for merging the plurality of first environmental data into fused data (RGB-D image or point cloud); means for performing edge detection on the fused data to detect edge features; means for extracting a first set of road features from the fused data; and means for saving the first set of road features into the initial map. In particular, the mapping mechanism can operate offline.

[0040] The positioning mechanism optionally includes means for collecting a plurality of second environmental data in the vicinity of the mobile target; means for identifying a second set of road features from the second environmental data; and means for matching the second set of road features of the unknown environment with the first set of road features in the initial map. In particular, the positioning mechanism can be operated online. In addition, the positioning mechanism can also include means for updating the position of the mobile target in the initial map.

[0041] The system may further include a device for updating the initial map to a first updated map when the unknown environment changes by more than a first predetermined threshold. In other words, if the change in the unknown environment is less than the first predetermined threshold, the device for updating the initial map is not enabled.

[0042] The system may further include means for updating the first update map to a second update map when the unknown environment changes by more than a second predetermined threshold. In other words, if the change in the unknown environment is less than the second predetermined threshold, the means for updating the first update map is not enabled. Similarly, the SMAL method may also include means for updating the second update map when the unknown environment changes by more than a third predetermined threshold and for updating other subsequent corresponding maps when the unknown environment changes by more than other subsequent predetermined thresholds.

[0043] As a third aspect, the present application discloses a computer program product, the computer program product comprising a non-transitory computer-readable storage medium, the medium containing computer program instructions and data to execute a sequential mapping and localization (SMAL) method for navigating a mobile target. The SMAL method comprises the steps of generating an initial map of an unknown environment during a mapping process; determining the position of the mobile target in the initial map during a localization process; and guiding the mobile target in an unknown environment.

[0044] Optionally, the mapping process is performed by the following steps: a step of collecting a plurality of first environmental data from a detection device; a step of merging the plurality of first environmental data into fused data (RGB-D image or point cloud); a step of performing edge detection on the fused data to detect edge features; a step of extracting a first set of road features from the fused data; and a step of saving the first set of road features to an initial map. In particular, the mapping process is an offline operation.

[0045] Optionally, the positioning process is performed by the following steps: a step of collecting a plurality of second environmental data near the mobile target; a step of identifying a second set of road features from the second environmental data; and a step of matching the second set of road features of the unknown environment with the first set of road features in the initial map. In particular, the positioning mechanism is an online operation. In addition, the positioning process may also include a step of updating the position of the mobile target in the initial map.

[0046] The positioning algorithm is shown below.

[0047] Pseudocode for the localization algorithm in SMAL.

[0048] Input: map M, a frame from a camera I and / or a point cloud C from a lidar.

[0049] Output: Pose (position p and orientation o)

[0050] Memory: Point P = {}

[0051] algorithm:

[0052]

[0053]

[0054] The SMAL method may also include the step of updating the initial map to a first updated map when the unknown environment changes by more than a first predetermined threshold. The SMAL method may also include the step of updating the first updated map to a second updated map when the unknown environment changes by more than a second predetermined threshold. Similarly, the SMAL method may also include the step of updating the second updated map when the unknown environment changes by more than a third predetermined threshold and updating other subsequent corresponding maps when the unknown environment changes by more than other subsequent predetermined thresholds.

[0055] The embodiments are shown in the accompanying drawings and are used to explain the principles of the disclosed embodiments. However, it should be understood that these drawings are only intended for illustrative purposes and are not intended to define the limitations of the relevant applications.

[0056] Figure 1 A schematic diagram of simultaneous localization and mapping (SLAM) navigation is shown;

[0057] Figure 2 A schematic diagram of sequential mapping and localization (SMAL) navigation is shown;

[0058] Figure 3 The mapping process of SMALL navigation is shown;

[0059] Figure 4 The positioning process of SMAL navigation is shown.

[0060] Figure 1 A schematic diagram of a simultaneous localization and mapping (SLAM) navigation 100 is shown. The SLAM navigation 100 is used to navigate an autonomous vehicle 102 from a starting position 112 to a destination 113 in an unknown environment 104. A map 106 of the unknown environment 104 is constructed and updated according to discrete time steps (t) so that the SLAM navigation 100 simultaneously keeps track of the position 108 of the autonomous vehicle 102 in the map 106.

[0061] In a first step 110, the autonomous vehicle 102 is located at a starting position 112 and obtains first sensor observations O1 around the starting position 112. The first sensor observations O1 are transmitted to the autonomous vehicle 102 for building a first map mi around the starting position 112. The position 108 of the autonomous vehicle 102 is calculated as a first position x in the first map mi. i The autonomous vehicle 102 then generates a first control ui according to the first position xi in the first map mi, so as to move the autonomous vehicle 102 from the starting position 112 in the unknown environment 104. The first map mi and the first position x therein are clearly shown. i In a first step 110 , the updating is simultaneous.

[0062] The autonomous vehicle 102 moves from the starting position 112 to the second position 122 after the first discrete time step 114. In the second step 120, the autonomous vehicle 102 is at the second position 122, and obtains a second sensor observation o2 from around the second position 122. The second sensor observation o2 is transmitted to the autonomous vehicle 102 for updating the first map mi to a second map m2 around the second position 122. The position 108 of the autonomous vehicle 102 in the second map m2 is correspondingly updated to the second position x2. The autonomous vehicle 102 then generates a second control u2 based on the second position x2 in the second map m2 to move the autonomous vehicle 102 from the second position 122 in the unknown environment 104. The second map m2 and the second position m2 are synchronously updated in the second step 120.

[0063] After the second discrete time step 124, the autonomous vehicle 102 moves from the second position 122 to the next position. In this step-by-step manner, the autonomous vehicle 102 continuously moves in the unknown environment 104. At the same time, the position 108 is also updated accordingly in the map 106. In the penultimate step 130, the penultimate sensor observation value is obtained near the penultimate position 132. t-1 . Update the second-to-last map m around the second-to-last position 132 t-1 Accordingly, the autonomous driving vehicle 102 is in the second-to-last map m t-1 The position 108 in the map is updated as the second-to-last position. Then the autonomous driving vehicle 102 updates the position 108 in the map according to the last map m. t-1 The second to last position x in t-1 Generate the penultimate control u t-1 , so that the autonomous driving vehicle 102 moves from the penultimate position 132. In the penultimate step 130, the penultimate map m is synchronously updated. t-1and the second position x t-1 .

[0064] In a final step 140, the final sensor observations are obtained from the unknown environment 104 after the penultimate discrete time step 134. t The final sensor observation value 0 is transmitted to the autonomous driving vehicle 102 for converting the penultimate map m t-1 Updated to final map m t . Accordingly, the final map m t The position 108 of the autonomous driving vehicle 102 in is updated to the final position x t Then the autonomous driving vehicle 102 uses the final map m t The final position x in t Generate the most common control u t , so as to stop the autonomous driving vehicle 102 at the destination 113. Therefore, in the final step 140, the final map m is updated synchronously t and the final position x t .

[0065] Figure 2 2 shows a schematic diagram of sequential mapping and localization (SMAL) navigation 200. SMAL navigation 200 is also used to navigate an autonomous vehicle 202 in an unknown environment 204. A map 206 of the unknown environment 204 is constructed and updated for the SMAL navigation 200 in order to maintain synchronized tracking of a position 208 of the autonomous vehicle 202 in the map 206.

[0066] SMAL navigation 200 is divided into three phases, an initial phase 210, a positioning phase 220, and a mapping phase 230. In the initial phase 210, initial sensor observations oi are obtained from the unknown environment 204 and transmitted to the autonomous driving vehicle 202 to generate an initial map m of the unknown environment 204. i The position 208 of the autonomous vehicle 202 is calculated as the initial map m i Then, the autonomous driving vehicle 202 calculates the initial position xt in the initial map m. i The initial position x in i Generate initial control u t , so as to move the autonomous driving vehicle 202 in the unknown environment 104.

[0067] In the localization phase 220, a first sensor observation o1 is obtained around a first location 222 in the unknown environment 204. Accordingly, the location 108 of the autonomous driving vehicle 102 is calculated in the initial map mi and is denoted as the first location x i Then, the autonomous driving vehicle 102 uses the initial map m i The initial position x ini Generate initial control u i , in order to move the autonomous driving vehicle 202 in the unknown environment 204. Compared with the SLAM navigation 100, the first sensor observation value o is used to update the initial map m i .

[0068] The autonomous vehicle 202 moves from the first location 222 to the subsequent location in discrete time without updating the initial map m i , until it moves to the final location 224. The final sensor observation result is obtained near the final location 224 in the unknown environment 204. t Similarly, the final sensor observation o t Not used to update the initial map m i The position 208 of the autonomous driving vehicle 202 in the unknown environment 204 is updated to the initial map m i The final position x in t Then, the autonomous driving vehicle 202 generates the final control u t , based on the initial map m i The final position x in t , moving the autonomous vehicle 202 to the next location around the last location 224.

[0069] In the mapping phase 230, the initial map m i Not applicable because the change in the unknown environment 104 significantly exceeds the first predetermined threshold. Get updated sensor observations from the unknown environment 204 n , and sends it to the autonomous driving vehicle 202 to update 232 the initial map m i First update map m to unknown environment 204 u Subsequently, the localization phase 220 is repeated 234 to guide the autonomous vehicle 102 to move in the unknown environment 104 using the first updated map mi. Similarly, when the unknown environment 104 significantly exceeds the second predetermined threshold and subsequent predetermined thresholds, the updated map is m u , updated to the second update map and subsequent update maps respectively.

[0070] Figure 3The mapping process 300 of the SMAL navigation 200 is shown. The steps of executing the mapping process 300 are as follows: the first step is to collect multiple first environmental data from the detection device; the second step 304 is to merge the multiple first environmental data into fused data (RGB-D image or point cloud); the third step 306 is to perform edge detection on the fused data to detect edge features; the fourth step 308 is to extract a first set of road features from the fused data; the fifth step 310 is to save the first set of road features to the initial map. For the SMAL navigation 200, the mapping process 300 can be used in the initial stage 210 to generate the initial map 212, and in the mapping stage to convert the initial map 232 into the initial map m. i Update to the first update map m ii Similarly, when the unknown environment undergoes a significant change, the mapping process 300 is also applicable to updating the first update map, the second update map, and subsequent update maps.

[0071] In the first step 302, first environmental data is collected frame by frame from LIDAR, camera or combination thereof as detection devices. In the second step 304, the frames are aligned and merged into a single frame. During the merging, real-time dynamic positioning (RTK); global navigation satellite system (GNSS) (including the United States' Global Positioning System (GPS), China's Beidou, Europe's Galileo and Russia's GLONASS) and inertial measurement unit (IMU) navigation system (RTK-GNSS / IMU navigation system for short) are also involved. Each frame is associated with the precise position of the RTK-GNSS / IMU navigation system (referred to as a normal frame). If the GNSS position of a certain frame is offset (referred to as a bad frame), the position of the bad frame is inaccurate. In this case, the precise position of the bad frame is determined based on the overlap of the bad frame and the normal frame. If no overlap or insufficient overlap is found, the bad frame is abandoned and a new frame is taken near the bad frame to replace it. In the third step 306, an edge detection algorithm is applied to the single frame to detect edge features. In the fourth step 308, a spot detection algorithm is applied to the edge features to extract road features from the edge features. In the fifth step 310, the road features are integrated into the initial map m i 、First update map m u , the second updated map and subsequent updated maps.

[0072] Figure 4 The mapping process 400 of the SMAL navigation 200 is shown. The positioning process 400 is performed by the following steps: a first step 402, collecting a plurality of second environmental data near the mobile target; a second step 404, identifying a second set of road features from the second environmental data; a third step 406, matching the second set of road features of the unknown environment with the first set of road features in the initial map; a fourth step, updating the position 108 of the autonomous driving vehicle 102 in the map 106, including the initial map m i、First update map m u , a second updated map, and a subsequent updated map. By repeating the localization phase 220 and the mapping phase 230 , the SMAL navigation 200 guides the autonomous driving vehicle 102 to move in the unknown environment 104 .

[0073] In this application, unless otherwise specified, the terms "comprise", "include" and their grammatical variations are used to represent "open" or "inclusive" language, such that it includes the listed elements but also allows for the inclusion of other elements not explicitly listed.

[0074] As used herein, in the context of formulation ingredient concentrations, the term "about" generally means + / - 5% of the stated value, more typically + / - 4% of the stated value, + / - 3% of the stated value, + / - 2% of the stated value, + / - 1% of the stated value, or even + / - 0.5% of the stated value.

[0075] In the present invention, certain embodiments may be disclosed in interval format. The description in interval format is for convenience and brevity only and should not be interpreted as a rigid limitation on the disclosed range. Accordingly, the description of an interval should be considered to have specifically disclosed all possible sub-intervals and the values ​​within the interval. For example, a description of an interval (e.g., from 1 to 6) should be considered to have specifically disclosed sub-intervals (e.g., from 1 to 3, from 1 to 4, from 1 to 5, from 2 to 4, from 2 to 6, from 3 to 6, etc.) and single numbers within the interval, such as 1, 2, 3, 4, 5, and 6. Regardless of the magnitude of the interval, this principle applies.

[0076] Obviously, after reading the above disclosure, various other modifications and adaptations of the present application will be obvious to those skilled in the art without departing from the spirit and scope of the present application, and all such modifications and adaptations are within the scope of the appended claims.

[0077] Reference numerals

[0078] 100 Simultaneous Localization and Mapping (SLAM) navigation;

[0079] 102 Autonomous vehicles;

[0080] 104 unknown environment;

[0081] 106 maps;

[0082] 108 positions;

[0083] 110 First step;

[0084] 112 starting position;

[0085] 113 destination;

[0086] 114 first discrete time step;

[0087] 120 Step 2;

[0088] 122 Second Place;

[0089] 124 second discrete time step;

[0090] 130 The penultimate step;

[0091] 132 Penultimate location;

[0092] 134 penultimate discrete time step;

[0093] 140 The last step;

[0094] 200 Sequential Mapping and Localization (SMAL) navigation;

[0095] 202 Autonomous vehicles;

[0096] 204 unknown environment;

[0097] 206 maps;

[0098] 208 position;

[0099] 210 Initial stage;

[0100] 212 Generate initial map;

[0101] 220 Positioning phase;

[0102] 222 First Place;

[0103] 224 Last Place;

[0104] 230 Mapping stage;

[0105] 232 Updated initial map;

[0106] 234 repeat positioning phase 220;

[0107] 300 Mapping process;

[0108] 302 First step;

[0109] 304 Step 2;

[0110] 306 Step 3;

[0111] 308 Step 4;

[0112] 310 Step 5;

[0113] 400 Positioning process;

[0114] 402 First step;

[0115] 404 Step 2;

[0116] 406 Step 3;

[0117] 408 Step 4;

[0118] O1 first sensor observation result;

[0119] M1 first map;

[0120] X1 first position;

[0121] U1 first control;

[0122] o2 Observation result of the second sensor;

[0123] m2 second map;

[0124] X2 second position;

[0125] U2 Second Control;

[0126] o t-i The penultimate sensor observations;

[0127] m t-i The second to last map;

[0128] x t-I The second to last position;

[0129] u t-i Penultimate control;

[0130] o t Final sensor observation results;

[0131] m t Final map;

[0132] x t Last position;

[0133] u t Final control;

[0134] o i Initial sensor observations;

[0135] m i Initial map;

[0136] o ii Update sensor observations;

[0137] m ii First updated map

Claims

1. A sequential mapping and localization (SMAL) method for navigating an autonomous vehicle, comprising the following steps: Generating (212) an initial map (m) of the unknown environment (204) in a map building process (300) i ), wherein the map construction process (300) comprises the following steps: Collecting (302) a plurality of first environmental data from observations of at least one sensor; merging (304) the plurality of first environmental data into fused data, wherein the fused data comprises an RGB-D image or a point cloud; Perform (306) edge detection on the fused data Extracting (308) a first set of road features from the fused data; and · Save (310) the first set of road features to the initial map (m i ); Determining in a positioning process (400) that the autonomous driving vehicle (202) is located in the initial map (m i ), The positioning process (400) comprises the following steps: Collecting (402) a plurality of second environmental data around the autonomous driving vehicle (200); identifying (404) a second set of road features from the second environmental data; and ·Compare the second set of road features of the unknown environment with the initial map (m i ) to match the first set of road features in (406); In the initial map (m i ) in updating (408) the position (220) of the autonomous vehicle (202); and guiding the autonomous vehicle (200) in the unknown environment (204) to control the movement of the autonomous vehicle (200), wherein the initial map (m i ); The sensor includes a distance-based sensor, a vision-based sensor or a combination thereof. The distance-based sensor includes a light detection and ranging (LIDAR), an acoustic sensor or a combination thereof. The vision sensor includes a monocular camera, an omnidirectional camera, an event camera or a combination thereof.

2. The method according to claim 1, wherein The unknown environment (204) includes an open area in which the road features are available.

3. The method according to claim 1, wherein The first set of road features includes markings, curbs, grass, lanes, points, road edges, different road surface edges, or any combination thereof.

4. The method according to claim 1, further comprising the steps of: A probabilistic method is applied to the first environmental data to eliminate noise in the first environmental data. The probabilistic method first distributes the noise and the first environmental data, then identifies the noise as abnormal information, and finally removes the abnormal information from the first environmental data to separate the noise from the first environmental data. The probabilistic method includes recursive Bayesian estimation.

5. The method according to claim 1, wherein: The collecting step (302) is performed in a frame-by-frame manner.

6. The method according to claim 1, wherein The merging step (304) is performed by aligning the frames to a world coordinate system.

7. The method according to claim 1, wherein The merging step (304) further comprises correcting the bad frame whose position is inaccurate in the world coordinate system, wherein the correcting step is performed by referring to the original overlap of the bad frame with the normal frame whose position is accurate.

8. The method according to claim 7, further comprising the steps of: Other good frames near the bad frame are collected to generate other overlaps.

9. The method according to claim 1, wherein: The edge detection includes blob detection for extracting the road features.

10. A system for navigating (200) an autonomous vehicle (202) using sequential mapping and localization (SMAL), comprising: · For generating (212) an initial map (m) of an unknown environment (204) through a map building mechanism i ), wherein the map construction mechanism is implemented by the following steps; Collecting (302) a plurality of first environmental data from observations of at least one sensor; merging (304) the plurality of first environmental data into fused data, wherein the fused data comprises an RGB-D image or a point cloud; Performing (306) edge detection on the fused data; Extracting (308) a first set of road features from the fused data; and · Save (310) the first set of road features to the initial map (m i ); ·Used to determine the location of the autonomous driving vehicle (202) in the initial map (m i ) in a position (220), wherein the positioning mechanism is implemented by the following steps: Collecting (402) a plurality of second environmental data near the autonomous driving vehicle (202); identifying (404) a second set of road features from the second environmental data; Matching (406) the second set of road features of the unknown environment with the first set of road features in the initial map (mi); and for guiding said device in said unknown environment, updating (408) the position (220) of the autonomous vehicle (202) in the initial map (mi); and Means for guiding the autonomous vehicle (202) in the unknown environment (204) in order to control the movement of the autonomous vehicle (202), wherein the initial map (mi) is constructed by means of the observations of the at least one sensor.

Images