A multi-sensor collaborative positioning method, device, storage medium and terminal device

Abstract

The present invention discloses a multi-sensor collaborative positioning method, device, storage medium and terminal device, including: constructing a probability grid map of a monitoring area based on a target to be positioned, where the monitoring area includes at least two monitoring sensors; initializing and marking the probability grid map according to the occupancy grid map and the monitoring grid map of the monitoring area; when any monitoring sensor detects the target to be positioned, obtaining a first target grid where the target to be positioned is located in the monitoring grid map; updating the marked probability grid map according to the first target grid; obtaining a positioning result of the target to be positioned according to the updated probability grid map; the grid marking values in the occupancy grid map, the monitoring grid map, and the marked probability grid map respectively represent whether there is an obstacle, whether it can be monitored, and the probability value of the existence of the target to be positioned. The present invention can more intuitively display the monitoring area and quickly and conveniently track and position the moving object.

Description

Technical Field

[0001] The present invention relates to the technical field of computer vision, and in particular, to a multi-sensor collaborative positioning method, device, computer-readable storage medium, and terminal device. Background Art

[0002] In a smart home or a monitoring system for a large scene, the positioning of moving objects / pedestrians is a very important function. Currently, almost all monitoring systems on the market use a split-screen display method similar to a nine-square grid. The intuitiveness of this split-screen display method is poor. If one is not familiar with the terrain environment of the monitored scene, or the number of monitoring cameras installed in the monitored scene is too large, it is relatively difficult to track and position moving objects / pedestrians. Summary of the Invention

[0003] The technical problem to be solved by the embodiments of the present invention is to provide a multi-sensor collaborative positioning method, device, computer-readable storage medium, and terminal device, which can more intuitively display the monitored area and quickly and conveniently track and position a moving object.

[0004] To solve the above technical problem, an embodiment of the present invention provides a multi-sensor collaborative positioning method, including:

[0005] Based on a target to be positioned, constructing a probability grid map corresponding to a monitored area; wherein, at least two monitoring sensors are included in the monitored area;

[0006] Initializing and marking the probability grid map according to a preset occupancy grid map and monitoring grid map corresponding to the monitored area;

[0007] When any monitoring sensor detects the target to be positioned, obtaining a first target grid where the target to be positioned is located in the monitoring grid map;

[0008] Updating the marked probability grid map according to the first target grid;

[0009] Obtaining a positioning result of the target to be positioned according to the updated probability grid map;

[0010] Wherein, the marking value of each grid in the occupancy grid map correspondingly represents whether there is an obstacle, the marking value of each grid in the monitoring grid map correspondingly represents whether it can be monitored, and the marking value of each grid in the marked probability grid map correspondingly represents the probability value of the existence of the target to be positioned.

[0011] Further, the method pre-obtains the occupancy grid map corresponding to the monitored area through the following steps:

[0012] Use the SLAM algorithm to map the environment of the monitored area to obtain an environmental grid map;

[0013] Mark all the grid cells with obstacles in the environmental grid map as 1, and mark all the grid cells without obstacles in the environmental grid map as 0;

[0014] Obtain the occupancy grid map according to the marked environmental grid map.

[0015] Further, the method pre-obtains the monitoring grid map corresponding to the monitored area through the following steps:

[0016] Obtain the grid position of each monitoring sensor in the occupancy grid map and the monitoring field of view corresponding to each monitoring sensor;

[0017] According to the grid position and monitoring field of view corresponding to each monitoring sensor, respectively obtain the grid cells that can be monitored by each monitoring sensor in the occupancy grid map;

[0018] Mark all the grid cells corresponding to the monitorable grid cells in the environmental grid map as 1, and mark all the non-monitorable grid cells in the environmental grid map as 0;

[0019] Obtain the monitoring grid map according to the marked environmental grid map.

[0020] Further, the initial marking of the probability grid map according to the preset occupancy grid map and monitoring grid map corresponding to the monitored area specifically includes:

[0021] Mark all the grid cells corresponding to the grid cells with a marked value of 1 in the occupancy grid map as 0 in the probability grid map;

[0022] Mark all the grid cells corresponding to the grid cells with a marked value of 1 in the monitoring grid map as 0 in the probability grid map;

[0023] Obtain M other grid cells in the probability grid map except the grid cells marked as 0, and mark each of the M grid cells as 1 / M; where M > 0.

[0024] Further, the update of the marked probability grid map according to the first target grid specifically includes:

[0025] Update the marked value of the grid cell corresponding to the first target grid in the marked probability grid map to 1;

[0026] Update the marking values of all grids in the marked probability grid map except the grids updated to 1 to 0.

[0027] Further, the method further includes:

[0028] When each monitoring sensor fails to detect the target to be located, obtain the second target grid where the target to be located is located in the probability grid map updated at the previous moment and the marking value corresponding to the second target grid according to the positioning result at the previous moment;

[0029] According to the preset moving speed, the second target grid and the marking value corresponding to the second target grid, update the probability grid map updated at the previous moment again;

[0030] Obtain the positioning result of the target to be located according to the probability grid map updated again.

[0031] Further, the step of updating the probability grid map updated at the previous moment again according to the preset moving speed, the second target grid and the marking value corresponding to the second target grid specifically includes:

[0032] According to the moving speed and the second target grid, and in combination with the occupancy grid map and the monitoring grid map, obtain the feasible region corresponding to the target to be located in the probability grid map updated at the previous moment; wherein, the feasible region includes N grids, and the feasible region represents the possible position area where the target to be located may exist at the current moment, N>0;

[0033] Update the marking value of each of the N grids to P / N, and update the marking values of all other grids in the probability grid map updated at the previous moment except the grids updated to P / N to 0; wherein, P represents the marking value corresponding to the second target grid, 0<P≤1.

[0034] To solve the above technical problems, an embodiment of the present invention further provides a multi-sensor collaborative positioning device, including:

[0035] A probability map construction module, configured to construct a probability grid map corresponding to a monitoring area based on a target to be located; wherein, at least two monitoring sensors are included in the monitoring area;

[0036] A probability map initialization module, configured to initialize and mark the probability grid map according to the preset occupancy grid map and monitoring grid map corresponding to the monitoring area;

[0037] A target grid acquisition module, configured to acquire a first target grid where the target to be located is located in the monitoring grid map when any monitoring sensor detects the target to be located;

[0038] A probability map update module, configured to update the marked probability grid map according to the first target grid;

[0039] A positioning result acquisition module, configured to acquire the positioning result of the target to be located according to the updated probability grid map;

[0040] Wherein, the marked value of each grid in the occupancy grid map correspondingly represents whether there is an obstacle, the marked value of each grid in the monitoring grid map correspondingly represents whether it can be monitored, and the marked value of each grid in the marked probability grid map correspondingly represents the probability value of the existence of the target to be located.

[0041] An embodiment of the present invention further provides a computer-readable storage medium, where the computer-readable storage medium includes a stored computer program; wherein, when the computer program runs, it controls the device where the computer-readable storage medium is located to execute the multi-sensor collaborative positioning method described in any one of the above.

[0042] An embodiment of the present invention further provides a terminal device, including a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor. When the processor executes the computer program, it implements the multi-sensor collaborative positioning method described in any one of the above.

[0043] Compared with the prior art, an embodiment of the present invention provides a multi-sensor collaborative positioning method, device, computer-readable storage medium, and terminal device. A probability grid map corresponding to a monitoring area is constructed based on a target to be located. The monitoring area includes at least two monitoring sensors, and the probability grid map is initially marked according to a preset occupancy grid map and monitoring grid map corresponding to the monitoring area. When any monitoring sensor detects the target to be located, a first target grid where the target to be located is located in the monitoring grid map is acquired, and the marked probability grid map is updated according to the first target grid, so as to acquire the positioning result of the target to be located according to the updated probability grid map; wherein, the marked value of each grid in the occupancy grid map correspondingly represents whether there is an obstacle, the marked value of each grid in the monitoring grid map correspondingly represents whether it can be monitored, and the marked value of each grid in the marked probability grid map correspondingly represents the probability value of the existence of the target to be located; the embodiment of the present invention can more intuitively display the monitoring area and quickly and conveniently track and locate a moving body. Description of the Drawings

[0044] Figure 1 It is a flowchart of a preferred embodiment of a multi - sensor collaborative positioning method provided by the present invention;

[0045] Figure 2 It is a monitoring schematic diagram of a monitoring sensor provided by an embodiment of the present invention;

[0046] Figure 3 It is a schematic diagram of updating a probability grid map provided by an embodiment of the present invention;

[0047] Figure 4 It is a structural block diagram of a preferred embodiment of a multi - sensor collaborative positioning device provided by the present invention;

[0048] Figure 5 It is a structural block diagram of a preferred embodiment of a terminal device provided by the present invention. Detailed implementation manners

[0049] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of protection of the present invention.

[0050] An embodiment of the present invention provides a multi - sensor collaborative positioning method. Refer to Figure 1 As shown, it is a flowchart of a preferred embodiment of a multi - sensor collaborative positioning method provided by the present invention. The method includes steps S11 to S15:

[0051] Step S11: Based on the target to be located, construct a probability grid map corresponding to the monitoring area; wherein, at least two monitoring sensors are included in the monitoring area;

[0052] Step S12: According to the preset occupancy grid map and monitoring grid map corresponding to the monitoring area, perform initialization marking on the probability grid map;

[0053] Step S13: When any monitoring sensor detects the target to be located, obtain the first target grid where the target to be located is located in the monitoring grid map;

[0054] Step S14: Update the marked probability grid map according to the first target grid;

[0055] Step S15: Obtain the positioning result of the target to be located according to the updated probability grid map;

[0056] Among them, the marking value of each grid in the occupancy grid map correspondingly represents whether there is an obstacle, the marking value of each grid in the monitoring grid map correspondingly represents whether it can be monitored, and the marking value of each grid in the marked probability grid map correspondingly represents the probability value of the existence of the target to be located.

[0057] It should be noted that before specifically executing steps S11 to S15, the embodiments of the present invention have pre-acquired the occupancy grid map and the monitoring grid map corresponding to the monitoring area. The marking value of each grid in the occupancy grid map represents whether there is an obstacle at the actual position corresponding to the grid in the monitoring area, and the marking value of each grid in the monitoring grid map represents whether the grid can be monitored at the actual position corresponding to the grid in the monitoring area (that is, whether it can be monitored by any monitoring sensor in the monitoring area).

[0058] In specific implementation, based on the target to be located, a probability grid map corresponding to the monitoring area is constructed, and according to the marking values of the grids in the occupancy grid map corresponding to the pre-set monitoring area, the marking values of the corresponding grids in the constructed probability grid map are initialized, and according to the marking values of the grids in the monitoring grid map corresponding to the pre-set monitoring area, the marking values of the corresponding grids in the constructed probability grid map are initialized. Correspondingly, the marking value of each grid in the initialized probability grid map correspondingly represents the probability value of the existence of the target to be located, that is, the probability value that the target to be located appears at the actual position corresponding to the grid in the monitoring area; at least two monitoring sensors (such as 2D / 3D laser sensors, depth sensors, RGB cameras, binocular cameras, cameras, etc.) are set in the monitoring area. When any one of the monitoring sensors detects the target to be located, the actual position of the target to be located is determined to correspond to the grid in the monitoring grid map, and the determined grid is used as the first target grid; according to the determined first target grid, the marking values of the corresponding grids in the initialized probability grid map are updated accordingly, so as to obtain the positioning result of the target to be located according to the marking values of the grids in the updated probability grid map. Correspondingly, the larger the marking value of a certain grid in the probability grid map, the greater the probability value that the target to be located appears at the actual position corresponding to the grid in the monitoring area, and the actual position corresponding to the grid with the largest marking value in the monitoring area can be used as the positioning result of the target to be located.

[0059] It can be understood that the target to be located can be a person or an object; there can be multiple targets to be located. For each target to be located, a separate probability grid map can be constructed to track and locate a specific target to be located. That is, the probability grid map corresponds one-to-one with the target to be located and is unique to each target to be located, while the occupancy grid map and the monitoring grid map can be shared. For example, assume that there is a person and a dog in a family that need to be tracked and located. Then these two targets to be located can share the same occupancy grid map and the same monitoring grid map. However, these two targets to be located need to construct their respective corresponding probability grid maps for tracking.

[0060] A multi-sensor collaborative positioning method provided by an embodiment of the present invention sets at least two monitoring sensors in a monitoring area, constructs a corresponding probability grid map based on the target to be located, and initializes and marks the probability grid map according to the occupancy grid map and the monitoring grid map. When any monitoring sensor detects the target to be located, the first target grid where the target to be located is located in the monitoring grid map is obtained, and the marked probability grid map is updated according to the first target grid, so as to obtain the positioning result of the target to be located according to the updated probability grid map. It can realize multi-sensor collaborative positioning of the target to be located, and through the probability grid map, the spatial position and possible appearance area of the target to be located in the monitoring area can be displayed more intuitively, so as to quickly and conveniently track and locate the moving body.

[0061] In another preferred embodiment, the method pre-obtains the occupancy grid map corresponding to the monitoring area through the following steps:

[0062] Use the SLAM algorithm to map the environment of the monitoring area to obtain an environmental grid map;

[0063] Mark all the grids with obstacles in the environmental grid map as 1, and mark all the grids without obstacles in the environmental grid map as 0;

[0064] Obtain the occupancy grid map according to the marked environmental grid map.

[0065] Specifically, in combination with the above embodiments, when pre-acquiring the occupancy grid map corresponding to the monitoring area, the environment of the monitoring area can be mapped first by using a mobile robot and a SLAM algorithm (Simultaneous Localization and Mapping), and the corresponding environmental grid map of the monitoring area can be obtained accordingly. Then, according to the actual positions of the obstacles in the monitoring area, the corresponding grids of the obstacles in the environmental grid map are determined. All the grids with obstacles in the environmental grid map are marked as 1, and all the grids without obstacles in the environmental grid map are marked as 0. Thus, the occupancy grid map corresponding to the monitoring area is obtained based on the marked environmental grid map.

[0066] For example, denote the occupancy grid map as Map_O, and Map_O(i, j) represents the grid at the position of the i-th row and j-th column in the occupancy grid map, where i > 0 and j > 0. Then, Map_O(i, j) = 1 indicates that there is an obstacle at the (i, j)-th grid position in the occupancy grid map, and Map_O(i, j) = 0 indicates that there is no obstacle at the (i, j)-th grid position in the occupancy grid map.

[0067] It should be noted that the obstacles in the embodiments of the present invention are preferably static obstacles. In this case, since the actual positions of static obstacles are generally fixed, the occupancy grid map only needs to be acquired once. However, the obstacles may also be dynamic obstacles. In this case, since the actual positions of dynamic obstacles are not fixed, the acquired occupancy grid map can be updated when the actual position of any dynamic obstacle changes, or the acquired occupancy grid map can be updated regularly.

[0068] In another preferred embodiment, the method pre-acquires the monitoring grid map corresponding to the monitoring area through the following steps:

[0069] Obtain the grid positions of each monitoring sensor in the occupancy grid map and the monitoring field of view corresponding to each monitoring sensor;

[0070] According to the grid positions and monitoring fields of view corresponding to each monitoring sensor, respectively obtain the grids that can be monitored by each monitoring sensor in the occupancy grid map;

[0071] Mark all the grids corresponding to the grids that can be monitored in the environmental grid map as 1, and mark all the grids that cannot be monitored in the environmental grid map as 0;

[0072] Obtain the monitoring grid map based on the marked environmental grid map.

[0073] Specifically, in combination with the above embodiments, when pre-acquiring the monitoring grid map corresponding to the monitoring area, the grid positions of each monitoring sensor in the occupancy grid map that has been acquired and the monitoring field of view corresponding to each monitoring sensor (i.e., the monitoring field angle) can be obtained first. Then, according to the grid positions and monitoring fields of view corresponding to each monitoring sensor in the occupancy grid map, and based on the grids with obstacles in the occupancy grid map, the grids that can be monitored by each monitoring sensor in the occupancy grid map are respectively obtained. All the grids corresponding to the grids that can be monitored in the environmental grid map are marked as 1, and all the grids corresponding to the grids that cannot be monitored in the environmental grid map are marked as 0, so as to obtain the monitoring grid map corresponding to the monitoring area according to the marked environmental grid map.

[0074] For example, denote the monitoring grid map as Map_C, and Map_C(i, j) represents the grid at the position of the i-th row and j-th column in the monitoring grid map, where i > 0 and j > 0. Then, Map_C(i, j) = 1 indicates that the grid at the (i, j) grid position in the monitoring grid map can be monitored by a certain monitoring sensor, and Map_C(i, j) = 0 indicates that the grid at the (i, j) grid position in the monitoring grid map is a monitoring dead angle and cannot be monitored by any monitoring sensor.

[0075] Combined Figure 2 As shown, it is a monitoring schematic diagram of a monitoring sensor provided by an embodiment of the present invention. Assume that there are two monitoring sensors C1 and C2 in the monitoring area, and the pose parameters and monitoring perspectives of C1 and C2 in the world coordinate system have been calculated through the sensor marking method. Then, the grid positions where C1 and C2 are located in the grid map can be determined. Taking C1 as an example, as Figure 2 shown, assume that the monitoring perspective of C1 in the figure is the first quadrant, and the grids in the gray area in the figure represent the grids with obstacles. Starting from the center of the grid position where C1 is located in the grid map, several rays are emitted within the monitoring perspective until they encounter an obstacle. The grids that the rays can pass through (or reach) indicate that these grids can be monitored by the monitoring sensor C1.

[0076] In yet another preferred embodiment, the initial marking of the probability grid map according to the preset occupancy grid map and monitoring grid map corresponding to the monitoring area specifically includes:

[0077] All the grids corresponding to the grids with a marked value of 1 in the occupancy grid map are marked as 0 in the probability grid map;

[0078] All the grids corresponding to the grids with a marked value of 1 in the monitoring grid map are marked as 0 in the probability grid map;

[0079] Obtain M other grids in the probability grid map except for the grids marked as 0, and mark each of the M grids as 1 / M; where M > 0.

[0080] Specifically, in combination with the above embodiments, when constructing a probability grid map corresponding to a monitoring area based on a target to be located, it is defaulted that all monitoring sensors in the monitoring area do not detect the target to be located; when initializing and marking the constructed probability grid map according to the occupancy grid map and the monitoring grid map, those skilled in the art can understand that there will definitely be no target to be located at the grid positions with obstacles in the occupancy grid map. Therefore, mark the grids corresponding to the grids with a marked value of 1 in the occupancy grid map in the constructed probability grid map as 0; although the monitorable grids in the monitoring grid map can be monitored by a certain monitoring sensor, it is defaulted that all monitoring sensors do not detect the target to be located during initialization. Therefore, mark the grids corresponding to the grids with a marked value of 1 in the monitoring grid map in the constructed probability grid map as 0; in the constructed probability grid map, except for the grids that have been initialized and marked as 0 according to the occupancy grid map and the monitoring grid map, there are still several unmarked grids. Then, obtain all other unmarked grids in the constructed probability grid map except for the grids marked as 0. Assume that there are M unmarked grids, M > 0, and mark each of the M grids as 1 / M, indicating that the probability value of the target to be located appearing at each of the positions of these M unmarked grids is equal during initialization.

[0081] For example, denote the probability grid map as Map_P, and Map_P(i, j) represents the grid at the position of the i-th row and the j-th column in the probability grid map, i > 0, j > 0. Then, mark the grids corresponding to the grids where Map_O(i, j) = 1 and Map_C(i, j) = 1 in Map_P as 0, that is, Map_P(i, j) = 0.

[0082] In another preferred embodiment, the updating of the marked probability grid map according to the first target grid specifically includes:

[0083] Update the marked value of the grid corresponding to the first target grid in the marked probability grid map to 1;

[0084] Update the marked values of all other grids in the marked probability grid map except for the grid updated to 1 to 0.

[0085] Specifically, in combination with the above embodiments, when updating the label value of the corresponding grid in the probability grid map after initialization labeling according to the determined first target grid, first update the label value of the grid corresponding to the first target grid in the probability grid map after initialization labeling to 1, and then update the label values of all other grids in the probability grid map after initialization labeling except the grid updated to 1 to 0. Correspondingly, when obtaining the positioning result of the target to be located according to the label values of the grids in the updated probability grid map, the grid with the largest label value is the grid with the label value of 1, and the positioning result of the target to be located can be obtained according to the actual position corresponding to the grid with the label value of 1 in the monitoring area.

[0086] It should be noted that when the monitoring sensor is a 2D / 3D laser sensor, a depth sensor or a binocular camera, a relatively accurate coordinate can be directly located according to the updated probability grid map. However, when the monitoring sensor is an RGB camera, since a single RGB camera can only locate that the target to be located is on a certain ray, therefore, the determined first target grid is actually several grids on the ray. Suppose there are X grids, X>1. Then, when updating the label values of the corresponding grids in the probability grid map after initialization labeling according to the determined first target grid, the probability value 1 can be directly evenly distributed to these X grids, that is, update the label values of the X grids corresponding to these X grids in the probability grid map after initialization labeling to 1 / X.

[0087] In another preferred embodiment, the method further includes:

[0088] When each monitoring sensor fails to detect the target to be located, obtain the second target grid where the target to be located is located in the probability grid map updated at the previous moment and the label value corresponding to the second target grid according to the positioning result at the previous moment;

[0089] According to the preset moving speed, the second target grid and the label value corresponding to the second target grid, update the probability grid map updated at the previous moment again;

[0090] Obtain the positioning result of the target to be located according to the probability grid map updated again.

[0091] Specifically, in combination with the above embodiments, when at least two monitoring sensors in the monitoring area do not detect the target to be located, that is, no monitoring sensor detects the target to be located anymore, it means that the target to be located has entered the monitoring blind area from the monitoring field of view of the monitoring sensor. At this time, the possible position where the target to be located may exist can be predicted according to the positioning result obtained at the previous moment; in specific implementation, first determine the second target grid where the target to be located is located in the probability grid map updated at the previous moment according to the positioning result obtained at the previous moment, and the marking value corresponding to the second target grid, and then, according to the preset moving speed, the determined second target grid and the marking value corresponding to the second target grid, update the probability grid map updated at the previous moment again, so as to obtain the positioning result of the target to be located according to the probability grid map updated again.

[0092] It should be noted that the actual moving speeds corresponding to different targets to be located may be different. Therefore, the moving speed can be set accordingly according to the actual moving speed of the target to be located.

[0093] As an improvement of the above solution, the step of updating the probability grid map updated at the previous moment again according to the preset moving speed, the second target grid and the marking value corresponding to the second target grid specifically includes:

[0094] According to the moving speed and the second target grid, and in combination with the occupancy grid map and the monitoring grid map, obtain the feasible region corresponding to the target to be located in the probability grid map updated at the previous moment; wherein, the feasible region includes N grids, and the feasible region represents the position area where the target to be located may exist at the current moment, N>0;

[0095] Update the marking value of each of the N grids to P / N, and update the marking values of all other grids except the grids updated to P / N in the probability grid map updated at the previous moment to 0; wherein, P represents the marking value corresponding to the second target grid, 0<P≤1.

[0096] Specifically, in combination with the above embodiments, when updating the probability grid map updated at the previous moment according to the preset moving speed, the determined second target grid, and the marking value corresponding to the second target grid, the feasible region corresponding to the target to be located in the probability grid map updated at the previous moment can be obtained according to the preset moving speed and the determined second target grid, in combination with the occupancy grid map and the monitoring grid map obtained in advance. The feasible region specifically represents the position area where the target to be located may exist at the current moment. Assuming that the feasible region includes N grids, N>0, and the marking value corresponding to the determined second target grid is P, 0<P≤1, then the marking value of each of these N grids is updated to P / N, and the marking values of all other grids in the probability grid map updated at the previous moment except for the grids updated to P / N are updated to 0.

[0097] Combined with Figure 3 As shown, it is a schematic diagram of probability grid map update provided by an embodiment of the present invention. Assuming that there are two monitoring sensors in the monitoring area, namely C1 and C2, Figure 3 The grids in the gray area in represent the grids with obstacles. The monitoring sensor C1 monitors the position of the target to be located at point A in at time t, Figure 3 and the marking value of the grid at point A is P(A)=1, and the marking values of other grids in the probability grid map are all 0. At time t+1, neither C1 nor C2 can monitor the target to be located. Assuming that the maximum moving distance of the target to be located per unit time is 2 grids, then except for the grids with obstacles and the grids that can be monitored, the feasible region corresponding to the target to be located in the probability grid map updated at time t can be determined as Figure 3 The shaded area in. The 4 grids in the shaded area represent the grid positions where the target to be located may exist at time t+1. Then, P(A)=1 is evenly distributed to these 4 grids, that is, the marking value of each of these 4 grids is updated to 0.25.

[0098] It can be understood that if at time t+2, neither C1 nor C2 can monitor the target to be located, the feasible region corresponding to the target to be located in the probability grid map updated at time t+1 can be determined according to the positioning result obtained at time t+1, that is, the corresponding feasible region is determined for each of the 4 grids in the shaded area, and the marking value 0.25 of the grid is evenly distributed to each grid in the corresponding feasible region. Correspondingly, if a certain grid is located in multiple feasible regions at the same time, the probability values assigned to this grid are accumulated.

[0099] In addition, if C1 and C2 have not monitored the target to be located for a long time, the probability value 1 can be evenly distributed to each grid in the probability grid map.

[0100] The embodiments of the present invention can be widely applied in practical scenarios. For example, they can be applied to the care of the elderly / children. By implementing the technical solutions provided by the present invention, if a certain monitoring camera detects an elderly person falling, the position where the elderly person falls can be quickly located, and a mobile robot can be dispatched to deliver medicine to the elderly person. If the elderly person falls within the blind area of the monitoring camera's field of view, it is also possible to start searching from the position with the highest probability of falling and predict the possible position of the elderly person, thereby reducing the rescue time.

[0101] The embodiments of the present invention also provide a multi-sensor collaborative positioning device. Refer to Figure 4 As shown, it is a structural block diagram of a preferred embodiment of a multi-sensor collaborative positioning device provided by the present invention. The device includes:

[0102] A probability map construction module 11, configured to construct a probability grid map corresponding to a monitoring area based on a target to be located; wherein, at least two monitoring sensors are included in the monitoring area;

[0103] A probability map initialization module 12, configured to perform initialization marking on the probability grid map according to a preset occupancy grid map and monitoring grid map corresponding to the monitoring area;

[0104] A target grid acquisition module 13, configured to acquire a first target grid where the target to be located is located in the monitoring grid map when any monitoring sensor detects the target to be located;

[0105] A probability map update module 14, configured to update the marked probability grid map according to the first target grid;

[0106] A positioning result acquisition module 15, configured to acquire a positioning result of the target to be located according to the updated probability grid map;

[0107] Wherein, the marking value of each grid in the occupancy grid map correspondingly represents whether there is an obstacle, the marking value of each grid in the monitoring grid map correspondingly represents whether it can be monitored, and the marking value of each grid in the marked probability grid map correspondingly represents the probability value of the existence of the target to be located.

[0108] Preferably, the device further includes an occupancy map acquisition module, configured to:

[0109] Use the SLAM algorithm to map the environment of the monitoring area to obtain an environmental grid map;

[0110] Mark all the grids with obstacles in the environmental grid map as 1, and mark all the grids without obstacles in the environmental grid map as 0;

[0111] Obtain the occupied grid map according to the marked environmental grid map.

[0112] Preferably, the device further includes a monitoring map acquisition module for:

[0113] Obtain the grid position of each monitoring sensor in the occupied grid map and the monitoring field of view corresponding to each monitoring sensor;

[0114] According to the grid position and monitoring field of view corresponding to each monitoring sensor, respectively obtain the grids that can be monitored by each monitoring sensor in the occupied grid map;

[0115] Mark all the grids corresponding to the grids that can be monitored in the environmental grid map as 1, and mark all the grids that cannot be monitored in the environmental grid map as 0;

[0116] Obtain the monitoring grid map according to the marked environmental grid map.

[0117] Preferably, the probability map initialization module 12 specifically includes:

[0118] The first initialization marking unit is used to mark all the grids corresponding to the grids with a marked value of 1 in the occupied grid map as 0 in the probability grid map;

[0119] The second initialization marking unit is used to mark all the grids corresponding to the grids with a marked value of 1 in the monitoring grid map as 0 in the probability grid map;

[0120] The third initialization marking unit is used to obtain M other grids in the probability grid map except for the grids marked as 0, and mark each of the M grids as 1 / M; where M>0.

[0121] Preferably, the probability map update module 14 specifically includes:

[0122] The first marking value update unit is used to update the marking value of the grid corresponding to the first target grid in the marked probability grid map to 1;

[0123] The second marking value update unit is used to update the marking values of all other grids in the marked probability grid map except for the grids updated to 1 to 0.

[0124] Preferably, the device further includes a positioning blind area processing module, and the positioning blind area processing module specifically includes:

[0125] A target grid acquisition unit, configured to, when none of the monitoring sensors detects the target to be located, acquire a second target grid in the probability grid map updated at the previous moment where the target to be located is located and a marking value corresponding to the second target grid according to the positioning result at the previous moment;

[0126] A probability map update unit, configured to update the probability grid map updated at the previous moment again according to a preset moving speed, the second target grid, and the marking value corresponding to the second target grid;

[0127] A positioning result acquisition unit, configured to acquire the positioning result of the target to be located according to the probability grid map updated again.

[0128] Preferably, the probability map update unit is specifically configured to:

[0129] According to the moving speed and the second target grid, and in combination with the occupancy grid map and the monitoring grid map, acquire a feasible region corresponding to the target to be located in the probability grid map updated at the previous moment; wherein, the feasible region includes N grids, the feasible region represents a position region where the target to be located may exist at the current moment, and N>0;

[0130] Update the marking value of each of the N grids to P / N, and update the marking values of all other grids except the grids updated to P / N in the probability grid map updated at the previous moment to 0; wherein, P represents the marking value corresponding to the second target grid, and 0<P≤1.

[0131] It should be noted that a multi-sensor collaborative positioning device provided in an embodiment of the present invention can implement all processes of the multi-sensor collaborative positioning method described in any of the above embodiments. The functions and achieved technical effects of each module and unit in the device respectively correspond to the functions and achieved technical effects of the multi-sensor collaborative positioning method described in the above embodiments, and will not be elaborated here.

[0132] An embodiment of the present invention further provides a computer-readable storage medium, where the computer-readable storage medium includes a stored computer program; wherein, when the computer program runs, it controls a device where the computer-readable storage medium is located to execute the multi-sensor collaborative positioning method described in any of the above embodiments.

[0133] An embodiment of the present invention further provides a terminal device, see Figure 5As shown, it is a structural block diagram of a preferred embodiment of a terminal device provided by the present invention. The terminal device includes a processor 10, a memory 20, and a computer program stored in the memory 20 and configured to be executed by the processor 10. When the processor 10 executes the computer program, the multi-sensor collaborative positioning method described in any of the above embodiments is implemented.

[0134] Preferably, the computer program can be divided into one or more modules / units (such as computer program 1, computer program 2,...). The one or more modules / units are stored in the memory 20 and executed by the processor 10 to complete the present invention. The one or more modules / units can be a series of computer program instruction segments capable of performing specific functions, and these instruction segments are used to describe the execution process of the computer program in the terminal device.

[0135] The processor 10 can be a Central Processing Unit (CPU), or can also be other general-purpose processors, Digital Signal Processors (DSPs), Application Specific Integrated Circuits (ASICs), Field-Programmable Gate Arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor can be a microprocessor, or the processor 10 can also be any conventional processor. The processor 10 is the control center of the terminal device, and connects various parts of the terminal device through various interfaces and lines.

[0136] The memory 20 mainly includes a program storage area and a data storage area. Among them, the program storage area can store an operating system, application programs required for at least one function, etc., and the data storage area can store relevant data, etc. In addition, the memory 20 can be a high-speed random access memory, or can also be a non-volatile memory, such as a plug-in hard disk, a Smart Media Card (SMC), a Secure Digital (SD) card, a Flash Card, etc., or the memory 20 can also be other volatile solid-state storage devices.

[0137] It should be noted that the above terminal device may include, but is not limited to, a processor and a memory. Those skilled in the art can understand, Figure 5The structural block diagram is only an example of the above terminal device, and does not constitute a limitation on the terminal device. It may include more or fewer components than shown, or combine certain components, or different components.

[0138] In summary, for a multi-sensor collaborative positioning method, device, computer-readable storage medium, and terminal device provided by an embodiment of the present invention, at least two monitoring sensors are set in a monitoring area, a corresponding probability grid map is constructed based on a target to be positioned, and the probability grid map is initially marked according to the occupancy grid map and the monitoring grid map. When any monitoring sensor detects the target to be positioned, the first target grid where the target to be positioned is located in the monitoring grid map is obtained, and the marked probability grid map is updated according to the first target grid, so as to obtain the positioning result of the target to be positioned according to the updated probability grid map. It can achieve multi-sensor collaborative positioning of the target to be positioned, and the spatial position and possible appearance areas of the target to be positioned in the monitoring area can be more intuitively displayed through the probability grid map, thereby quickly and conveniently tracking and positioning a moving object.

[0139] The above are only the preferred embodiments of the present invention. It should be noted that for those of ordinary skill in the art, without departing from the technical principle of the present invention, several improvements and deformations can be made, and these improvements and deformations should also be regarded as the protection scope of the present invention.

Claims

1. A multi-sensor collaborative positioning method, characterized in that, Including: Based on the target to be located, construct a probability grid map corresponding to the monitoring area; wherein, at least two monitoring sensors are included in the monitoring area; According to the preset occupancy grid map and monitoring grid map corresponding to the monitoring area, initialize and mark the probability grid map; When any monitoring sensor detects the target to be located, obtain the first target grid where the target to be located is located in the monitoring grid map; Update the marked probability grid map according to the first target grid; Obtain the positioning result of the target to be located according to the updated probability grid map; Wherein, the marked value of each grid in the occupancy grid map correspondingly represents whether there is an obstacle, the marked value of each grid in the monitoring grid map correspondingly represents whether it can be monitored, and the marked value of each grid in the marked probability grid map correspondingly represents the probability value of the existence of the target to be located.

2. The multi-sensor collaborative positioning method according to claim 1, characterized in that The method pre-obtains the occupancy grid map corresponding to the monitoring area through the following steps: Use the SLAM algorithm to map the monitoring area environment to obtain an environmental grid map; Mark all the grids with obstacles in the environmental grid map as 1, and mark all the grids without obstacles in the environmental grid map as 0; Obtain the occupancy grid map according to the marked environmental grid map.

3. The multi-sensor collaborative positioning method according to claim 2, characterized in that The method pre-obtains the monitoring grid map corresponding to the monitoring area through the following steps: Obtain the grid position of each monitoring sensor in the occupancy grid map and the monitoring field of view corresponding to each monitoring sensor; According to the grid position and monitoring field of view corresponding to each monitoring sensor, respectively obtain the grids that can be monitored by each monitoring sensor in the occupancy grid map; Mark all the grids corresponding to the grids that can be monitored in the environmental grid map as 1, and mark all the grids that cannot be monitored in the environmental grid map as 0; Obtain the monitoring grid map according to the marked environmental grid map.

4. The multi-sensor collaborative positioning method according to claim 3, wherein The initializing and marking of the probability grid map according to the preset occupancy grid map and monitoring grid map corresponding to the monitoring area specifically includes: Mark all the grids corresponding to the grids with a marked value of 1 in the occupancy grid map as 0 in the probability grid map; Mark all the grids corresponding to the grids with a marked value of 1 in the monitoring grid map as 0 in the probability grid map; Obtain M other grids in the probability grid map except the grids marked as 0, and mark each of the M grids as 1 / M; where M>0.

5. The multi-sensor collaborative positioning method according to claim 4, wherein The updating of the marked probability grid map according to the first target grid specifically includes: Update the marked value of the grid corresponding to the first target grid in the marked probability grid map to 1; Update the marked values of all other grids in the marked probability grid map except the grid updated to 1 to 0.

6. The multi-sensor collaborative positioning method according to any one of claims 1 to 5, characterized in that The method further includes: When none of the monitoring sensors detects the target to be located, obtain the second target grid where the target to be located is located in the probability grid map updated at the previous moment and the corresponding marking value of the second target grid according to the positioning result at the previous moment; According to the preset moving speed, the second target grid, and the corresponding marking value of the second target grid, update the probability grid map updated at the previous moment again; Obtain the positioning result of the target to be located according to the probability grid map updated again.

7. The multi-sensor collaborative positioning method according to claim 6, characterized in that, The step of updating the probability grid map updated at the previous moment according to the preset moving speed, the second target grid, and the corresponding marking value of the second target grid specifically includes: According to the moving speed and the second target grid, and in combination with the occupancy grid map and the monitoring grid map, obtain the feasible region corresponding to the target to be located in the probability grid map updated at the previous moment; wherein, the feasible region includes N grids, and the feasible region represents the position area where the target to be located may exist at the current moment, N>0; Update the marking value of each of the N grids to P / N, and update the marking values of all other grids in the probability grid map updated at the previous moment except the grids updated to P / N to 0; wherein, P represents the marking value corresponding to the second target grid, 0<P≤1.

8. A multi-sensor collaborative positioning device, characterized in that, including: A probability map construction module, configured to construct a probability grid map corresponding to a monitoring area based on a target to be located; wherein, at least two monitoring sensors are included in the monitoring area; A probability map initialization module, configured to perform initialization marking on the probability grid map according to the preset occupancy grid map and monitoring grid map corresponding to the monitoring area; A target grid acquisition module, configured to obtain the first target grid where the target to be located is located in the monitoring grid map when any monitoring sensor detects the target to be located; A probability map update module, configured to update the marked probability grid map according to the first target grid; A positioning result acquisition module, configured to obtain the positioning result of the target to be located according to the updated probability grid map; Wherein, the marking value of each grid in the occupancy grid map correspondingly represents whether there is an obstacle, the marking value of each grid in the monitoring grid map correspondingly represents whether it can be monitored, and the marking value of each grid in the marked probability grid map correspondingly represents the probability value of the existence of the target to be located.

9. A computer-readable storage medium, characterized in that, The computer-readable storage medium includes a stored computer program; wherein, the computer program controls the device where the computer-readable storage medium is located to execute the multi-sensor collaborative positioning method according to any one of claims 1 to 7 when running.

10. A terminal device, characterized in that, It includes a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor. The processor implements the multi-sensor collaborative positioning method according to any one of claims 1 to 7 when executing the computer program.

Images