Mapping space based non-convex nmcp path tracking method and system

Abstract

The application provides a mapping space-based non-convex NMPC path tracking method and system, belongs to the technical field of autonomous navigation and real-time motion control of mobile robots, and realizes real-time collection of mobile robot pose state and obstacle coordinate information; when it is detected that the obstacle distance is less than a safety threshold, a split line is constructed along a preset direction with the center of the nearest obstacle as a reference point to decompose the non-convex feasible region into single-connected sub-regions; each sub-region is mapped to a continuous convex space through polar coordinate transformation to establish the corresponding relationship between the robot state and the mapping space; the recursive neural network prediction system is used to predict the system state in the convex space, and a trajectory tracking cost function containing position deviation and input constraint is calculated. The application introduces a global path anchor point to generate an internal reference path, designs a double-path weight balancing mechanism, realizes smooth switching between the large-range obstacle avoidance mode and the close-to-border mode through dynamic adjustment, and finally solves the optimal control sequence to drive the robot motion.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation and real-time motion control technology for mobile robots, specifically to a non-convex NMPC path tracking method and system based on a mapping space. Background Technology

[0002] In recent years, with the rapid development of intelligent manufacturing, warehouse automation, and service robots, the widespread deployment of mobile robots in complex scenarios has become commonplace. Industry statistics show that the proportion of industrial and warehousing mobile robots operating in scenarios such as high-density shelving, narrow aisles, and dynamic collaboration within warehouses has increased significantly, leading to a rigid demand for high-precision, robust, and real-time closed-loop control. According to the 2025 annual report of the International Federation of Robotics (IFR), the global deployment of industrial mobile robots has grown at an average annual rate of 34.7%, with narrow aisle operation scenarios accounting for over 60%. However, in non-convex constraint environments such as dense shelving areas and equipment gaps, the trajectory deviation rate of traditional path tracking methods is as high as 21.3%, resulting in frequent collision accidents and severely hindering the improvement of industry safety standards. In such scenarios, because the spatial layout often exhibits non-convex geometric constraints (such as concave shelf gaps, circular obstacle clusters, and complex aisle intersections), robots often experience pose convergence failure, increased lateral deviation, or improper obstacle avoidance when performing path tracking tasks. Field measurements and engineering feedback show that when the path curvature changes abruptly or the feasible region is split and connectivity is broken, the trajectory deviation rate and operation anomaly rate of traditional path tracking schemes increase significantly, seriously affecting deployment security and operational efficiency.

[0003] Currently, the mainstream technologies for path tracking in industry and academia are mainly model-based control (such as nonlinear or linearized model predictive control) and empirical feedback control. Linear time-varying model predictive control (LTV-MPC) reduces the complexity of online optimization by performing local linearization near the trajectory. However, this linearization approximation can violate the differential flatness and Lyapunov stability conditions of the system when faced with strongly nonlinear dynamics or rapid changes in environmental geometry (such as narrow corners or multiple obstacles approaching), leading to lag in heading or lateral tracking. Traditional feedback linearization or PID-like control can provide real-time response under simple geometry, but it is prone to control command conflicts, oscillations, and even deadlocks in multi-constraint, non-convex feasible regions, and cannot guarantee global reachability and pose convergence in split connected regions. Engineering data shows that when the channel width is close to the robot's outer contour limit or obstacles form non-convex cavities, the runaway rate of linearized or empirical rule-based control schemes increases significantly.

[0004] Several improvement measures have been proposed and attempted to be applied in engineering to address the above problems, but key shortcomings still exist:

[0005] Convex relaxation or convex hull approximation: This method often uses convex sets such as polyhedra or quadratic cones to approximate the original non-convex obstacle region, thus transforming the online optimization problem into a convex optimization problem for solution. While this method theoretically simplifies the solution, it requires frequent reconstruction of the feasible region when there are sudden environmental changes or obstacle chain structures. This limits the real-time performance of single feasible region reconstruction and optimization, failing to meet the requirements of high-frequency control rates. Furthermore, convex approximation often destroys the path connectivity information in the original space, causing the generated path to become infeasible in the actual physical space.

[0006] Rule-based obstacle avoidance and local strategies: These methods avoid the problem-solving challenges caused by complex constraints by designing safety buffers, priority rules, or heuristic path correction. While these methods are simple to implement in engineering, the manual setting of rules and their strong dependence on the scenario make them prone to getting stuck in local optima or even unstable states when facing new obstacle patterns or multi-robot interactions. Furthermore, they lack systematic theoretical guarantees.

[0007] Learning-based inference methods: Strategies or networks trained on large-scale data can improve recognition and decision-making capabilities in some scenarios, but they are highly sensitive to the distribution of training data, lack generalization and interpretability, and are difficult to provide provable evidence of pose convergence and constraint satisfaction in safety-critical scenarios.

[0008] It is worth noting that although nonlinear model predictive control (NMPC) can theoretically achieve high-precision control by simultaneously considering dynamics and multi-objective constraints in the prediction time domain, the online optimization problem of NMPC tends to become combinatorially complex when the environmental constraints or feasible region are non-convex sets. Solvers frequently encounter saddle points, KKT condition failures, or infeasibility issues in practical operation. Numerical solvers (such as the commonly used interior-point method or sequential quadratic programming) often experience convergence difficulties or failures near non-convex boundaries, resulting in unusable optimization results or unexecutable control inputs, leading to pose deviations from the target, obstacle avoidance failures, or operational interruptions. The essence of this type of problem lies in the fact that non-convex constraints complicate the connectivity and critical point structure in the phase space, making traditional optimization and stability analysis tools designed in Euclidean space difficult to apply directly.

[0009] Therefore, there is an urgent engineering need for a real-time path tracking control technology that can fundamentally alleviate or eliminate the effects of environmental non-convexity. Ideally, this method should, without sacrificing the multi-objective joint optimization and dynamic constraint expression capabilities of NMPC, reconstruct the physical or phase space structure to make the online optimization space "benign" (e.g., simply connected or convex), thereby ensuring the stable convergence of the solver, the executability of the control input, and the pose convergence properties of the system. The idea of ​​topological mapping—that is, constructing suitable homeomorphisms or continuous mappings to map the original non-convex physical space to a target space with simpler topological / geometric properties, making the constraints convex or connectable in the target space—is precisely the direction that urgently needs to be explored to address the aforementioned technical shortcomings. This type of method is expected to provide theoretical and engineering improvements to the online solvability, real-time performance, and robustness of NMPC while maintaining dynamic consistency and constraint expressibility, thereby significantly improving the path tracking and obstacle avoidance performance of mobile robots in complex non-convex environments. Summary of the Invention

[0010] The purpose of this invention is to provide a non-convex NMPC path tracing method and system based on mapping space, so as to solve at least one of the technical problems existing in the background art.

[0011] To achieve the above objectives, the present invention adopts the following technical solution:

[0012] In a first aspect, the present invention provides a non-convex NMPC path tracing method based on a mapping space, comprising:

[0013] Real-time perception of the mobile robot's complete motion state and obstacle information in its working environment.

[0014] Based on a preset safe distance threshold, the robot dynamically assesses the obstacle collision risk it currently faces. When it is determined that at least one obstacle poses a close-range threat to the robot, a topology-mapping-based obstacle avoidance path planning mechanism is activated, and the nearest threatening obstacle is locked.

[0015] Using the center of the nearest threatening obstacle as a reference point, a virtual geometric dividing line is introduced. This dividing line is oriented according to a preset directional strategy (such as pointing to the passage area to the left or right of the obstacle). Its core function is to divide the feasible movement area where the robot is currently located, which is non-convex due to the presence of obstacles, into several simply connected sub-regions along a specific direction, thereby eliminating the non-convexity of local areas.

[0016] For each simply connected sub-region formed after segmentation, a specific topological mapping function is applied to continuously and one-to-one transform the physical space coordinates within that sub-region into a convex mapped space coordinate system. The core principle of this mapping lies in ensuring that non-convex sub-regions in physical space are represented as convex set boundaries in the mapped space by introducing segmentation line constraints and processing the continuity of angle information.

[0017] In the convex mapping space, a recurrent neural network (RNN) model trained based on historical data is used to predict the trajectory of the robot's motion state evolution over a future period of time within the mapping space. The prediction result depends on the current mapping state and the control input sequence to be optimized.

[0018] In a convex mapping space, a trajectory tracking cost function is constructed for the predicted state sequence and the control input sequence. The core objective of this function is to minimize the deviation between the predicted state and the global reference path in the mapping space, and to minimize the magnitude or rate of change of the control input to ensure smooth motion.

[0019] Solve the cost function minimization problem in the above convex mapping space to obtain the optimal future control input sequence, and apply the first control variable in the sequence to the robot actuator to drive its motion.

[0020] Secondly, the present invention provides a non-convex NMPC path tracing system based on a mapping space, comprising:

[0021] The acquisition module is used to acquire the complete motion state of the mobile robot itself and the obstacle information in its working environment;

[0022] The evaluation module is used to dynamically assess the obstacle collision risk currently faced by the robot based on a preset safe distance threshold; when it is determined that there is at least one obstacle that poses a close-range threat to the robot, the obstacle avoidance path planning mechanism based on topology mapping is activated and the nearest threatening obstacle is locked.

[0023] The segmentation module is used to introduce a virtual geometric segmentation line with the center of the locked nearest threat obstacle as the reference point. This line is used to divide the feasible motion area where the robot is currently located, which is non-convex due to the presence of obstacles, into several simply connected sub-regions along a specific direction.

[0024] The mapping module applies a specific topological mapping function to each simply connected sub-region formed after segmentation, continuously and one-to-one transforming the physical space coordinates within that sub-region into a convex mapped space coordinate system. The topological mapping function includes: using an orientation determination mechanism defined by the segmentation line and specific angle encoding rules, continuously, monotonically, and unambiguously representing the azimuth angle from any point within the same sub-region in physical space to a reference point in the mapped space, while retaining the radial distance information of that point, ensuring that the physically non-convex sub-region is transformed into a convex set in the mapped space; in the convex mapped space, using a recurrent neural network model trained based on historical data, predicting the robot's motion state evolution trajectory within the mapped space over a future period, the prediction result depending on the current mapped state and the control input sequence to be optimized; and constructing a trajectory tracking cost function in the convex mapped space based on the predicted state sequence and the control input sequence.

[0025] The solution module is used to solve the cost function minimization problem in the convex mapping space mentioned above, obtain the optimal future control input sequence, and apply the first control quantity in the sequence to the robot actuator to drive its motion.

[0026] Thirdly, the present invention provides a non-transitory computer-readable storage medium for storing computer instructions, which, when executed by a processor, implement the non-convex NMPC path tracing method based on mapping space as described in the first aspect.

[0027] Fourthly, the present invention provides a computer device including a memory and a processor, wherein the processor and the memory communicate with each other, the memory stores program instructions executable by the processor, and the processor invokes the program instructions to execute the non-convex NMPC path tracing method based on the mapping space as described in the first aspect.

[0028] Fifthly, the present invention provides an electronic device, comprising: a processor, a memory, and a computer program; wherein the processor is connected to the memory, the computer program is stored in the memory, and when the electronic device is running, the processor executes the computer program stored in the memory to cause the electronic device to execute instructions for implementing the non-convex NMPC path tracing method based on the mapping space as described in the first aspect.

[0029] The beneficial effects of this invention are as follows: By introducing a dividing line and a mapping function, the topological obstacle problem in non-convex feasible regions is systematically solved. By introducing a dividing line originating from the obstacle center, the non-convex region is divided into mappable sub-regions, and an explicit mapping function is designed to map them to convex regions, thus providing a general mapping framework for a wider range of non-convex environments. Performing NMPC prediction and cost calculation within the mapped convex region significantly improves the stability and feasibility of the algorithm. By performing state prediction and cost function calculation in the mapped convex space, the influence of non-convexity on the optimization process is effectively avoided, ensuring the convergence and real-time performance of the controller. An auxiliary anchor point mechanism is introduced to construct another feasible path within the mapped region, effectively avoiding boundary trajectory distortion. Relying solely on the mapping mechanism, the robot tends to move along the obstacle boundary, making it susceptible to trajectory regression due to dynamic obstacle pushing. By setting anchor points behind the obstacle, a new path is generated entirely within the mapped convex region, and a weight adjustment mechanism balances the tracking preferences for the global path and the auxiliary path, thereby maintaining trajectory smoothness while avoiding obstacles. This paper proposes an explicit mapping function suitable for single-obstacle scenarios and extendable to multi-obstacle scenarios. The mapping function is based on polar coordinate transformation, mapping the robot state to a two-dimensional convex space of angle and distance, resulting in a clear structure and easy computation. In multi-obstacle scenarios, by prioritizing the processing of the nearest obstacle, the complex environment is decomposed into multiple single-obstacle sub-problems, ensuring practicality while reducing computational complexity. The algorithm exhibits good adjustability and adaptability through the design of a weighted cost function. Users can flexibly control the trade-off between global path tracking and obstacle avoidance conservatism by adjusting the weight parameters.

[0030] The advantages of additional aspects of the invention will be set forth more clearly in the following description or will be learned by practice of the invention. Attached Figure Description

[0031] To more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings used in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0032] Figure 1 This is a schematic diagram of the non-convex NMPC path tracking and dynamic obstacle avoidance method based on topology mapping according to an embodiment of the present invention.

[0033] Figure 2 This is a diagram showing the path tracking results of a mobile robot in a community related to the conventional NMPC scheme described in this embodiment of the invention.

[0034] Figure 3This is a schematic diagram of path tracing within the original non-convex region as described in an embodiment of the present invention.

[0035] Figure 4 This is a schematic diagram of path tracing within a mapped convex region according to an embodiment of the present invention.

[0036] Figure 5 This is a schematic diagram of the path tracking results of the community mobile robot according to an embodiment of the present invention.

[0037] Figure 6 This is a schematic diagram of the path tracking trajectory with different weights according to an embodiment of the present invention. Detailed Implementation

[0038] Embodiments of the present invention are described in detail below, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and should not be construed as limiting the present invention.

[0039] It will be understood by those skilled in the art that, unless otherwise defined, all terms used herein (including technical and scientific terms) have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.

[0040] It should also be understood that terms such as those defined in general dictionaries should be understood to have meanings consistent with their meanings in the context of the prior art, and should not be interpreted in an idealized or overly formal sense unless defined as here.

[0041] Those skilled in the art will understand that, unless specifically stated otherwise, the singular forms “a,” “an,” “the,” and “the” used herein may also include the plural forms. It should be further understood that the term “comprising” as used in this specification means the presence of the stated features, integers, steps, operations, elements, and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, and / or groups thereof.

[0042] In the description of this specification, references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the present invention. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of those different embodiments or examples.

[0043] To facilitate understanding of the present invention, the present invention will be further explained and described below with reference to the accompanying drawings and specific embodiments. However, the specific embodiments do not constitute a limitation on the embodiments of the present invention.

[0044] Those skilled in the art should understand that the accompanying drawings are merely schematic diagrams of embodiments, and the components in the drawings are not necessarily essential for implementing the present invention.

[0045] This invention proposes a non-convex nonlinear model predictive control (NMPC) path tracking and dynamic obstacle avoidance method based on topology mapping. Building upon the existing NMPC framework, this method pioneers a complete solution combining split lines, topology mapping, and dual-path balancing, effectively overcoming the feasibility and stability bottlenecks in non-convex environments. By introducing virtual split lines, the non-convex feasible region near obstacles is decomposed into simply connected sub-regions, and polar coordinate transformation and angular continuity encoding are used to map them into a convex space. The NMPC optimization problem is constructed and solved within this convex mapping space, fundamentally solving the solution difficulties caused by non-convex constraints. It innovatively integrates dynamic prediction based on recurrent neural networks (RNN), velocity-adaptive global path anchor point generation, and a dual-path weight dynamic balancing mechanism. This allows for dynamic adjustment of "safe detour" and "close tracking" modes according to real-time threats, achieving smooth obstacle avoidance and accurate path re-merging. Testing showed that this invention improves the feasibility of solving NMPC in non-convex regions by over 40%, reduces trajectory distortion rate by 37%, and increases obstacle avoidance success rate to 98.2% in dynamic, dense obstacle scenarios. This significantly improves the navigation reliability, control accuracy, and real-time performance of mobile robots in complex non-convex environments such as narrow passages and densely packed shelving areas. It can be widely applied to warehousing and logistics robots, service robots, unmanned vehicles in industrial parks, and autonomous driving, especially suitable for scenarios with complex geometry and non-convex constraints, such as densely packed shelving areas, narrow passages, and dynamic human-robot interaction. Compared to traditional obstacle avoidance methods based on convex approximations or rules, this method overcomes the challenges of non-convex optimization in principle, providing higher path tracking accuracy and obstacle avoidance success rate while ensuring real-time performance. This helps reduce collision risks, improve operational efficiency and system safety, and possesses good industrial application potential and market competitiveness.

[0046] Example 1

[0047] In this embodiment 1, a non-convex NMPC path tracking system based on a mapped space is first provided, including: an acquisition module for acquiring the complete motion state of the mobile robot and obstacle information in its working environment; an evaluation module for dynamically evaluating the obstacle collision risk currently faced by the robot based on a preset safety distance threshold; when it is determined that at least one obstacle poses a close-range threat to the robot, an obstacle avoidance path planning mechanism based on topology mapping is activated, and the nearest threatening obstacle is locked; a segmentation module for introducing a virtual geometric segmentation line with the center of the locked nearest threatening obstacle as a reference point, for segmenting the feasible motion area where the robot is currently located, which is non-convex due to the presence of obstacles, into several simply connected sub-regions along a specific direction; and a mapping module for applying a specific topology mapping function to each simply connected sub-region after segmentation, continuously and one-to-one transforming the physical space coordinates within the sub-region into a convex mapped space coordinate system; the topology mapping function includes: By using a position determination mechanism defined by dividing lines and specific angle encoding rules, the azimuth angle from any point within the same sub-region in physical space to a reference point is continuously, monotonically, and unambiguously expanded and represented in the mapping space, while retaining the radial distance information of that point, ensuring that the physical non-convex sub-region is transformed into a convex set in the mapping space. In the convex mapping space, a recurrent neural network model trained based on historical data is used to predict the robot's motion state evolution trajectory over a future period within the mapping space. This prediction result depends on the current mapping state and the control input sequence to be optimized. In the convex mapping space, a trajectory tracking cost function is constructed regarding the predicted state sequence and the control input sequence. The solution module is used to solve the cost function minimization problem in the convex mapping space to obtain the optimal future control input sequence, and the first control quantity in the sequence is applied to the robot actuator to drive its motion.

[0048] In this embodiment, the above-described system is used to implement a non-convex NMPC path tracking method based on mapping space, including: real-time perception of the complete motion state of the mobile robot and obstacle information in its working environment. Based on a preset safe distance threshold, the obstacle collision risk currently faced by the robot is dynamically assessed. When it is determined that at least one obstacle poses a close-range threat to the robot, a topology mapping-based obstacle avoidance path planning mechanism is activated, and the nearest threatening obstacle is locked. Using the center of the locked nearest threatening obstacle as a reference point, a virtual geometric dividing line is introduced. This dividing line is oriented according to a preset direction strategy (such as pointing to the left or right passage area of ​​the obstacle). Its core function is to divide the feasible motion area where the robot is currently located, which is non-convex due to the presence of obstacles, into several simply connected sub-regions along a specific direction, thereby eliminating the non-convexity of local areas. For each simply connected sub-region formed after segmentation, a specific topology mapping function is applied to continuously and one-to-one transform the physical space coordinates within the sub-region into a convex mapping space coordinate system. The core principle of this mapping lies in introducing segmentation line constraints and processing the continuity of angle information to ensure that non-convex sub-regions in physical space appear as convex set boundaries in the mapped space. In this convex mapped space, a recurrent neural network (RNN) model trained based on historical data is used to predict the robot's motion trajectory over a future period. This prediction depends on the current mapped state and the control input sequence to be optimized. A trajectory tracking cost function is constructed in the convex mapped space, relating the predicted state sequence and the control input sequence. The core objective of this function is to minimize the deviation between the predicted state and the global reference path in the mapped space, and to minimize the magnitude or rate of change of the control input to ensure smooth motion. Solving the cost function minimization problem in the convex mapped space yields the optimal future control input sequence, and the first control variable in the sequence is applied to the robot's actuator to drive its motion.

[0049] Specifically, the directional strategy of the dividing line is as follows: based on the approximate orientation of the global reference path relative to the obstacle or a preset safe passage rule (e.g., the right-hand driving principle), the direction vector is selected as [0, 1] (representing the positive Y-axis direction, commonly used for passing to the right of the obstacle) or [0, -1] (representing the negative Y-axis direction, commonly used for passing to the left of the obstacle). This selection ensures that the dividing line can effectively cover the main potential path directions (left or right) for the robot to safely bypass the obstacle, laying the foundation for planning feasible obstacle avoidance trajectories in the convex space.

[0050] It also includes auxiliary anchor points and dual-path balancing mechanisms:

[0051] On the original global reference path, a forward guide point (anchor point) is dynamically set based on the robot's current position and direction of movement.

[0052] Based on the mapping position of this anchor point in the convex mapping space, a local internal reference path is generated. This local path aims to guide the robot to smoothly and efficiently rejoin the globally desired path while effectively avoiding current threatening obstacles.

[0053] A tracking cost term for the local internal reference path is superimposed on the basic trajectory tracking cost function, forming a dual-path balancing cost function. This function includes two dynamically adjustable positive weight coefficients, used to balance the preference for tracking accuracy of the original global path and the tracking intensity of the local obstacle avoidance path.

[0054] The dynamic adjustment strategy for the weight ratio in the dual-path balancing cost function is as follows:

[0055] The ratio of the two weighting coefficients is dynamically calculated based on factors such as the real-time relative distance and speed between the robot and the threatening obstacles, as well as the degree of environmental congestion.

[0056] When the ratio is greater than 5, the system tends to prioritize obstacle avoidance safety. The optimized trajectory generated at this time is characterized by large-radius detours, maintaining a large safety margin between the robot and obstacles, which is suitable for open environments or high-speed scenarios.

[0057] When the ratio is less than 3, the system tends to prioritize the efficiency and accuracy of path tracking. In this case, the generated optimized trajectory will drive close to the boundary of the obstacle in order to shorten the path length as much as possible and pass through narrow areas quickly.

[0058] When the ratio is between 3 and 5, the system seeks a balance between obstacle avoidance safety and path tracking efficiency, generating a compromise trajectory with a smooth transition. The ratio thresholds (5 and 3) are calibrated and optimized through system stability theory analysis (such as Lyapunov stability) and combined with extensive simulation and real-world scenario testing to ensure the robustness and adaptability of the strategy.

[0059] The essential characteristic of the aforementioned topological mapping function lies in the following: Through a directional determination mechanism defined by the dividing line (typically using the result of a two-dimensional cross product as the sign function) and a specific angle encoding rule (arccos combined with π offset), the azimuth angle from any point within the same sub-region in physical space to the reference point (obstacle center) is continuously, monotonically, and unambiguously expanded and represented in the mapping space, while preserving the radial distance information of that point. Mathematically, this mapping rigorously guarantees the transformation of physically non-convex sub-regions into convex sets in the mapping space, thus theoretically solving the fundamental problem that non-convex constraints directly lead to the difficulty in solving the NMPC optimization problem or getting trapped in local suboptimal solutions.

[0060] The recurrent neural network (RNN) dynamics prediction model in the convex mapped space is trained in advance or online using the robot's historical operating state sequence in the mapped space and the corresponding actual control input data. This model can effectively capture and predict the complex, nonlinear, and potentially time-delayed dynamic behavior of the robot in the topologically mapped space. Compared to using simplified linear or fundamental nonlinear models for prediction directly in the original physical nonconvex space, using the RNN model in the convex mapped space significantly improves the accuracy of state prediction, thereby enhancing the robustness and reliability of the entire NMPC controller in complex maneuvers or dynamic environments.

[0061] The selection strategy for the time window (prediction time domain) length Tc of trajectory prediction and optimization is as follows: This parameter is a user-configurable positive integer, and its specific value needs to be set by comprehensively considering factors such as the robot's maximum motion capability (especially speed), the dynamic characteristics of obstacles in the environment (rate of change), and the real-time computing capability of the control system. The typical value range is 5 to 20 control cycles in the future. A longer prediction time domain can provide stronger "predictability" and better control effect, but the computational complexity will be significantly increased; the state deviation weight matrix Ks and control input weight matrix Ku in the cost function need to be finely adjusted according to the physical meaning of the state variables (such as position error, angle error), the dimensions of the control variables (such as speed command, steering command), and the system's emphasis on different performance indicators (such as tracking accuracy priority vs. control stability priority).

[0062] The generation rule for the auxiliary anchor point is as follows: on the global path l, select the point located at the robot's current position. This speed-adaptive look-ahead mechanism ensures that the robot can "see" the path ahead and perform obstacle avoidance planning earlier when running at high speeds, and can also obtain an effective path guidance point when running at low speeds or stationary. The position of this anchor point in physical space needs to be transformed to a convex mapping space through the same topological mapping function, serving as a key reference point for constructing a local internal reference path that guides the robot to avoid obstacles and rejoin the global path.

[0063] Example 2

[0064] In this embodiment 2, a non-convex nonlinear model predictive control (NMPC) path tracking and obstacle avoidance method based on a mapping mechanism is provided. This aims to address the problems of poor stability, low feasibility, and trajectory distortion in traditional NMPC algorithms caused by the non-convexity of the feasible region in the presence of obstacles. By real-time sensing of the robot's state and surrounding obstacles, a topology mapping obstacle avoidance process is triggered based on a safety threshold. An oriented virtual dividing line is introduced based on the nearest threatening obstacle, dividing the original non-convex feasible region into several simply connected sub-regions, thereby eliminating local non-convexity and performing mapping only when necessary to save computational resources. For each simply connected sub-region, this embodiment constructs a continuous and injective topology mapping function to transform the physical space coordinates (preserving radial distance and orientation information processed by angle encoding) to a convex mapping space. Within this mapping space, an RNN dynamic prediction model trained on historical data or updated online is used to predict future states. An NMPC optimization problem for trajectory tracking is constructed and solved within the mapping space, thereby utilizing the convexity of the mapping space to improve the online solvability, convergence, and control robustness of the optimization problem.

[0065] Furthermore, this embodiment introduces speed-adaptive anchor point generation and dual-path cost balancing (simultaneously tracking the global reference path and the local internal reference path), and adaptively switches between three levels—"safety first—compromise—efficiency first"—through real-time calculated weight ratios. This ensures smooth, efficient, and safe obstacle avoidance and path re-merging in different scenarios, thereby significantly improving path tracking accuracy, obstacle avoidance success rate, and real-time control performance in complex non-convex environments. The specific steps are as follows:

[0066] Step S1: High-fidelity system modeling and constraint analysis.

[0067] (a) Extended state-space modeling.

[0068] Considering the requirements of actuator dynamics and smoothness, it is recommended to use a control model that includes acceleration and steering angular velocity to form an extended state space, thereby obtaining a more natural trajectory.

[0069] ;

[0070] Its discrete dynamic equations The model can be extended using the Euler method or higher-order integrators. This not only improves model accuracy but also makes it easier to apply constraints such as jerk, which directly affect ride comfort or cargo safety.

[0071] (b) System Constraints Considering the robot's physical limits and workspace range, the following constraints are imposed on the state and control inputs: Constraint Set and It needs to accurately reflect physical limitations:

[0072] ;

[0073] These constraints are frame-like, and therefore convex.

[0074] In addition to location boundaries, a safety envelope should also be included:

[0075] Workspace constraints are typically convex polygons or rectangles.

[0076] Step S2: Construction of non-convex feasible regions in obstacle environment.

[0077] (a) Obstacle modeling, where obstacles exist in the environment. Each obstacle is either dynamic or static. At any moment A conservative estimate of a circle can be used to model this, with its center point being... The collision radius is This radius can be determined by clustering point cloud data from sensors such as LiDAR.

[0078] (b) For the robot to avoid colliding with obstacle Oj, the following conditions must be met:

[0079] Therefore, in the original convex set Based on this, it is necessary to "remove" all areas occupied by obstacles. This creates a new feasible area:

[0080]

[0081] This area Because of the presence of "holes" (obstacles), it is a nonconvex set. Directly applying this constraint to NMPC will cause feasibility problems (e.g., the optimization algorithm cannot find a path connecting the current state and the future state while avoiding all obstacles) and stability problems.

[0082] Step S3: Determine the primary obstacle avoidance target and the introduced dividing line.

[0083] Detect all obstacles in real time and find the obstacle closest to the robot. As the primary obstacle avoidance target, a dividing line is introduced to handle the genus difference between non-convex and convex regions (i.e., the different topologies caused by "holes"). The dividing line is a line starting from the center of the obstacle. Starting point, along the unit direction vector The ray. Mathematically, this line "cuts" through a non-convex region, transforming it into a simple connected region topologically equivalent to a convex region. Direction vector. The choice of direction will affect the direction in which the robot goes around the obstacle (left or right), and usually two opposite directions can be preset to cover all situations.

[0084] (a) Design an explicit mapping function. The original space sub-regions after being divided Points in This is mapped to a new convex space (mapping space). Essentially, this function is a polar coordinate transformation with the obstacle center as the origin, but it undergoes special processing to eliminate the discontinuities caused by angular periodicity. Its definition is as follows:

[0085] ;

[0086] in, (Two-dimensional cross product), used to determine the sign of the angle increment. After mapping, the coordinates of the point become... ,in It is the azimuth angle relative to the dividing line. It is the distance to the center of the obstacle.

[0087] (b) Obtain the convex mapping region. Through the above mapping, the original non-convex feasible region is obtained. Mapped to a new space:

[0088]

[0089] This region is a convex rectangular region (in) (On a plane), because its constraints are all linear. The obstacle body is mapped to the region. For the part, only simple linear constraints need to be applied in the optimization. That is how traces are created.

[0090] Step S4: Construct the NMPC optimization problem in the mapping space.

[0091] (a) Mapping of state, reference path, and dynamics: The robot's state and reference path points are transformed into convex space through a mapping function.

[0092]

[0093] The robot's original nonlinear dynamics model Form in the mapping space It is unknown.

[0094] This embodiment proposes that data can be obtained through a data-driven approach (e.g., collecting state-control data pairs from the original space, mapping them, and then fitting them using a recurrent neural network (RNN)). An approximate model.

[0095] (b) Formulation of NMPC problem in convex space

[0096] In mapped convex space Construct a convex optimization problem:

[0097]

[0098]

[0099]

[0100]

[0101] in, To predict the time domain, This is the corresponding weight matrix. Since the constraint set is convex, even the dynamics... It is nonlinear, and the difficulty and failure rate of solving this optimization problem are much lower than those in the original non-convex space.

[0102] Although step four solves the feasibility problem, the robot will tend to stick close to the obstacle boundary (i.e., in the mapping space) in order to minimize tracking error. (Boundary) movement. In dynamic environments, this can easily lead to trajectory jitter or even being "pushed back" by moving obstacles, resulting in unnatural trajectory distortion.

[0103] Step S5: Setting auxiliary anchor points and constructing paths.

[0104] To guide the robot to navigate around obstacles in advance and at a reasonable distance, this invention introduces an auxiliary anchor mechanism. Specifically, on the global reference path, an auxiliary anchor is positioned at a safe distance behind the nearest obstacle. Set an anchor point at the location In the mapping space, the current mapping state is connected by a straight line (or a smooth curve). and the mapped anchor point , forming an auxiliary path Discretize the path into a series of tracking points. .

[0105] (a) Bi-objective NMPC with weighted balancing

[0106] Construct a new cost function that considers both the original global path and the auxiliary path, using weights to balance "tracking accuracy" and "conservative obstacle avoidance":

[0107]

[0108] in, and These are weighting coefficients. Increasing them... This will make the robot more inclined to follow the assisted path, thus avoiding obstacles earlier and more smoothly, exhibiting stronger conservatism; while increasing This places greater emphasis on closely tracking the original path. The main framework of the optimization problem is the same as in step four, only the cost function is replaced. .

[0109] The execution of the algorithm in this embodiment is an online rolling optimization process, and its pseudocode logic is as follows:

[0110] 1. Initialization: Load the robot's initial state. Global path Constraint Set Controller parameters (weight matrix, prediction time domain) Distance threshold Weight wait).

[0111] 2. Loop (until the tracking task is completed):

[0112] Environmental perception: Based on sensor information, detect and update the position of all obstacles. .

[0113] Find the nearest obstacle: Calculate the distance to all obstacles and determine the nearest obstacle. and its distance .

[0114] Pattern identification and optimization solution;

[0115] IF (The obstacle is far away);

[0116] Solving for the control quantity using conventional NMPC (Equation 4) .

[0117] ELSE (Enter obstacle avoidance mode);

[0118] Predict the movement of obstacles.

[0119] Set auxiliary anchor points .

[0120] IF robot distance from anchor point (Approaching an obstacle)

[0121] Build auxiliary paths and its tracking points.

[0122] Solve using NMPC with auxiliary anchor points (Equations 12, 13) .

[0123] ELSE (Obstacle bypassed):

[0124] Solve using standard NMPC (Equation 4) .

[0125] Control implementation: Take the first element of the optimized control sequence. It is applied to robot systems.

[0126] State Update: The state at the next moment is derived based on the robot dynamics model (Equation 2). .

[0127] Proceed to the next time step.

[0128] Step S6: Robust algorithm integration and real-time decision-making logic.

[0129] (a) Hysteresis design for mode switching: To avoid control jitter caused by frequent mode switching near the thresholds γ and ξ, hysteresis comparison is used. For example, the threshold for switching from "accessibility" to "obstacle avoidance" mode is γenter, while the threshold for switching from "obstacle avoidance" back to "accessibility" is γexit>γenter.

[0130] (b) Fail-safe and degradation strategies:

[0131] Mapped NMPC solution failure: Fallback to regular NMPC and trigger emergency stop or detour. Sensor failure: Switch to obstacle state prediction based on historical data, or trigger safe stop. No nearest obstacle: Always use regular NMPC mode. Mapped NMPC solution may take longer. Solution time needs to be monitored in real time. If timeout occurs, output the control value from the previous moment or a filtered conservative control value to ensure the real-time performance of the system. NMPC outputs a control sequence, and only the first value is executed. To prevent high-frequency jitter, the optimal control value can be low-pass filtered before being sent to the underlying actuator.

[0132] In this embodiment, a simulation is performed using a community as an example to provide a detailed explanation of the specific implementation of the above method.

[0133] like Figure 1As shown, it includes the following steps:

[0134] 1. Real-time perception and threat assessment: In path tracking and obstacle avoidance systems, real-time perception and threat assessment are the primary steps to ensure safe robot navigation. This module acquires environmental information through multi-sensor fusion technology and identifies potential threats based on a dynamic risk assessment strategy. For example... Figure 2 (Community scene simulation diagram) As shown, the robot needs to identify moving pedestrians (black circles) and assess their threat level in a complex and dynamic environment.

[0135] The robot is equipped with sensors such as GNSS / IMU, LiDAR, cameras, and millimeter-wave radar. It fuses positioning and odometry data through extended Kalman filtering (EKF) or unscented Kalman filtering (UKF) to obtain high-precision state estimation.

[0136]

[0137] Point cloud clustering algorithms (such as Euclidean clustering) and deep learning models (such as YOLO or PointPillars) are used to extract the geometric and motion information of obstacles, including the center position Ci, collision radius, velocity vector Vi, etc., and time series trajectories are maintained for each obstacle to support dynamic prediction.

[0138] Threat assessment strategy defines a dynamic security distance model:

[0139]

[0140] And calculate the comprehensive collision risk index:

[0141]

[0142] when > When using Rthresh, mark the obstacle as a threat and select the nearest or highest-risk obstacle as the primary threat obstacle. For example... Figure 2 As shown, when a pedestrian approaches the robot, the threat assessment module triggers obstacle avoidance behavior.

[0143] 2. To address the optimization challenges posed by non-convex feasible regions, this system introduces a partitioning mechanism to divide the original non-convex region into simply connected subdomains, thus providing a foundation for subsequent convex mapping. This process is as follows: Figure 3 As shown, the area around the obstacle is divided into mappable convex sub-regions by dividing lines.

[0144] The dividing line is defined with the center of the main threat obstacle. Using the global path relative position and traffic rules as the base point, select the splitting direction (e.g., splitting to the right) and construct an infinitely long splitting line:

[0145]

[0146] The original non-convex feasible region is divided into several simply connected subdomains along the dividing line. Each subdomain is represented by a polygon, and its geometric center, boundary point set, and adjacency relationships are maintained. The dividing line divides the region surrounding the obstacle into two symmetrical convex subdomains. As the obstacle moves or the robot's state changes, the dividing line and subdomain divisions need to be dynamically updated. The update frequency is determined by the obstacle's movement speed and the degree of environmental change, to balance computational efficiency and accuracy.

[0147] 3. Construction and execution of topological mapping functions: Topological mapping is the core innovation of this system. Through mathematical transformations, non-convex subdomains are mapped to convex spaces, thus enabling the NMPC optimization problem to be solved within convex regions. For example... Figure 4 As shown, the mapped region becomes a regular convex region (such as a sector or rectangle), which facilitates the optimization algorithm. The mapping function is designed for each subdomain. Using polar coordinates:

[0148]

[0149] in For radial monotonic transformations (such as linear scaling). The angle encoding function ensures the injectivity and continuity of the mapping.

[0150] The subdomain boundary is discretized, and its convexity is verified after applying a mapping (e.g., through convex hull detection). Simultaneously, an inverse mapping is implemented to map the optimization results back to the physical space. For example... Figure 3 As shown, after mapping, the obstacle area is compressed to the bottom, and the robot trajectory smoothly bypasses this area in the mapped space.

[0151] 4. RNN Prediction and NMPC Optimization Solution in Mapped Space: Within the mapped convex space, an RNN is used to predict the robot's dynamic behavior, and an NMPC optimization problem is constructed to achieve dual-path tracking and obstacle avoidance. This part corresponds to... Figure 5 (Simulation results of the community scene) show the optimized trajectory of the robot in the mapped space.

[0152] RNN dynamics modeling, defining states in the mapping space Predict future Tc-step states using LSTM or GRU networks:

[0153]

[0154] NMPC optimizes the construction of a dual-path cost function:

[0155]

[0156] in: For global path tracking error, For local anchor point path tracking error, To control the input regularization term, a real-time QP or SQP solver (such as OSQP) is used to solve the optimization problem, and the first term of the output optimal control sequence is taken. Execute the algorithm and verify its physical feasibility through inverse mapping. For example... Figure 6 As shown, the robot smoothly tracks two paths within the mapped space and successfully avoids pedestrians.

[0157] 5. Weight Adaptation, Anchor Point Update, and Loop Execution: To achieve adaptive obstacle avoidance in dynamic environments, this system introduces a weight adaptation and anchor point update mechanism to balance security and tracking efficiency. This part corresponds to... Figure 6 (Comparison of trajectories under different weights) demonstrates the impact of weights on robot behavior. The dual-path weight adaptive method dynamically adjusts the weight ratio of the global and local paths based on real-time risk indicators.

[0158]

[0159] Anchor point generation and updating, setting the forward distance based on the robot speed Vk:

[0160]

[0161] Sampling anchor points along the global path or feasible direction constructs a local reference path. For example... Figure 6 As shown, the greater the weight, the more likely the robot is to move away from the obstacle.

[0162] Cross-domain switching and stability: When the robot approaches the boundary of a subdomain, it smoothly switches to a neighboring domain, using a transition function to blend the reference trajectory and avoid abrupt control changes. The system updates perception, mapping, prediction, and weights in real time in the main loop until the threat is eliminated or the task is completed.

[0163] The system's main loop executes S1→S2→S3→S4→S5 sequentially, updating environmental information and controller status in real time. When threats or obstacles disappear or the task is completed, the mapping process exits, and regular NMPC tracing resumes. If optimization failure or mapping instability is detected, a conservative strategy is activated and the anomaly is logged. This solution achieves safe, smooth, and efficient path tracing in non-convex environments through multi-module collaboration, demonstrating strong practicality and scalability.

[0164] Example 3

[0165] This embodiment 3 provides a non-transitory computer-readable storage medium for storing computer instructions. When the computer instructions are executed by a processor, they implement the non-convex NMPC path tracing method based on the mapping space as described above. The method includes:

[0166] Acquire the complete motion state of the mobile robot and information about obstacles in its working environment;

[0167] Based on a preset safe distance threshold, the robot dynamically assesses the obstacle collision risk it currently faces; when it is determined that at least one obstacle poses a close-range threat to the robot, a topology-mapping-based obstacle avoidance path planning mechanism is activated, and the nearest threatening obstacle is locked;

[0168] Using the center of the nearest threatening obstacle as a reference point, a virtual geometric dividing line is introduced to divide the robot's current feasible motion region, which is non-convex due to the presence of obstacles, into several simply connected sub-regions along a specific direction.

[0169] For each simply connected sub-region formed after segmentation, a specific topological mapping function is applied to continuously and one-to-one transform the physical space coordinates within the sub-region into a convex mapped space coordinate system. The topological mapping function includes: using the orientation determination mechanism defined by the segmentation line and specific angle encoding rules, continuously, monotonically, and unambiguously expanding and representing the azimuth angle from any point in the same sub-region in the physical space to the reference point in the mapped space, while retaining the radial distance information of the point, ensuring that the non-convex physical sub-region is transformed into a convex set in the mapped space.

[0170] In the convex mapping space, a recurrent neural network model trained based on historical data is used to predict the trajectory of the robot's motion state evolution over a future period of time within the mapping space. The prediction result depends on the current mapping state and the control input sequence to be optimized. In the convex mapping space, a trajectory tracking cost function is constructed regarding the predicted state sequence and the control input sequence.

[0171] Solve the cost function minimization problem in the above convex mapping space to obtain the optimal future control input sequence, and apply the first control variable in the sequence to the robot actuator to drive its motion.

[0172] Example 4

[0173] This embodiment 4 provides a computer device, including a memory and a processor, wherein the processor and the memory communicate with each other, and the memory stores program instructions that can be executed by the processor. The processor calls the program instructions to execute the non-convex NMPC path tracing method based on the mapping space as described above, the method including:

[0174] Acquire the complete motion state of the mobile robot and information about obstacles in its working environment;

[0175] Based on a preset safe distance threshold, the robot dynamically assesses the obstacle collision risk it currently faces; when it is determined that at least one obstacle poses a close-range threat to the robot, a topology-mapping-based obstacle avoidance path planning mechanism is activated, and the nearest threatening obstacle is locked;

[0176] Using the center of the nearest threatening obstacle as a reference point, a virtual geometric dividing line is introduced to divide the robot's current feasible motion region, which is non-convex due to the presence of obstacles, into several simply connected sub-regions along a specific direction.

[0177] For each simply connected sub-region formed after segmentation, a specific topological mapping function is applied to continuously and one-to-one transform the physical space coordinates within the sub-region into a convex mapped space coordinate system. The topological mapping function includes: using the orientation determination mechanism defined by the segmentation line and specific angle encoding rules, continuously, monotonically, and unambiguously expanding and representing the azimuth angle from any point in the same sub-region in the physical space to the reference point in the mapped space, while retaining the radial distance information of the point, ensuring that the non-convex physical sub-region is transformed into a convex set in the mapped space.

[0178] In the convex mapping space, a recurrent neural network model trained based on historical data is used to predict the trajectory of the robot's motion state evolution over a future period of time within the mapping space. The prediction result depends on the current mapping state and the control input sequence to be optimized. In the convex mapping space, a trajectory tracking cost function is constructed regarding the predicted state sequence and the control input sequence.

[0179] Solve the cost function minimization problem in the above convex mapping space to obtain the optimal future control input sequence, and apply the first control variable in the sequence to the robot actuator to drive its motion.

[0180] Example 5

[0181] This embodiment 5 provides an electronic device, including: a processor, a memory, and a computer program; wherein, the processor is connected to the memory, and the computer program is stored in the memory. When the electronic device is running, the processor executes the computer program stored in the memory to cause the electronic device to execute instructions implementing the non-convex NMPC path tracing method based on the mapping space as described above. The method includes:

[0182] Acquire the complete motion state of the mobile robot and information about obstacles in its working environment;

[0183] Based on a preset safe distance threshold, the robot dynamically assesses the obstacle collision risk it currently faces; when it is determined that at least one obstacle poses a close-range threat to the robot, a topology-mapping-based obstacle avoidance path planning mechanism is activated, and the nearest threatening obstacle is locked;

[0184] Using the center of the nearest threatening obstacle as a reference point, a virtual geometric dividing line is introduced to divide the robot's current feasible motion region, which is non-convex due to the presence of obstacles, into several simply connected sub-regions along a specific direction.

[0185] For each simply connected sub-region formed after segmentation, a specific topological mapping function is applied to continuously and one-to-one transform the physical space coordinates within the sub-region into a convex mapped space coordinate system. The topological mapping function includes: using the orientation determination mechanism defined by the segmentation line and specific angle encoding rules, continuously, monotonically, and unambiguously expanding and representing the azimuth angle from any point in the same sub-region in the physical space to the reference point in the mapped space, while retaining the radial distance information of the point, ensuring that the non-convex physical sub-region is transformed into a convex set in the mapped space.

[0186] In the convex mapping space, a recurrent neural network model trained based on historical data is used to predict the trajectory of the robot's motion state evolution over a future period of time within the mapping space. The prediction result depends on the current mapping state and the control input sequence to be optimized. In the convex mapping space, a trajectory tracking cost function is constructed regarding the predicted state sequence and the control input sequence.

[0187] Solve the cost function minimization problem in the above convex mapping space to obtain the optimal future control input sequence, and apply the first control variable in the sequence to the robot actuator to drive its motion.

[0188] In summary, the non-convex NMPC path tracking and dynamic obstacle avoidance method based on topology mapping described in this embodiment of the invention specifically includes: real-time acquisition of the mobile robot's pose state (position, heading angle, velocity) and obstacle coordinate information; when the obstacle distance is less than a 1.5-meter safety threshold, using the nearest obstacle center as the reference point, constructing a dividing line along a preset direction to decompose the non-convex feasible region into simply connected sub-regions; mapping each sub-region to a continuous convex space through polar coordinate transformation, establishing a correspondence between the robot state and the mapped space; using a recurrent neural network to predict the system state within the convex space, calculating the trajectory tracking cost function including position deviation and input constraints; innovatively introducing global path anchor points to generate internal reference paths, designing a dual-path weight balancing mechanism, and achieving smooth switching between large-scale obstacle avoidance and boundary-hugging modes through dynamic adjustment; finally solving for the optimal control sequence to drive robot motion. This method improves the feasibility of solving non-convex region NMPC by more than 40%, reduces the trajectory distortion rate by 37%, and increases the dynamic obstacle avoidance success rate to 98.2%.

[0189] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0190] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0191] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0192] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment, whereby a series of operational steps are performed to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0193] While the specific embodiments of the present invention have been described above in conjunction with the accompanying drawings, this is not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that, based on the technical solutions disclosed in the present invention, various modifications or variations that can be made by those skilled in the art without creative effort should be included within the scope of protection of the present invention.

Claims

1. A non-convex NMPC path tracing method based on mapping space, characterized in that, include: Acquire the complete motion state of the mobile robot and information about obstacles in its working environment; Based on a preset safe distance threshold, the robot's current risk of collision with obstacles is dynamically assessed. When it is determined that at least one obstacle poses a close-range threat to the robot, the obstacle avoidance path planning mechanism based on topology mapping is activated, and the nearest threatening obstacle is locked. Using the center of the nearest threatening obstacle as a reference point, a virtual geometric dividing line is introduced to divide the robot's current feasible motion region, which is non-convex due to the presence of obstacles, into several simply connected sub-regions along a specific direction. For each simply connected sub-region formed after segmentation, a specific topological mapping function is applied to continuously and one-to-one transform the physical space coordinates within the sub-region into a convex mapped space coordinate system. The topological mapping function includes: using a directional determination mechanism defined by the dividing line and specific angle encoding rules, the azimuth angle from any point in the same sub-region in the physical space to the reference point is continuously, monotonically, and unambiguously expanded and represented in the mapping space, while retaining the radial distance information of the point, ensuring that the physical non-convex sub-region is transformed into a convex set in the mapping space; In the convex mapping space, a recurrent neural network model trained based on historical data is used to predict the trajectory of the robot's motion state evolution over a future period of time within the mapping space. The prediction result depends on the current mapping state and the control input sequence to be optimized. In the convex mapping space, a trajectory tracking cost function is constructed regarding the predicted state sequence and the control input sequence. Solve the cost function minimization problem in the above convex mapping space to obtain the optimal future control input sequence, and apply the first control variable in the sequence to the robot actuator to drive its motion.

2. The non-convex NMPC path tracing method based on mapping space according to claim 1, characterized in that, The directional strategy of the dividing line is as follows: based on the approximate orientation of the global reference path relative to the obstacle or the preset safe passage rules, the direction vector is selected as [0, 1] or [0, -1]; [0, 1] represents the positive Y-axis direction, used for passage to the left of the obstacle, and [0, -1] represents the negative Y-axis direction, often used for passage to the left of the obstacle. This ensures that the dividing line can effectively cover the main potential path directions for the robot to safely bypass the obstacle, laying the foundation for planning feasible obstacle avoidance trajectories in the convex space.

3. The non-convex NMPC path tracing method based on mapping space according to claim 1, characterized in that, It also includes auxiliary anchor points and a dual-path balancing mechanism: On the original global reference path, a forward guidance point is dynamically set according to the robot's current position and direction of movement. Based on the mapping position of this anchor point in the convex mapping space, a local internal reference path is generated. This local path aims to guide the robot to smoothly and efficiently rejoin the global desired path while effectively avoiding current threatening obstacles. On top of the basic trajectory tracking cost function, a tracking cost term for the local internal reference path is superimposed to form a dual-path balancing cost function. This function contains two dynamically adjustable positive weight coefficients, which are used to balance the preference for tracking accuracy of the original global path and the tracking and following strength of the local obstacle avoidance path, respectively.

4. The non-convex NMPC path tracing method based on mapping space according to claim 3, characterized in that, The dynamic adjustment strategy for the weight ratio in the dual-path balancing cost function is as follows: The ratio of the two weight coefficients is dynamically calculated based on the real-time relative distance and speed between the robot and the threatening obstacle, as well as the degree of environmental congestion. When the ratio is greater than 5, the system prioritizes obstacle avoidance safety, resulting in an optimized trajectory with a large radius detour, maintaining a large safety margin between the robot and the obstacle, suitable for open environments or high-speed scenarios. When the ratio is less than 3, the system prioritizes path tracking efficiency and accuracy, resulting in an optimized trajectory that closely follows the obstacle boundary to shorten the path length and quickly pass through narrow areas. When the ratio is between 3 and 5, the system seeks a balance between obstacle avoidance safety and path tracking efficiency, generating a smooth-transitioning compromise trajectory.

5. The non-convex NMPC path tracing method based on mapping space according to claim 1, characterized in that, The selection strategy for the time window length Tc of trajectory prediction and optimization is as follows: This parameter is a user-configurable positive integer, and its specific value needs to be set by comprehensively considering factors such as the robot's maximum motion capability, the dynamic characteristics of obstacle motion in the environment, and the real-time computing capability of the control system.

6. The non-convex NMPC path tracing method based on mapping space according to claim 5, characterized in that, The state deviation weight matrix Ks and control input weight matrix Ku in the cost function need to be adjusted according to the physical meaning of the state variables, the dimensions of the control variables, and the system's emphasis on different performance indicators.

7. A non-convex NMPC path tracing system based on a mapping space, characterized in that, include: The acquisition module is used to acquire the complete motion state of the mobile robot itself and the obstacle information in its working environment; The evaluation module is used to dynamically assess the risk of collision with obstacles currently facing the robot based on a preset safe distance threshold; When it is determined that at least one obstacle poses a close-range threat to the robot, the obstacle avoidance path planning mechanism based on topology mapping is activated, and the nearest threatening obstacle is locked. The segmentation module is used to introduce a virtual geometric segmentation line with the center of the locked nearest threat obstacle as the reference point. This line is used to divide the feasible motion area where the robot is currently located, which is non-convex due to the presence of obstacles, into several simply connected sub-regions along a specific direction. The mapping module is used to apply a specific topological mapping function to each simply connected sub-region formed after segmentation, so as to continuously and one-to-one transform the physical space coordinates within the sub-region into a convex mapped space coordinate system. The topology mapping function includes: using a position determination mechanism defined by dividing lines and specific angle encoding rules, the azimuth angle from any point within the same sub-region in physical space to a reference point is continuously, monotonically, and unambiguously expanded and represented in the mapping space, while retaining the radial distance information of the point, ensuring that the physical non-convex sub-region is transformed into a convex set in the mapping space; in the convex mapping space, a recurrent neural network model trained based on historical data is used to predict the trajectory of the robot's motion state evolution over a future period of time within the mapping space, and the prediction result depends on the current mapping state and the control input sequence to be optimized; in the convex mapping space, a trajectory tracking cost function is constructed regarding the predicted state sequence and the control input sequence; The solution module is used to solve the cost function minimization problem in the convex mapping space mentioned above, obtain the optimal future control input sequence, and apply the first control quantity in the sequence to the robot actuator to drive its motion.

8. A non-transitory computer-readable storage medium, characterized in that, The non-transitory computer-readable storage medium is used to store computer instructions, which, when executed by a processor, implement the non-convex NMPC path tracing method based on a mapping space as described in any one of claims 1-6.

9. A computer device, characterized in that, The method includes a memory and a processor, the processor and the memory communicating with each other, the memory storing program instructions executable by the processor, and the processor calling the program instructions to execute the non-convex NMPC path tracing method based on the mapping space as described in any one of claims 1-6.

10. An electronic device, characterized in that, include: The device includes a processor, a memory, and a computer program; wherein the processor is connected to the memory, the computer program is stored in the memory, and when the electronic device is running, the processor executes the computer program stored in the memory to cause the electronic device to execute instructions that implement the non-convex NMPC path tracing method based on the mapping space as described in any one of claims 1-6.

Images