Intelligent flow measuring robot path planning method and system based on ADCP

Abstract

This invention discloses a path planning method and system for an intelligent flow measurement robot based on ADCP. The planning method includes: S1 acquiring the ship's flow velocity and relative velocity measured by ADCP, the absolute motion velocity provided by the GPS module, and the distance and angle from the ship to the obstacle detected by the forward-looking obstacle avoidance sonar, and performing preprocessing; S2 constructing a multi-layer fusion cost map using the preprocessed data; S3 generating a global reference path for the ship based on the multi-layer fusion cost map; S4 determining whether there are dynamic obstacles at a preset distance in the global reference path. If so, proceed to S5; otherwise, proceed to S6; S5 sampling the current velocity to generate several candidate velocities, then generating candidate trajectories, controlling the ship to travel at the candidate velocity corresponding to the candidate path with the minimum cost value, and then returning to S4; S6 controlling the ship to continue traveling along the global reference path, and returning to S4 until the ship reaches the end of the global reference path before reaching the end of the global reference path.

Description

Technical Field

[0001] This invention relates to the field of hydrological monitoring technology, specifically to a path planning method and system for an intelligent flow measurement robot based on ADCP. Background Technology

[0002] Traditional hydrological flow measurement methods, such as manual cableways or manned vessels equipped with ADCPs, suffer from low efficiency, high risk, high cost, and inability to meet sudden emergency monitoring needs. In recent years, unmanned surface vessels (USVs) equipped with ADCPs have become a trend. However, existing flow measurement USVs mostly use preset routes or simple remote control methods, lacking sufficient intelligence. Specifically: Rigid path: Unable to dynamically adjust the course according to real-time flow field changes, which may lead to measurement failure or equipment damage in strong currents or complex waters; Inefficient: Unable to autonomously identify and prioritize the measurement of key sections with drastic changes in flow velocity (such as river narrowing and confluence), resulting in low measurement value density; Weak obstacle avoidance capability: Slow to react to dynamic obstacles on the water surface (such as floating objects and fishing boats), and safety depends on operator experience.

[0003] Existing path planning algorithms (such as AStar and traditional genetic algorithms) have limitations in hydrological flow measurement scenarios: AStar algorithm has difficulty handling dynamically changing flow velocities; traditional genetic algorithms are prone to premature convergence and the planned paths are not smooth, failing to meet ship kinematic constraints. Summary of the Invention

[0004] To address the aforementioned shortcomings in existing technologies, the present invention provides an intelligent flow measurement robot path planning method and system based on ADCP, which solves the problem of weak obstacle avoidance capability in existing path planning methods.

[0005] To achieve the above-mentioned objectives, the technical solution adopted by this invention is as follows: Firstly, a path planning method for an intelligent flow measurement robot based on ADCP is provided. The intelligent flow measurement robot is mounted on a ship's hull, and the method includes the following steps: S1. Obtain the hull flow velocity and relative velocity measured by the ADCP of the intelligent flow measurement robot, the absolute motion velocity provided by the GPS module, and the distance and angle from the hull to the obstacle detected by the forward-looking obstacle avoidance sonar, and perform preprocessing. S2. Using the preprocessed data, construct three layers with the same geographical range and raster resolution: a static obstacle layer, a dynamic flow rate cost layer, and a measurement value layer. Then, perform weighted fusion on the three layers to obtain a multi-layer fusion cost map. S3. Based on the multi-layer fusion cost map, the adaptive genetic simulated annealing algorithm is used for global path planning to generate the optimal current measurement path of the hull as the global reference path. S4. Receive real-time data from forward-looking obstacle avoidance sonar and visual sensors, and determine whether there are dynamic obstacles at a preset distance in the global reference path. If so, proceed to step S5; otherwise, proceed to step S6. S5. Sample the current speed to generate several candidate speeds, and generate a candidate trajectory for each candidate speed. Control the ship to travel at the candidate speed corresponding to the candidate path with the minimum cost value, and then return to step S4. S6. Control the ship to continue traveling the preset distance along the global reference path, and return to step S4 if the ship has not reached the end of the global reference path, until the end of the global reference path is reached.

[0006] In one possible implementation, step S2 further includes: S21. Generate three layers with the same geographic range and raster resolution: a static barrier layer, a dynamic flow cost layer, and a measurement value layer. S22. In the static obstacle layer, read the static obstacles in the prior map and assign the corresponding grid value to 255. Mark the dynamic ships detected by the forward-looking obstacle avoidance sonar in real time as 254 in the corresponding grid and restore the grid to 0 when the ship moves out. S23. In the dynamic velocity cost layer, the surface velocity vector field measured by ADCP is meshed, and the dynamic velocity cost of each grid is calculated based on the surface velocity corresponding to the grid and the angle between the ship's desired course and the direction of the current. in, The dynamic flow rate cost of the grid; The surface velocity measured by ADCP in the grid; The angle between the ship's desired course and the direction of the water current; For upstream and downstream cost coefficients; The risk coefficient for crossflow; S24. In the measurement value layer, each grid is assigned a measurement value based on the prior hydrological map or the main channel centerline, bend vertices and confluences preset by the task. S25. Weighted fusion of the static obstacle layer, dynamic flow velocity cost layer, and measurement value layer is performed to obtain a multi-layer fused cost map.

[0007] In one possible implementation, step S3 further includes: S31. Based on the multi-layer fusion cost map, a number of paths are randomly generated in free space using a segmented real number encoding method based on measurement sections to form an initial population. The paths represent the path points that need to be passed in sequence. S32. The fitness of each individual in the population is calculated using an evaluation function. The expression for the evaluation function is: in, R is the fitness value; L is the path length; E is the total energy consumption based on flow rate cost; R1 is the risk value of approaching obstacles and strong crossflow areas; R2 is the benefit of the path covering high-value measurement areas; , , and All are adjustable weights; S33. Dynamically adjust the crossover probability P based on the variance of population fitness. c With the probability of mutation P m The next generation population is generated by crossover and mutation of individuals selected based on fitness, based on crossover and mutation probabilities. S34. Update the annealing temperature and determine whether the current iteration has reached the termination condition. If so, output the global optimal solution as the optimal flow measurement path and use it as the global reference path for the flow measurement robot. Otherwise, return to step S32.

[0008] In one possible implementation, methods for calculating the total energy consumption E based on flow rate cost include: A1. For two adjacent path points in the path, determine the coordinates of their midpoint and obtain the surface velocity vector of the midpoint from the dynamic velocity cost layer using bilinear interpolation. A2. Determine the direction angle of the path segment based on the coordinates of the two path points, and determine the direction angle of the flow velocity based on the surface velocity vector at the midpoint. Use the absolute value of the difference between the direction angle of the path segment and the direction angle of the flow velocity as the direction angle of the ship in the water flow. A3. Calculate the velocity cost at the two path points based on the surface velocity vector at the midpoint and the hull's orientation angle in the current. : Where α and β are both empirical coefficients; The surface velocity vector at the midpoint; The azimuth angle of the ship in the current; To take the absolute value; A4. Calculate the total energy consumption of the path based on the flow rate cost of all adjacent path points: in, This represents the total number of path points on the path. and The first on the path and 1 path point; It is the Euclidean norm.

[0009] In one possible implementation, the expression for calculating the risk value near the obstacle and the strong crossflow region is: in, and All are weighting coefficients; path point Euclidean distance to the nearest obstacle in the static obstacle layer; The obstacle distance penalty function; This is the safe distance threshold; path point Surface velocity vector; path point The azimuth angle of the ship's hull in the current; This represents the total number of path points on the path.

[0010] In one possible implementation, the revenue from calculating the path coverage of high-value measurement areas is calculated. The expression is: in, To measure value layer The coordinates of the j-th grid cell; m is the total number of grid cells covered by the path; Let j be the measurement value of the j-th grid. Let be the path weight factor for the j-th grid. This is the reward coefficient.

[0011] In one possible implementation, the expressions for calculating the crossover probability and the mutation probability are as follows: , in, and These are the crossover probability and the mutation probability, respectively. Basic crossover probability; It is an exponential function; This represents the standard deviation of the current population fitness. The reference standard deviation; Basic mutation probability; It is a very small positive number.

[0012] In one possible implementation, step S5 further includes: S51. The data collected by the GPS module and the inertial measurement unit (IMU) are fused to obtain the heading angle, the speed u along the bow direction and the yaw rate r of the ship in the geodetic coordinate system. The current speed is constructed using u and r. Several candidate speeds are generated within the preset range of the current speed according to the preset sampling step size. S52. For each candidate velocity, a discrete kinematics model is used to predict the state sequence for the next N steps, forming a candidate path. The expression for the state sequence is: in, and Let i be the position coordinates at step i; Let be the heading angle at step i; and The coordinates of the position at step i-1; Let be the heading angle at step i-1; k is the time corresponding to the current speed; u′ and r′ constitute candidate speeds, which are the speed along the bow direction and the yaw rate, respectively; and This represents the absolute velocity component measured by ADCP. The discrete time step; S53. Calculate the cost of each candidate path using a multi-objective cost function: in, For multi-objective cost functions; Given the position coordinates and heading angle of the candidate path at step j, Depend on , and constitute; Given the position coordinates and heading angle at the j-th step on the global reference path, starting from the current time; for Distance to the obstacle; This is the safe distance threshold; , and All are cost weighting coefficients; The square of the Euclidean distance between the two points; S54. Based on the cost of all candidate paths, select the candidate speed of the candidate path with the minimum cost value, and control the ship to travel at the candidate speed, then return to step S4.

[0013] In one possible implementation, the preprocessing of the collected data in step S1 includes: The relative velocity is transformed from the ship's coordinate system to the geodetic coordinate system, and the transformed relative velocity is fused with the absolute motion velocity provided by the GPS module to obtain the absolute velocity vector in the geodetic coordinate system. The sonar detected by the forward-looking obstacle avoidance sonar is converted to the sonar rectangular coordinate system, then to the ship coordinate system to obtain the obstacle coordinates in the ship coordinate system, and then to the geodetic coordinate system to obtain the obstacle coordinates in the geodetic coordinate system. Based on the geodetic coordinates of the origin and the coordinates of the obstacle, the obstacle is mapped onto the global map grid.

[0014] Secondly, an intelligent flow measurement robot path planning system based on ADCP is provided, which includes an intelligent flow measurement robot arranged on the hull. The intelligent flow measurement robot includes an airborne computing module connected to an acoustic Doppler current profiler (ADCP), an inertial measurement unit (IMU), a global positioning system (GPS) module, a forward-looking obstacle avoidance sonar, and a visual sensor. The airborne computing module is used to execute the intelligent flow measurement robot path planning method based on ADCP.

[0015] Compared with the prior art, the present invention has the following beneficial effects: The multi-layered fusion cost map generated by this scheme takes ADCP velocity data into account. This allows the subsequent adaptive genetic simulated annealing algorithm to consider real-time flow field conditions during global path planning, thus avoiding the possibility of measurement failure or equipment damage in strong currents or complex waters. At the same time, when traveling according to the planned global reference path, this scheme also considers dynamic obstacles that may appear on the water surface at any time, and makes timely fine adjustments to the global reference path at the location of dynamic obstacles to achieve obstacle avoidance and ensure the safety of the travel process.

[0016] When constructing the dynamic flow velocity cost layer, this scheme obtains the surface flow velocity vector field in real time through ADCP, and establishes a cost function C_flow=α|v|cosθ+β|v|sinθ based on the flow velocity magnitude |v| and the angle θ between the robot's expected heading and the flow direction. This function simultaneously quantifies the energy consumption of upstream navigation (the first term) and the yaw risk caused by crossflow (the second term), enabling path planning to actively avoid strong upstream and crossflow areas, thereby significantly reducing the energy consumption of the ship during navigation.

[0017] When generating the global reference path, this scheme considers energy consumption, obstacles, and strong crossflow regions when calculating fitness using an evaluation function. This helps generated individuals avoid obstacles and strong crossflow regions as much as possible, thus reducing the possibility of path measurement failure or equipment damage. During population generation, this scheme dynamically adjusts the crossover probability Pc and mutation probability Pm using the population fitness standard deviation σ. When population diversity decreases, Pm is automatically increased to introduce new genes and avoid premature convergence. When the population diverges, Pc is decreased to protect superior individuals, thereby improving convergence speed and thus increasing the efficiency of path planning.

[0018] This scheme constructs a multi-objective cost function that integrates path tracking error, control energy, and obstacle distance into the optimization framework, S ref,k+j The global path derived from AGA-SA, which integrates the measurement value layer and the flow rate cost layer, enables local planning to follow not only geometry but also task; the d in the cost function obs (·) This includes not only physical obstacles detected by forward-looking sonar, but also cost layers M through dynamic flow velocity. flow Mapping these obstacles as "virtual obstacles" enables the robot to proactively avoid areas of strong crossflow, achieving dual obstacle avoidance against both physical and hydrological environments. The optimal control command is re-solved in each control cycle, allowing the intelligent flow measurement robot to respond in real-time to dynamic obstacles and changing flow fields.

[0019] This scheme constructs a measurement value layer, assigning differentiated measurement values ​​to different water areas based on prior hydrological knowledge (such as the centerline of the main channel, the apex of a bend, and the confluence). The evaluation function guides the path to actively cover high-value areas through the benefit term R2, enabling the intelligent flow measurement robot to autonomously identify and prioritize the measurement of key sections with rich hydrological information. In this way, more effective hydrological data can be obtained under the same path length, which can significantly improve the spatial representativeness of the measurement data. Attached Figure Description

[0020] Figure 1 This is a flowchart of a path planning method for an intelligent flow measurement robot based on ADCP.

[0021] Figure 2 This is a framework diagram of the trajectory optimization algorithm in the obstacle avoidance process of dynamic obstacles in this scheme.

[0022] Figure 3 This is a comparison diagram of the path planned by the intelligent flow measurement robot in a simulated river channel according to the method of this scheme and the actual navigation trajectory. Detailed Implementation

[0023] The specific embodiments of the present invention are described below to enable those skilled in the art to understand the present invention. However, it should be understood that the present invention is not limited to the scope of the specific embodiments. For those skilled in the art, various changes are obvious as long as they are within the spirit and scope of the present invention as defined and determined by the appended claims. All inventions utilizing the concept of the present invention are protected.

[0024] refer to Figure 1 , Figure 1 The flowchart illustrates a path planning method for an intelligent flow measurement robot based on ADCP. In this scheme, the intelligent flow measurement robot is mounted on the ship's hull. Figure 1 As shown, the method S provided in this solution includes steps S1 to S6.

[0025] In step S1, the flow velocity and relative velocity of the ship measured by the ADCP of the intelligent flow measurement robot, the absolute motion velocity provided by the GPS module, and the distance and angle from the ship to the obstacle detected by the forward-looking obstacle avoidance sonar are obtained and preprocessed. Transforming the relative velocity from the ship's coordinate system to the geodetic coordinate system, we obtain the relative velocity components in the geodetic coordinate system as follows: , Among them, (u rel v rel () represents the relative velocity; ψ represents the heading angle; It is a rotation matrix; , These represent the northward and eastward components of the velocity vector of water relative to the ship in the geodetic coordinate system, in m / s.

[0026] and The expression after temporary opening is: , .

[0027] The converted relative velocity is then fused with the absolute velocity provided by the GPS module to obtain the absolute velocity vector converted to the geodetic coordinate system. : , , in, The absolute velocity is obtained by fusing the converted relative velocity with the absolute motion velocity provided by the GPS module, i.e., the absolute flow velocity in the geodetic coordinate system, in m / s; and The absolute velocity provided to the GPS module, i.e., the velocity of the ship relative to the ground (in geodetic coordinates), in m / s.

[0028] Transform the sonar detected by the forward-looking obstacle avoidance sonar into a sonar Cartesian coordinate system: , in, and d represents the coordinates obtained after transforming the distance to the obstacle detected by the forward-looking obstacle avoidance sonar; d is the distance to the obstacle detected by the forward-looking obstacle avoidance sonar, in meters. This is the horizontal azimuth angle relative to the sonar centerline.

[0029] Next and Transform to the ship's coordinate system to obtain the obstacle coordinates in the ship's coordinate system. , ): in, and This indicates the installation location of the forward-looking obstacle avoidance sonar on the hull. The fixed installation angle α between the sonar coordinate system and the ship's coordinate system (i.e., the angle between the sonar centerline and the ship's centerline, usually 0°).

[0030] Then set the obstacle coordinates ( , Transform to the geodetic coordinate system to obtain the obstacle coordinates in the geodetic coordinate system. , ): Expand: Where (X,Y) are the geodetic coordinates of the ship's center of gravity; ψ is the heading angle, with north-east as positive.

[0031] Based on the geodetic coordinates of the origin and the coordinates of the obstacles, the obstacles are mapped onto the global map grid: in: r is the index of the grid cell containing the obstacle; r is the grid resolution, in meters per grid cell.

[0032] In step S2, three layers with the same geographical range and raster resolution—a static obstacle layer, a dynamic flow cost layer, and a measurement value layer—are constructed using the preprocessed data. The three layers are then weighted and fused to obtain a multi-layer fused cost map. In one embodiment of the present invention, step S2 further includes: S21. Generate three layers with the same geographic range (e.g., 100m×100m) and raster resolution (e.g., 1m×1m): a static barrier layer, a dynamic flow cost layer, and a measurement value layer. S22. In the static obstacle layer, read the static obstacles in the prior map (the map pre-imported by the system), such as shoreline, shoal, and reef, and assign the corresponding grid value to 255. Mark the dynamic ships detected in real time by the forward-looking obstacle avoidance sonar as 254 in the corresponding grid, and restore the grid to 0 when the ship moves out. S23. In the dynamic velocity cost layer, the surface velocity vector field measured by ADCP is meshed, and the dynamic velocity cost of each grid is calculated based on the surface velocity corresponding to the grid and the angle between the ship's desired course and the direction of the current. in, The dynamic flow rate cost of the grid; The surface velocity measured by ADCP in the grid; The angle between the ship's desired course and the direction of the water current; The upstream and downstream cost coefficient reflects the impact of water flow along the course on energy consumption (positive for cost, negative for benefit), 0.8; The crossflow risk coefficient reflects the impact of lateral water flow on maneuvering stability (positive is cost), 0.5; the first term α∣v∣cosθ: high cost in counterflow (θ≈π), low cost in downstream flow (θ≈0); the second term β∣v∣sinθ: high cost in crossflow (θ≈π / 2), zero when there is no crossflow.

[0033] S24. In the measurement value layer, each grid is assigned a measurement value based on the prior hydrological map (the hydrological map pre-imported by the system) or the main channel centerline, bend vertices and confluences preset by the task. S25. Weighted fusion of the static obstacle layer, dynamic flow velocity cost layer, and measurement value layer yields a multi-layered fused cost map. The expression for the weighted fusion is: Among them, W obs W flow W value These are the fusion weights; This is the final comprehensive cost value of the grid coordinates (x, y) in the geodetic coordinate system, which is directly used by subsequent path planners (such as AGA-SA); This represents the obstacle occupancy value of the static obstacle layer at grid (x, y); This represents the flow cost value of the dynamic flow cost layer at grid (x, y). To measure the value of the value layer at grid (x, y), the value range is usually 0 to 100. The higher the value, the greater the hydrological measurement value at that location (such as the centerline of the main channel, the apex of a bend, the confluence, etc.).

[0034] In step S3, based on the multi-layer fusion cost map, the adaptive genetic simulated annealing algorithm is used for global path planning to generate the optimal current measurement path of the hull as the global reference path. In one embodiment of the present invention, step S3 further includes: S31. Based on the multi-layer fusion cost map, a number of paths are randomly generated in free space using a segmented real number encoding method based on measurement sections to form an initial population. The paths represent the path points that need to be passed in sequence. S32. The fitness of each individual in the population is calculated using an evaluation function. The expression for the evaluation function is: in, R is the fitness value; L is the path length; E is the total energy consumption based on flow rate cost; R1 is the risk value of approaching obstacles and strong crossflow areas; R2 is the benefit of the path covering high-value measurement areas; , , and All weights are adjustable; W1-W4 are dynamically adjusted according to the task stage. For example: On the way to the flow measurement section: W1=0.4, W2=0.4, W3=0.2, W4=0.0 (ignore the benefits); During the formal flow measurement phase: W1=0.2, W2=0.2, W3=0.2, W4=0.4 (emphasize coverage of high-value areas); Emergency obstacle avoidance mode: temporarily increase W3.

[0035] In implementation, the preferred method for calculating the total energy consumption E based on flow rate cost in this scheme includes: A1. For two adjacent path points P in the path i and P i+1 Determine the coordinates of the midpoint between the two. , ( , ), ( , () are path points P i and P i+1 The coordinates are determined, and the surface velocity vector at the midpoint is obtained from the dynamic velocity cost layer using bilinear interpolation. .

[0036] A2. Determine the direction angle of the path segment based on the coordinates of the two path points. The velocity direction angle is determined based on the surface velocity vector at the midpoint. The absolute value of the difference between the path segment direction angle and the flow velocity direction angle is used. As the azimuth angle of the ship in the water flow.

[0037] A3. Calculate the velocity cost at the two path points based on the surface velocity vector at the midpoint and the hull's orientation angle in the current. : Where α and β are both empirical coefficients (e.g., α=0.8, β=0.5); The surface velocity vector at the midpoint; The azimuth angle of the ship in the current; To take the absolute value.

[0038] A4. Calculate the total energy consumption of the path based on the flow rate cost of all adjacent path points: in, This represents the total number of path points on the path. and The first on the path and 1 path point; It is the Euclidean norm.

[0039] The expression for calculating the risk value of approaching obstacles and strong crossflow areas is: in, and All are weighting coefficients. , ; path point Euclidean distance to the nearest obstacle in the static obstacle layer; The obstacle distance penalty function; The preferred safe distance threshold is 2m; path point Surface velocity vector; path point The azimuth angle of the ship's hull in the current; This represents the total number of path points on the path.

[0040] Calculate the revenue from covering high-value measurement areas. The expression is: in, To measure value layer The coordinates of the j-th grid cell; m is the total number of grid cells covered by the path; Let j be the measurement value of the j-th grid. Let be the path weight factor for the j-th grid. The reward coefficient is preferably set to 0.5. The larger the value, the more high-value measurement areas the path passes through, the more abundant the hydrological data can be obtained, and therefore the higher the reward.

[0041] S33. Dynamically adjust the crossover probability P based on the variance of population fitness. c With the probability of mutation P m The next generation population is generated by crossover and mutation of individuals selected based on fitness, based on crossover and mutation probabilities. In implementation, this scheme preferably uses the following expressions to calculate the crossover probability and mutation probability: , in, and These are the crossover probability and the mutation probability, respectively. Basic crossover probability; It is an exponential function; This represents the standard deviation of the current population fitness. The reference standard deviation; Basic mutation probability; It is a very small positive number; when σ is very small (poor population diversity), σ ref / σ is very large, thus significantly improving P m To escape local optima, random perturbations are introduced; when σ is large, P m Approaching P m_base To avoid damaging excellent individuals.

[0042] S34, Update annealing temperature Where γ is the cooling coefficient, 0.95; k is the number of iterations; The initial temperature T0 is the annealing temperature at the k-th iteration. The initial temperature T0 is set according to the initial population fitness variance (e.g., T0=100). The higher the temperature, the greater the probability of accepting inferior solutions, which is beneficial for global search; as the temperature gradually decreases, the algorithm tends to converge.

[0043] Determine if the current iteration has reached the termination condition. If so, output the globally optimal solution as the optimal flow measurement path and use it as the global reference path for the flow measurement robot; otherwise, return to step S32. Preferably, the algorithm terminates when the number of iterations reaches 200, or the optimal solution has not improved for 50 consecutive iterations, and the current optimal path is output as the global reference path P. global .

[0044] In step S4, real-time data from forward-looking obstacle avoidance sonar and visual sensors are received, and it is determined whether there are dynamic obstacles at a preset distance in the global reference path. If so, proceed to step S5; otherwise, proceed to step S6. In step S5, the current speed is sampled to generate several candidate speeds, and a candidate trajectory for each candidate speed is generated. The ship is controlled to travel at the candidate speed corresponding to the candidate path with the minimum cost value, and then the process returns to step S4. In step S6, the ship is controlled to continue traveling a preset distance along the global reference path, and if the ship does not reach the end of the global reference path, it returns to step S4 until it reaches the end of the global reference path.

[0045] like Figure 2 As shown, in one embodiment of the present invention, step S5 further includes: S51. The data collected by the GPS module and the inertial measurement unit (IMU) are fused to obtain the heading angle, the velocity u along the bow direction and the yaw rate r of the ship in the geodetic coordinate system. The current velocity is constructed using u and r. Based on the robot dynamics constraints (maximum acceleration and maximum velocity), several candidate velocities are generated within the preset range of the current velocity according to the preset sampling step size. S52. For each candidate velocity, a discrete kinematics model is used to predict the state sequence for the next N steps (each step Δt = 0.1 s) to form a candidate path. The expression for the state sequence is: in, and Let i be the position coordinates at step i; Let be the heading angle at step i; and The coordinates of the position at step i-1; Let be the heading angle at step i-1; k is the time corresponding to the current speed; u′ and r′ constitute candidate speeds, which are the speed along the bow direction and the yaw rate, respectively; and This represents the absolute velocity component measured by ADCP. The discrete time step; S53. Calculate the cost of each candidate path using a multi-objective cost function: in, For multi-objective cost functions; Given the position coordinates and heading angle of the candidate path at step j, Depend on , and constitute; Given the position coordinates and heading angle at the j-th step on the global reference path, starting from the current time; for Distance to the obstacle; This is the safe distance threshold; , and These are all cost weighting coefficients, which can be dynamically adjusted according to the task stage (e.g., increase W1 when measuring flow, and increase W3 when avoiding obstacles). It is the square of the Euclidean distance between the two points.

[0046] S54. Based on the cost of all candidate paths, select the candidate speed of the candidate path with the minimum cost value (i.e., MPC trajectory rolling optimization), so that the robot achieves optimal performance in terms of tracking the global path, avoiding obstacles, resisting water flow disturbances, and saving energy. Control the ship to travel at the candidate speed (specifically, convert the candidate speed into thruster speed and rudder angle commands), and then return to step S4, or return to step S4 in the next control cycle (e.g., after 0.1 s).

[0047] This solution also provides an intelligent flow measurement robot path planning system based on ADCP, which includes an intelligent flow measurement robot deployed on the hull. The intelligent flow measurement robot includes an airborne computing module connected to an acoustic Doppler current profiler (ADCP), an inertial measurement unit (IMU), a global positioning system (GPS) module, a forward-looking obstacle avoidance sonar, and a visual sensor. The airborne computing module is used to execute the ADCP-based intelligent flow measurement robot path planning method.

[0048] The following section uses the ADCP-based intelligent flow measurement robot path planning method provided in this solution to simulate a river scenario in order to verify its performance.

[0049] Hardware platform: In this embodiment, the hull carrying the intelligent flow measurement robot is a catamaran unmanned surface vessel (USV) with the following main parameters: length 1.0 meter, width 0.5 meters, and good stability.

[0050] Equipped with sensors and computing units: Acoustic Doppler Current Profiler (ADC): A 600 kHz downward-looking acoustic Doppler current profiler integrated at the center of the ship's bottom, with a sampling frequency of 1 Hz, capable of acquiring surface current profiles at a depth of 0.5 m, and outputting the relative velocity component u in the ship's coordinate system. rel v rel .

[0051] Inertial Measurement Unit (IMU): A six-degree-of-freedom inertial measurement unit that provides yaw angle ψ and roll angle. Pitch angle θ and triaxial angular velocity.

[0052] Global Positioning System (GPS) module: Multi-satellite dual-frequency RTK-GPS, horizontal positioning accuracy ±0.02 m, velocity accuracy ±0.03 m / s, combined with IMU to output ship hull pose (X, Y, ψ) and ground velocity (v) in geodetic coordinate system. b,N ,v b,E ).

[0053] Forward-looking obstacle avoidance sonar: detection range 50 m, horizontal opening angle 120°, angular resolution 1°, distance resolution 0.1 m, outputs obstacle polar coordinates. .

[0054] Visual sensor: Monocular color camera, used to assist in identifying dynamic targets on the water surface.

[0055] Onboard computing unit module: Intel NUC industrial embedded computer, 2.8 GHz, 16 GB memory, running Ubuntu + ROS system, all planning algorithms are implemented in C++, control cycle 0.1 s.

[0056] The specific implementation steps of the path planning method for intelligent flow measurement robots are as follows: Step S1: Construction of Multi-Layer Fusion Cost Map S1.1 Data Preprocessing and Spatiotemporal Registration Time synchronization: The high-precision clock of IMU (3) serves as the master clock, and a unified time t is assigned to each frame of data from ADCP (2), GPS (4), forward-looking sonar (5), and vision sensor (6).

[0057] Spatial unification -- absolute velocity conversion: Let u be measured at the current time t. rel =0.5 m / s, u rel =0.5m / s, v rel =0.1 m / s, v rel =0.1m / s, the ship's velocity relative to the ground is v b,N =2.0 m / s, v b,N=2.0m / s, v b,E =1.0 m / s, v b,E =1.0 m / s, heading angle ψ=30°. Calculate the relative velocity components in the geodetic coordinate system: v w / b,N =u rel cosψ-v rel sinψ=0.5×0.8660-0.1×0.5=0.3830 m / s, v w / b,E =u rel sinψ+v rel cosψ=0.5×0.5+0.1×0.8660=0.3366 m / s, Synthetic absolute flow rate: v w,N =v b,N +v w / b,N =2.0 + 0.3830 = 2.3830 m / s v w,E =v b,E +v w / b,E =1.0 + 0.3366 = 1.3366 m / s Obtain the absolute velocity vector =(2.3830, 1.3366) m / s.

[0058] Spatial unification -- sonar obstacle grid mapping: Suppose a sonar detects an obstacle with d = 10 m. (Port side is positive), sonar installation position x sonar =0.3 m, y sonar =0 m, installation angle α=0. Sonar rectangular coordinates: x s =10cos15∘=9.659 m, y s =10sin15∘=2.588 m. Ship coordinates: x b =0.3 + 9.659 = 9.959 m, y b =0 + 2.588 = 2.588 m. Let the current geodetic coordinates of the ship be X = 50 m, Y = 30 m, and ψ = 30, then: X obs =50+9.959cos30 ∘ -2.588sin30 ∘ =50+8.625-1.294=57.331m; Y obs =30+9.959sin30 ∘ +2.588cos30 ∘=30+4.980+2.241=37.221 m; Given a map origin (X0, Y0) = (0, 0) and a resolution r = 1 m / grid, then the grid index is: col= =57, row= =37.

[0059] S1.2 Map Initialization and Layer Definition Three 100×100 grid layers with a resolution of 1 m are allocated in the onboard computing unit memory, corresponding to static obstacle layers M respectively. obs Dynamic flow rate cost layer M flow Measurement value layer M value The initial values ​​are all 0.

[0060] S1.3 Each information layer is constructed independently. Static obstacle layer: Load the prior electronic navigation map and assign the corresponding grid values ​​of shoreline, bridge pier, etc. to 255; the dynamic vessels detected in real time by the forward-looking obstacle avoidance sonar are temporarily marked as 254, and restored to 0 after being removed from the field of view.

[0061] Dynamic flow rate cost layer: according to formula C flow (θ, |v|) = α |v|cosθ + β |v|sinθ calculates the flow velocity cost for each grid cell. Let α = 0.8 and β = 0.5. If the flow velocity in a certain grid cell is |v| = 0.8 m / s, and the robot's desired angle between its heading and the flow direction is θ = 120° (counter-current skew), then: C flow =0.8×0.8×cos120∘+0.5×0.8×sin120 ∘ =-0.4+0.554=0.154; Measurement Value Layer: Based on the preset parameters of the flow measurement task, the grid near the centerline of the main channel is assigned a value of 90, the area at the apex of the curve is assigned a value of 100, and the nearshore area is assigned a value of 10.

[0062] S1.4 Multi-layer weighted fusion Take weight w obs =100, w flow =1.0, w value =-0.5 (negative values ​​represent gains), then the final cost is: M final (x,y)=100M obs (x,y)+1.0M flow (x,y)-0.5M value The (x,y) obstacle grid (255) results in M final Extremely large; the path planner will automatically avoid it.

[0063] Step S2: AGA-SA Global Path Planning S2.1 Encoding and Initialization The starting point is P1=(10,10), the ending point is Pn=(90,90), the population size is N=50, and each individual is a sequence of 20 waypoints, randomly generated in free space.

[0064] S2.2 Evaluation Function We assign weights W1=0.3, W2=0.3, W3=0.2, and W4=0.2. For each individual, calculate the following sequentially: Path length L: Sum of Euclidean distances between adjacent waypoints; Energy consumption estimate E: Calculated by taking the midpoint velocity of each segment and then calculating C. flow Then multiply by the segment length and sum; Risk value R1: take λ1=1.0, λ2=0.5, obstacle penalty Penalty(d)=max(0,2-d) 2 Lateral flow velocity v side = |v|sinθ; Profit value R2: All grid cells M traversed by the path value Accumulate.

[0065] S2.3 Adaptive Genetic Operations Crossover probability: Pc = 0.8·exp(-σ / 10); Mutation probability: Pm = 0.05·(1+10 / (σ+0.001)), where σ is the standard deviation of population fitness; Selection: Tournament selection (k=3); Crossover: Segmented arithmetic crossover; Mutation: Gaussian mutation (standard deviation 0.5 m).

[0066] S2.4 Simulated Annealing Receiver The initial temperature T0 = 100°C, the cooling coefficient γ = 0.95, and the temperature is reduced once every 10 generations. For new individuals, if the fitness difference ΔF < 0, inferior solutions are accepted with probability exp(-ΔF / T).

[0067] S2.5 Termination Condition If the optimal solution shows no improvement after 200 iterations or 50 consecutive iterations, the optimal path is output as the global reference path.

[0068] Step S3: Local MPC-DWA Real-time Planning and Control S3.1 State Awareness Read the following data per control cycle (0.1 s): pose (x, y, ψ), velocity (u, r), ADCP local flow velocity. Sonar obstacle range field d obs And obtain the lookahead point 5 m away from the current position on the global path as the reference pose s ref .

[0069] S3.2 Candidate Trajectory Generation Velocity space sampling: u′∈[0,1.2] m / s, step size 0.1 m / s; r′∈[-0.8,0.8] rad / s, step size 0.05 rad / s. For each group (u′,r′), predict the trajectory for the next 20 steps (Δt=0.1 s) using a kinematic model with water flow: x k+1 =x k +(u′cosψk+v w,x )Δt;y k+1 =y k +(u′sinψk+v w,y )Δt;ψ k+1 =ψ k +r′Δt S3.3 Optimal Control Selection The cost function J takes the values ​​W1=1, W2=0.1, W3=10, and d safe =1.5 m. Calculate J for each candidate trajectory and select the (u∗,r∗) corresponding to the minimum value.

[0070] S3.4 Command Output Send (u∗,r∗) to the thruster and servo motor, and repeat steps S3.1~S3.4 in the next cycle.

[0071] Effect verification: In the simulated winding river scenario (see...) Figure 3 Traditional S-shaped routes are typical navigation paths for current measurement sections, reciprocating in an "S" shape. However, they are easily affected by currents and deviate from the course when passing through areas with strong crossflows, and they do not provide sufficient coverage of high-value areas (such as bend apex and main channel centerline). The intelligent planned route, generated by the AGA-SA algorithm of this invention, can proactively increase the density of waypoints in high-value areas (more concentrated sampling) and provide advance deviation compensation in areas with strong crossflows, making the actual trajectory more closely match the planned survey line.

[0072] exist Figure 3 The starting and ending points are both (100, 100), indicating that the starting and ending points of the task are the same, and the comparison focuses on the differences in the intermediate navigation process. Compared with the traditional measurement scheme that uses a pre-set "S" shaped path, the path planned by the method proposed in this invention exhibits the following advantages: Quantization performance comparison: Spatial representativeness (+25%): By actively covering high-value areas, the intelligent path makes the collected data more representative of the true flow field structure of the entire cross section.

[0073] Spatial representativeness = (Reconstruction accuracy of this invention - Reconstruction accuracy of traditional methods) / Reconstruction accuracy of traditional methods × 100% In this embodiment, a high-precision real velocity field is first established in the river channel using single-point measurements. Surface velocities at each point are collected using both a traditional "S"-shaped path and the intelligent planning path of this invention. Then, Kriging interpolation is used to reconstruct the cross-sectional flow field from the sampling points of both paths, and the root mean square error (RMSE) between the reconstructed and real flow fields is calculated. The average deviation between the reconstructed and real flow fields of this invention is 20% lower than that of traditional methods, which translates to an improvement in representative accuracy of approximately 25%.

[0074] Operation time (-15%): Due to the pre-emptive offsetting of flow pressure and efficient planning of the path, the time spent on ineffective navigation and correction is reduced, and the overall mission is completed faster.

[0075] Operation time reduction rate = (Traditional time - Invention time) / Traditional time × 100% In the examples, the conventional method takes about 120 seconds, while the present invention takes about 102 seconds, a reduction of 15%.

[0076] (3) Waypoint utilization efficiency (+60%): Densify measurements in key areas and sparsely pass through flat areas to maximize the data value of each waypoint.

[0077] Waypoint utilization efficiency = Total survey value / Total number of waypoints (or total length) of the route In the embodiments, the total measurement value obtained by the present invention under the same path length is 60% higher than that of the conventional method; or conversely, the number of waypoints required to achieve the same measurement value is reduced by 60%.

[0078] (4) Current resistance stability: The intelligent path maintains higher trajectory stability in actual navigation through pre-compensation design.

[0079] Path behavior comparison: (1) Curve behavior: Traditional paths "pass through" high-value areas, while intelligent paths actively increase waypoints to achieve refined measurement.

[0080] (2) Crossflow behavior: Under the influence of crossflow, the actual trajectory of the traditional path deviates significantly from the planned measurement line, resulting in data failure. The intelligent path deviates upstream in advance, so that the actual trajectory falls exactly on the planned measurement line after the flow pressure is canceled out.

[0081] (3) Obstacle avoidance behavior: When a dynamic obstacle appears, the system (MPC-DWA) generates a smooth obstacle avoidance trajectory within 3 seconds and guides the robot back to the global path within 10 seconds after avoiding the obstacle. The measurement interruption time is extremely short and the task continuity is high.

Claims

1. A path planning method for an intelligent flow measurement robot based on ADCP, characterized in that, The intelligent flow measurement robot is mounted on the ship's hull, and the method includes the following steps: S1. Obtain the hull flow velocity and relative velocity measured by the ADCP of the intelligent flow measurement robot, the absolute motion velocity provided by the GPS module, and the distance and angle from the hull to the obstacle detected by the forward-looking obstacle avoidance sonar, and perform preprocessing. S2. Using the preprocessed data, construct three layers with the same geographical range and raster resolution: a static obstacle layer, a dynamic flow rate cost layer, and a measurement value layer. Then, perform weighted fusion on the three layers to obtain a multi-layer fused cost map. S3. Based on the multi-layer fusion cost map, the adaptive genetic simulated annealing algorithm is used for global path planning to generate the optimal current measurement path of the hull as the global reference path. S4. Receive real-time data from forward-looking obstacle avoidance sonar and visual sensors, and determine whether there are dynamic obstacles at a preset distance in the global reference path. If so, proceed to step S5; otherwise, proceed to step S6. S5. Sample the current speed to generate several candidate speeds, and generate a candidate trajectory for each candidate speed. Control the ship to travel at the candidate speed corresponding to the candidate path with the minimum cost value, and then return to step S4. S6. Control the ship to continue traveling the preset distance along the global reference path, and return to step S4 if the ship has not reached the end of the global reference path until it reaches the end of the global reference path. Step S2 further includes: S21. Generate three layers with the same geographic range and raster resolution: a static barrier layer, a dynamic flow cost layer, and a measurement value layer. S22. In the static obstacle layer, read the static obstacles in the prior map and assign the corresponding grid value to 255. Mark the dynamic ships detected by the forward-looking obstacle avoidance sonar in real time as 254 in the corresponding grid and restore the grid to 0 when the ship moves out. S23. In the dynamic velocity cost layer, the surface velocity vector field measured by ADCP is meshed, and the dynamic velocity cost of each grid is calculated based on the surface velocity corresponding to the grid and the angle between the ship's desired heading and the direction of the water flow. in, The dynamic flow rate cost of the grid; The surface velocity measured by ADCP in the grid; The angle between the ship's desired course and the direction of the water current; For upstream and downstream cost coefficients; The risk coefficient for crossflow; S24. In the measurement value layer, each grid is assigned a measurement value based on the prior hydrological map or the main channel centerline, bend vertices and confluences preset by the task. S25. Weighted fusion of the static obstacle layer, dynamic flow velocity cost layer, and measurement value layer is performed to obtain a multi-layer fused cost map.

2. The path planning method for an intelligent flow measurement robot based on ADCP according to claim 1, characterized in that, Step S3 further includes: S31. Based on the multi-layer fusion cost map, a number of paths are randomly generated in free space using a segmented real number encoding method based on measurement sections to form an initial population. The paths represent the path points that need to be passed in sequence. S32. The fitness of each individual in the population is calculated using an evaluation function. The expression for the evaluation function is: in, R is the fitness value; L is the path length; E is the total energy consumption based on flow rate cost; R1 is the risk value of approaching obstacles and strong crossflow areas; R2 is the benefit of the path covering high-value measurement areas; , , and All are adjustable weights; S33. Dynamically adjust the crossover probability P based on the variance of population fitness. c With the probability of mutation P m The next generation population is generated by crossover and mutation of individuals selected based on fitness, based on crossover and mutation probabilities. S34. Update the annealing temperature and determine whether the current iteration has reached the termination condition. If so, output the global optimal solution as the optimal flow measurement path and use it as the global reference path for the flow measurement robot. Otherwise, return to step S32.

3. The path planning method for an intelligent flow measurement robot based on ADCP according to claim 2, characterized in that, Methods for calculating total energy consumption E based on flow rate cost include: A1. For two adjacent path points in the path, determine the coordinates of their midpoint and obtain the surface velocity vector of the midpoint from the dynamic velocity cost layer using bilinear interpolation. A2. Determine the direction angle of the path segment based on the coordinates of the two path points, and determine the direction angle of the flow velocity based on the surface velocity vector at the midpoint. Use the absolute value of the difference between the direction angle of the path segment and the direction angle of the flow velocity as the direction angle of the ship in the water flow. A3. Calculate the velocity cost at the two path points based on the surface velocity vector at the midpoint and the hull's orientation angle in the current. : in, For upstream and downstream cost coefficients; The risk coefficient for crossflow; The surface velocity vector at the midpoint; The azimuth angle of the ship in the water flow; To take the absolute value; A4. Calculate the total energy consumption of the path based on the flow rate cost of all adjacent path points: in, This represents the total number of path points on the path. and The first on the path and 1 path point; It is the Euclidean norm.

4. The path planning method for an intelligent flow measurement robot based on ADCP according to claim 2, characterized in that, The expression for calculating the risk value of approaching obstacles and strong crossflow areas is: in, and All are weighting coefficients; path point Euclidean distance to the nearest obstacle in the static obstacle layer; The obstacle distance penalty function; This is the safe distance threshold; path point Surface velocity vector; path point The azimuth angle of the ship's hull in the current; This represents the total number of path points on the path.

5. The path planning method for an intelligent flow measurement robot based on ADCP according to claim 2, characterized in that, Calculate the revenue from covering high-value measurement areas. The expression is: in, To measure value layer The coordinates of the j-th grid cell; m is the total number of grid cells covered by the path; Let j be the measurement value of the j-th grid. Let be the path weight factor for the j-th grid. This is the reward coefficient.

6. The path planning method for an intelligent flow measurement robot based on ADCP according to claim 2, characterized in that, The expressions for calculating the crossover probability and the mutation probability are as follows: ， in, and These are the crossover probability and the mutation probability, respectively. Basic crossover probability; It is an exponential function; This represents the standard deviation of the current population fitness. The reference standard deviation; Basic mutation probability; It is a very small positive number.

7. The path planning method for an intelligent flow measurement robot based on ADCP according to claim 1, characterized in that, Step S5 further includes: S51. The data collected by the GPS module and the inertial measurement unit (IMU) are fused to obtain the heading angle, the speed u along the bow direction and the yaw rate r of the ship in the geodetic coordinate system. The current speed is constructed using u and r. Several candidate speeds are generated within the preset range of the current speed according to the preset sampling step size. S52. For each candidate velocity, a discrete kinematics model is used to predict the state sequence for the next N steps, forming a candidate path. The expression for the state sequence is: in, and Let i be the position coordinates at step i; Let be the heading angle at step i; and The coordinates of the position at step i-1; Let be the heading angle at step i-1; k is the time corresponding to the current speed; u′ and r′ constitute candidate speeds, which are the speed along the bow direction and the yaw rate, respectively; and This represents the absolute velocity component measured by ADCP. The discrete time step; S53. Calculate the cost of each candidate path using a multi-objective cost function: in, For multi-objective cost functions; Given the position coordinates and heading angle of the candidate path at step j, Depend on , and constitute; Given the position coordinates and heading angle at the j-th step on the global reference path, starting from the current time; for Distance to the obstacle; This is the safe distance threshold; , and All are cost weighting coefficients; The square of the Euclidean distance between the two points; S54. Based on the cost of all candidate paths, select the candidate speed of the candidate path with the minimum cost value, and control the ship to travel at the candidate speed, then return to step S4.

8. The path planning method for an intelligent flow measurement robot based on ADCP according to claim 1, characterized in that, In step S1, the preprocessing of the collected data includes: The relative velocity is transformed from the ship's coordinate system to the geodetic coordinate system, and the transformed relative velocity is fused with the absolute motion velocity provided by the GPS module to obtain the absolute velocity vector in the geodetic coordinate system. The sonar detected by the forward-looking obstacle avoidance sonar is converted to the sonar rectangular coordinate system, then to the ship coordinate system to obtain the obstacle coordinates in the ship coordinate system, and then to the geodetic coordinate system to obtain the obstacle coordinates in the geodetic coordinate system. Based on the geodetic coordinates of the origin and the coordinates of the obstacle, the obstacle is mapped onto the global map grid.

9. A path planning system for an intelligent flow measurement robot based on ADCP, characterized in that, The invention includes an intelligent flow measurement robot mounted on the hull. The intelligent flow measurement robot includes an onboard computing module connected to an acoustic Doppler current profiler (ADCP), an inertial measurement unit (IMU), a global positioning system (GPS) module, a forward-looking obstacle avoidance sonar, and a visual sensor. The onboard computing module is used to execute the path planning method for the intelligent flow measurement robot based on ADCP as described in any one of claims 1-8.

Images