Method for motion planning of inspection robot based on hybrid A* and model predictive control

Abstract

The application discloses a kind of motion planning method of inspection robot based on hybrid A* and model predictive control, constructs grid map and carries out obstacle inflation processing;By constructing continuous state discretization method, initialize data structure, vectorize motion mode and introduce direction change and reverse penalty term definition motion mode consistency penalty, dynamically adjust heuristic weight based on search progress;Calculate the actual cost including path length, consistency penalty and heading angle smoothing penalty, generate initial global path;Path is smoothed using B-spline;Establish discrete-time prediction model of differential robot, introduce progress variable to quantify task completion, dynamically adjust look-ahead distance based on path curvature, dynamically calculate reference state within the constraint forward window;Construct MPC objective function, solve optimal control sequence by rolling optimization and execute.The method can realize kinematically feasible, geometrically smooth global path generation and high-precision, strong robustness local trajectory tracking.

Description

Technical Field

[0001] This invention belongs to the field of intelligent mobile robot path planning and autonomous navigation technology, specifically involving a motion planning method for inspection robots based on hybrid A* and model predictive control. Background Technology

[0002] In the petrochemical industry, mobile inspection robots are widely used for periodic monitoring and anomaly warning of equipment, pipelines, and valves in high-temperature, high-pressure, flammable, explosive, or toxic areas. In such applications, the robot's autonomous navigation capabilities face extremely high demands: it must not only navigate safely in complex environments with narrow passages and both static and dynamic obstacles, but also generate feasible paths that conform to vehicle kinematic constraints based on preset inspection points, achieving high-precision and high-stability trajectory tracking to ensure the reliability of sensor data acquisition and the inherent safety of the operational process.

[0003] Path planning, as a core module of mobile robot navigation systems, is an indispensable key technology for mobile robots to perform tasks. From the perspective of robot task execution, autonomous navigation is usually required before starting a task. This process is generally divided into two levels: global path planning and local trajectory tracking. Global path planning uses prior map information to generate a roughly feasible path from the starting pose to the target pose; local trajectory tracking combines real-time perception information and uses control algorithms to drive the robot to accurately follow the reference trajectory and avoid sudden obstacles.

[0004] In the field of global planning, traditional graph search algorithms (such as the A* algorithm) can effectively handle discrete grid maps, but the paths they generate often ignore the robot's non-holonomic constraints, making them difficult to apply directly to practical mobile robots. To address this, the Hybrid A* algorithm was developed. This algorithm incorporates a kinematic model into the continuous state space for node expansion and uses Reeds-Shepp curves to accelerate convergence. It can generate feasible trajectories that meet physical constraints such as minimum turning radius, and has applications in autonomous driving and special-purpose robots.

[0005] In local tracking, Model Predictive Control (MPC) has become one of the mainstream methods for trajectory tracking due to its ability to explicitly handle multivariable system constraints, optimize future time-domain performance, and possess good anti-interference capabilities. MPC solves the optimal control sequence in the finite time domain online, making the robot state as close as possible to the desired reference trajectory while satisfying actuator constraints such as velocity and acceleration.

[0006] However, to further improve the overall performance of the system, it is necessary to organically integrate the Hybrid A* global path planning method with the Model Predictive Control (MPC) local trajectory tracking method to construct a coordinated and consistent motion planning system. On the one hand, if the path generated by the global planning involves frequent turns, direction changes, or abrupt curvature changes, it will increase the tracking difficulty of the local controller, thereby affecting the smoothness of motion and execution efficiency. On the other hand, if the local tracker lacks awareness of the global task progress and path geometric features, it will be difficult to maintain reliable progress in complex looping paths, and may even lead to task interruption or deviation. Therefore, how to further improve the geometric smoothness of the path while ensuring its kinematic feasibility, and achieve efficient coordination between the front-end and back-end through reasonable trajectory preprocessing and tracking strategies, has become a key technical challenge for high-risk industrial inspection scenarios. Summary of the Invention

[0007] To address the problems existing in the prior art, this invention provides a motion planning method for inspection robots based on hybrid A* and model predictive control. This method can achieve kinematically feasible and geometrically smooth global path generation and high-precision, robust local trajectory tracking, which can effectively improve the autonomous navigation capability, safety, and mission success rate of mobile robots in petrochemical inspection scenarios.

[0008] To achieve the above objectives, this invention provides a motion planning method for inspection robots based on hybrid A* and model predictive control, comprising the following steps: Step 1: Environmental modeling and grid map construction; By establishing a Cartesian coordinate system, the inspection area is divided into regular grid cells, environmental information is recorded and marked, and obstacles are expanded to generate a structured safety cost map. Step 2: Hybrid based on heuristic function optimization Global path planning: First, the starting point and ending point are processed as inputs, a continuous state discretization method is constructed and discretization transformation is completed, and the search data structure and starting node are initialized. Then, the six discrete motion modes of the robot are vectorized, a motion mode consistency penalty term is constructed and the motion modes are modeled uniformly. At the same time, a dynamic weight adjustment strategy is designed based on the search progress to dynamically balance the convergence speed and global optimality in the path search. Subsequently, through node expansion, multi-dimensional actual cost calculation and ReedsShepp curve heuristic cost calculation, the search nodes are updated and the initial global path is backtracked to generate. Step 3: Global path smoothing; The initial global path is fitted with a cubic B-spline curve, and a cumulative arc length sequence is constructed by uniform sampling and arc length parameterization to generate a smooth reference trajectory with continuous curvature and high-order differentiability. The tangent direction and curvature of the path are then calculated. Step 4: Integrating Progress Excitation and Adaptive Forward-Looking Nonlinear MPC Local Trajectory Tracking; First, a continuous-time nonlinear model is established based on the kinematics principle of differential drive robots. Discrete-time state update equations are obtained through first-order Euler discretization, and the future multi-step state sequence is recursively calculated. Next, the projected arc length of the robot's current position onto the nearest point on the arc-length parameterized reference path is defined as the progress variable. The task completion degree and relative position are quantified by minimizing the Euclidean distance. Then, the forward-looking distance is dynamically adjusted based on the path curvature, shortening in curves and extending in straight lines, and the arc length of the forward-looking point is calculated to determine the target range for future trajectory tracking. Further, in the prediction time domain, the predicted position is projected onto a specific forward window of the reference path to generate a reference state sequence matching the robot's future trajectory. Then, by calculating the state error vector, an objective function containing state deviation, control energy consumption, and progress excitation terms is constructed. Kinematic, control, and forward-looking point constraints are applied to enable the optimizer to select the control sequence that maximizes future progress. Finally, the optimal control problem is solved online in each control cycle. The first control variable is sent to the underlying driver to update the state and environmental information, and the cycle continues until all inspection tasks are completed.

[0009] Furthermore, in order to achieve a standardized discretized representation of the environmental space and provide a unified, accurate, and computationally efficient benchmark framework for subsequent path search, the environmental modeling and raster map construction process in step 1 is as follows: S11: Establish a global Cartesian coordinate system with the starting point as the origin, and define the boundary of the raster map. , and According to the predefined resolution The inspection area is divided into regular grid units and the environmental information of each unit is recorded. S12: Mark obstacles as impassable units and expand them to generate a structured safety cost map.

[0010] Furthermore, in order to efficiently generate kinematically feasible, smooth, and optimized global paths in complex environments, in step 2, a heuristic function-optimized Hybrid path is used. The global path planning process is as follows: S21: Input and processing of starting and ending points and initialization of data structures; construct a continuous state discretization method including discretization calculation formula, coordinate offset calculation formula and continuous coordinate restoration formula, and complete the input verification and discretization transformation of the starting and ending points; initialize the core data structures of open and closed lists, and create a starting search node based on the starting discrete data and add it to the open list; S22: Discrete motion pattern vectorization; abstracting the robot's six discrete motion patterns into two-dimensional unit pattern vectors; S23: Define the motion pattern consistency penalty term; by constructing a motion pattern consistency penalty term consisting of direction change penalty and direction reversal penalty, the six discrete motion patterns are abstracted into two-dimensional unit pattern vectors for unified modeling; S24: Define a dynamic weight adjustment strategy; based on the current node to the target node. The ratio of distance to the global shortest path length defines the search progress index. By dynamically adjusting the weights of the actual cumulative cost and the heuristic estimated cost, a balance is achieved between strong directional rapid convergence in the early stage of the search and fine optimization of the global optimum in the later stage of the search. S25: Node Expansion and Cost Calculation; Select the optimal node from the open list and restore continuous coordinates; Generate child node trajectories based on six motion modes; Complete child node discretization and validity checks; Calculate the actual cost including path length, motion mode consistency penalty, and heading angle smoothness penalty; Calculate the incompleteness heuristic cost using the Reeds-Shepp curve analytical search method based on the normalized coordinate system; Determine the total cost by combining a dynamic weight strategy; Manage the addition and updating of child nodes in the open list; Generate the initial global path through path backtracking after the termination condition is met.

[0011] Furthermore, in order to achieve accurate representation and efficient management of the search space, and to use hybrid... The algorithm provides a solid foundation for fast convergence and optimal path search in complex environments. In step S21 of step 2, the input and processing of the starting and ending points and the initialization of the data structures are as follows: S211: Construct a continuous state discretization method; construct the discretization calculation formula, coordinate offset calculation formula, and continuous coordinate restoration formula according to formula (1), formula (2), and formula (3), respectively; , , (1); In the formula, , Location grid resolution; It is the discretization step size of the heading angle; , , (2); In the formula, ; ; ; , , (3).

[0012] S212: Input, verification, and discretization of the start and end points; receiving continuous coordinates of the start point. and endpoint continuous coordinates As input; verify whether the position coordinates are within the environmental boundaries. At the same time, check the heading angle. Is it within physical limitations? The continuous state discretization method is used to separate the continuous coordinates of the starting point. and endpoint continuous coordinates Convert to discrete coordinates and offsets, denoted as the starting point discrete data respectively. and endpoint discrete data This serves as baseline data for subsequent searches; S213: Initialize the core data structures; initialize the open list `open_list` as a min-heap priority queue, with each node storing discrete coordinates. Offset ( ), actual cost Inspirational Cost Total cost and parent node pointers; initialize the closed list (closed_list) as a hash table to store the discrete coordinates of explored nodes. To quickly detect duplicate access; S214: Create the starting search node and add it to the open list; based on the starting discrete data. Create the starting search node Set the starting search node of ; Calculate the starting search node The cost of inspiration As shown in formula (4); (4); In the formula, Starting search node Ignore obstacle heuristics for incompleteness constraints; ; Starting search node Based on minimum turning radius Heuristic value for the length of the Reeds-Shepp curve; Calculate the starting search node Total cost As shown in formula (5); (5); Start search node Add it to the open list (open_list) as the starting point for path searching.

[0013] Furthermore, in order to accurately suppress path jagged oscillations and unnecessary reversing behavior, and to significantly reduce the complexity controlled by mechanical stress while ensuring the geometric smoothness of the path, the process of defining the motion mode consistency penalty term in step S23 of step 2 is as follows: Pattern vector based on parent node and the pattern vector of the current node Define motion pattern consistency penalty As shown in formula (6); (6); In the formula, The weighting coefficient for the motion pattern consistency penalty term; As a comprehensive index of consistency in movement patterns, , , , To reverse the penalty weights, and These are the components of the pattern vector along the Y-axis in the robot's local coordinate system. It is a symbolic function; To achieve an adaptive balance between strong directional rapid convergence in the early stages of the search and fine-grained optimization in the later stages to approach the global optimum, and to effectively balance planning efficiency and path optimality in complex environments, the dynamic weight adjustment strategy is defined in step S24 of step 2 as follows: Define search progress metrics As shown in formula (7); (7); In the formula, To start from the current node To the target node The shortest path length; To start from the starting point To the target node The global shortest path length; The total cost function for constructing nodes is shown in formula (8); (8); In the formula, , For threshold parameters; ; For the current node The cost of inspiration, , For the current node Ignore barrier heuristics for incompleteness constraints. For the current node Based on minimum turning radius Heuristic value for the length of the Reeds-Shepp curve; Furthermore, in order to achieve accurate generation and efficient optimization of a kinematically feasible, smooth, and safety-constrained global path, the node expansion and cost calculation process in step S25 of step 2 is as follows: S251: Select the current node and restore its continuous state; select the total cost from the open list open_list. The smallest node is used as the current node. The current node is discretized according to the continuous state discretization method. Stored discrete coordinates and offset ( Calculate its continuous coordinates ; S252: Generate child node trajectories; assuming the turning radius of the mobile robot is... The movement step size is And it must meet the minimum turning radius constraint. Based on the kinematic model of the mobile robot, generate sub-nodes for forward, backward, left turn, and right turn motion patterns respectively. continuous coordinates As shown in formulas (9), (10), (11) and (12) respectively; where the geometric relationship between the left rear turn extension node and the right turn extension node is equivalent, and the geometric relationship between the right rear turn extension node and the left turn extension node is equivalent. , , (9); , , (10); , , (11); , , (12); In the formula, ; S253: Child node discretization and validity check; using a continuous state discretization method to discretize child nodes. continuous coordinates Convert to discrete coordinates and offset Simultaneously, the validity of child nodes is checked, including detecting grid boundaries: Is it within the map area?; Check for obstacle collisions: use the inflated raster map query unit. Is it an obstacle or is the distance to an obstacle less than the safe range? Check if it exists in the closed_list; if it does, it means that the child node has been fully explored. S254: Calculate the actual cost of the child node; Calculate child nodes The actual cost As shown in formula (13); (13); In the formula, The actual cost of the parent node reflects the path length from the starting point to the current node and is a core indicator of path quality. As a smoothness penalty term, , The cost of path length directly influences the direction of path optimization. child node The pattern vector defines the motion pattern consistency penalty term. , , , , and These are the pattern vectors for the current node and the new node, respectively. This refers to the Y-axis component of the current node's pattern vector in the local coordinate system. The Y-axis component of the new node pattern vector in the local coordinate system; This is a penalty term for the smoothness of the heading angle change. , The weighting coefficient for the heading angle change smoothness penalty term; S255: Calculate the heuristic cost and total cost of the child nodes; Calculate child nodes The cost of inspiration As shown in formula (14); child nodes are constructed based on a dynamic weight adjustment strategy. Total cost As shown in formula (15); (14); In the formula, child node Ignore obstacle heuristics for incompleteness constraints; ; child node Based on minimum turning radius The heuristic value for the length of the Reeds-Shepp curve. ; (15); In the formula, , , S256: Add child nodes to or update the open list; Check if the child node is in the open list (open_list) or the closed list (closed_list). If not, create a new node and store its child nodes. Relevant node information Set the parent node as Add to open_list; if the child node is already in open_list and If the child node is smaller than the original node, update the node's relevant information settings and reorder the open list (open_list); if the child node is already in the closed list (closed_list), skip the child node and proceed to the next round of search. S257: Termination condition judgment for Hybrid A* algorithm; Check termination conditions: If the endpoint node If the path is added to the closed list (closed_list), the algorithm terminates successfully and outputs the path; otherwise, the open list (open_list) is empty and the endpoint node is not found. If the algorithm fails, it terminates and returns a message indicating that no feasible path exists.

[0014] S258: Path Backtracking and Continuous Coordinate Transformation When the end node When a node is added to the closed list, a sequence of discrete nodes is generated by backtracking from the endpoint through the parent node pointer. The discrete nodes are then restored to continuous coordinates using a continuous state discretization method, resulting in an initial global path composed of discrete points that meets the preset inspection point traversal requirements, is kinematically feasible, and conforms to the explosion-proof safety distance.

[0015] As a preferred embodiment, in step 2, S255, during the calculation... At that time, the Reeds-Shepp curve analytical search method based on the normalized coordinate system was adopted, and the process is as follows: S255-1: First input the current node pose. and target point pose This serves as the boundary condition for path planning; then, the current node pose is normalized to a coordinate system with the starting point as the origin through coordinate translation and rotation. The initial heading is aligned with the local coordinate system along the x-axis, and the spatial dimensions are divided by the minimum turning radius. This transforms the problem into a standard problem involving unit circle turning, yielding the normalized target position. ; S255-2: Traverse all basic path templates defined in the Reeds-Shepp theory, analytically solve for the length of each segment for each template, and determine whether there is a real solution; for all candidate paths with valid solutions, further check whether they meet the vehicle's kinematic constraints and task requirements; S255-3: Select the shortest total length path from all feasible paths as the optimal RS curve; multiply the path at this unit scale by... By restoring the actual scale and mapping it back to the original global coordinate system through an inverse transformation, a series of continuous state points are generated, forming a complete RS curve that can be used for heuristic estimation or trajectory generation. Its total length is [the total length of the curve]. .

[0016] In this technical solution, by normalizing the poses of the current node and the target point to a local coordinate system with the starting point as the origin and scaling it to a unit transformation radius, the complex RS curve solution can be transformed into a standardized analytical search problem. This method, by traversing all path templates and selecting the optimal solution that satisfies kinematic constraints and task requirements, can efficiently obtain the shortest incomplete path length while ensuring path reliability. This provides accurate and computationally efficient dynamic heuristic information for the A* algorithm, significantly improving the guidance quality and search efficiency of the heuristic function.

[0017] Furthermore, in order to provide an ideal reference input for subsequent high-precision tracking control, effectively eliminate jagged edges, and ensure the smoothness and stability of motion execution, the global path smoothing process in step 3 is as follows: First, cubic B-spline curves are used to smoothly connect the discrete points of the initial global path, and then the arc length is parameterized to generate a smooth parametric trajectory. Subsequently, the number of sampling points on the smoothed curve was uniformly increased. For each point, calculate its corresponding spatial coordinates, and construct a cumulative arc length sequence by progressively accumulating Euclidean distances. The total path length is denoted as . Simultaneously, a one-dimensional interpolation function is constructed to ultimately obtain the interpolation result based on the arc length. Reference path for parameters As shown in formula (16); (16); In the formula, and Representing the x-axis and y-axis respectively, For path parameters, ; Furthermore, the path tangent direction is calculated using numerical differentiation. and curvature As shown in formulas (17) and (18) respectively; (17); (18).

[0018] Furthermore, in order to achieve high-precision, efficient, and robust local trajectory tracking for the inspection robot on complex paths, the process of integrating progress excitation and adaptive forward look-ahead nonlinear MPC local trajectory tracking in step 4 is as follows: S41: Establish a discrete-time state-space prediction model for the controlled object; establish a continuous-time nonlinear model based on the kinematics principle of differential drive robot, obtain the discrete-time state update equation by discretization using the first-order Euler method, and recursively calculate the state sequence of future multiple steps in the prediction time domain. S42: Introduce a progress variable; define the progress variable as the projected arc length of the nearest point on the arc length parameterized reference path of the robot's current position, and achieve accurate projection by minimizing the Euclidean distance to quantify the robot's task completion degree and relative position on the inspection path. S43: Adaptive adjustment of forward sight distance; dynamically adjusts the forward sight distance based on the curvature of the current path, automatically shortens it in curved areas to improve tracking accuracy, automatically extends it in straight areas to improve response efficiency, and calculates the arc length of the forward sight point to define the target range for future trajectory tracking; S44: Dynamic calculation of reference state; For each step in the prediction time domain, the predicted position is projected onto the reference path and the search interval is limited to the forward window to dynamically generate a reference state sequence that matches the robot's future trajectory. S45: Construct and solve the nonlinear MPC optimization problem; by calculating the state error vector, construct a nonlinear MPC objective function that includes state deviation, control energy consumption and schedule excitation terms, and apply kinematic constraints, control constraints and forward view constraints so that the optimizer prioritizes the control sequence that maximizes future schedule; S46: Rolling optimization and execution; Solve the finite-time optimal control problem online in each control cycle, send the first control quantity of the optimal control sequence to the underlying driver, and then update the state and environmental information in a loop until the robot completes all inspection tasks.

[0019] Furthermore, in order to provide the model controller with an accurate and computationally efficient predictive basis for describing the robot's motion characteristics, and to ensure the accuracy and real-time performance of subsequent trajectory tracking control, the process of establishing the discrete-time state-space prediction model of the controlled object in step S41 of step 4 is as follows: For the status The control variables are Based on the differential drive kinematics principle, a continuous-time nonlinear kinematic model of the robot is constructed, as shown in formula (19); assuming the sampling period is... The continuous-time nonlinear kinematic model is discretized using the first-order Euler method to obtain the discrete-time state update equation, as shown in formula (20); let Within the prediction time domain, the future is calculated recursively. The state sequence of the steps is shown in formula (21); (19); Discrete state is : (20); In the formula, , and Let x, y, and y represent the robot's x-coordinate, y-coordinate, and heading angle at time k, respectively. , These represent the speeds of the left and right wheels at time k, respectively. (twenty one); In the formula, In order to be in Always Predicting the state at any given time.

[0020] In order to achieve continuous quantitative perception of task completion and provide a key feedback benchmark for subsequent progress incentives and adaptive forward look-ahead, the process of introducing progress variables in step S42 of step 4 is as follows: Define the arc length coordinates corresponding to the nearest point on the arc length parameterized reference path to the robot's current position as the progress variable. As shown in formula (22); (twenty two); In the formula, Let K be the actual position coordinates of the robot at time k; To achieve the optimal match between tracking performance and path geometry, the adaptive adjustment of the forward look distance in step 4, S43, is as follows: Based on the current path curvature Dynamically adjust forward viewing distance As shown in formula (23); at the same time, calculate the arc length of the foreseeability point. As shown in formula (24); (twenty three); In the formula, The baseline forward sight distance for inspection scenarios; This is the curvature adjustment coefficient. >0; , These are the minimum and maximum forward viewing distances for adapting to narrow passages; (twenty four); To effectively avoid tracking deviations caused by over-looking ahead, and to provide a causally consistent reference benchmark for MPC that accurately matches the robot's future trajectory, the dynamic calculation process of the reference state in step S44 of step 4 is as follows: For each step in the prediction time domain Predicted location Projected onto the reference path, the search range is limited to the forward window. Calculate the arc length and arc length reference state As shown in formulas (25) and (26) respectively; (25); (26).

[0021] To achieve accurate tracking of complex trajectories and proactive progress advancement of the task, effectively address the tracking stagnation problem under complex paths, and ensure control feasibility, the process of constructing and solving the nonlinear MPC optimization problem in step S45 of step 4 is as follows: Calculation in Time prediction State error vector at time step As shown in formula (27); at the same time, a schedule incentive term is introduced to construct a nonlinear MPC objective function, as shown in formula (28); (27); (28); In the formula, This is the weight matrix for the state deviation; The weight matrix for controlling the input; The weighting coefficients for the schedule incentives are... >0; In order to be in Time prediction Time-based control input; Construct the optimization problem constraint conditions based on kinematic constraints, control constraints and forward view constraints, as shown in formula (29), so that the optimization problem satisfies the composite condition constraints; (29); In the formula, It is a nonlinear discrete kinematic model; , These are the minimum and maximum speed vectors allowed for the left and right wheels, respectively. To achieve real-time response to dynamic environments and continuous updates to optimal control, thereby ensuring the robustness of trajectory tracking and the continuity of task execution, the rolling optimization and execution process in step S46 of step 4 is as follows: In each control cycle The controller solves the finite-time optimal control problem described by equations (28) and (29) online to obtain the optimal control input sequence. and the first control variable in the sequence The signal is sent to the underlying driver to drive the robot's movement; then, the next control cycle begins. The robot status and surrounding environment information are updated, and the MPC optimization solution is performed again. This process is repeated until the robot has traversed all inspection points and reached the final target location.

[0022] This invention provides a motion planning method for inspection robots based on hybrid A* and model predictive control, aiming to solve key technical bottlenecks of traditional motion planning methods in high-constraint, high-risk scenarios such as petrochemical inspections. These bottlenecks include low global path search efficiency, poor trajectory smoothness, easy local tracking stagnation, and insufficient control robustness. Traditional Hybrid A* algorithms suffer from the following drawbacks in practical applications: numerous redundant nodes during the search process lead to long computation time; the generated path involves frequent turning and direction switching, resulting in an uneven trajectory; and the fixed heuristic function weights cannot adapt to the needs of different search stages. At the local tracking control level, traditional MPC methods also face tracking failure problems due to poor reference trajectory quality, lack of task-level perception, and rigid look-forward strategies.

[0023] To address the aforementioned issues, this invention proposes a systematic optimization scheme from global planning to local tracking. At the global path planning level, by constructing a motion pattern consistency penalty term that includes penalties for direction changes and reversals, unnecessary steering jitter and frequent forward / backward switching in the path are effectively suppressed, significantly improving the coherence and smoothness of the generated trajectory. By designing a heading angle change smoothness penalty term, abrupt changes in continuous angles are further constrained, ensuring the smoothness of trajectory curvature changes and better aligning with vehicle dynamics characteristics. Simultaneously, an innovative heuristic weight dynamic adjustment strategy based on search progress is proposed. In the early stages of the search, the goal orientation is enhanced for rapid convergence, while in the later stages, the actual cost is emphasized for fine-tuning, thus balancing planning efficiency and path optimality in complex environments.

[0024] At the local trajectory tracking level, this invention constructs a nonlinear model predictive control framework for differential drive robots. First, the global path planned by Hybrid A* is smoothed using cubic B-spline and parameterized by arc length to generate a geometrically continuous, curvature-bounded, high-order differentiable reference trajectory, thus providing an ideal tracking benchmark for the controller. Second, by introducing an explicit optimization mechanism using progress variables, the task completion rate is directly embedded into the MPC objective function, fundamentally solving the problem of robots stalling, wandering, or failing to reach the destination on complex paths. Furthermore, a curvature-adaptive forward-looking mechanism is designed, enabling the controller's forward-looking point to intelligently adjust according to the path curvature, and using this as the upper limit of the forward projection search, ensuring the causality and local optimum of the reference state. By solving the optimal control sequence in the finite-time domain online, the robot is driven to accurately track the reference trajectory, avoid sudden obstacles, and ensure the stability of sensor data acquisition.

[0025] This invention does not simply concatenate Hybrid A* and MPC, but rather achieves complementary advantages through deep coupling. The Hybrid A* algorithm, employing multi-scale penalty and dynamic heuristic optimization, serves as the front-end global planner, outputting a kinematically feasible, structurally simple, and highly smooth global path. The nonlinear MPC, integrating progress incentives and adaptive look-forward, acts as the back-end tracking controller, utilizing a high-quality reference trajectory processed by B-splines to achieve accurate, stable, and oscillatory tracking execution. This deep collaboration between the two forms a complete motion planning framework, from intelligent front-end planning to precise back-end execution.

[0026] This method is simple to implement and low in cost, effectively improving the autonomous navigation capability, safety, and mission success rate of mobile robots in petrochemical inspection scenarios. Through multi-dimensional collaborative optimization, this invention can effectively solve the key technical bottlenecks of mobile robot motion planning methods in petrochemical inspection scenarios, achieving kinematically feasible and geometrically smooth global path generation and high-precision, robust local trajectory tracking. It is particularly suitable for mobile inspection scenarios in high-temperature, high-pressure, flammable, explosive, or toxic areas in the petrochemical industry, as well as practical application scenarios with extremely high requirements for safety, stability, and mission reliability, such as emergency rescue and unmanned driving. Attached Figure Description

[0027] Figure 1 This is the overall flowchart of the motion planning fusion of Hybrid A* and MPC in this invention; Figure 2 This is a flowchart of the Hybrid A* algorithm in this invention; Figure 3 This is a flowchart of the MPC algorithm in this invention; Figure 4 The figure shows the numerical simulation results of this invention on the MATLAB platform; Figure 5 This is a graph showing the MPC path tracking error of the present invention on the MATLAB platform; Figure 6 This is a diagram of the control signal curves for the MPC in the MATLAB platform according to the present invention. Detailed Implementation

[0028] The invention will now be further described with reference to the accompanying drawings.

[0029] like Figures 1 to 6 As shown, this invention provides a motion planning method for inspection robots based on hybrid A* and model predictive control, comprising the following steps: Step 1: Environment modeling and raster map construction; By establishing a Cartesian coordinate system, the inspection area is divided into regular grid cells, environmental information is recorded and marked, and obstacles are expanded to generate a structured safety cost map. To achieve a standardized, discretized representation of the environmental space and provide a unified, accurate, and computationally efficient baseline framework for subsequent path search, the specific process of environmental modeling and raster map construction is as follows: S11: Establish a global Cartesian coordinate system with the starting point as the origin, and define the boundary of the raster map. , and According to the predefined resolution The inspection area is divided into regular grid units and the environmental information of each unit is recorded. S12: Mark obstacles as impassable units and expand them to generate a structured safety cost map.

[0030] Step 2: Hybrid based on heuristic function optimization Global path planning; First, the starting point and ending point are processed as inputs, a continuous state discretization method is constructed and discretization transformation is completed, and the search data structure and starting node are initialized. Then, the six discrete motion modes of the robot are vectorized, a motion mode consistency penalty term is constructed and the motion modes are modeled uniformly. At the same time, a dynamic weight adjustment strategy is designed based on the search progress to dynamically balance the convergence speed and global optimality in the path search. Subsequently, through node expansion, multi-dimensional actual cost calculation and ReedsShepp curve heuristic cost calculation, the search nodes are updated and the initial global path is backtracked to generate. To efficiently generate kinematically feasible, smooth, and optimized global paths in complex environments, specifically, a heuristic function-optimized Hybrid... The global path planning process is as follows: S21: Input and processing of starting and ending points, and initialization of data structures; constructing a continuous state discretization method including discretization calculation formulas, coordinate offset calculation formulas, and continuous coordinate restoration formulas, and completing input verification and discretization transformation of the starting and ending points; initializing the core data structures of open and closed lists, and creating a starting search node based on the starting point discrete data and adding it to the open list, for hybrid... The search lays the foundation; To achieve accurate representation and efficient management of the search space, and in order to use hybrid... The algorithm provides a solid foundation for fast convergence and optimal path search in complex environments. The input and processing of the starting and ending points, as well as the initialization of the data structures, are as follows: S211: Construct a continuous state discretization method; construct the discretization calculation formula, coordinate offset calculation formula, and continuous coordinate restoration formula according to formula (1), formula (2), and formula (3), respectively; , , (1); In the formula, , Location grid resolution; It is the discretization step size of the heading angle; , , (2); In the formula, ; ; ; , , (3).

[0031] S212: Input, verification, and discretization of the start and end points; receiving continuous coordinates of the start point. and endpoint continuous coordinates As input; verify whether the position coordinates are within the environmental boundaries. At the same time, check the heading angle. Is it within physical limitations? The continuous state discretization method is used to separate the continuous coordinates of the starting point. and endpoint continuous coordinates Convert to discrete coordinates and offsets, denoted as the starting point discrete data respectively. and endpoint discrete data This serves as baseline data for subsequent searches; S213: Initialize core data structures; initialize the open list `open_list` as a min-heap priority queue (based on...). (value sorted), each node stores discrete coordinates Offset ( ), actual cost Inspirational Cost Total cost and parent node pointers; initialize the closed list (closed_list) as a hash table to store the discrete coordinates of explored nodes. This process enables rapid detection of duplicate visits, ensuring efficient management of node states and avoiding redundant computations. S214: Create the starting search node and add it to the open list; based on the starting discrete data. Create the starting search node Set the starting search node of ; Calculate the starting search node The cost of inspiration As shown in formula (4); (4); In the formula, Starting search node Ignore obstacle heuristics for incompleteness constraints; ; Starting search node Based on minimum turning radius Heuristic value for the length of the Reeds-Shepp curve; Calculate the starting search node Total cost As shown in formula (5); (5); Start search node Add it to the open list (open_list) as the starting point for path searching.

[0032] S22: Discrete motion pattern vectorization; The six discrete motion patterns of the robot (including forward, left turn, right turn, backward, left-back turn, and right-back turn) are abstracted into two-dimensional unit pattern vectors, thereby transforming the complex pattern judgment problem into a concise vector operation, effectively simplifying the calculation process; S23: Define a motion pattern consistency penalty term; by constructing a motion pattern consistency penalty term consisting of a direction change penalty and a direction reversal penalty, the six discrete motion patterns are abstracted into two-dimensional unit pattern vectors for unified modeling, so as to effectively suppress frequent turning and direction reversal in path planning and significantly improve the geometric smoothness and motion coherence of the path. Among them, the obtained model vector The correspondence between the motion patterns is shown in Table 1. Table 1: Model Vectors Table of correspondences with sports modes To achieve precise suppression of path jagged oscillations and unnecessary reversing behavior, and to significantly reduce the complexity controlled by mechanical stress while ensuring the geometric smoothness of the path, the process of defining a motion pattern consistency penalty term is as follows: To improve the smoothness and motion coherence of the generated path and avoid frequent turns and mode switching, a mode vector based on the parent node is used. and the pattern vector of the current node Define motion pattern consistency penalty As shown in formula (6); (6); In the formula, The weighting coefficient for the motion pattern consistency penalty term; A comprehensive index for consistency of movement patterns, including penalties for changes in direction. and direction reversal penalty item ; Direction change penalty To suppress drastic changes in heading angle, a linear penalty is applied when adjacent nodes move in similar directions (with an acute angle), allowing for minor turns. When the directions tend to be opposite (with an obtuse angle), a quadratic nonlinear penalty is switched to significantly increase the cost of sharp turns or U-turns, thereby effectively suppressing sawtooth oscillations and improving path geometric smoothness. Reversal penalty This algorithm specifically identifies complete transitions between forward and backward modes. By detecting changes in the sign of the Y component of the mode vector in the local coordinate system, a high-amplitude penalty is applied upon sign reversal to suppress unnecessary direction reversals, reducing mechanical stress and control complexity. This penalty mechanism, without increasing state dimensions or logical complexity, guides the search process to prioritize generating high-quality paths that are directionally continuous, structurally simple, and motion-coherent, better fitting the kinematic characteristics of actual mobile robots, thus improving the algorithm's practicality and robustness in complex industrial scenarios. , , , To reverse the penalty weights, and These are the components of the pattern vector along the Y-axis in the robot's local coordinate system. It is a symbolic function; S24: Define a dynamic weight adjustment strategy; to balance the directionality in the early stage of the search (rapid convergence to the target) and the optimality in the later stage of the search (ensuring path quality), a dynamic weight adjustment strategy based on search progress is proposed; based on the distance from the current node to the target node... The ratio of distance to the global shortest path length defines the search progress index. By dynamically adjusting the weights of actual cumulative cost and heuristic estimated cost, a balance is achieved between strong directional rapid convergence in the early stage of the search and fine optimization of global optimum in the later stage of the search, so as to take into account both search efficiency and path quality. To achieve an adaptive balance between strong directional and rapid convergence in the early stages of the search and fine-grained optimization in the later stages to approach the global optimum, and to effectively balance planning efficiency and path optimality in complex environments, the dynamic weight adjustment strategy is defined as follows: Define search progress metrics As shown in formula (7); (7); In the formula, To start from the current node To the target node The shortest path length (ignoring kinematic constraints); To start from the starting point To the target node The global shortest path length; The total cost function for constructing nodes is shown in Equation (8). Based on the search progress, the actual cumulative cost is dynamically adjusted. weight and the cost of inspiration weight Therefore, reducing the initial search volume. ,promote To enhance directionality for faster convergence and avoid local redundant expansion, the search efficiency is gradually improved in the later stages. By regressing to a fine comparison of the actual cumulative costs, we can ensure that the path is close to the global optimum, thereby achieving a synergistic improvement in efficiency and quality in complex environments. (8); In the formula, , For threshold parameters, This is used to prevent weights from converging too early; ; For the current node The cost of inspiration, By taking the maximum of the two values, the acceptability and informativeness of the heuristic value are ensured. For the current node Ignore barrier heuristics for incompleteness constraints. For the current node Based on minimum turning radius Heuristic value for the length of the Reeds-Shepp curve; Thus, the algorithm reduces redundant nodes and accelerates convergence in complex environments, while generating high-quality trajectories with smoother turns and more continuous curvature, taking into account search efficiency, path optimality, and scene adaptability.

[0033] S25: Node expansion and cost calculation; Select the optimal node from the open list and restore the continuous coordinates, generate the child node trajectory based on six motion modes, complete the discretization and validity check of the child nodes, calculate the actual cost including path length, motion mode consistency penalty and heading angle smoothness penalty, use the Reeds-Shepp curve analytical search method based on the normalized coordinate system to calculate the incompleteness heuristic cost, combine the dynamic weight strategy to determine the total cost, manage the addition and update of child nodes in the open list, and generate the initial global path through path backtracking after the termination condition is met; To achieve accurate generation and efficient optimization of kinematically feasible, smooth, and safety-constrained global paths, the node expansion and cost calculation process is as follows: S251: Select the current node and restore its continuous state; select the total cost from the open list open_list. The smallest node is used as the current node. The current node is discretized according to the continuous state discretization method. Stored discrete coordinates and offset ( Calculate its continuous coordinates This ensures the precise application of kinematic constraints in continuous space; S252: Generate child node trajectories; assuming the turning radius of the mobile robot is... The movement step size is And it must meet the minimum turning radius constraint. Based on the kinematic model of the mobile robot, generate sub-nodes for forward, backward, left turn, and right turn motion patterns respectively. continuous coordinates As shown in formulas (9), (10), (11) and (12) respectively; where the geometric relationship between the left rear turn extension node and the right turn extension node is equivalent, and the geometric relationship between the right rear turn extension node and the left turn extension node is equivalent. , , (9); , , (10); , , (11); , , (12); In the formula, ; S253: Child node discretization and validity check; using a continuous state discretization method to discretize child nodes. continuous coordinates Convert to discrete coordinates and offset Simultaneously, the validity of child nodes is checked, including detecting grid boundaries: Is it within the map area?; Check for obstacle collisions: use the inflated raster map query unit. Is it an obstacle or is the distance to an obstacle less than the safe range? Check if it exists in the closed_list; if it does, it means that the child node has been fully explored. This process is used to filter out invalid and duplicate nodes, avoid invalid expansion, and improve search efficiency. S254: Calculate the actual cost of the child node; Calculate child nodes The actual cost As shown in formula (13); (13); In the formula, The actual cost of the parent node reflects the path length from the starting point to the current node and is a core indicator of path quality. As a smoothness penalty term, , The cost of path length directly influences the direction of path optimization. child node The pattern vector defines the motion pattern consistency penalty term. , , , , and These are the pattern vectors for the current node and the new node, respectively. This refers to the Y-axis component of the current node's pattern vector in the local coordinate system. The Y-axis component of the new node pattern vector in the local coordinate system; This is a penalty term for the smoothness of the heading angle change. , The weighting coefficient for the heading angle change smoothness penalty term; S255: Calculate the heuristic cost and total cost of the child nodes; Calculate child nodes The cost of inspiration As shown in formula (14); child nodes are constructed based on a dynamic weight adjustment strategy. Total cost As shown in formula (15); (14); In the formula, child node Ignore obstacle heuristics for incompleteness constraints; ; child node Based on minimum turning radius The heuristic value for the length of the Reeds-Shepp curve. This cost estimates the optimal path length from the current point to the target, guiding the search to expand towards the target. (15); In the formula, , , This mechanism enhances directionality in the early stages of the search. Larger), later enhanced path optimization ( (Larger), dynamically balancing search efficiency and path quality.

[0034] As a preferred option, in calculation To improve the accuracy and computational efficiency of the heuristic values, an analytical search method based on the Reeds-Shepp curve using a normalized coordinate system is adopted. The process is as follows: S255-1: First input the current node pose. and target point pose This serves as the boundary condition for path planning; then, the current node pose is normalized to a coordinate system with the starting point as the origin through coordinate translation and rotation. The initial heading is aligned with the local coordinate system along the x-axis, and the spatial dimensions are divided by the minimum turning radius. This transforms the problem into a standard problem involving unit circle turning, yielding the normalized target position. ; S255-2: Traverse all basic path templates defined in the Reeds-Shepp theory (48 combinations in total, such as LSL, RSR, LRL, RSLR, etc., where L, R, and S represent left-turn arcs, right-turn arcs, and straight lines, respectively, and forward and backward motions are allowed). Analyze the lengths of each segment for each template and determine if a real solution exists. For all candidate paths with valid solutions, further check whether they satisfy the vehicle's kinematic constraints (such as minimum turning radius) and task requirements (such as whether reversing is allowed or whether there is a conflict with obstacles). S255-3: Select the shortest total length path from all feasible paths as the optimal RS curve; multiply the path at this unit scale by... The actual scale is restored and mapped back to the original global coordinate system through an inverse transformation, generating a series of continuous state points, forming a complete RS curve that can ultimately be used for heuristic estimation or trajectory generation, such as... Figure 5 As shown, its total length is .

[0035] In this technical solution, by normalizing the poses of the current node and the target point to a local coordinate system with the starting point as the origin and scaling it to a unit transformation radius, the complex RS curve solution can be transformed into a standardized analytical search problem. This method, by traversing all path templates and selecting the optimal solution that satisfies kinematic constraints and task requirements, can efficiently obtain the shortest incomplete path length while ensuring path reliability. This provides accurate and computationally efficient dynamic heuristic information for the A* algorithm, significantly improving the guidance quality and search efficiency of the heuristic function.

[0036] S256: Add child nodes to or update the open list; Check if the child node is in the open list (open_list) or the closed list (closed_list). If not, create a new node and store its child nodes. Relevant node information Set the parent node as Add to open_list; if the child node is already in open_list and If the node is smaller than the original node, update the node's relevant information and reorder the open list (open_list); if the child node is already in the closed list (closed_list), skip the child node and proceed to the next round of search; this process ensures that the optimal path is explored and avoids the generation of redundant nodes. S257: Termination condition judgment for Hybrid A* algorithm; Check termination conditions: If the endpoint node If the path is added to the closed list (closed_list), the algorithm terminates successfully and outputs the path; otherwise, the open list (open_list) is empty and the endpoint node is not found. If the algorithm fails, it terminates and returns a message indicating that no feasible path exists.

[0037] S258: Path Backtracking and Continuous Coordinate Transformation When the end node When a node is added to the closed list, a sequence of discrete nodes is generated by backtracking from the endpoint through the parent node pointer. The discrete nodes are then restored to continuous coordinates using a continuous state discretization method, resulting in an initial global path consisting of discrete points that meets the preset inspection point traversal requirements, is kinematically feasible, and conforms to the explosion-proof safety distance.

[0038] Step 3: Global path smoothing; The initial global path is fitted with a cubic B-spline curve, and a cumulative arc length sequence is constructed by uniform sampling and arc length parameterization to generate a smooth reference trajectory with continuous curvature and high-order differentiability. The path tangent direction and curvature are calculated to provide a benchmark for subsequent tracking control. In order to provide an ideal reference input for subsequent high-precision tracking control, effectively eliminate jagged edges, and ensure the smoothness and stability of motion execution, the global path smoothing process is as follows: First, cubic B-spline curves are used to smoothly connect the discrete points of the initial global path, and then the arc length is parameterized to generate a smooth parametric trajectory. Subsequently, the number of sampling points on the smoothed curve was uniformly increased. For each point, calculate its corresponding spatial coordinates, and construct a cumulative arc length sequence by progressively accumulating Euclidean distances. The total path length is denoted as . Simultaneously, a one-dimensional interpolation function is constructed to ultimately obtain the interpolation result based on the arc length. Reference path for parameters As shown in formula (16); (16); In the formula, and Representing the x-axis and y-axis respectively, For path parameters, ; Furthermore, the path tangent direction is calculated using numerical differentiation. and curvature As shown in formulas (17) and (18) respectively; (17); (18).

[0039] Step 4: Integrate progress excitation with adaptive forward look-ahead nonlinear MPC local trajectory tracking; First, a continuous-time nonlinear model is established based on the kinematics principle of differential drive robot. Discrete-time state update equations are obtained through first-order Euler discretization, and the future multi-step state sequence is recursively calculated. Next, the projected arc length of the robot's current position onto the nearest point on the arc-length parameterized reference path is defined as the progress variable. The task completion degree and relative position are quantified by minimizing the Euclidean distance. Then, the forward-looking distance is dynamically adjusted based on the path curvature, shortening it on curves and extending it on straight sections, and the arc length of the forward-looking point is calculated to determine the target range for future trajectory tracking. Further, in the prediction time domain, the predicted position is projected onto a specific forward window of the reference path, generating a reference state sequence that matches the robot's future trajectory. Then, by calculating the state error vector, an objective function containing state deviation, control energy consumption, and progress excitation terms is constructed. Kinematic, control, and forward-looking point constraints are applied, allowing the optimizer to select the control sequence that maximizes future progress. Finally, the optimal control problem is solved online in each control cycle. The first control variable is sent to the underlying driver, and the state and environmental information are updated and executed cyclically until all inspection tasks are completed.

[0040] To achieve high-precision, efficient, and robust local trajectory tracking for inspection robots on complex paths, the specific process of nonlinear MPC local trajectory tracking that integrates progress excitation and adaptive forward look is as follows: S41: Establish a discrete-time state-space prediction model for the controlled object; establish a continuous-time nonlinear model based on the kinematics principle of differential drive robot, obtain the discrete-time state update equation by discretization using the first-order Euler method, and recursively calculate the state sequence of future multiple steps in the prediction time domain to provide a prediction model for the controlled object for model predictive control. To provide a computationally efficient predictive basis for accurately describing the robot's motion characteristics for the model controller, and to ensure the accuracy and real-time performance of subsequent trajectory tracking control, the process of establishing a discrete-time state-space predictive model of the controlled object is as follows: For the status The control variables are Based on the differential drive kinematics principle, a continuous-time nonlinear kinematic model of the robot is constructed, as shown in formula (19). This model reflects the physical characteristics of the robot achieving steering by adjusting the speed difference between the left and right wheels. To adapt to the discretization requirements of the MPC controller, the sampling period is set to be... The continuous-time nonlinear kinematic model is discretized using the first-order Euler method to obtain the discrete-time state update equation, as shown in formula (20); let Within the prediction time domain, the future is calculated recursively. The state sequence of the steps is shown in formula (21); (19); Discrete state is : (20); In the formula, , and Let x, y, and y represent the robot's x-coordinate, y-coordinate, and heading angle at time k, respectively. , These represent the speeds of the left and right wheels at time k, respectively. (twenty one); In the formula, In order to be in Always Predicting the state at any given time.

[0041] S42: Introduce a progress variable; define the progress variable as the projected arc length of the nearest point on the arc length parameterized reference path of the robot's current position, and achieve accurate projection by minimizing the Euclidean distance to quantify the robot's task completion degree and relative position on the inspection path. In order to achieve continuous quantitative perception of task completion and provide a key feedback benchmark for subsequent progress incentives and adaptive forward look-ahead, the process of introducing progress variables is as follows: In the narrow passage scenarios of petrochemical plants, the progress variable is a key state auxiliary quantity in the model control framework for path tracking. It is used to quantify the progress of the mobile robot on the reference path. The progress variable is defined as the arc length coordinates corresponding to the nearest point on the arc length parameterized reference path where the robot's current position is located. As shown in formula (22); (twenty two); In the formula, Let be the actual position coordinates of the robot at time k; this projection operation is achieved by minimizing the Euclidean distance between the robot's current position and points on the path, ensuring... It accurately reflects the robot's progress along the global path.

[0042] S43: Adaptive adjustment of forward sight distance; dynamically adjusts the forward sight distance based on the curvature of the current path, automatically shortens it in curved areas to improve tracking accuracy, automatically extends it in straight areas to improve response efficiency, and calculates the arc length of the forward sight point to define the target range for future trajectory tracking; To achieve the optimal match between tracking performance and path geometry, the adaptive adjustment of the look-ahead distance is as follows: When the robot is in a high-curvature area (such as a sharp bend or a U-turn), if a fixed forward look-ahead distance is still used, the forward look-ahead point may cross the inside of the bend, pointing to the outside of the path or behind, causing the controller to generate incorrect directional commands. Conversely, in straight areas, an excessively short forward look-ahead distance will weaken the system's ability to predict changes in the path ahead, based on the current path curvature. Dynamically adjust forward viewing distance To balance tracking accuracy and response speed, as shown in formula (23); during the iterative calculation of MPC, it is necessary to ensure that the reference state at future time moments is located after the current reference state on the reference path. Therefore, a foreground constraint needs to be applied, which forces all reference states to be located between the current progress and the foreground to ensure the stability of the optimization problem. At the same time, the arc length of the foreground is calculated. As shown in formula (24); (twenty three); In the formula, The baseline forward sight distance for inspection scenarios; This is the curvature adjustment coefficient. >0; , These are the minimum and maximum forward sight distances for adapting to narrow passages; this design incorporates a curvature term. This allows the forward sight distance to automatically shorten in curved areas to improve tracking accuracy, and to automatically extend in straight sections to improve response efficiency. (twenty four); This value defines the target range for future trajectory tracking, ensuring that the control strategy always focuses on path segments near the robot's current progress.

[0043] S44: Dynamic calculation of reference state; For each step in the prediction time domain, the predicted position is projected onto the reference path and the search interval is limited to the forward window to dynamically generate a reference state sequence that matches the robot's future trajectory, providing a benchmark for subsequent error calculation; To effectively avoid tracking deviations caused by over-looking ahead, and to provide a causally consistent reference benchmark for MPC that accurately matches the robot's future trajectory, the dynamic calculation process of the reference state is as follows: For each step in the prediction time domain Predicted location Projected onto the reference path, the search range is limited to the forward window. Calculate the arc length and arc length reference state As shown in formulas (25) and (26) respectively; This constraint ensures that the reference state is always behind the current progress and does not exceed the forward look-ahead, avoiding tracking deviations caused by excessive looking ahead.

[0044] (25); (26).

[0045] S45: Construct and solve the nonlinear MPC optimization problem; by calculating the state error vector, construct a nonlinear MPC objective function that includes state deviation, control energy consumption and schedule excitation terms, and apply kinematic constraints, control constraints and forward view constraints so that the optimizer prioritizes the control sequence that maximizes future schedule; To achieve accurate tracking of complex trajectories and proactive task progress, effectively addressing the tracking stagnation problem under complex paths and ensuring control feasibility, the process of constructing and solving the nonlinear MPC optimization problem is as follows: In each control cycle, an optimal control problem in the finite time domain is solved online; schedule variable It is not only used for state feedback, but also deeply integrated into the MPC optimization objective and constraint system, in the design of the objective function; Calculation in Time prediction State error vector at time step As shown in formula (27); at the same time, a schedule incentive term is introduced to construct a nonlinear MPC objective function, as shown in formula (28); (27); (28); In the formula, This is the weight matrix for state deviation, used to balance the priority of position deviation and heading deviation. By assigning different weight coefficients to position error and heading error, the controller's emphasis on lateral tracking accuracy and heading stability can be adjusted. To control the input weight matrix, the magnitude of input variation is penalized to suppress mechanical wear and energy consumption caused by frequent adjustments, thereby improving system robustness and operating efficiency. The weighting coefficients for the schedule incentives are... >0; In order to be in Time prediction The control input at any given time is achieved by explicitly introducing schedule stimuli, which will affect the future schedule. As an explicit optimization objective in the objective function, the optimizer is guided to prioritize the selection of control sequences that maximize task progress in the feasible solution set, thereby prompting the robot to move towards the endpoint during trajectory tracking. Construct the optimization problem constraint conditions based on kinematic constraints, control constraints and forward view constraints, as shown in formula (29), so that the optimization problem satisfies the composite condition constraints; (29); In the formula, It is a nonlinear discrete kinematic model; , These are the minimum and maximum speed vectors allowed for the left and right wheels, respectively. The schedule incentive term uses a negative sign mechanism to make the optimizer prioritize selecting the feasible trajectory that maximizes future progress. The control sequence generates a continuous forward-driving force, effectively avoiding stagnation in complex looping paths.

[0046] S46: Rolling optimization and execution; Solve the finite-time optimal control problem online in each control cycle, send the first control quantity of the optimal control sequence to the underlying driver, and then update the state and environmental information in a loop until the robot completes all inspection tasks.

[0047] To achieve real-time response to dynamic environments and continuous updates to optimal control, thereby ensuring the robustness of trajectory tracking and the continuity of task execution, the rolling optimization and execution process is as follows: In each control cycle The controller solves the finite-time optimal control problem described by equations (28) and (29) online to obtain the optimal control input sequence. and the first control variable in the sequence The data is sent to the underlying driver to drive the robot's movement. The left and right sides can be controlled separately to achieve corresponding speeds, driving the motors to rotate at the set speed, thereby adjusting the robot's motion state. Then, the next control cycle begins. The robot's state and surrounding environment information are updated, and the MPC optimization solution is re-performed. This process is repeated until the robot has traversed all inspection points and reached the final target location, thus completing the entire inspection task. Figure 6 As shown, it is a graph of the output control signal of the MPC algorithm in one embodiment of the present invention.

[0048] This invention provides a motion planning method for inspection robots based on hybrid A* and model predictive control, aiming to solve key technical bottlenecks of traditional motion planning methods in high-constraint, high-risk scenarios such as petrochemical inspections. These bottlenecks include low global path search efficiency, poor trajectory smoothness, easy local tracking stagnation, and insufficient control robustness. Traditional Hybrid A* algorithms suffer from the following drawbacks in practical applications: numerous redundant nodes during the search process lead to long computation time; the generated path involves frequent turning and direction switching, resulting in an uneven trajectory; and the fixed heuristic function weights cannot adapt to the needs of different search stages. At the local tracking control level, traditional MPC methods also face tracking failure problems due to poor reference trajectory quality, lack of task-level perception, and rigid look-forward strategies.

[0049] To address the aforementioned issues, this invention proposes a systematic optimization scheme from global planning to local tracking. At the global path planning level, by constructing a motion pattern consistency penalty term that includes penalties for direction changes and reversals, unnecessary steering jitter and frequent forward / backward switching in the path are effectively suppressed, significantly improving the coherence and smoothness of the generated trajectory. By designing a heading angle change smoothness penalty term, abrupt changes in continuous angles are further constrained, ensuring the smoothness of trajectory curvature changes and better aligning with vehicle dynamics characteristics. Simultaneously, an innovative heuristic weight dynamic adjustment strategy based on search progress is proposed. In the early stages of the search, the goal orientation is enhanced for rapid convergence, while in the later stages, the actual cost is emphasized for fine-tuning, thus balancing planning efficiency and path optimality in complex environments.

[0050] At the local trajectory tracking level, this invention constructs a nonlinear model predictive control framework for differential drive robots. First, the global path planned by Hybrid A* is smoothed using cubic B-spline and parameterized by arc length to generate a geometrically continuous, curvature-bounded, high-order differentiable reference trajectory, thus providing an ideal tracking benchmark for the controller. Second, by introducing an explicit optimization mechanism using progress variables, the task completion rate is directly embedded into the MPC objective function, fundamentally solving the problem of robots stalling, wandering, or failing to reach the destination on complex paths. Furthermore, a curvature-adaptive forward-looking mechanism is designed, enabling the controller's forward-looking point to intelligently adjust according to the path curvature, and using this as the upper limit of the forward projection search, ensuring the causality and local optimum of the reference state. By solving the optimal control sequence in the finite-time domain online, the robot is driven to accurately track the reference trajectory, avoid sudden obstacles, and ensure the stability of sensor data acquisition.

[0051] This invention does not simply concatenate Hybrid A* and MPC, but rather achieves complementary advantages through deep coupling. The Hybrid A* algorithm, employing multi-scale penalty and dynamic heuristic optimization, serves as the front-end global planner, outputting a kinematically feasible, structurally simple, and highly smooth global path. The nonlinear MPC, integrating progress incentives and adaptive look-forward, acts as the back-end tracking controller, utilizing a high-quality reference trajectory processed by B-splines to achieve accurate, stable, and oscillatory tracking execution. This deep collaboration between the two forms a complete motion planning framework, from intelligent front-end planning to precise back-end execution.

[0052] This method is simple to implement and low in cost, effectively improving the autonomous navigation capability, safety, and mission success rate of mobile robots in petrochemical inspection scenarios. Through multi-dimensional collaborative optimization, this invention can effectively solve the key technical bottlenecks of mobile robot motion planning methods in petrochemical inspection scenarios, achieving kinematically feasible and geometrically smooth global path generation and high-precision, robust local trajectory tracking. It is particularly suitable for mobile inspection scenarios in high-temperature, high-pressure, flammable, explosive, or toxic areas in the petrochemical industry, as well as practical application scenarios with extremely high requirements for safety, stability, and mission reliability, such as emergency rescue and unmanned driving.

Claims

1. A motion planning method for an inspection robot based on hybrid A* and model predictive control, characterized in that, Includes the following steps: Step 1: Environment modeling and raster map construction; By establishing a Cartesian coordinate system, the inspection area is divided into regular grid cells, environmental information is recorded and marked, and obstacles are expanded to generate a structured safety cost map. Step 2: Hybrid based on heuristic function optimization Global path planning: First, the starting point and ending point are processed as inputs, a continuous state discretization method is constructed and discretization transformation is completed, and the search data structure and starting node are initialized. Then, the six discrete motion modes of the robot are vectorized, a motion mode consistency penalty term is constructed and the motion modes are modeled uniformly. At the same time, a dynamic weight adjustment strategy is designed based on the search progress to dynamically balance the convergence speed and global optimality in the path search. Subsequently, through node expansion, multi-dimensional actual cost calculation and ReedsShepp curve heuristic cost calculation, the search nodes are updated and the initial global path is backtracked to generate. Step 3: Global path smoothing; The initial global path is fitted with a cubic B-spline curve, and a cumulative arc length sequence is constructed by uniform sampling and arc length parameterization to generate a smooth reference trajectory with continuous curvature and high-order differentiability. The tangent direction and curvature of the path are then calculated. Step 4: Integrating Progress Excitation and Adaptive Forward-Looking Nonlinear MPC Local Trajectory Tracking; First, a continuous-time nonlinear model is established based on the kinematics principle of differential drive robots. Discrete-time state update equations are obtained through first-order Euler discretization, and the future multi-step state sequence is recursively calculated. Next, the projected arc length of the robot's current position onto the nearest point on the arc-length parameterized reference path is defined as the progress variable. The task completion degree and relative position are quantified by minimizing the Euclidean distance. Then, the forward-looking distance is dynamically adjusted based on the path curvature, shortening in curves and extending in straight lines, and the arc length of the forward-looking point is calculated to determine the target range for future trajectory tracking. Further, in the prediction time domain, the predicted position is projected onto a specific forward window of the reference path to generate a reference state sequence matching the robot's future trajectory. Then, by calculating the state error vector, an objective function containing state deviation, control energy consumption, and progress excitation terms is constructed. Kinematic, control, and forward-looking point constraints are applied to enable the optimizer to select the control sequence that maximizes future progress. Finally, the optimal control problem is solved online in each control cycle. The first control variable is sent to the underlying driver to update the state and environmental information, and the cycle continues until all inspection tasks are completed.

2. The motion planning method for an inspection robot based on hybrid A* and model predictive control according to claim 1, characterized in that, In step 1, the process of environmental modeling and raster map construction is as follows: S11: Establish a global Cartesian coordinate system with the starting point as the origin, and define the boundary of the raster map. , and According to the predefined resolution The inspection area is divided into regular grid units and the environmental information of each unit is recorded. S12: Mark obstacles as impassable units and expand them to generate a structured safety cost map.

3. A motion planning method for an inspection robot based on hybrid A* and model predictive control as described in claim 1 or 2, characterized in that, In step 2, the Hybrid is optimized based on the heuristic function. The global path planning process is as follows: S21: Input and processing of starting and ending points and initialization of data structures; construct a continuous state discretization method including discretization calculation formula, coordinate offset calculation formula and continuous coordinate restoration formula, and complete the input verification and discretization transformation of the starting and ending points; initialize the core data structures of open and closed lists, and create a starting search node based on the starting discrete data and add it to the open list; S22: Discrete motion pattern vectorization; The six discrete motion modes of the robot are abstracted into two-dimensional unit pattern vectors; S23: Define the motion pattern consistency penalty; By constructing a motion pattern consistency penalty term consisting of direction change penalty and direction reversal penalty, the six discrete motion patterns are abstracted into two-dimensional unit pattern vectors for unified modeling; S24: Define the dynamic weight adjustment strategy; Based on the current node to the target node The ratio of distance to the global shortest path length defines the search progress index. By dynamically adjusting the weights of the actual cumulative cost and the heuristic estimated cost, a balance is achieved between strong directional rapid convergence in the early stage of the search and fine optimization of the global optimum in the later stage of the search. S25: Node Expansion and Cost Calculation; The optimal node is selected from the open list and its continuous coordinates are restored. Sub-node trajectories are generated based on six motion modes. Sub-node discretization and validity checks are performed. The actual cost, including path length, motion mode consistency penalty, and heading angle smoothness penalty, is calculated. The incompleteness heuristic cost is calculated using the Reeds-Shepp curve analytical search method based on the normalized coordinate system. The total cost is determined by combining a dynamic weight strategy. The addition and updating of sub-nodes in the open list are managed until the termination condition is met. Then, the initial global path is generated through path backtracking.

4. The motion planning method for an inspection robot based on hybrid A* and model predictive control according to claim 3, characterized in that, In step 2, S21, the input and processing of the starting and ending points, as well as the initialization of the data structure, are as follows: S211: Construct a continuous state discretization method; construct the discretization calculation formula, coordinate offset calculation formula, and continuous coordinate restoration formula according to formula (1), formula (2), and formula (3), respectively; ， ， （1）； In the formula, , Location grid resolution; It is the discretization step size of the heading angle; ， ， （2）； In the formula, ; ; ; ， ， （3）； S212: Input, verification, and discretization of the start and end points; receiving continuous coordinates of the start point. and endpoint continuous coordinates As input; verify whether the position coordinates are within the environmental boundaries. At the same time, check the heading angle. Is it within physical limitations? ; The continuous state discretization method is used to separate the continuous coordinates of the starting point. and endpoint continuous coordinates Convert to discrete coordinates and offsets, denoted as the starting point discrete data respectively. and endpoint discrete data This serves as baseline data for subsequent searches; S213: Initialize the core data structures; initialize the open list `open_list` as a min-heap priority queue, with each node storing discrete coordinates. Offset ( ), actual cost Inspirational Cost Total cost and parent node pointers; initialize the closed list (closed_list) as a hash table to store the discrete coordinates of explored nodes. To quickly detect duplicate access; S214: Create the starting search node and add it to the open list; Based on the starting discrete data Create the starting search node Set the starting search node of ; Calculate the starting search node The cost of inspiration As shown in formula (4); （4）； In the formula, Starting search node Ignore obstacle heuristics for incompleteness constraints; ; Starting search node Based on minimum turning radius Heuristic value for the length of the Reeds-Shepp curve; Calculate the starting search node Total cost As shown in formula (5); （5） Start search node Add it to the open list (open_list) as the starting point for path searching.

5. The motion planning method for an inspection robot based on hybrid A* and model predictive control according to claim 4, characterized in that, In step 2, S23, the process of defining the motion pattern consistency penalty term is as follows: Pattern vector based on parent node and the pattern vector of the current node Define motion pattern consistency penalty As shown in formula (6); （6）； In the formula, The weighting coefficient for the motion pattern consistency penalty term; As a comprehensive index of consistency in movement patterns, ， ， ， To reverse the penalty weights, and These are the components of the pattern vector along the Y-axis in the robot's local coordinate system. It is a symbolic function; In step 2, S24, the process of defining the dynamic weight adjustment strategy is as follows: Define search progress metrics As shown in formula (7); （7）； In the formula, To start from the current node To the target node The shortest path length; To start from the starting point To the target node The global shortest path length; The total cost function for constructing nodes is shown in formula (8); （8）； In the formula, , For threshold parameters; ; For the current node The cost of inspiration, , For the current node Ignore barrier heuristics for incompleteness constraints. For the current node Based on minimum turning radius The heuristic value for the length of the Reeds-Shepp curve.

6. The motion planning method for an inspection robot based on hybrid A* and model predictive control according to claim 5, characterized in that, In step 2, S25, the node expansion and cost calculation process is as follows: S251: Select the current node and restore its continuous state; select the total cost from the open list open_list. The smallest node is used as the current node. The current node is discretized according to the continuous state discretization method. Stored discrete coordinates and offset ( Calculate its continuous coordinates ; S252: Generate child node trajectories; assuming the turning radius of the mobile robot is... The movement step size is And it must meet the minimum turning radius constraint. ; Based on the kinematic model of the mobile robot, sub-nodes are generated to generate forward, backward, left turn, and right turn motion patterns, respectively. continuous coordinates As shown in formulas (9), (10), (11) and (12) respectively; where the geometric relationship between the left rear turn extension node and the right turn extension node is equivalent, and the geometric relationship between the right rear turn extension node and the left turn extension node is equivalent. ， ， （9）； ， ， （10）； ， ， （11）； ， ， （12）； In the formula, ; S253: Child node discretization and validity check; using a continuous state discretization method to discretize child nodes. continuous coordinates Convert to discrete coordinates and offset Simultaneously, the validity of child nodes is checked, including detecting grid boundaries: Is it within the map area?; Check for obstacle collisions: use the inflated raster map query unit. Is it an obstacle or is the distance to an obstacle less than the safe range? Check if it exists in the closed_list; if it does, it means that the child node has been fully explored. S254: Calculate the actual cost of the child node; Calculate child nodes The actual cost As shown in formula (13); （13）； In the formula, The actual cost of the parent node reflects the path length from the starting point to the current node and is a core indicator of path quality. As a smoothness penalty term, , The cost of path length directly influences the direction of path optimization. child node The pattern vector defines the motion pattern consistency penalty term. , , , , and These are the pattern vectors for the current node and the new node, respectively. This refers to the Y-axis component of the current node's pattern vector in the local coordinate system. The Y-axis component of the new node pattern vector in the local coordinate system; This is a penalty term for the smoothness of the heading angle change. , The weighting coefficient for the heading angle change smoothness penalty term; S255: Calculate the heuristic cost and total cost of the child nodes; Calculate child nodes The cost of inspiration As shown in formula (14); child nodes are constructed based on a dynamic weight adjustment strategy. Total cost As shown in formula (15); （14）； In the formula, child node Ignore obstacle heuristics for incompleteness constraints; ; child node Based on minimum turning radius The heuristic value for the length of the Reeds-Shepp curve. ; （15）； In the formula, , , S256: Add child nodes to or update the open list; Check if the child node is in the open list (open_list) or the closed list (closed_list). If not, create a new node and store its child nodes. Relevant node information Set the parent node as Add to open_list; if the child node is already in open_list and If the child node is smaller than the original node, update the node's relevant information settings and reorder the open list (open_list); if the child node is already in the closed list (closed_list), skip the child node and proceed to the next round of search. S257: Termination condition judgment for Hybrid A* algorithm; Check termination conditions: If the endpoint node If the path is added to the closed list (closed_list), the algorithm terminates successfully and outputs the path; otherwise, the open list (open_list) is empty and the endpoint node is not found. If the algorithm fails, it terminates and returns a message indicating that no feasible path exists. S258: Path Backtracking and Continuous Coordinate Transformation When the end node When a node is added to the closed list, a sequence of discrete nodes is generated by backtracking from the endpoint through the parent node pointer. The discrete nodes are then restored to continuous coordinates using a continuous state discretization method, resulting in an initial global path composed of discrete points that meets the preset inspection point traversal requirements, is kinematically feasible, and conforms to the explosion-proof safety distance.

7. The motion planning method for an inspection robot based on hybrid A* and model predictive control according to claim 6, characterized in that, In step 2, S255, during the calculation At that time, the Reeds-Shepp curve analytical search method based on the normalized coordinate system was adopted, and the process is as follows: S255-1: First input the current node pose. and target point pose This serves as the boundary condition for path planning; then, the current node pose is normalized to a coordinate system with the starting point as the origin through coordinate translation and rotation. The initial heading is aligned with the local coordinate system along the x-axis, and the spatial dimensions are divided by the minimum turning radius. This transforms the problem into a standard problem involving unit circle turning, yielding the normalized target position. ; S255-2: Traverse all basic path templates defined in the Reeds-Shepp theory, analytically solve for the length of each segment for each template, and determine whether there is a real solution; for all candidate paths with valid solutions, further check whether they meet the vehicle's kinematic constraints and task requirements; S255-3: Select the shortest total length path from all feasible paths as the optimal RS curve; multiply the path at this unit scale by... By restoring the actual scale and mapping it back to the original global coordinate system through an inverse transformation, a series of continuous state points are generated, forming a complete RS curve that can be used for heuristic estimation or trajectory generation. Its total length is [the total length of the curve]. .

8. The motion planning method for an inspection robot based on hybrid A* and model predictive control according to claim 1, characterized in that, In step 3, the global path smoothing process is as follows: First, cubic B-spline curves are used to smoothly connect the discrete points of the initial global path, and then the arc length is parameterized to generate a smooth parametric trajectory. Subsequently, the number of sampling points on the smoothed curve was uniformly increased. For each point, calculate its corresponding spatial coordinates, and construct a cumulative arc length sequence by progressively accumulating Euclidean distances. The total path length is denoted as . Simultaneously, a one-dimensional interpolation function is constructed to ultimately obtain the interpolation result based on the arc length. Reference path for parameters As shown in formula (16); （16）； In the formula, and Representing the x-axis and y-axis respectively, For path parameters, ; Furthermore, the path tangent direction is calculated using numerical differentiation. and curvature As shown in formulas (17) and (18) respectively; （17）； （18）。 9. The motion planning method for an inspection robot based on hybrid A* and model predictive control according to claim 1, characterized in that, In step 4, the process of fusing progress excitation and adaptive forward look-ahead nonlinear MPC local trajectory tracking is as follows: S41: Establish a discrete-time state-space prediction model for the controlled object; establish a continuous-time nonlinear model based on the kinematics principle of differential drive robot, obtain the discrete-time state update equation by discretization using the first-order Euler method, and recursively calculate the state sequence of future multiple steps in the prediction time domain. S42: Introduce a schedule variable; The progress variable is defined as the projected arc length of the nearest point on the arc length parameterized reference path of the robot's current position. Accurate projection is achieved by minimizing the Euclidean distance, which quantifies the robot's task completion degree and relative position on the inspection path. S43: Adaptive adjustment of forward sight distance; dynamically adjusts the forward sight distance based on the curvature of the current path, automatically shortens it in curved areas to improve tracking accuracy, automatically extends it in straight areas to improve response efficiency, and calculates the arc length of the forward sight point to define the target range for future trajectory tracking; S44: Dynamic calculation of reference state; For each step in the prediction time domain, the predicted position is projected onto the reference path and the search interval is limited to the forward window to dynamically generate a reference state sequence that matches the robot's future trajectory. S45: Construct and solve the nonlinear MPC optimization problem; by calculating the state error vector, construct a nonlinear MPC objective function that includes state deviation, control energy consumption and schedule excitation terms, and apply kinematic constraints, control constraints and forward view constraints so that the optimizer prioritizes the control sequence that maximizes future schedule; S46: Rolling optimization and execution; Solve the finite-time optimal control problem online in each control cycle, send the first control quantity of the optimal control sequence to the underlying driver, and then update the state and environmental information in a loop until the robot completes all inspection tasks.

10. The motion planning method for an inspection robot based on hybrid A* and model predictive control according to claim 1, characterized in that, In step 4, S41, the process of establishing the discrete-time state-space prediction model of the controlled object is as follows: For the status The control variables are Based on the differential drive kinematics principle, a continuous-time nonlinear kinematic model of the robot is constructed, as shown in formula (19); assuming the sampling period is... The continuous-time nonlinear kinematic model is discretized using the first-order Euler method to obtain the discrete-time state update equation, as shown in formula (20); let Within the prediction time domain, the future is calculated recursively. The state sequence of the steps is shown in formula (21); （19）； Discrete state is : （20）； In the formula, , and Let x, y, and y represent the robot's x-coordinate, y-coordinate, and heading angle at time k, respectively. , These represent the speeds of the left and right wheels at time k, respectively. （21）； In the formula, In order to be in Always Predicting the state at any given moment; In step 2, S42, the process of introducing the progress variable is as follows: Define the arc length coordinates corresponding to the nearest point on the arc length parameterized reference path to the robot's current position as the progress variable. As shown in formula (22); （22）； In the formula, Let K be the actual position coordinates of the robot at time k. In step 4, S43, the adaptive adjustment of the forward sight distance is as follows: Based on the current path curvature Dynamically adjust forward viewing distance As shown in formula (23); at the same time, calculate the arc length of the foreseeability point. As shown in formula (24); （23）； In the formula, The baseline forward sight distance for inspection scenarios; This is the curvature adjustment coefficient. >0; , These are the minimum and maximum forward viewing distances for adapting to narrow passages; （24）； In step 4, S44, the dynamic calculation process of the reference state is as follows: For each step in the prediction time domain Predicted location Projected onto the reference path, the search range is limited to the forward window. Calculate the arc length and arc length Reference state As shown in formulas (25) and (26) respectively; （25）； （26）； In step 4, S45, the process of constructing and solving the nonlinear MPC optimization problem is as follows: Calculation in Time prediction State error vector at time step As shown in formula (27); at the same time, a schedule incentive term is introduced to construct a nonlinear MPC objective function, as shown in formula (28); （27）； （28）； In the formula, This is the weight matrix for the state deviation; The weight matrix for controlling the input; The weighting coefficients for the schedule incentives are... >0; In order to be in Time prediction Time-based control input; Construct the optimization problem constraint conditions based on kinematic constraints, control constraints and forward view constraints, as shown in formula (29), so that the optimization problem satisfies the composite condition constraints; （29）； In the formula, It is a nonlinear discrete kinematic model; , These are the minimum and maximum speed vectors allowed for the left and right wheels, respectively. In step 4, S46, the rolling optimization and execution process is as follows: In each control cycle The controller solves the finite-time optimal control problem described by equations (28) and (29) online to obtain the optimal control input sequence. and the first control variable in the sequence Send the data to the underlying driver to drive the robot's movement; Then, the next control cycle begins. The robot status and surrounding environment information are updated, and the MPC optimization solution is performed again. This process is repeated until the robot has traversed all inspection points and reached the final target location.

Images