METHOD AND SYSTEM FOR ADAPTIVE MAP-BASED ROUTE PLANNING

Abstract

A method for determining a route for a surveyed map includes receiving an initial survey plan and commanding the vehicle to move to survey a terrain following the initial survey plan. The method further includes surveying the terrain with a plurality of sensors and determining a confidence level for each of the plurality of map areas surveyed with the plurality of sensors of the vehicle. Furthermore, the method includes predicting a confidence level for each of the plurality of map areas still to be surveyed according to the initial survey plan. The method also includes determining a confidence loss based on the confidence data and determining whether the confidence loss is greater than a predetermined threshold. The method further includes adjusting the initial survey plan to create an updated survey plan.

Description

INTRODUCTION

[0001] This description generally relates to a system and method for route planning. More specifically, this description relates to a system and method for adaptive map-based route planning based on the cost of terrain navigation and the confidence in map and sensor data.

[0002] This introduction generally presents the context of the description. Works by the inventors named herein, to the extent described in this introduction, as well as aspects of the description that are not prior art at the time of application, are neither expressly nor implicitly acknowledged as prior art against this description.

[0003] Certain open-loop static survey schemes perform poorly when limited prior information about the terrain features of the area to be surveyed is available. Furthermore, mobile terrain survey platforms have limited onboard computing and storage resources. For these reasons, it is desirable to develop a system and method for efficiently surveying relatively unknown terrain with limited computing and storage resources.

[0004] US 2023 / 0 096 982 A1 shows a method for generating an exploration path for a robot, in which a point cloud is first created from individual image data, the points are classified as ground or obstacle points, a ground point cloud is formed, a trust map is generated from this, and access nodes and an exploration path are determined on the basis of this.

[0005] US 2021 / 0 141 389 A1 shows a robot with a drive system and data processing hardware that processes image data during movement and sets waypoints on a map using a heuristic. When the heuristic is triggered, a waypoint is stored with sensor data, with an associated edge path describing the movement between two waypoints.

[0006] ORISATOKI, Mobolaji O.; AMOUZADI, Mahdi; DIZQAH, Arash M.: A heuristic informative path-planning algorithm for autonomous mapping of unknown areas. August 2023. URL: https: / / arxiv.org / pdf / 2308.12209 [accessed on February 12, 2025] describes a heuristic path-planning algorithm that enables autonomous robots to efficiently map unknown areas. DESCRIPTION

[0007] The present description describes a method for adaptive route planning. The method includes receiving an initial survey plan by a control unit of a vehicle. The initial survey plan includes a plurality of initial waypoints. The vehicle includes a plurality of sensors. The method further includes commanding the vehicle to move to survey a terrain following the initial survey plan. The method also includes surveying the terrain with the plurality of sensors while the vehicle moves according to the initial survey plan to create a map of the terrain. The map of the terrain includes a plurality of map areas. The method also includes determining a confidence level for each of the plurality of map areas surveyed with the plurality of sensors of the vehicle to determine real-time confidence data.The method also includes predicting a confidence level for each of the plurality of map areas still to be surveyed according to the initial survey plan to determine predicted confidence data. The method also includes merging the predicted confidence data with the real-time confidence data to determine a confidence loss. The method also includes determining whether the confidence loss is greater than a predetermined threshold. The method also includes adjusting the initial survey plan to produce an updated survey plan in response to determining that the confidence loss is greater than the predetermined threshold. The method also includes instructing the vehicle to move according to the updated survey plan.Other embodiments of this aspect include corresponding computer systems, devices, and programs recorded on one or more computer storage devices, each configured to perform the actions of the methods.

[0008] In a particular aspect of the present description, the method may include determining the next two waypoints using the updated survey plan and updating the predicted confidence data based on the next two waypoints of the updated survey plan to develop updated predicted confidence data. The updated predicted confidence data is determined based on the virtual line between the next two waypoints of the updated survey plan. Adjusting the initial survey plan to generate an updated survey plan includes adding a new waypoint to the initial survey plan to create the updated survey plan, wherein the new waypoint includes location data in response to determining that the confidence loss is greater than the predetermined threshold.The method may include identifying a sequence of nodes in a search graph that represents the minimum cost of travel between a first waypoint and a last waypoint. The confidence level of each of the plurality of map areas monitored with the plurality of sensors of the vehicle is based on a distance between the vehicle and a corresponding one of the plurality of map areas monitored with the plurality of sensors. The confidence level of each of the plurality of map areas monitored with the plurality of sensors of the vehicle is based on a number of sensor measurements taken in the corresponding one of the plurality of map areas served by the plurality of sensors.The confidence level of each of the plurality of map areas sensed by the plurality of sensors of the vehicle is based on a number of sensor modalities used to sense the corresponding one of the plurality of map areas served by the plurality of sensors. The confidence level of each of the plurality of map areas monitored by the plurality of sensors of the vehicle is based on data sensed by the plurality of sensors in the corresponding one of the plurality of map areas served by the plurality of sensors. The method may also include determining that the vehicle is unable to reach at least one of the plurality of initial waypoints.The method may further include updating the initial survey plan to generate the updated survey plan in response to determining that the vehicle is unable to reach at least one of the plurality of initial waypoints.

[0009] This description further describes a vehicle with an adaptive route planning system. The vehicle includes sensors and a control unit. The control unit is programmed to execute the method described above.

[0010] Further areas of applicability of the present disclosure will become apparent from the detailed description below. It should be understood that the detailed description and specific examples are for purposes of illustration only and are not intended to limit the scope of the disclosure.

[0011] The above features and advantages, as well as other features and advantages of the presently disclosed system and method, are readily apparent from the detailed description, including the claims, and the exemplary embodiments taken in conjunction with the accompanying drawings. BRIEF DESCRIPTION OF THE CHARACTERS

[0012] The present disclosure will be better understood from the detailed description and the accompanying drawings, in which: Fig. Figure 1 is a schematic diagram of a vehicle with a video streaming system. Fig. 2 is a flowchart for an adaptive route planning method. Fig. Figure 3 is a flowchart of a method for predicting data reliability for the remaining survey plan. Fig. Figure 4 is a flowchart for a method for adapting a survey plan. DETAILED DESCRIPTION

[0013] The following describes in detail several examples of the description, illustrated in the accompanying drawings. Wherever possible, the same or similar reference numbers are used in the drawings and the description to refer to the same or similar parts or steps.

[0014] Fig. Figure 1 shows a vehicle 10 having a body 12 and a plurality of wheels 14 connected to the body 12. The vehicle 10 may be an autonomous vehicle. In the illustrated embodiment, the vehicle 10 is depicted as a lunar mobility vehicle, but it should be understood that other vehicles, including autonomous underwater vehicles (AUVs), trucks, sport utility vehicles (SUVs), lunar terrain vehicles (LUVs), etc., may also be used.

[0015] The vehicle 10 includes a control unit 34 having at least one vehicle processor 44 and a non-transitory, computer-readable device or medium 46. The vehicle processor 44 may be a custom or off-the-shelf processor, a central processing unit (CPU), a graphics processing unit (GPU), an auxiliary processor among a plurality of processors connected to the control unit 34, a semiconductor-based microprocessor (in the form of a microchip or chipset), a macroprocessor, a combination thereof, or generally a device for executing instructions. The vehicle computer-readable device or medium 46 may include volatile and non-volatile memory, such as read-only memory (ROM), random access memory (RAM), and keep-alive memory (KAM).KAM is a persistent or non-volatile memory that can be used to store various operating variables while the vehicle processor 44 is off. The vehicle's computer-readable storage device or media 46 can be implemented using a variety of storage devices such as PROMs (Programmable Read-Only Memory), EPROMs (Electrically Erasable PROMs), EEPROMs (Electrically Erasable PROMs), flash memory, or other electrical, magnetic, optical, or combination storage devices capable of storing data, some of which are executable instructions used by the control unit 34 in controlling the vehicle 10. The control unit 34 of the vehicle 10 can be programmed to execute part or all of the method 100 (. Fig. 2) as described in detail below.

[0016] The instructions may comprise one or more separate programs, each comprising an ordered collection of executable instructions for implementing logical functions. When executed by the vehicle processor 44, the instructions receive and process signals from sensors, perform logic, calculations, methods, and / or algorithms for automatically controlling the components of the vehicle 10, and generate control signals for automatically controlling the components of the vehicle 10 based on the logic, calculations, methods, and / or algorithms. Although in Fig. 1 depicts a single control unit 34, embodiments of the vehicle 10 may include a plurality of control units 34 communicating via a suitable communication medium or combination of communication media and cooperating to process the sensor signals, perform logic, calculations, methods, and / or algorithms, and generate control signals to automatically control features of the vehicle 10. The control unit 34 is part of an adaptive route planning system 49.

[0017] The vehicle 10 also includes one or more sensors 26 connected to the body 12. The sensors 26 are in communication with the control unit 34 and sense observable conditions of the external environment and / or the internal environment of the vehicle 10. As non-limiting examples, the sensors 26 may include one or more cameras, one or more LIDAR (Light Detection and Ranging) sensors, one or more proximity sensors, one or more ultrasonic sensors, one or more thermal imaging sensors, transceivers, and / or other sensors. Each sensor 26 is configured to generate a signal indicative of the sensed observable conditions (i.e., sensor data) of the external environment and / or the internal environment of the vehicle 10. The signal is indicative of the sensor data sensed by the sensors 24.

[0018] Fig. 2 is a flowchart of a method 100 for adaptive route planning. The method 100 begins at block 102. The method 100 then proceeds to block 104. At block 104, the control unit 34 of the vehicle 10 receives an initial survey plan. The initial survey plan includes a plurality of initial waypoints. Further, at block 104, the control unit 34 of the vehicle 10 initializes the confidence data structure, thereby setting the confidence level to zero or no confidence. The method 100 then proceeds to block 106.

[0019] In block 106, the control unit 34 of the vehicle 10 determines whether any waypoints remain in the waypoint list. If no more waypoints remain in the waypoint list, the method 100 continues to block 108. In block 108, the control unit 34 stores the final terrain map and the confidence data associated with the final terrain map in a quad tree or other suitable data structure. The method 100 then proceeds to block 110. The method 100 ends in block 110. If some waypoints remain in the waypoint list in block 106, the method 100 continues to block 112.

[0020] In block 112, the control unit 34 commands the vehicle 10 to move to survey an unknown terrain following the initial survey plan. As the vehicle 10 moves along the unknown terrain according to the initial survey plan, the vehicle 10 surveys the terrain with the sensors 26 while moving according to the initial survey plan (or a later adjusted survey plan) to create a map a of the terrain, thereby generating map data. The map of the terrain includes a plurality of map areas. Further, in block 112, the control unit 34 of the vehicle 10 determines a confidence level for each of the plurality of map areas sensed by the vehicle's sensors 26 to determine real-time confidence data.The confidence level of each of the plurality of map areas or cells sensed by sensors 26 of vehicle 10 is based on the distance between vehicle 10 and the map area sensed by sensors 26, the number of sensor modalities (e.g., lidar, camera, etc.) used to sense the map area sensed by sensors 26, the number of sensor measurements used to sense the map area sensed by sensors 26, the data consistency of the sensor data sensed by sensors 26, and / or the sensing performance of each sensor 26. For example, sensors 26 with high accuracy and resolution will provide data with higher confidence. The more consistent the sensor data is between the different sensor modalities (e.g., camera, lidar, radar, etc.) for a given map area, the higher the confidence level for that particular map area.The more sensor modality data used to capture a particular map area, the higher the confidence level for that area. The greater the distance between the vehicle 10 and the captured map area, the lower the confidence level for that particular map area. The real-time confidence data is an indicator of the confidence of the sensor data for each captured map area. The map data and the real-time confidence data may be stored in a quadtree or other suitable data structure. Method 100 then proceeds to block 114.

[0021] In block 114, the control unit 34 of the vehicle 10 checks whether a predetermined milestone has been reached. The milestone may be a predetermined confidence level for the map areas or map cells detected by the sensors 26 of the vehicle 10. For example, the milestone may be that the confidence level for each of the map areas detected by the sensors 26 is greater than eighty percent. If the milestone has not been reached, the method 100 returns to block 112. If the milestone has been reached, the method 100 continues to block 116.

[0022] In block 116, the control unit 34 of the vehicle 10 predicts a confidence level for each of the plurality of map areas still to be monitored according to the initial monitoring plan to determine the predicted confidence data. The confidence level of each of the map areas surveyed with the sensors 26 of the vehicle 10 is based on the distance between the vehicle 10 and the map area being surveyed with the sensors 26 and / or the number of sensor modalities (e.g., lidar, camera, etc.) used to survey the map area with the sensors 26. The more sensor modalities used to survey a particular map area, the higher the confidence level for that particular map area. The greater the distance between the vehicle 10 and the map area to be monitored, the lower the confidence level for that map area.The predicted confidence data is an indicator of the confidence of the sensor data for the individual map areas to be monitored. Method 100 then continues with block 118.

[0023] In block 118, the control unit 34 of the vehicle 10 combines the predicted confidence data with the real-time confidence data to achieve a data merge. The method 100 then proceeds to block 120. In block 120, the control unit 34 of the vehicle 10 determines a confidence loss based on the merged data. The method 100 then proceeds to block 122.

[0024] In block 122, the control unit 34 of the vehicle 10 determines whether the confidence loss is greater than a predetermined threshold (e.g., 80%). If the confidence loss is greater than the predetermined threshold, the method 100 continues to block 124. In block 124, the control unit 34 of the vehicle 10 adjusts the survey plan (i.e., changes the original survey plan) if it is determined that the confidence loss is greater than the predetermined threshold. After block 124, the method 100 returns to block 112. If the confidence loss is not greater than the predetermined threshold, the method 100 continues to block 126.

[0025] In block 126, the control unit 34 of the vehicle 10 determines whether the vehicle 10 has reached the first waypoint of the initial survey plan and surveyed the map area around the first waypoint of the initial survey plan. If the vehicle 10 has not reached the first waypoint of the initial survey plan or has not surveyed the map area around the first waypoint of the initial survey plan, the method 100 returns to block 112. If the vehicle 10 has reached the first waypoint of the initial survey plan and surveyed the map area around the first waypoint of the initial survey plan, the method 100 continues to block 128. In block 128, the control unit 34 of the vehicle 10 removes the waypoint around the map area surveyed by the sensors 26 from the waypoint list. The method 100 then returns to block 106.

[0026] Fig. 3 is a subroutine or method 200 for predicting the confidence data of map areas that will be surveyed in the future according to the survey plan (e.g., the original or adjusted survey plan). The method 200 begins in block 202 and describes the specifics of the method described above with respect to the method 100 of Fig. 2. Method 200 then proceeds to block 204. In block 204, control unit 34 of vehicle 10 generates a predicted waypoint list by equating the remaining waypoints to be measured with the predicted waypoint list. Method 200 then proceeds to block 206.

[0027] In block 206, the control unit 34 determines whether at least two waypoints remain in the list of predicted waypoints. If fewer than two waypoints remain in the list of predicted waypoints, the method 100 proceeds to block 208. In block 208, the method 200 ends, and the control unit 34 continues in the method 100 by executing block 118. If at least two waypoints remain in the list of predicted waypoints, the method 200 proceeds to block 210.

[0028] In block 210, the controller 34 determines and selects the next two immediately adjacent waypoints in the list of prediction waypoints. The method 200 then proceeds to block 212. In block 212, the controller 34 updates the map area (or cell) confidence level for each map area or cell in the confidence data structure. In other words, the predicted confidence data is updated based on the next two immediately adjacent waypoints of the updated survey plan to develop updated predicted confidence data. To this end, the controller 34 constructs a virtual line between the next two immediately adjacent waypoints of the updated survey plan for each map area or cell in the confidence data structure.The control unit 34 then determines a new confidence level based on this virtual line between the next two immediately adjacent waypoints in the predictive waypoint list. The updated confidence level is then set equal to the larger of the current confidence level and the updated confidence level determined on the virtual line between the next two immediately adjacent waypoints in the predictive waypoint list. As described above, the confidence level of each of the plurality of map areas or cells sensed by the sensors 26 of the vehicle 10 is based on the distance between the vehicle 10 and the map area sensed by the sensors 26, the number of sensor modalities (e.g., lidar, camera, etc.) used to sense the map area served by the sensors 26, and / or the data consistency of the sensor data sensed by the sensors 26.Method 200 then proceeds to block 214. In block 214, control unit 34 removes the first prediction waypoint from the prediction waypoint list. Method 100 then returns to block 206.

[0029] Fig. 4 is a flowchart of a subroutine or method 400 for adapting a survey plan. The method 300 begins with block 304 and describes the specifics of block 124 described above with respect to the method 100 of Fig.2. Then, the method 400 proceeds to block 304. In block 304, the control unit 34 develops a new "adapt" confidence data structure at a particular level of abstraction from the existing confidence data structure. The "adapt" confidence data structure contains only the map regions or cells where the confidence loss is greater than the predetermined threshold. To this end, the control unit 34 decomposes and / or integrates the map regions or cells as needed. Then, the method 400 proceeds to block 306.

[0030] In block 306, the controller 34 inserts a node (e.g., a quadtree node) with location data into a search graph for each "adjusted" map area or cell where the confidence loss is greater than a predetermined threshold. The method 300 then proceeds to block 308. In block 308, the controller 34 determines whether the search graph is empty. If the search graph is empty, the method 300 proceeds to block 310. In block 310, the method 300 ends, and the controller 34 proceeds to block 112 in the method 100. If the search graph is not empty, the method 300 proceeds to block 312.

[0031] In block 312, the control unit 34 identifies a sequence of nodes (e.g., quadtree nodes) in the search graph that represents the lowest-cost path between all nodes (e.g., the first waypoint and the last waypoint). To do this, the control unit 34 applies a minimum path algorithm (e.g., the traveling salesman heuristic algorithm) to create a new customized map summary with ordered waypoints. The control unit 34 then determines the travel cost based on the distance and navigation cost to create a navigation cost map. The control unit 34 creates an updated survey plan by selecting the lowest-cost path. The method 300 then continues with block 314.

[0032] In block 314, the control unit 34 updates the search node graph for any required user-defined maneuvers. To do so, the control unit 34 determines that the vehicle 10 is physically unable to reach at least one of the waypoints (e.g., initial waypoints or adjusted waypoints), and in response, the control unit 34 executes an A* algorithm or other suitable informed search algorithm to determine detailed waypoints, taking into account the known navigation costs, and thus create an updated overview map that avoids the waypoint or waypoints that the vehicle 10 physically cannot reach. In other words, the control unit 34 adjusts the original survey plan (or another adjusted survey plan) to create the updated survey plan. The method 300 then proceeds to block 316.

[0033] While exemplary embodiments are described above, these embodiments are not intended to describe all possible forms encompassed by the claims. The words used in the description are words of description rather than limitation, and it is understood that various changes may be made without departing from the spirit and scope of the description. As previously described, the features of various embodiments may be combined to form other embodiments of the presently disclosed system and method that may not be expressly described or illustrated.While various embodiments might be described as advantageous or preferred over other prior art embodiments or implementations with respect to one or more desired characteristics, those skilled in the art will recognize that one or more characteristics or features may be compromised to achieve desired overall system characteristics depending on the specific application and implementation. These characteristics may include, but are not limited to, cost, strength, durability, life cycle cost, marketability, appearance, packaging, size, serviceability, weight, manufacturability, ease of assembly, etc. Therefore, embodiments described as less desirable than other prior art embodiments or implementations with respect to one or more features are not outside the scope of the description and may be desirable for certain applications.

[0034] The drawings are simplified and not to scale. For the sake of simplicity and clarity, directional terms such as top, bottom, left, right, above, above, below, below, behind, and front may be used in the drawings. These and similar directional terms are not to be construed as limiting the scope of the description in any way.

[0035] Embodiments of the present specification are described herein. However, it should be understood that the disclosed embodiments are merely examples, and other embodiments may take various and alternative forms. The drawings are not necessarily to scale; some features may be exaggerated or reduced to show details of particular components. Therefore, specific structural and functional details disclosed herein are not to be interpreted as limiting, but merely as a representative basis for teaching one skilled in the art how to variously employ the presently disclosed system and method.As those skilled in the art will appreciate, various features illustrated and described with reference to one of the figures may be combined with features illustrated in one or more other figures to produce embodiments not explicitly illustrated or described. The illustrated feature combinations represent representative embodiments for typical applications. However, various combinations and modifications of the features consistent with the teachings of this description may be desirable for particular applications or implementations.

[0036] Embodiments of the present description may be described herein in terms of functional and / or logical block components and various processing steps. Such block components may be implemented by a variety of hardware, software, and / or firmware components configured to perform the specified functions. For example, an embodiment of the present description may utilize various integrated circuit components, such as memory elements, digital signal processing elements, logic elements, lookup tables, or the like, capable of performing a variety of functions under the control of one or more microprocessors or other control units.Furthermore, those skilled in the art will recognize that embodiments of the present description may be used in connection with a variety of systems and that the systems described herein are merely exemplary embodiments of the present description.

[0037] For the sake of brevity, techniques for signal processing, data fusion, signaling, control, and other functional aspects of the systems (and the individual operating components of the systems) are not described in detail here. Furthermore, the connecting lines depicted in the various figures are intended to represent exemplary functional relationships and / or physical couplings between the various elements. It should be noted that alternative or additional functional relationships or physical connections may be present in an embodiment of the present description.

[0038] This description is for illustrative purposes only and is not intended to limit the disclosure, its application, or uses in any way. The broad teachings of the description may be embodied in a variety of forms. Although this description contains specific examples, the true scope of the disclosure should not be so limited, since other modifications will become apparent upon study of the drawings, the description, and the following claims.

Claims

[1] A method (100) for determining a route for a surveyed map, comprising: Receiving, by a control unit (34) of a vehicle (10), an initial survey plan, the initial survey plan including a plurality of initial waypoints and the vehicle including a plurality of sensors; Instructing the vehicle (10) to move to survey a terrain following the initial survey plan; surveying the terrain with the plurality of sensors (26) while the vehicle (10) moves according to the initial survey plan to create a map of the terrain, the map of the terrain including a plurality of map areas; determining a confidence level for each of the plurality of map areas sensed by the plurality of sensors of the vehicle to determine real-time confidence data; predicting a confidence level for each of the plurality of map areas yet to be surveyed according to the initial survey plan to determine predicted confidence data; Merging the predicted confidence data with the real-time confidence data to determine a confidence loss; Determine whether the confidence loss is greater than a predetermined threshold; in response to determining that the confidence loss is greater than the specified threshold, adjusting the original monitoring plan to create an updated monitoring plan; Instruct the vehicle to move according to the updated survey plan. [2] The method (100) of claim 1, further comprising: Determining the next two waypoints using the updated survey plan; and Update the predicted confidence data based on the next two waypoints of the updated survey plan to develop updated predicted confidence data. [3] The method (100) of claim 2, further comprising constructing a virtual line between the next two waypoints of the updated survey plan, wherein the updated predicted confidence data is determined based on the virtual line between the next two waypoints of the updated survey plan. [4] The method (100) of claim 3, wherein adapting the initial survey plan to create an updated survey plan comprises: in response to determining that the confidence loss is greater than the predetermined threshold, adding a new waypoint to the original survey plan to create the updated survey plan, wherein the new waypoint includes location data. [5] The method (100) of claim 4, further comprising identifying a sequence of nodes in a search graph that represent a minimum cost for travel between a first waypoint and a last waypoint. [6] The method (100) of claim 5, wherein the confidence level of each of the plurality of map areas sensed by the plurality of sensors of the vehicle is based on a distance between the vehicle and a corresponding one of the plurality of map areas sensed by the plurality of sensors. [7] The method (100) of claim 6, wherein the confidence level of each of the plurality of map areas monitored by the plurality of sensors (26) of the vehicle is based on a number of sensor measurements taken in the corresponding one of the plurality of map areas served by the plurality of sensors (26). [8] The method (100) of claim 7, wherein the confidence level of each of the plurality of map areas monitored with the plurality of sensors (26) of the vehicle (10) is based on a number of sensor modalities used to monitor the corresponding one of the plurality of map areas served by the plurality of sensors. [9] The method (100) of claim 8, wherein the confidence level of each of the plurality of map areas monitored by the plurality of sensors (26) of the vehicle (10) is based on a data consistency of sensor data collected by the plurality of sensors in the corresponding one of the plurality of map areas served by the plurality of sensors (26). [10] The method (100) of claim 9, wherein adapting the initial survey plan to create an updated survey plan comprises: Determining that the vehicle (10) is unable to reach at least one of the plurality of initial waypoints; and in response to determining that the vehicle (10) is unable to reach the at least one of the plurality of initial waypoints, updating the initial survey plan to create the updated survey plan.

Images