An underwater robot trajectory obstacle detection method

Abstract

The application discloses an underwater robot trajectory obstacle detection method, comprising the following steps: (1) generating a feasible trajectory set; (2) generating a footprint grid map set; (3) generating a probability footprint grid map set; (4) saving the probability footprint grid map set; (5) constructing a sensor grid map set; (6) integrating sensor data; and (7) collision detection. In step (2), the footprint shape of a robot moving along a trajectory in different shapes is modeled, so that the method is suitable for robots in various shapes. In step (3), the uncertainty of the robot moving in water is considered. In step (5), the uncertainty of sensor measurement is considered. In addition, steps (1)-(5) are offline calculation steps, only steps (6)-(7) need to consume online computing power, and step (6)-(7) is simple to calculate. Therefore, the method has the characteristics of high real-time performance, and can provide strong support for the safe operation of the underwater robot.

Description

Technical Field

[0001] This invention relates to a method for detecting obstacles on the trajectory of an underwater robot. Background Technology

[0002] Underwater robots are of great significance for seabed exploration, rescue, deep-sea aquaculture, and offshore platform cleaning and inspection. To ensure the safe and reliable operation of underwater robots, underwater obstacle detection capability is essential. When the robot is operated by a human, underwater obstacle detection can provide the operator with candidate trajectories with a low probability of collision; the operator only needs to select one to avoid a collision. When the robot is operating autonomously, obstacle detection capability is even more crucial, as it needs to provide the robot with collision-free candidate trajectories in real time for the path planning layer to select a safe path.

[0003] Most current obstacle detection methods do not consider the shape of the robot, or simply assume that the robot is square or round. In addition, for underwater robots, the obstacle recognition sensors are generally sonar, which has high measurement uncertainty. At the same time, due to the influence of water flow, the movement trajectory of underwater robots also has great uncertainty. These two aspects are not considered simultaneously by most obstacle avoidance methods.

[0004] In summary, based on current methods for detecting obstacles on the trajectory of underwater robots, determining whether a robot can operate safely involves considerable uncertainty. Summary of the Invention

[0005] To address the aforementioned problems, this invention proposes a method for detecting obstacles on the trajectory of an underwater robot that can reduce uncertainty, thereby providing stronger support for the safe operation of underwater robots.

[0006] This invention solves its technical problem through the following technical solution: a method for detecting obstacles on the trajectory of an underwater robot, comprising the following steps:

[0007] (1) Generation of feasible trajectory sets;

[0008] (2) Footprint raster map generation

[0009] Mark the footprints of each trajectory in the feasible trajectory set on the grid map, and record the warning level of each footprint position. Construct a footprint grid map for each trajectory. The warning level is set according to the reaction time of the robot to avoid the corresponding footprint position.

[0010] (3) Generation of probabilistic footprint raster map atlas

[0011] Mark the areas that the underwater robot may actually pass through when moving along a certain trajectory on a grid map, and store the probability of the trajectory passing through each grid cell to construct a trajectory probability grid map. Then, based on the trajectory probability grid map Generate a corresponding footprint raster map, which is called a probabilistic footprint raster map. And ultimately obtain a probabilistic footprint raster atlas;

[0012] (4) Saving of probabilistic footprint raster atlas;

[0013] (5) Sensor grid atlas construction

[0014] Given sensor measurements, determine the possible location of obstacles in the sensor coordinate system, and mark the probability of obstacles at each grid in the coordinate system. This map is called a sensor grid map. The collection of sensor grid maps corresponding to different measurements is called a sensor grid map set; (6) Sensor data integration

[0015] Collect measurement values ​​from various sensors on the robot. And index the corresponding sensor grid map based on the measured value. Based on the pose of each sensor coordinate system in the robot coordinate system ,Will Transform to the robot coordinate system and determine the occupancy probability of the corresponding grid. Accumulate them to form a sensor fusion grid map. And normalize the occupancy probability in the sensor fusion grid map;

[0016] (7) Collision detection

[0017] The probability value of the corresponding grid cell in each grid map of the sensor fusion grid map and the probabilistic footprint grid map set is multiplied to obtain the probability of collision for each footprint grid cell of each trajectory.

[0018] In this invention, the specific steps for generating the feasible trajectory set in step (1) are as follows:

[0019] Discretize the continuous trajectory. The robot's first Continuous trajectory The set of discretized sample points. Let i be the i-th sample point in this set, which includes three position coordinates x and x. i y i z i and three attitude coordinates row i pitch iyaw i Position represents the coordinates of the selected robot coordinate system origin in the specified coordinate system, and attitude represents the robot coordinate system's roll angle, pitch angle, and yaw angle in the specified coordinate system. The number of sample points in this set; the feasible trajectory set consists of a series of feasible trajectories, i.e. ,in This indicates the number of feasible trajectories in the feasible trajectory set.

[0020] It is recommended to determine this in the following way: ,in, Represents two adjacent trajectory sample points and The time interval, The magnitude of the value affects the accuracy of the approximation of the discrete trajectory, and also affects the computational cost. N represents the time it takes for the robot to come to a complete stop from its maximum speed; N is a safety margin set for this purpose. The magnification factor is so that its value should be greater than 1.

[0021] Tra j Determined in the following manner:

[0022] (1.a) Discretize the control variables of the robot to obtain a set of discrete control variables. , To control the number of quantities;

[0023] (1.b) Obtain discrete trajectory points based on the control input and the robot's motion model f().

[0024] When the control quantity is When the discrete trajectory is obtained, it is:

[0025] .

[0026] In this invention, the specific steps for generating the footprint raster atlas in step (2) are as follows:

[0027] 2.a Construct the raster data structure and determine the raster coordinate system. Grid coordinate system Robot coordinate system coincide

[0028] Each grid cell stores the following vector ,in For the corresponding raster coordinates The probability of finding footprints in the image. For the corresponding raster coordinates The warning level; the initial value for each grid is [value missing]. ;

[0029] 2.b Construct a raster map of the j-th trajectory.

[0030] First, obtain each sample point of the j-th trajectory. Then align the robot's shape to each sample point. At this point, if the coordinates are... Where the grid and the robot shape intersect, then the grid value at this point... A value of 1 is assigned to the grid cell on the robot's footprint along the j-th trajectory. If they do not intersect, the initial value is maintained. Furthermore, the time when a given trajectory sample point is found is used to assign a value to the grid cell covered by the robot's footprint as it reaches that sample point along the trajectory. value.

[0031] Regarding the aforementioned reaction time, this invention sets three levels: Danger, Warning, and Safety, and defines them as follows: Reaction time within... Inside, the level is dangerous; Inside, the level is warning; The internal security level is Safe.

[0032] In this invention, the specific steps for generating the probabilistic footprint raster map atlas in step (3) are as follows:

[0033] 3.a First, regarding the control quantity The trajectory probability raster map is thresholded for segmentation. Within each raster value, the probability is... The above is reserved, less than or equal to The value is set to 0;

[0034] 3.b Then, align the robot's shape to each non-zero grid cell in the trajectory probability grid map. For the robot's pose in the trajectory probability grid, set it as follows: obtain the coordinates of the aligned grid cells. Find the control quantity Corresponding trajectory Distance between sample points The nearest sample point is used to obtain its pose information, and this pose is used as a grid. Alignment posture;

[0035] 3.c Set the intersection of the robot shape and trajectory probability grid map. Values, where raster coordinates The probability of footprints Set as the trajectory probability at the corresponding location, grid coordinates Alert level The approximate method is used to obtain the distance in the footprint raster map in step (2). The most recent footprint point, with the warning level of that point as... The value;

[0036] 3.d Construct a probabilistic footprint raster map for each control variable to obtain a probabilistic footprint raster map set.

[0037] In step (7), a threshold is further set. The result of multiplying the probabilities is greater than If the grid cell is not found to be in a collision state, then the grid cell is considered to be in a collision state.

[0038] The method of this invention is set up in two stages: the first stage is the preprocessing stage, and the second stage is the online processing stage. The preprocessing stage is the calculation and storage stage before the robot works, which does not require online computing power. Steps (1)-(5) are performed in the preprocessing stage, and steps (6) and (7) are performed in the online processing stage. This invention concentrates the main data processing steps in the offline stage, and the online calculation steps are simple. Therefore, the online detection speed of trajectory obstacles is relatively fast.

[0039] Compared with existing collision detection technologies, this invention has the following advantages:

[0040] The method takes into account the uncertainties of sensor measurements and the uncertainties of robot movement in water, is applicable to robots of various shapes, and has high real-time performance, which can provide stronger support for the safe operation of underwater robots. Attached Figure Description

[0041] Figure 1 Figure 1 shows a schematic diagram of obstacle recognition on a trajectory. Figure 2 shows the obstacle recognition trajectory form, and Figure 3 shows the obstacle recognition trajectory footprint form.

[0042] Figure 2 This is a schematic diagram of the feasible trajectory set;

[0043] Figure 3 This is a diagram of a footprint grid map;

[0044] Figure 4 This is a schematic diagram of a trajectory probability grid map;

[0045] Figure 5 Figure (a) shows a sensor grid map with a distance of 600 mm, and Figure (b) shows a sensor grid map with a distance of 1400 mm. Detailed Implementation

[0046] Obstacle detection on the trajectory of an underwater robot is not simply detecting whether there is an obstacle in front of the robot, but rather detecting whether there is an obstacle on the trajectory along which the robot will move, and how far away (the degree of danger) the robot is from colliding with the nearest obstacle while moving along that trajectory.

[0047] like Figure 1 As shown in (a), a robot (black) moves along a trajectory (dashed line). Obstacle 1 in front is not on the trajectory, so it is not a collision obstacle. Obstacles 2 and 3 are on the trajectory and are collision obstacles. Obstacle 2 will collide with obstacle 3 first. Therefore, on the trajectory, obstacle 2 is closer to the robot's current position than obstacle 3. Figure 1 (b) illustrates the concept of a trajectory footprint, which is the spatial range covered by the robot's body (considered as a rigid body) when the robot moves along a trajectory. Therefore, obstacle detection on a given trajectory means detecting whether the trajectory footprint corresponding to that trajectory interferes with the obstacle.

[0048] Because underwater robots vary in shape and are affected by water flow, their actual movement trajectory deviates from the planned trajectory. Therefore, the working conditions of underwater robots and terrestrial robots are very different, and the uncertainty of the trajectory and the shape of the robot need to be considered. On the other hand, most underwater collision detection sensors are sonar sensors, which have large distance measurement uncertainty. Therefore, this invention will design an obstacle recognition method for underwater robots based on their characteristics.

[0049] This method has two stages: a preprocessing stage and an online processing stage. Preprocessing involves calculations and data storage before the robot begins its operation, thus requiring no online computing power. The online processing stage, which is the actual movement stage of the robot, demands high real-time performance; therefore, the algorithm must be fast.

[0050] The preprocessing stage has five steps: (1) generating feasible trajectory sets; (2) generating footprint raster maps; (3) generating probabilistic footprint raster map sets; (4) saving probabilistic footprint raster maps; and (5) constructing sensor raster map sets.

[0051] Online processing has two steps: (6) Sensor data integration; (7) Collision detection.

[0052] The main steps of this patent are focused on offline calculations, with very few online steps. Therefore, online obstacle detection for a single trajectory can be very fast.

[0053] Each step will be described in detail below.

[0054] (1) Generation of feasible trajectory sets

[0055] This invention discretizes a continuous trajectory. Let... Let be the i-th trajectory sample point on the discrete trajectory, including three position coordinates and three attitude coordinates. The three positions represent the rectangular coordinates of the robot coordinate system in the specified coordinate system, and the attitudes represent the roll angle, pitch angle, and yaw angle of the robot coordinate system in the specified coordinate system. Continuous trajectory It can be discretized into a set of poses. .in, This represents the number of discrete points in the pose. Two adjacent trajectory sample points are defined. and The time interval is , The smaller the value, the more accurately the discrete trajectory can represent the continuous trajectory, but the computational cost will increase. As the value increases, the approximation of the discrete trajectory becomes less accurate. However, considering that this step is an offline computation step, the increase in computational cost will not affect the online computational efficiency. Using an interval of 0.05s, for Based on the time it takes for the robot to go from top speed to a complete stop and To determine, specifically , Will The purpose of quadrupling the size is to allow for sufficient safety margin, enabling the robot to observe a larger area to detect obstacles.

[0056] The feasible trajectory set refers to all the possible trajectories a robot can take within a short time, given its dynamic configuration and motion constraints. The feasible trajectory set consists of a series of feasible trajectories. ,in This indicates the number of feasible trajectories in the feasible trajectory set, which is related to the number of discretized control variables. See below for a detailed description. Each trajectory in the trajectory set specifically refers to the trajectory of the robot's control center (usually the geometric center). Figure 2 This diagram illustrates a set of feasible trajectories for a dual-thrust underwater robot. The robot achieves turning by generating different thrusts from its two thrusters, and travels in a straight line when the thrusts are equal. Therefore, its short-term trajectory can be well represented by both arcs and straight lines. In the diagram, the black area represents the robot body, and multiple dashed lines form the set of feasible trajectories.

[0057] The specific steps for generating a feasible trajectory set are as follows:

[0058] (1.a) Discretize the control variables of the robot to obtain a set of valid discrete control variables. ,For example, Figure 2 The control variables of the robot shown ,in Input the velocity of the left thruster. Input the speed of the right thruster; the range of left and right speeds is... , Therefore, the control quantity can be discretized based on the range and speed interval. Since the control quantities differ for different robots, the discretization method depends on the specific situation; this patent only illustrates the general approach.

[0059] (1.b) Based on the control input and the robot's motion model, discrete trajectory points are obtained, and further, a set of discrete trajectories is obtained. The robot's motion model can be expressed as a linear model. A and B are parameters related to robot kinematics, and are related to the robot's thruster arrangement and type. Additionally, Also and discrete time interval Related, and Let be the robot's pose at time t and time t+1. This refers to the robot control variable mentioned in (1.a). When the control variable is When the discrete trajectory is obtained, it is:

[0060]

[0061] Furthermore, according to One control variable can obtain a set of feasible trajectories. .

[0062] (2) Footprint raster map generation

[0063] Since robots have shapes, obstacle detection essentially involves determining whether the trajectory footprints interfere with obstacles. Therefore, it is necessary to pre-construct the trajectory footprints for each trajectory in the feasible trajectory set, as well as the warning level for each footprint position. The resulting map is called a footprint grid map.

[0064] This invention patent categorizes obstacle warning levels into three levels: Danger, Warning, and Safe, corresponding to three areas of the obstacle. Once a track is marked as Danger, it means that moving along that track... A collision with an obstacle is inevitable within a certain timeframe; the warning indicates that you must continue moving along that trajectory. A collision is possible during this time period. Safety indicates that following this trajectory will result in a collision. Within a given timeframe, there is ample reaction time.

[0065] The method for constructing a footprint raster atlas is as follows:

[0066] 2.a Constructing a raster data structure (e.g.) Figure 3 (grid structure in the image), and determine the grid coordinate system. (like Figure 3As shown), at this time and (Robot coordinate system) coincides. The resolution of the grid map is set according to the actual situation. The higher the resolution, the more detailed the grid detection of obstacles, but the more storage space it consumes, and vice versa. Considering that underwater obstacles are relatively sparse, this patent recommends a grid resolution of 50mm. When obstacles are dense, a higher resolution grid is recommended. Due to and The grid coordinate system coincides; the origin of the grid coordinate system represents the robot's current position. The axis represents the robot's current pose. Each grid cell stores the following vector. ,in For the corresponding raster coordinates The probability of finding footprints in the image. For the corresponding raster coordinates The warning levels (Danger, Warning, Safe) are set, and the initial value for each grid cell is [value missing]. .

[0067] 2.b Based on the feasible trajectory set In Trajectory, Construction Each track corresponds to one raster map, forming a footprint raster atlas. The specific method for constructing a raster map based on the j-th trajectory is as follows: First, obtain the trajectory sample points for each of the j-th trajectories. Then, align the robot's shape to each trajectory sample point (align the centroid to the position of the trajectory sample point, and the pose to the pose of the trajectory sample point). At this point, if the coordinates are... Where the grid and the robot shape intersect, then the grid value at this point... A value of 1 is assigned to indicate that the grid cell lies on the footprint of the robot's j-th trajectory. If they do not intersect, the initial value is retained. Furthermore, based on a given trajectory sample point, the time to that trajectory sample point can be retrieved. If the time is... All the grids covered by the trajectory sample points Assigning values ​​is dangerous, especially if the time interval is... ,but The value is assigned as a warning, and so on. It's important to note that footprints from different trajectory points may overlap, therefore the corresponding grid... Values ​​can also be overwritten, with the rule being that higher alert levels can overwrite lower alert levels, but the reverse is not true. A footprint raster map is as follows: Figure 3As shown, the left side represents danger, the middle represents warning, and the right side represents safety. Different grayscale values ​​have been used to represent these in the image. The remaining blank grids represent areas not covered by the trajectory footprints. (Note that the trajectory sample points in this image are relatively sparse, which will miss many grids that should be on the trajectory. The sparse processing is to make the image clearer.)

[0068] (3) Generation of probabilistic footprint raster map atlas

[0069] like Figure 4 As shown, when the robot control quantity is In theory, the robot will move along a predetermined trajectory (the line in the middle). However, due to the influence of water flow, the actual movement trajectory of its control center (usually the centroid) may pass through a certain area. Figure 4 The image shows a gray-black grid, where darker areas indicate locations more likely to be traversed along the central line trajectory. Therefore, a trajectory needs to be constructed for each control variable, as shown below. Figure 4 The grid map shown is referred to in this patent as a trajectory probability grid map. Each grid cell stores the probability that the trajectory passes through that point. Each control quantity has indivual This grid map can be obtained experimentally or through online learning. Online learning requires online computing power. Experimental methods involve placing the robot in water with fixed control parameters. Starting from a fixed point, multiple repeated experiments are conducted, and the robot's movement trajectory is measured using external measuring equipment. By statistically analyzing the points traversed by the trajectory in these multiple experiments, a trajectory probability grid map for a given control variable can be obtained. In principle, the trajectory probability grid map needs to consider attitude uncertainty; however, to simplify the algorithm, this patent only considers position uncertainty.

[0070] Given control quantity The probability footprint raster map can be constructed by combining the trajectory probability raster map and the footprint raster map generated in step (2). Note that the coordinate system of the probability footprint raster map and the trajectory probability raster map are the same.

[0071] The construction steps are as follows: First, for the control variable... Threshold segmentation is performed on the trajectory probability raster map, that is, the probability of the trajectory passing through a raster is greater than a certain threshold. Reserved, less than or equal to The trajectory probability is set to 0; then, following the method in step (2), the robot's shape is aligned to each non-zero grid cell in the trajectory probability grid map. Note that the trajectory probability grid does not have attitude information. This patent uses the following scheme for attitude alignment: the coordinates of the aligned grid are obtained as follows. So, find Distance between trajectory sample points The nearest sample point is used to obtain its pose, which is then used as a grid. The alignment posture; since the coordinate system of the probabilistic footprint raster map and the trajectory probabilistic raster map coincide, therefore and The coordinates coincide one by one, and similar to the method in (2), the intersection of the robot shape and trajectory probability grid map can be set. Values, where raster coordinates The probability of footprints in Set as the trajectory probability at the corresponding location, grid coordinates Alert level This is obtained using an approximate method, namely by finding the raster map of the footprint trajectory. Mid-range The most recent footprint point, with the warning level of that point as... The value of .

[0072] Build By analyzing the probability footprint raster map of each control variable in the set, we can obtain the probability footprint raster map atlas. .

[0073] (4) Saving of probabilistic footprint raster atlas

[0074] Save all the probability footprint grid maps generated in step (3) to the hard disk. When the robot starts working, it can be read from the hard disk all at once.

[0075] (5) Sensor grid atlas construction

[0076] Sonar measurements underwater are subject to significant uncertainty. If a reading indicates an obstacle 1 meter ahead, then obstacles may exist within a range of 0.8-1.2 meters ahead. Therefore, the uncertainty of sonar ranging needs to be considered during obstacle avoidance. Constructing a sensor grid map of the sonar in an offline step can improve the efficiency of online calculations.

[0077] The specific steps for creating a sensor grid atlas are as follows:

[0078] 5.a Initialize the raster map data structure and determine the sensor coordinate system. Each grid cell stores the probability of whether a location is an obstacle, i.e. The obstacle probability of each grid cell is initialized to 0.

[0079] 5.b Construct sensor grid maps with different ranging distances to form a sensor grid map atlas. Considering that obstacle detection does not require data from the full range of the sonar sensor, this invention limits the sensor range to... ,in To determine the robot's fastest movement speed, the range is divided into 100mm intervals, which can be further divided into... There are discrete distances, therefore the sensor grid atlas and the number of data points are... ,Right now The interval between divisions can also be adjusted according to the actual situation. For specific distance samples... This allows the construction of a corresponding sensor grid map. The construction of this grid map requires actual experimentation, specifically fixing the sonar sensor underwater and then using the sensor coordinate system as the center, with a radius ranging from... Small obstacles are placed within the designated area (the distance between the obstacles and the center of the sensor coordinate system is known; this distance can be measured using other high-precision sensors or fixed using mechanical devices, the dimensions of which determine the distance between the obstacles and the sensors). After placement, sensor measurements are collected, and statistical analysis is performed based on the (measured values ​​and actual values) data to construct a distance measurement system. The corresponding sensor grid map. Figure 5 The image shows two sensor grid maps with different ranging distances. The meaning is that when the sensor measurement value is 600mm or 1400mm (due to errors, the measurement is not entirely accurate), the probability of the obstacle's true location in each position in the sensor coordinate system is represented. The darker the area, the higher the probability. Therefore, it can be seen that when the sensor ranging value is specific, obstacles may also exist in the area near that value.

[0080] (6) Sensor data integration

[0081] This step is an online calculation step, the purpose of which is to collect the measurement values ​​of each sonar sensor. And index the sensor grid atlas based on the measured values. Sensor grid map in which This indicates that it is based on distance. The generated index is then used to determine the pose of each sensor coordinate system in the robot coordinate system. ,Will Transform to the robot coordinate system and determine the occupancy probability of the corresponding grid. Accumulation (e.g., the probabilities of two sonar heads having obstacles at the same location are respectively) and Then the probability that there is an obstacle at that location is Ultimately, this will create a sensor fusion grid map. (At this point, the coordinate system is switched to the robot coordinate system.) Furthermore, it is necessary to normalize the occupancy probability in the sensor fusion grid map.

[0082] (7) Collision detection.

[0083] This step is an online calculation step, which involves multiplying the probability values ​​of corresponding grid cells in each grid map from the sensor fusion grid map and the probabilistic footprint grid map set to obtain the probability of a collision for each footprint grid cell along each trajectory. Specifically, ,in This represents the product of the probabilities of corresponding points in the grid, and a threshold is further set. The result of multiplying the probabilities is greater than If a cell is found to be in a collision state, it is considered to have collided with another cell; otherwise, it is not. This allows for collision detection on the trajectory, thus providing the answer: given... The system identifies which tracks might collide with obstacles and the warning levels for collisions, allowing users to choose a safe track to drive on.

[0084] This invention considers the uncertainty of sensor measurements and the uncertainty of robot movement in water, and is applicable to robots of various shapes. The method has high real-time performance and can provide stronger support for the safe operation of underwater robots. "Considering the uncertainty of sensor measurements" is mainly reflected in the construction of sensor grid map in step (5), which uses sensor grid map to clearly model the uncertainty of sensor measurements. "Considering the uncertainty of robot movement in water" is mainly reflected in the generation of probabilistic footprint grid map in step (3), which uses probabilistic footprint grid map to clearly model the uncertainty of trajectory generated by a given control quantity. "Applicable to robots of various shapes" is mainly reflected in the generation of footprint grid map in step (2), which uses footprint grid map to model the footprint shape of robots of different shapes moving according to the trajectory. "High real-time performance" is mainly reflected in the fact that steps (1)-(5) are all offline calculation steps, and the calculation steps (6)-(7) are simple and highly parallel, which can be easily accelerated by GPU, thus achieving high real-time performance.

Claims

1. A method for detecting obstacles on the trajectory of an underwater robot, characterized in that, Includes the following steps: (1) Generation of feasible trajectory sets; (2) Footprint raster map generation Mark the footprints of each trajectory in the feasible trajectory set on the grid map, and record the warning level of each footprint position. Construct a footprint grid map for each trajectory. The warning level is set according to the reaction time of the robot to avoid the corresponding footprint position. (3) Generation of probabilistic footprint raster atlas Mark the areas that the underwater robot may actually pass through when moving along a certain trajectory on a grid map, and store the probability of the trajectory passing through each grid cell to construct a trajectory probability grid map. Then, based on the trajectory probability grid map Generate a corresponding footprint raster map, which is called a probabilistic footprint raster map. And ultimately obtain a probabilistic footprint raster atlas; (4) Saving of probabilistic footprint raster atlas; (5) Sensor grid atlas construction Given sensor measurements, determine the possible location of an obstacle in the sensor coordinate system, and mark the probability of an obstacle at each grid in the coordinate system. This map is called a sensor grid map, and the collection of sensor grid maps corresponding to different measurements is called a sensor grid map set. (6) Sensor data integration Collect measurement values ​​from various sensors on the robot. And index the corresponding sensor grid map based on the measured value. Based on the pose of each sensor coordinate system in the robot coordinate system ,Will Transform to the robot coordinate system and determine the occupancy probability of the corresponding grid. Accumulate them to form a sensor fusion grid map. And normalize the occupancy probability in the sensor fusion grid map; (7) Collision detection The probability value of the corresponding grid cell in each grid map of the sensor fusion grid map and the probabilistic footprint grid map set is multiplied to obtain the probability of collision for each footprint grid cell of each trajectory.

2. The obstacle detection method on the trajectory of an underwater robot according to claim 1, characterized in that, The specific steps for generating the feasible trajectory set in step (1) are as follows: Discretize the continuous trajectory. The robot's first Continuous trajectory The set of discretized sample points. Let i be the i-th sample point in this set, which includes three position coordinates x and x. i y i z i and three attitude coordinates row i pitch i yaw i Position represents the coordinates of the selected robot coordinate system origin in the specified coordinate system, and attitude represents the robot coordinate system's roll angle, pitch angle, and yaw angle in the specified coordinate system. The number of sample points in this set; the feasible trajectory set consists of a series of feasible trajectories, i.e. ,in This indicates the number of feasible trajectories in the feasible trajectory set.

3. The obstacle detection method on the trajectory of an underwater robot according to claim 2, characterized in that, Determined in the following manner: ,in, Represents two adjacent trajectory sample points and The time interval, N represents the time it takes for the robot to come to a complete stop from its maximum speed; N is a safety margin set for this purpose. The magnification factor.

4. The obstacle detection method on the trajectory of an underwater robot according to claim 3, characterized in that, Tra j Determined in the following manner: (1.a) Discretize the control variables of the robot to obtain a set of discrete control variables. , To control the number of quantities; (1.b) Obtain discrete trajectory points based on the control input and the robot's motion model f(). When the control quantity is When the discrete trajectory is obtained, it is: 。 5. The obstacle detection method on the trajectory of an underwater robot according to claim 4, characterized in that, The specific steps for generating the footprint raster atlas in step (2) are as follows: 2.a Construct the raster data structure and determine the raster coordinate system. Grid coordinate system Robot coordinate system coincide Each grid cell stores the following vector ,in For the corresponding raster coordinates The probability of finding footprints in the image. For the corresponding raster coordinates The warning level; the initial value for each grid is [value missing]. ; 2.b Construct a raster map of the j-th trajectory. First, obtain each sample point of the j-th trajectory. Then align the robot's shape to each sample point. At this point, if the coordinates are... Where the grid and the robot shape intersect, then the grid value at this point... A value of 1 is assigned to the grid cell on the robot's footprint along the j-th trajectory. If they do not intersect, the initial value is maintained. Furthermore, the time when a given trajectory sample point is found is used to assign a value to the grid cell covered by the robot's footprint as it reaches that sample point along the trajectory. value.

6. The obstacle detection method on the trajectory of an underwater robot according to claim 2 or 3, characterized in that, For the aforementioned reaction time, three levels are defined: Danger, Warning, and Safe, and are defined as follows: Reaction time within... Inside, the level is dangerous; Inside, the level is warning; The internal security level is Safe.

7. The obstacle detection method on the trajectory of an underwater robot according to claim 5, characterized in that, The specific steps for generating the probabilistic footprint raster atlas in step (3) are as follows: 3.a First, regarding the control quantity The trajectory probability raster map is thresholded for segmentation. Within each raster value, the probability falls within a set threshold. The above is reserved, less than or equal to The value is set to 0; 3.b Then, align the robot's shape to each non-zero grid cell in the trajectory probability grid map. For the robot's pose in the trajectory probability grid, set it as follows: obtain the coordinates of the aligned grid cells. Find the control quantity Corresponding trajectory Distance between sample points The nearest sample point is used to obtain its pose information, and this pose is used as a grid. Alignment posture; 3.c Set the intersection of the robot shape and trajectory probability grid map. Values, where raster coordinates The probability of footprints Set as the trajectory probability at the corresponding location, grid coordinates Alert level The approximate method is used to obtain the distance in the footprint raster map in step (2). The most recent footprint point, with the warning level of that point as... The value; 3.d Construct a probabilistic footprint raster map for each control variable to obtain a probabilistic footprint raster map set.

8. The obstacle detection method on the trajectory of an underwater robot according to claim 7, characterized in that, In step (7), a threshold is further set. The result of multiplying the probabilities is greater than If the grid cell is not found to be in a collision state, then the grid cell is considered to be in a collision state.

9. The obstacle detection method on the trajectory of an underwater robot according to claim 8, characterized in that, The detection method is set up in two stages: the first stage is the preprocessing stage and the second stage is the online processing stage. The preprocessing stage is the stage of calculation and storage before the robot works. Steps (1)-(5) are performed in the preprocessing stage, and steps (6) and (7) are performed in the online processing stage.

Images