Path planning method, device and navigation system for mobile device

Abstract

The present application provides a path planning method, device and navigation system for a movable device, wherein a laser radar is installed on the movable device, and the method comprises obtaining an occupied grid map, wherein the occupied grid map is used to characterize the probability that the position corresponding to each grid is occupied by an obstacle; obtaining occupied abnormal grids in the occupied grid map, wherein the occupied abnormal grids refer to grids whose probability of being occupied by an obstacle is variable; determining invalid positions according to the occupied abnormal grids, wherein the invalid positions refer to positions where the laser radar cannot receive echoes; determining the path of the movable device based at least on the invalid positions, and determining the invalid positions by occupying the abnormal grids, wherein the invalid positions include the situation where reflective obstacles exist, and finally determining the path that the movable device is to travel according to the invalid positions, thereby solving the problem in the prior art that the accuracy of path planning for the movable device is reduced due to ignoring the processing of reflective obstacles.

Description

Technical Field

[0001] The present application relates to the field of laser radar technology, and more specifically, to a path planning method, device, computer-readable storage medium, processor, and navigation system for a movable device. Background Art

[0002] In laser SLAM, because the laser radar is far away from the target object, or the target object's surface is reflective and the laser radar cannot receive an echo when emitting laser rays to the target object, an invalid position occurs. The common processing method in the prior art regards the invalid position where no echo is received as a point at infinity, which ignores the processing of reflective obstacles and affects the accuracy of path planning for movable equipment (including intelligent robots). Summary of the invention

[0003] The main purpose of the present application is to provide a path planning method, device, computer-readable storage medium, processor and navigation system for a movable device, so as to solve the problem in the prior art that the accuracy of path planning for a movable device is reduced due to neglecting the processing of reflective obstacles.

[0004] In order to achieve the above-mentioned purpose, according to one aspect of the present application, a path planning method for a movable device is provided, wherein a laser radar is installed on the movable device, including: obtaining an occupied grid map, wherein the occupied grid map is used to characterize the probability that the position corresponding to each grid is occupied by an obstacle; obtaining an abnormal occupied grid in the occupied grid map, wherein the abnormal occupied grid refers to a grid whose probability of being occupied by the obstacle undergoes an abnormal change, wherein the abnormal change refers to a change in the probability of being occupied by the obstacle that is greater than a predetermined change; determining an invalid position based on the abnormal occupied grid, wherein the invalid position refers to a position where the laser radar cannot receive an echo; and determining the path of the movable device based at least on the invalid position.

[0005] Furthermore, at least based on the invalid position, determining the path of the movable device includes: constructing an invalid point grid map based on the invalid position, the invalid point grid map being used to characterize the probability that the position corresponding to each grid is identified as the invalid position; and determining the path of the movable device according to the invalid point grid map and the occupied grid map.

[0006] Furthermore, determining the path of the movable device according to the invalid point grid map and the occupied grid map includes: determining a first probability that the position corresponding to the target grid is occupied by the obstacle according to the occupied grid map; determining a second probability that the position corresponding to the target grid is determined as the invalid position according to the invalid point grid map; calculating a weighted average of the first probability and the second probability to obtain a fusion probability; and determining the path of the movable device based on the fusion probability.

[0007] Further, based on the fusion probability, determining the path of the movable device includes: determining that a grid whose fusion probability is greater than or equal to a first threshold is an occupied grid; determining that a grid whose fusion probability is less than or equal to a second threshold is an idle grid; determining that a grid whose fusion probability is greater than the second threshold and less than the first threshold is an unknown grid, and the first threshold is greater than the second threshold; and determining that the path of the movable device includes only the idle grid.

[0008] Furthermore, obtaining the abnormal occupied grid in the occupied grid map includes: determining that the probability that a predetermined grid is occupied by the obstacle is a third probability according to a first occupied grid map, wherein the first occupied grid map is determined based on a plurality of frames of laser data that are earlier in time; determining that the probability that a predetermined grid is occupied by the obstacle is a fourth probability according to a second occupied grid map, wherein the second occupied grid map is determined based on a plurality of frames of laser data that are later in time; and determining that the predetermined grid is the abnormal occupied grid when the difference between the third probability and the fourth probability is greater than a predetermined difference.

[0009] Furthermore, after obtaining the abnormal occupied grid in the occupied grid map, the method further includes: obtaining the origin of the movable device; and obtaining a connection line between the origin of the movable device and the abnormal occupied grid.

[0010] Further, determining the invalid position according to the occupied abnormal grids includes: determining all grids passed by the connecting line as the occupied abnormal grids.

[0011] Further, determining the invalid position according to the occupied abnormal grid includes: determining a grid corresponding to an intersection of at least two of the connecting lines as the occupied abnormal grid.

[0012] According to another aspect of the present application, a path planning device for a movable device is also provided, wherein a laser radar is installed on the movable device, and the path planning device comprises: a first acquisition unit, used to acquire an occupation grid map, wherein the occupation grid map is used to characterize the probability that the position corresponding to each grid is occupied by an obstacle; a second acquisition unit, used to acquire occupation abnormal grids in the occupation grid map, wherein the occupation abnormal grids refer to grids whose probability of being occupied by the obstacle is variable; a first determination unit, used to determine an invalid position based on the occupation abnormal grids, wherein the invalid position refers to a position where the laser radar cannot receive an echo; and a second determination unit, used to determine the path of the movable device based at least on the invalid position.

[0013] According to another aspect of the present application, a computer-readable storage medium is provided, wherein the computer-readable storage medium includes a stored program, wherein when the program is executed, the device where the computer-readable storage medium is located is controlled to execute any one of the above-mentioned methods.

[0014] According to another aspect of the present application, a processor is provided, which is used to run a program, wherein the program executes any one of the above methods when running.

[0015] According to another aspect of the present application, a navigation system is also provided, which includes a removable device, one or more processors, a memory and one or more programs, wherein the one or more programs are stored in the memory and are configured to be executed by the one or more processors, and the one or more programs include methods for executing any one of the methods described above.

[0016] By applying the technical solution of the present application, an occupation grid map is obtained, and the occupation grid map is used to characterize the probability that the position corresponding to each grid is occupied by an obstacle; an abnormal occupation grid in the occupation grid map is obtained, and the abnormal occupation grid refers to a grid whose probability of being occupied by the obstacle changes abnormally; an invalid position is determined based on the abnormal occupation grid, and the invalid position refers to a position where the laser radar cannot receive an echo; at least based on the invalid position, the path of the movable device is determined, and the occupation grid map is used to represent the probability that the position corresponding to each grid is occupied by an obstacle, and then the abnormal occupation grid is obtained, and the abnormal occupation grid refers to a grid whose probability of being occupied by the obstacle changes abnormally, and then the invalid position is determined by the abnormal occupation grid, and the invalid position includes the situation where there is a reflective obstacle. Finally, the path to be traveled by the movable device is determined based on the invalid position, thereby solving the problem in the prior art that the accuracy of path planning of the movable device is reduced due to ignoring the processing of reflective obstacles. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] The drawings constituting part of the present application are used to provide a further understanding of the present application. The exemplary embodiments and descriptions of the present application are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0018] Figure 1 A flow chart of a path planning method for a movable device according to an embodiment of the present application is shown;

[0019] Figure 2 A schematic diagram of a path planning device for a movable device according to an embodiment of the present application is shown;

[0020] Figure 3 An occupancy grid map when there are no obstacles according to an embodiment of the present application is shown;

[0021] Figure 4 An occupancy grid map when there are obstacles according to an embodiment of the present application is shown. DETAILED DESCRIPTION

[0022] It should be noted that, in the absence of conflict, the embodiments and features in the embodiments of the present application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0023] In order to enable those skilled in the art to better understand the solution of the present application, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present application.

[0024] It should be noted that the terms "first", "second", etc. in the specification and claims of the present application and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequential order. It should be understood that the data used in this way can be interchanged where appropriate, so that the embodiments of the present application described here. In addition, the terms "including" and "having" and any of their variations are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device comprising a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0025] It should be understood that when an element (such as a layer, film, region, or substrate) is described as being "on" another element, the element may be directly on the other element, or there may be intermediate elements. Moreover, in the specification and claims, when an element is described as being "connected" to another element, the element may be "directly connected" to the other element, or "connected" to the other element through a third element.

[0026] As introduced in the background technology, the prior art ignores the handling of reflective obstacles, resulting in a problem of reduced accuracy in path planning of a movable device. To solve the above problem of reduced accuracy in path planning of a movable device, an embodiment of the present application provides a path planning method, device, computer-readable storage medium, processor and navigation system for a movable device.

[0027] According to an embodiment of the present application, a path planning method for a movable device is provided.

[0028] Figure 1 is a flow chart of a path planning method for a mobile device according to an embodiment of the present application. Figure 1 As shown, the method comprises the following steps:

[0029] Step S101, obtaining an occupancy grid map, wherein the occupancy grid map is used to represent the probability that a position corresponding to each grid is occupied by an obstacle;

[0030] Step S102, obtaining an abnormally occupied grid in the above-mentioned occupied grid map, wherein the abnormally occupied grid refers to a grid whose probability of being occupied by the above-mentioned obstacle changes abnormally, and the above-mentioned abnormal change means that the change amount of the probability of being occupied by the above-mentioned obstacle is greater than a predetermined change amount;

[0031] Specifically, the predetermined change amount may be set to 40%, 50%, etc. For example, the probability of being occupied by the above obstacles changes from 80% to 40%. Those skilled in the art may set a suitable predetermined change amount according to actual needs.

[0032] Step S103, determining an invalid position according to the above-mentioned abnormally occupied grid, wherein the above-mentioned invalid position refers to a position where the laser radar cannot receive an echo;

[0033] Step S104, determining a path of the movable device based at least on the invalid position.

[0034] In the above steps, the occupancy grid map is used to represent the probability that the position corresponding to each grid is occupied by an obstacle, and then the occupancy abnormal grid is obtained. The occupancy abnormal grid refers to a grid whose probability of being occupied by the above obstacle is variable. Then, the invalid position is determined through the above occupancy abnormal grid, and the above invalid position includes the situation where there is a reflective obstacle. Finally, the path that the above movable device is to travel is determined based on the above invalid position, thereby solving the problem in the prior art that the accuracy of path planning for the movable device is reduced due to ignoring the processing of reflective obstacles.

[0035] It should be noted that the steps shown in the flowcharts of the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and that, although a logical order is shown in the flowcharts, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0036] In one embodiment of the present application, determining the path of the movable device at least based on the invalid position includes: constructing an invalid point grid map based on the invalid position, the invalid point grid map being used to characterize the probability that the position corresponding to each grid is identified as the invalid position; determining the path of the movable device according to the invalid point grid map and the occupied grid map, that is, constructing an invalid point grid map through the invalid position, and through the invalid point grid map and the occupied grid map, it is ensured that the path of the movable device is not only the optimal path (e.g., the shortest path) but also can bypass the invalid position.

[0037] In one embodiment of the present application, determining the path of the movable device according to the invalid point grid map and the occupied grid map includes: determining a first probability that the position corresponding to the target grid is occupied by the obstacle according to the occupied grid map; determining a second probability that the position corresponding to the target grid is determined as the invalid position according to the invalid point grid map; calculating a weighted average of the first probability and the second probability to obtain a fusion probability; determining the path of the movable device based on the fusion probability, comprehensively considering the first probability of being occupied by the obstacle and the second probability of being determined as the invalid position, so as to ensure that the movable device does not collide with the obstacle.

[0038] In one embodiment of the present application, based on the above-mentioned fusion probability, determining the path of the above-mentioned movable device includes: determining that the grid whose fusion probability is greater than or equal to the first threshold is an occupied grid; determining that the grid whose fusion probability is less than or equal to the second threshold is an idle grid; determining that the grid whose fusion probability is greater than the second threshold and less than the first threshold is an unknown grid, and the above-mentioned first threshold is greater than the above-mentioned second threshold; determining that the path of the above-mentioned movable device only includes the above-mentioned idle grid, setting the first threshold to the boundary for determining the occupied grid, and the second threshold to the boundary for determining the idle grid, when the fusion probability is greater than or equal to the first threshold, it is determined to be an occupied grid, and when the fusion probability is less than or equal to the second threshold, it is determined to be an unknown grid. By controlling the movable device to travel at the position corresponding to the idle grid, it can be ensured that the movable device does not collide with an obstacle.

[0039] In one embodiment of the present application, obtaining the abnormal grid occupied in the above-mentioned occupancy grid map includes: determining the probability that the predetermined grid is occupied by the above-mentioned obstacle as a third probability according to the first occupancy grid map, and the above-mentioned first occupancy grid map is determined according to the multiple frames of laser data in the previous time; determining the probability that the predetermined grid is occupied by the above-mentioned obstacle as a fourth probability according to the second occupancy grid map, and the above-mentioned second occupancy grid map is determined according to the multiple frames of laser data in the later time; when the difference between the above-mentioned third probability and the above-mentioned fourth probability is greater than the predetermined difference, determining the above-mentioned predetermined grid as the above-mentioned abnormal grid occupied, determining the third probability and the fourth probability respectively according to the multiple frames of laser data different in time, and finally comparing the values ​​of the third probability and the fourth probability, as long as the difference between the third probability and the fourth probability is greater than the predetermined difference, the above-mentioned predetermined grid is determined to be the above-mentioned abnormal grid occupied, thereby realizing the accurate determination of the abnormal grid occupied. Specifically, the predetermined difference can be set to 40%, 50%, etc. For example, the third probability is 80% and the fourth probability is 40%.

[0040] In one embodiment of the present application, after obtaining the abnormal occupied grid in the above-mentioned occupied grid map, the above-mentioned method also includes: obtaining the origin of the above-mentioned movable device; obtaining the connection line between the origin of the above-mentioned movable device and the above-mentioned abnormal occupied grid, so as to facilitate subsequent path planning.

[0041] In one embodiment of the present application, determining the invalid position based on the above-mentioned occupied abnormal grid includes: determining all the grids passed by the above-mentioned connecting line as the above-mentioned occupied abnormal grids, that is, by determining all the grids passed by the connecting line as the above-mentioned occupied abnormal grids, controlling the movable device not to move in the position corresponding to the occupied grid to ensure that the movable device does not collide with obstacles.

[0042] In one embodiment of the present application, determining the invalid position based on the above-mentioned occupied abnormal grid includes: determining the grid corresponding to the intersection of at least two of the above-mentioned lines as the above-mentioned occupied abnormal grid, that is, if several of the above-mentioned lines intersect and thus generate an intersection, such as two of the above-mentioned lines generate an intersection, then the grid corresponding to the above-mentioned intersection is confirmed as the above-mentioned occupied abnormal grid, thereby greatly improving the probability that the path of the movable device is a path that can bypass the invalid position.

[0043] The present application also provides a path planning device for a mobile device. It should be noted that the path planning device for a mobile device in the present application can be used to execute the path planning method for a mobile device provided in the present application. The path planning device for a mobile device provided in the present application is introduced below.

[0044] Figure 2 Schematic diagram of a path planning device for a mobile device according to an embodiment of the present application. Figure 2 As shown, the movable device is equipped with a laser radar, and the laser radar includes: a first acquisition unit 10, a second acquisition unit 20, a first determination unit 30 and a second determination unit 40;

[0045] The first acquisition unit 10 is used to acquire an occupancy grid map, where the occupancy grid map is used to represent the probability that a position corresponding to each grid is occupied by an obstacle;

[0046] The second acquisition unit 20 is used to acquire the abnormally occupied grid in the above-mentioned occupied grid map, wherein the abnormally occupied grid refers to a grid whose probability of being occupied by the above-mentioned obstacle changes abnormally, and the above-mentioned abnormal change means that the change amount of the probability of being occupied by the above-mentioned obstacle is greater than a predetermined change amount;

[0047] Specifically, the predetermined change amount may be set to 40%, 50%, etc. For example, the probability of being occupied by the above obstacles changes from 80% to 40%. Those skilled in the art may set a suitable predetermined change amount according to actual needs.

[0048] The first determination unit 30 is used to determine an invalid position according to the above-mentioned abnormal grid occupation, and the above-mentioned invalid position refers to a position where the laser radar cannot receive an echo;

[0049] The second determining unit 40 is configured to determine a path of the movable device at least based on the invalid position.

[0050] The above-mentioned device uses each grid of the occupation grid map to represent the probability that the corresponding position is occupied by an obstacle. According to the occupation grid map, it is used to represent the probability that the position corresponding to each grid is occupied by an obstacle, and then obtains the occupation abnormal grid. The occupation abnormal grid refers to a grid whose probability of being occupied by the above-mentioned obstacle is variable. Then, through the above-mentioned occupation abnormal grid, an invalid position is determined, and the above-mentioned invalid position includes the situation where a reflective obstacle exists. Finally, according to the above-mentioned invalid position, the path that the above-mentioned movable device is to travel is determined, thereby solving the problem in the prior art that the accuracy of the path planning of the movable device is reduced due to ignoring the processing of reflective obstacles.

[0051] In one embodiment of the present application, the second determination unit includes a first processing module and a second processing module. The first processing module constructs an invalid point grid map based on the invalid position, and the invalid point grid map is used to characterize the probability that the position corresponding to each grid is identified as the invalid position; the second processing module is used to determine the path of the movable device according to the invalid point grid map and the occupied grid map, that is, through the invalid position, an invalid point grid map is constructed, and through the invalid point grid map and the occupied grid map, it is ensured that the path of the movable device is not only the optimal path (for example, the shortest path) but also can bypass the invalid position.

[0052] In one embodiment of the present application, the second processing module includes a first processing submodule, a second processing submodule, a calculation submodule and a third processing submodule. The first processing submodule determines the first probability that the position corresponding to the target grid is occupied by the above-mentioned obstacle according to the above-mentioned occupied grid map; the second processing submodule determines the second probability that the position corresponding to the above-mentioned target grid is determined as the above-mentioned invalid position according to the above-mentioned invalid point grid map; the calculation submodule calculates the weighted average of the above-mentioned first probability and the above-mentioned second probability to obtain a fusion probability; the third processing submodule determines the path of the above-mentioned movable device based on the above-mentioned fusion probability, and comprehensively considers the first probability of being occupied by the above-mentioned obstacle and the second probability of being determined as the above-mentioned invalid position, so as to ensure that the movable device does not collide with the obstacle.

[0053] In one embodiment of the present application, the third processing submodule includes a fourth processing submodule, a fifth processing submodule, a sixth processing submodule and a seventh processing submodule, the fourth processing submodule determines that the grid whose fusion probability is greater than or equal to the first threshold is an occupied grid; the fifth processing submodule determines that the grid whose fusion probability is less than or equal to the second threshold is an idle grid; the sixth processing submodule determines that the grid whose fusion probability is greater than the second threshold and less than the first threshold is an unknown grid, and the first threshold is greater than the second threshold; the seventh processing submodule determines that the path of the movable device only includes the idle grid, sets the first threshold as the boundary for determining the occupied grid, and sets the second threshold as the boundary for determining the idle grid. When the fusion probability is greater than or equal to the first threshold, it is determined to be an occupied grid, and when the fusion probability is less than or equal to the second threshold, it is determined to be an unknown grid. By controlling the movable device to travel at the position corresponding to the idle grid, it can be ensured that the movable device does not collide with the obstacle.

[0054] In one embodiment of the present application, the second acquisition unit includes a third processing module, a fourth processing module and a fifth processing module. The third processing module determines the probability that the predetermined grid is occupied by the above-mentioned obstacle as the third probability according to the first occupied grid map, and the above-mentioned first occupied grid map is determined according to the multiple frames of laser data in the previous time; the fourth processing module determines the probability that the predetermined grid is occupied by the above-mentioned obstacle as the fourth probability according to the second occupied grid map, and the above-mentioned second occupied grid map is determined according to the multiple frames of laser data in the later time; the fifth processing module determines that the above-mentioned predetermined grid is the above-mentioned abnormal occupation grid when the difference between the above-mentioned third probability and the above-mentioned fourth probability is greater than the predetermined difference, and determines the third probability and the fourth probability respectively according to the multiple frames of laser data before and after different times, and finally compares the values ​​of the third probability and the fourth probability. As long as the difference between the third probability and the fourth probability is greater than the predetermined difference, the above-mentioned predetermined grid is determined to be the above-mentioned abnormal occupation grid, thereby realizing the accurate determination of the abnormal occupation grid. Specifically, the predetermined difference can be set to 40%, 50%, etc. For example, the third probability is 80% and the fourth probability is 40%.

[0055] In one embodiment of the present application, the above-mentioned laser radar also includes a third acquisition unit and a fourth acquisition unit. After the second acquisition unit acquires the occupied abnormal grid in the above-mentioned occupied grid map, the third acquisition unit acquires the origin of the above-mentioned movable device; the fourth acquisition unit acquires the connection line between the origin of the above-mentioned movable device and the above-mentioned occupied abnormal grid to facilitate subsequent path planning.

[0056] In one embodiment of the present application, the first determination unit includes a sixth processing module, which determines all grids passed by the above-mentioned connecting line as the above-mentioned occupied abnormal grids, that is, by determining all grids passed by the connecting line as the above-mentioned occupied abnormal grids, the movable device is controlled not to move in the position corresponding to the occupied grid to ensure that the movable device does not collide with the obstacle.

[0057] In one embodiment of the present application, the first determination unit also includes a seventh processing module, which determines the grid corresponding to the intersection of at least two of the above-mentioned lines as the above-mentioned occupied abnormal grid, that is, if several of the above-mentioned lines intersect and thus generate an intersection, such as two of the above-mentioned lines generate an intersection, then the grid corresponding to the above-mentioned intersection is confirmed as the above-mentioned occupied abnormal grid, thereby greatly improving the probability that the path of the movable device is a path that can bypass the invalid position.

[0058] The path planning device of the above-mentioned movable device includes a processor and a memory. The first acquisition unit, the second acquisition unit, the first determination unit, the second determination unit, etc. are all stored in the memory as program units, and the processor executes the above-mentioned program units stored in the memory to realize corresponding functions.

[0059] The processor includes a kernel, which retrieves the corresponding program unit from the memory. One or more kernels can be set, and the problem of reduced accuracy of path planning for movable devices due to neglect of processing of reflective obstacles in the prior art is solved by adjusting kernel parameters.

[0060] The memory may include non-permanent memory in a computer-readable medium, random access memory (RAM) and / or non-volatile memory, such as read-only memory (ROM) or flash RAM, and the memory includes at least one memory chip.

[0061] An embodiment of the present invention provides a computer-readable storage medium, wherein the computer-readable storage medium includes a stored program, wherein when the program is executed, the device where the computer-readable storage medium is located is controlled to execute the path planning method for the movable device.

[0062] An embodiment of the present invention provides a processor, and the processor is used to run a program, wherein the path planning method of the movable device is executed when the program is run.

[0063] An embodiment of the present invention provides a device, which includes a processor, a memory, and a program stored in the memory and executable on the processor. When the processor executes the program, at least the following steps are implemented: obtaining an occupied grid map, wherein the occupied grid map is used to characterize the probability that a position corresponding to each grid is occupied by an obstacle; obtaining abnormal occupied grids in the occupied grid map, wherein the abnormal occupied grids refer to grids in which the probability of being occupied by the obstacle changes abnormally, wherein the abnormal change refers to a change in the probability of being occupied by the obstacle greater than a predetermined change; determining an invalid position based on the abnormal occupied grids, wherein the invalid position refers to a position where a laser radar cannot receive an echo; and determining a path of the movable device based at least on the invalid position.

[0064] Furthermore, at least based on the above-mentioned invalid positions, determining the path of the above-mentioned movable device includes: constructing an invalid point grid map based on the above-mentioned invalid positions, the above-mentioned invalid point grid map being used to characterize the probability that the position corresponding to each grid is identified as the above-mentioned invalid position; determining the path of the above-mentioned movable device according to the above-mentioned invalid point grid map and the above-mentioned occupied grid map.

[0065] Furthermore, according to the invalid point grid map and the occupied grid map, determining the path of the movable device includes: determining a first probability that the position corresponding to the target grid is occupied by the obstacle according to the occupied grid map; determining a second probability that the position corresponding to the target grid is determined as the invalid position according to the invalid point grid map; calculating a weighted average of the first probability and the second probability to obtain a fusion probability; and determining the path of the movable device based on the fusion probability.

[0066] Further, based on the above-mentioned fusion probability, determining the path of the above-mentioned movable device includes: determining that the grid whose fusion probability is greater than or equal to the first threshold is an occupied grid; determining that the grid whose fusion probability is less than or equal to the second threshold is an idle grid; determining that the grid whose fusion probability is greater than the second threshold and less than the first threshold is an unknown grid, and the above-mentioned first threshold is greater than the above-mentioned second threshold; determining that the path of the above-mentioned movable device only includes the above-mentioned idle grid.

[0067] Furthermore, obtaining the abnormal occupied grid in the above-mentioned occupied grid map includes: determining that the probability that the predetermined grid is occupied by the above-mentioned obstacle is a third probability according to the first occupied grid map, and the above-mentioned first occupied grid map is determined according to multiple frames of laser data that are earlier in time; determining that the probability that the predetermined grid is occupied by the above-mentioned obstacle is a fourth probability according to the second occupied grid map, and the above-mentioned second occupied grid map is determined according to multiple frames of laser data that are later in time; when the difference between the third probability and the fourth probability is greater than the predetermined difference, determining that the predetermined grid is the abnormal occupied grid.

[0068] Furthermore, after obtaining the abnormally occupied grid in the above-mentioned occupied grid map, the above-mentioned method further includes: obtaining the origin of the above-mentioned movable device; and obtaining a connection line between the origin of the above-mentioned movable device and the abnormally occupied grid.

[0069] Further, determining the invalid position according to the above-mentioned occupied abnormal grids includes: determining all grids passed by the above-mentioned connecting line as the above-mentioned occupied abnormal grids.

[0070] Further, determining the invalid position according to the above-mentioned abnormally occupied grid includes: determining the grid corresponding to the intersection of at least two of the above-mentioned lines as the above-mentioned abnormally occupied grid. The device in this article can be a server, a PC, a PAD, a mobile phone, and the like.

[0071] The present application also provides a navigation system, which includes a removable device, one or more processors, a memory and one or more programs, wherein the one or more programs are stored in the memory and are configured to be executed by the one or more processors, and the one or more programs include methods for executing any one of the methods mentioned above: obtaining an occupation grid map, the occupation grid map is used to characterize the probability that the position corresponding to each grid is occupied by an obstacle; obtaining an abnormal occupation grid in the occupation grid map, the abnormal occupation grid refers to a grid with a variable probability of being occupied by the obstacle; determining an invalid position based on the abnormal occupation grid, the invalid position refers to a position where the laser radar cannot receive an echo; and determining the path of the removable device based at least on the invalid position.

[0072] Furthermore, at least based on the above-mentioned invalid positions, determining the path of the above-mentioned movable device includes: constructing an invalid point grid map based on the above-mentioned invalid positions, the above-mentioned invalid point grid map being used to characterize the probability that the position corresponding to each grid is identified as the above-mentioned invalid position; determining the path of the above-mentioned movable device according to the above-mentioned invalid point grid map and the above-mentioned occupied grid map.

[0073] Furthermore, according to the invalid point grid map and the occupied grid map, determining the path of the movable device includes: determining a first probability that the position corresponding to the target grid is occupied by the obstacle according to the occupied grid map; determining a second probability that the position corresponding to the target grid is determined as the invalid position according to the invalid point grid map; calculating a weighted average of the first probability and the second probability to obtain a fusion probability; and determining the path of the movable device based on the fusion probability.

[0074] Further, based on the above-mentioned fusion probability, determining the path of the above-mentioned movable device includes: determining that the grid whose fusion probability is greater than or equal to the first threshold is an occupied grid; determining that the grid whose fusion probability is less than or equal to the second threshold is an idle grid; determining that the grid whose fusion probability is greater than the second threshold and less than the first threshold is an unknown grid, and the above-mentioned first threshold is greater than the above-mentioned second threshold; determining that the path of the above-mentioned movable device only includes the above-mentioned idle grid.

[0075] Furthermore, obtaining the abnormal occupied grid in the above-mentioned occupied grid map includes: determining that the probability that the predetermined grid is occupied by the above-mentioned obstacle is a third probability according to the first occupied grid map, and the above-mentioned first occupied grid map is determined according to multiple frames of laser data that are earlier in time; determining that the probability that the predetermined grid is occupied by the above-mentioned obstacle is a fourth probability according to the second occupied grid map, and the above-mentioned second occupied grid map is determined according to multiple frames of laser data that are later in time; when the above-mentioned third probability is different from the above-mentioned fourth probability, determining that the above-mentioned predetermined grid is the above-mentioned abnormal occupied grid.

[0076] Furthermore, after obtaining the abnormally occupied grid in the above-mentioned occupied grid map, the above-mentioned method further includes: obtaining the origin of the above-mentioned movable device; and obtaining a connection line between the origin of the above-mentioned movable device and the abnormally occupied grid.

[0077] Further, determining the invalid position according to the above-mentioned occupied abnormal grids includes: determining all grids passed by the above-mentioned connecting line as the above-mentioned occupied abnormal grids.

[0078] Further, determining the invalid position according to the above-mentioned occupied abnormal grid includes: determining the grid corresponding to the intersection of at least two of the above-mentioned connecting lines as the above-mentioned occupied abnormal grid.

[0079] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application may adopt the form of a computer program product implemented in one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that include computer-usable program code.

[0080] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0081] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0082] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process in the computer or other programmable device. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0083] In a typical configuration, a computing device includes one or more processors (CPU), input / output interfaces, network interfaces, and memory.

[0084] The memory may include non-permanent memory in a computer-readable medium, random access memory (RAM) and / or non-volatile memory in the form of read-only memory (ROM) or flash RAM. The memory is an example of a computer-readable medium.

[0085] Computer readable media include permanent and non-permanent, removable and non-removable media that can be implemented by any method or technology to store information. Information can be computer readable instructions, data structures, program modules or other data. Examples of computer storage media include, but are not limited to, phase change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technology, compact disk read-only memory (CD-ROM), digital versatile disk (DVD) or other optical storage, magnetic cassettes, magnetic tape magnetic disk storage or other magnetic storage devices or any other non-transmission media that can be used to store information that can be accessed by a computing device. As defined herein, computer readable media does not include temporary computer readable media (transitory media), such as modulated data signals and carrier waves.

[0086] It should also be noted that the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, commodity or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, commodity or device. In the absence of more restrictions, the elements defined by the sentence "comprises a ..." do not exclude the existence of other identical elements in the process, method, commodity or device including the elements.

[0087] Embodiment This embodiment relates to a laser radar data processing solution, which includes the following steps:

[0088] Step 1: Obtain an occupancy grid map, which is used to represent the probability that the position corresponding to each grid is occupied by an obstacle;

[0089] Step 2: determining, according to a first occupied grid map, a probability that a predetermined grid is occupied by the obstacle as a third probability, wherein the first occupied grid map is determined according to a plurality of frames of laser data that are prior in time;

[0090] Step 3: determining the probability that the predetermined grid is occupied by the obstacle as a fourth probability according to a second occupied grid map, wherein the second occupied grid map is determined according to a plurality of frames of laser data at a later time;

[0091] Step 4: when the difference between the third probability and the fourth probability is greater than a predetermined difference, determining the predetermined grid as the abnormal occupancy grid;

[0092] Step 5: Get the origin of the above movable device;

[0093] Step 6: Obtain a line between the origin of the movable device and the abnormal grid;

[0094] Step 7: All grids that the above-mentioned connecting line passes through are determined as the above-mentioned abnormally occupied grids;

[0095] Step 8: Determine the grid corresponding to the intersection of at least two of the above lines as the above-mentioned abnormal occupation grid;

[0096] Step 9: Based on the invalid positions, construct an invalid point grid map, where the invalid point grid map is used to represent the probability that the position corresponding to each grid is identified as the invalid position;

[0097] Step 10: determining a first probability that a position corresponding to the target grid is occupied by the obstacle according to the occupied grid map;

[0098] Step 11: determining a second probability that the position corresponding to the target grid is determined as the invalid position according to the invalid point grid map;

[0099] Step 12: Calculate the weighted average of the first probability and the second probability to obtain a fusion probability;

[0100] Step 13: Determine the grid whose fusion probability is greater than or equal to the first threshold as an occupied grid;

[0101] Step 14: Determine the grid whose fusion probability is less than or equal to the second threshold as an idle grid;

[0102] Step 15: Determine that the grid whose fusion probability is greater than the second threshold and less than the first threshold is an unknown grid, and the first threshold is greater than the second threshold;

[0103] Step 16: Determine that the path of the movable device only includes the idle grids.

[0104] In the prior art, grid maps divide the environment into a series of grids, where each grid is given a possible value, and a typical example is an occupied grid map. Regarding the scheme of occupied grid maps, specifically, in an occupied grid map, we grid the map, and the state of each grid is occupied, idle, or unknown. We use p(s=1) to represent the probability of occupied state, and p(s=0) to represent the probability of idle state, and the sum of the two is 1. It is not appropriate to use two values ​​to represent the state of a grid, so the ratio of the two is introduced to represent the state of the grid: When the laser radar measurement observation result z arrives, the relevant grid needs to update the grid state. The updated grid state is Through the Bayesian formula, we can get So we use simple addition to update the state of the grid. In addition, the initial state S of a grid init , since the default probability of grid idleness and occupancy is 0.5, Similar to the grid map, p(x=1) is used to represent the probability of invalid state, and p(x=0) is used to represent the probability of valid state. The grid state update formula is

[0105] Commonly used path planning methods include Dijkstra algorithm, BFS algorithm, A* algorithm, RRT algorithm, BUG algorithm, etc. The principle of Dijkstra algorithm is to save the shortest distance after traversing all points; BFS algorithm is based on heuristic depth-first search to find a fast path, but it cannot be guaranteed to be the shortest path; A* algorithm is the most popular choice in path planning. It combines Dijkstra and BFS to search for a shorter path quickly; RRT algorithm uses incremental sampling search method and uses random sequence to complete the search tree construction to the target state; BUG algorithm idea is to move straight to the target and move around when encountering obstacles. The following is a specific example of A*: A* algorithm uses heuristic search, and its evaluation function is f(n) = g(n) + h(n), where f(n) is the evaluation function of node n, g(n) is the actual cost from the starting point to the nth node, and h(n) is the estimated cost of the best path from node n to the target node. When h(n) = 0, A* becomes the Dijkstra algorithm, which ensures that the shortest path is found. When h(n) is much larger than g(n), A* becomes the BFS algorithm, so A* needs to balance the optimal path and the fastest search for path planning. The actual cost g(n) can often be directly obtained. h(n) can be used as a heuristic function, and the distance formula can be used. For example, the Euclidean distance is h(n) = D*sqrt((nx-goal.x) 2 +(ny-goal.y) 2 ), where D is the proportionality coefficient, which determines the weight of the estimated cost.

[0106] In the above steps, each grid of the occupied grid map represents the probability that the corresponding position is occupied by an obstacle. The above-mentioned occupied abnormal grid refers to a grid whose probability of being occupied by the above-mentioned obstacle is variable. The position where the laser radar cannot receive an echo is used to refer to the above-mentioned occupied abnormal grid, and the path of the above-mentioned movable device is determined based on the above-mentioned invalid position, which solves the problem in the prior art that the accuracy of path planning of the movable device is reduced due to ignoring the processing of reflective obstacles.

[0107] Fusion probability in the above steps: Since the occupied grid map indicates the probability of each grid being occupied on the map, and the invalid value grid map indicates the probability of each grid detecting an invalid value, in order to minimize collisions, the simplest method is to equate the probability of grid invalid values ​​with obstacles. Therefore, the probability values ​​of the occupied grid map and the invalid value grid map can be directly added in a certain proportion to generate a fused map. Then, path planning is performed directly on the fused map: Update grid status of the occupied grid map: Invalid value grid map grid status update: Analogizing invalidity to occupation, grid probability fusion, the fused occupancy probability is p(z|s′=1)=α*p(z|s′=1)+β*p(z|x=1), α represents the proportion of the first probability occupied by the above-mentioned obstacle, β represents the proportion of the second probability determined as the above-mentioned invalid position, the size of the two can be determined according to the performance of the lidar sensor and the influence of the invalid value grid map on the fusion probability. If the lidar sensor performance is poor, there are many reflective objects that cannot receive echo signals. At this time, the invalid value grid map has a greater impact on the fusion probability. At this time, α and β can both be taken as 0.5; if the lidar sensor performance is good, the echo signals of reflective objects can basically be received, and the influence of the invalid value grid map is not that great, then α and β can be taken as 0.9 and 0.1 respectively.

[0108] When the probability is greater than the first threshold, the grid is considered to be occupied, that is, an obstacle area; when the probability is less than the second threshold, the grid is considered to be idle, that is, a free area; if it is between the two, it is considered to be an unknown state.

[0109] Instantaneous path planning based on single-frame lidar can also avoid generating invalid value grid maps. When a frame of lidar data is received each time, the invalid values ​​in it are converted into rays in the world coordinate system and the grid sequence N{n1,n2,……,n k} Temporarily mark all as occupied, and then perform a path planning. You can also use the method in the first article, when determining a point n of the ray t When there is an obstacle, a segment O{n t ,n t+1 ,......,n k} is set to occupied state. The above path planning is performed every several frames. Compared with building an invalid value grid map, this method is fast, but it may lead to low path finding efficiency.

[0110] There are two methods for constructing an invalid value grid map in this application: the first method is used in the map construction process, that is, when there is no relatively complete occupied grid map, the occupied grid map and the invalid value grid map are updated at the same time. Each grid in the invalid value grid map records the number of times or probability information that the laser radar identifies the invalid value. For each grid that the ray corresponding to the invalid point of each frame of the laser radar passes through, the invalid value probability is increased; for each grid that the valid point of each frame of the laser radar passes through, the invalid value probability is reduced. The probability value is updated in the same way as the occupied grid map. Figure 3 FIG. 2 shows an occupancy grid map when there are no obstacles according to an embodiment of the present application; Figure 3 As shown; p(x=1) is used to represent the probability of invalid state, and p(x=0) is used to represent the probability of valid state. The grid state update formula is

[0111] If we already have a relatively complete occupancy grid map, the steps to construct the invalid value grid map are relatively simple, because we already know which points on the map are occupied and which points are free. Figure 4 FIG. 4 shows an occupancy grid map when there are obstacles according to an embodiment of the present application. Figure 4 As shown in Figure 2, if we already know that there is an obstacle at the hexagonal mark through the occupancy grid map, and the laser radar emits a ray at the diamond mark point (the ray refers to Figure 4 If an invalid value is received by the laser radar (marked by the arrow in the figure), it is certain that there is a (reflective) obstacle in a grid between the laser radar and the obstacle, because Figure 3 and Figure 4 Similarly, so no longer Figure 3 To elaborate.

[0112] From the above description, it can be seen that the above embodiments of the present application achieve the following technical effects:

[0113] 1) The path planning method of the movable device of the present application, the method obtains an occupation grid map, the above-mentioned occupation grid map is used to characterize the probability that the position corresponding to each grid is occupied by an obstacle; obtains the occupation abnormal grid in the above-mentioned occupation grid map, the above-mentioned occupation abnormal grid refers to the grid whose probability of being occupied by the above-mentioned obstacle changes abnormally; determines the invalid position according to the above-mentioned occupation abnormal grid, and the above-mentioned invalid position refers to the position where the laser radar cannot receive the echo; at least based on the above-mentioned invalid position, determines the path of the above-mentioned movable device, the above-mentioned method is used to represent the probability that the position corresponding to each grid is occupied by an obstacle according to the occupation grid map, and then obtains the occupation abnormal grid, the occupation abnormal grid refers to the grid whose probability of being occupied by the above-mentioned obstacle changes abnormally, and then determines the invalid position through the above-mentioned occupation abnormal grid, the above-mentioned invalid position includes the situation where there is a reflective obstacle, and finally determines the path to be driven by the above-mentioned movable device according to the above-mentioned invalid position, thereby solving the problem of reduced accuracy of path planning of the movable device due to ignoring the processing of reflective obstacles in the prior art.

[0114] 2) A path planning device for a movable device of the present application, wherein the movable device is provided with a laser radar, wherein the laser radar comprises a first acquisition unit, a second acquisition unit, a first determination unit and a second determination unit; the first acquisition unit is used to acquire an occupied grid map, wherein the occupied grid map is used to characterize the probability that the position corresponding to each grid is occupied by an obstacle; the second acquisition unit is used to acquire an abnormal occupied grid in the occupied grid map, wherein the abnormal occupied grid refers to a grid whose probability of being occupied by the obstacle changes abnormally; the first determination unit is used to determine an invalid position according to the abnormal occupied grid, wherein the invalid position refers to a position where the laser radar cannot receive an echo The second determination unit is used to determine the path of the movable device at least based on the invalid position. The device is based on the occupation grid map to indicate the probability that the position corresponding to each grid is occupied by an obstacle, and then obtains the abnormal occupation grid. The abnormal occupation grid refers to a grid in which the probability of being occupied by the obstacle changes abnormally. The invalid position is determined by the abnormal occupation grid. The invalid position includes the situation where there is a reflective obstacle. Finally, the path of the movable device to be driven is determined according to the invalid position, thereby solving the problem in the prior art that the accuracy of the path planning of the movable device is reduced due to ignoring the processing of reflective obstacles.

[0115] The above description is only the preferred embodiment of the present application and is not intended to limit the present application. For those skilled in the art, the present application may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A path planning method for a movable device, characterized in that: The movable device is equipped with a laser radar, including: Obtaining an occupancy grid map, where the occupancy grid map is used to represent the probability that a position corresponding to each grid is occupied by an obstacle; Acquire an abnormally occupied grid in the occupied grid map, wherein the abnormally occupied grid refers to a grid in which the probability of being occupied by the obstacle changes abnormally, and the abnormal change refers to a change amount of the probability of being occupied by the obstacle greater than a predetermined change amount; Determine the position corresponding to the occupied abnormal grid as an invalid position, where the invalid position refers to a position where the laser radar cannot receive an echo, wherein the reasons why the laser radar cannot receive an echo include: the laser radar is far away from the target object and the surface of the target object is reflective; determining a path of the movable device based at least on the invalid location; Wherein, determining the path of the movable device at least based on the invalid position is specifically implemented as follows: Based on the invalid positions, construct an invalid point grid map, wherein the invalid point grid map is used to characterize the probability that the position corresponding to each grid is identified as the invalid position; Determining, according to the occupancy grid map, a first probability that a position corresponding to the target grid is occupied by the obstacle; Determine, according to the invalid point grid map, a second probability that the position corresponding to the target grid is determined as the invalid position; Calculating a weighted average of the first probability and the second probability to obtain a fusion probability; Based on the fusion probability, a path of the movable device is determined.

2. The method according to claim 1, characterized in that Determining a path of the movable device based on the fusion probability includes: Determine a grid whose fusion probability is greater than or equal to a first threshold as an occupied grid; Determine that a grid whose fusion probability is less than or equal to a second threshold is an idle grid; Determine that a grid whose fusion probability is greater than the second threshold and less than the first threshold is an unknown grid, and the first threshold is greater than the second threshold; A path of the movable device is determined to include only the idle grids.

3. The method according to claim 1 or 2, characterized in that: Obtaining the abnormally occupied grid in the occupied grid map, including: Determining a probability that a predetermined grid is occupied by the obstacle as a third probability according to a first occupancy grid map, wherein the first occupancy grid map is determined according to a plurality of frames of laser data that precede in time; Determining the probability that the predetermined grid is occupied by the obstacle as a fourth probability according to a second occupied grid map, wherein the second occupied grid map is determined according to a plurality of frames of laser data at a later time; In a case where a difference between the third probability and the fourth probability is greater than a predetermined difference, the predetermined grid is determined to be the occupancy abnormal grid.

4. The method according to claim 1, characterized in that: After obtaining the abnormally occupied grid in the occupied grid map, the method further includes: Obtaining the origin of the movable device; A line between the origin of the movable device and the occupied abnormal grid is obtained.

5. The method according to claim 4, characterized in that After obtaining the connection line between the origin of the movable device and the abnormal grid, the method includes: The grids through which the connecting line passes are all determined as the abnormally occupied grids.

6. The method according to claim 4, characterized in that After obtaining the connection line between the origin of the movable device and the abnormal grid, the method includes: The grid corresponding to the intersection of at least two of the connecting lines is determined as the abnormal occupation grid.

7. A path planning device for a movable device, characterized in that: The movable device is equipped with a laser radar, including: A first acquisition unit is used to acquire an occupancy grid map, where the occupancy grid map is used to represent the probability that a position corresponding to each grid is occupied by an obstacle; A second acquisition unit is used to acquire an abnormally occupied grid in the occupied grid map, wherein the abnormally occupied grid refers to a grid whose probability of being occupied by the obstacle changes abnormally, and the abnormal change refers to a change amount of the probability of being occupied by the obstacle greater than a predetermined change amount; A first determining unit is used to determine the position corresponding to the occupied abnormal grid as an invalid position, where the invalid position refers to a position where the laser radar cannot receive an echo, wherein the reasons why the laser radar cannot receive an echo include: the laser radar is far away from the target object and the surface of the target object is reflective; a second determining unit, configured to determine a path of the movable device based at least on the invalid position; The second determining unit is specifically configured to: Based on the invalid positions, construct an invalid point grid map, wherein the invalid point grid map is used to characterize the probability that the position corresponding to each grid is identified as the invalid position; Determining, according to the occupancy grid map, a first probability that a position corresponding to the target grid is occupied by the obstacle; Determine, according to the invalid point grid map, a second probability that the position corresponding to the target grid is determined as the invalid position; Calculating a weighted average of the first probability and the second probability to obtain a fusion probability; Based on the fusion probability, a path of the movable device is determined.

8. A computer-readable storage medium, characterized in that: The computer-readable storage medium includes a stored program, wherein when the program is executed, the device where the computer-readable storage medium is located is controlled to execute the method according to any one of claims 1 to 6.

9. A processor, characterized in that: The processor is used to run a program, wherein the program executes the method according to any one of claims 1 to 6 when running.

10. A navigation system, characterized in that: The device comprises a removable device, one or more processors, a memory and one or more programs, wherein the one or more programs are stored in the memory and are configured to be executed by the one or more processors, and the one or more programs include methods for executing any one of claims 1 to 6.

Images