Complete track recording and fusion navigation method for autonomous mobile robot

Abstract

The invention relates to a complete track recording and fusion navigation method for an autonomous mobile robot. The method comprises a track dynamic recording and storage mechanism; a trajectory filtering and smoothing optimization method; a trajectory merging and global path generation mechanism: combining multi-section historical trajectory fusion with global path planner ROS calling to calculate a connection path between adjacent trajectories, and when the system global planner ROS fails due to map update or environment change, the system selects linear interpolation to combine quaternion interpolation rollback through an interpolation connection mechanism; and publishing visualization is carried out. According to the invention, the global planning service of the global path planner ROS is called to calculate the connection path between the adjacent tracks, and the direction uses linear interpolation and the position uses quaternion interpolation back-off, so that the comprehensive requirements of real-time recording, dynamic storage, smooth optimization and multi-track fusion navigation of the complete track of the robot are met; and the continuity, robustness and global availability of path planning are improved.

Description

Technical Field

[0001] This invention relates to the interdisciplinary field of high-precision and robust trajectory recording and reproduction technology for multi-sensor data, and particularly to a method for complete trajectory recording and fusion navigation of an autonomous mobile robot. Background Technology

[0002] With the continuous development of artificial intelligence, autonomous driving, and mobile robot technologies, autonomous mobile robots have been widely used in scenarios such as warehousing and logistics, park delivery, industrial inspection, and indoor services. In order to achieve high-precision and autonomous path planning and navigation in complex environments, mobile robots typically rely on a variety of sensors (such as LiDAR, IMU, vision sensors, and odometry) for localization and path generation.

[0003] In existing technologies, robot path planning and navigation mainly rely on the following types of methods: 1. Navigation methods based on SLAM (Simultaneous Localization and Mapping): These methods construct environmental maps in real time using sensor information and utilize algorithms such as loop closure detection and graph optimization to achieve robot localization and path generation. Typical algorithms include GMAPping, Cartographer, and Hector SLAM. These methods have good mapping and navigation capabilities in unknown environments, but they are sensitive to noise and have difficulty reusing historical trajectory data.

[0004] 2. Navigation methods based on global path planning algorithms: Robots use algorithms such as A*, Dijkstra, or DWA (Dynamic Window Approach) on a known map to plan the globally optimal path from the starting point to the target point. These methods are computationally efficient and have a clear structure, but they require replanning for each navigation task and lack a trajectory reuse mechanism.

[0005] 3. Dynamic programming method based on local obstacle avoidance and trajectory optimization: Algorithms such as Timed Elastic Band (TEB) or Model Predictive Control (MPC) improve obstacle avoidance and path smoothness by optimizing parameters such as trajectory curvature and velocity constraints within a local range. However, these methods are computationally complex and typically only focus on short-term local paths, offering insufficient support for path optimization in long-term repetitive tasks.

[0006] 4. Track recording and playback methods: To improve the efficiency of repetitive tasks, some systems record robot motion trajectories and reuse them directly in the next execution. For example, the robot path is saved through the `nav_msgs::Path` message in the system's global planner (ROS) to achieve path playback navigation. However, such methods typically only support single-segment recording or offline storage, cannot dynamically manage trajectory data during operation, and lack effective smoothing and fusion mechanisms.

[0007] In summary, while current mobile robot navigation systems can achieve localization and path planning, they still have significant shortcomings in terms of the real-time performance, smoothness, reusability, and fusion of multiple trajectory segments. This limits the robot's autonomy and efficiency in long-term operation, complex environments, and repetitive task scenarios. Summary of the Invention

[0008] This invention provides a complete trajectory recording and fusion navigation method that runs under the ROS framework of the system global planner, in order to solve and overcome the defects of traditional methods such as discontinuous trajectory, inability to reuse, and lack of robustness in merging.

[0009] The technical solution adopted in this invention is an autonomous mobile robot trajectory recording and fusion navigation method, which includes three innovative modules: 1. Dynamic trajectory recording and storage mechanism: Based on position and direction difference, trajectory points are recorded in real time, supporting segmented recording of start and stop of trajectory, automatic naming, and formation of trajectory library.

[0010] 2. Trajectory filtering and smoothing optimization methods: Window filtering and quaternion directional smoothing are applied to the saved or loaded trajectory to reduce noise and jitter and enhance path continuity.

[0011] 3. Trajectory merging and global path generation: Multiple historical trajectories are merged sequentially, and connection segments are generated by the system's global planner ROS. If planning fails, linear interpolation combined with quaternion interpolation is used to backtrack, generating a continuous and executable global path and publishing it.

[0012] According to the present invention, a method for recording and fusing the complete trajectory of an autonomous mobile robot is provided, wherein the trajectory merging and global path generation mechanism includes: Read each segment from the trajectory library in sequence; For any two adjacent segments endpoints and The system global planner ROS is invoked to generate the connection path C; A set of independent connection path validity thresholds are applied: number of points, point spacing, corner / curvature, and cost map conflict to determine whether C is directly acceptable. If the number of points and shape of the connection path C formed by the system global planner ROS meet the validity thresholds, then the connection path C is inserted into the merged path. Furthermore, when the system's global planner ROS fails due to map updates or environmental changes, the system automatically falls back to the interpolation join mechanism, where location is determined using linear interpolation.

[0013] In the formula, , For the two endpoints, It is the spatial position of the robot at time t during trajectory interpolation, and the parameter is... ; Direction is determined using spherical linear interpolation: , In the formula, This represents spherical linear interpolation, where the quaternion for interpolation is... , ,make ,parameter For the starting attitude quaternion and the end point The inner product of directional quaternions is negative. Take − first To ensure the shortest angle interpolation; The merged overall path is then subjected to another global filter to eliminate seam spikes and improve smoothness; Publish the final path to / global_path and save it as merged_track.txt for reproduction and offline analysis.

[0014] According to the present invention, a method for recording and fusing the complete trajectory of an autonomous mobile robot is provided, wherein the trajectory dynamic recording and storage mechanism includes: TF is used as the source of position and direction; Whether to record the current position and direction is determined by the displacement threshold; Furthermore, this invention employs a dynamic trajectory recording strategy based on an Euclidean distance threshold, which calculates the current position. Compared to the last recorded position The displacement difference is calculated using the following formula:

[0015] When the displacement difference ∆ is greater than the threshold, a trajectory point is recorded; Segmented recording is used. When recording begins, the current segment buffer is cleared. When recording ends, the segment is filtered, automatically named, and persisted as a file named track_. <n>The file is .txt, and a track_index is maintained in the file directory to prevent overwriting; It supports management interfaces such as external triggering, loading, and clearing through the ROS global planner services / topics, facilitating integration with host computers or task scheduling modules.

[0016] According to the present invention, a complete trajectory recording and fusion navigation method for an autonomous mobile robot is provided, wherein the threshold is 0.16m~0.25m, which is read by a parameter server during the constructor, facilitating on-site parameter adjustment.

[0017] According to the present invention, a method for recording and fusing the complete trajectory of an autonomous mobile robot is provided, wherein the trajectory filtering and smoothing optimization method includes: Position smoothing: Apply a sliding window weighted average to the trajectory positions before saving the trajectory segments. The specific formula is as follows:

[0018] In the formula, For trajectory indexing, For local standard deviation, For fixed windows; Adaptive window: The window width is adaptively determined by the local position variance to enhance smoothness in jittery areas. The specific formula is as follows:

[0019] In the formula, For fixed windows, Indicates restriction on between, This is an empirical coefficient. For trajectory indexing, The threshold is... For local standard deviation, calculation is performed using a short window. Furthermore, perform directional (quaternion) smoothing: because and Equivalently, the average of quaternions cannot be directly averaged over the components; therefore, we first align the signs: if but Then perform a weighted sum and normalization, as shown in the following formula: --------------------------------------------------------------------- It is the final average pose quaternion obtained after smoothing, used to represent the target pose of the robot after weighting and normalization. It is the first The weighting coefficients of each pose quaternion.

[0020] It is the original pose quaternion. It is the first one after "alignment with same sign" processing. A quaternion of poses. It is a reference attitude quaternion. It is the quaternion dot product, used to determine sign consistency. It is a weighted sum The modulus is used to normalize the weighted sum result. --------------------------------------------------------------------- This invention also provides an autonomous mobile robot trajectory recording and fusion navigation system, comprising: The dynamic recording and adaptive storage module for trajectories is used to: record trajectory points, automatically name them, and form a trajectory library; The trajectory filtering and smoothing optimization module is used to: apply window filtering and quaternion directional smoothing to saved or loaded trajectories to reduce noise and jitter and enhance path continuity; The trajectory merging and global path generation module is used to merge multiple historical trajectories in sequence to generate connecting segments.

[0021] The present invention also provides an electronic device, including a processor and a memory storing a computer program, wherein the processor executes the computer program to implement any of the above-described methods for recording and fusing the complete trajectory of an autonomous mobile robot.

[0022] The present invention also provides a non-transitory computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements any of the above-described methods for recording and fusing the complete trajectory of an autonomous mobile robot.

[0023] The present invention also provides a computer program product, which includes a computer program that can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer is able to execute any of the above-described methods for recording and fusing the complete trajectory of an autonomous mobile robot.

[0024] This invention proposes an autonomous mobile robot trajectory recording and fusion navigation method, which realizes dynamic trajectory recording, adaptive smoothing optimization and multi-segment trajectory fusion navigation under the system global planner ROS framework, overcoming the defects of traditional methods such as discontinuous trajectory, inability to reuse, and lack of robustness in merging.

[0025] The present invention provides a method for recording and fusing the complete trajectory of an autonomous mobile robot, which has at least the following beneficial effects: First, it achieves dynamic recording and adaptive storage of trajectories, improving trajectory reuse efficiency. 1. This invention adopts a dynamic trajectory recording strategy based on Euclidean distance threshold. By calculating the displacement difference between the current position and the previous recorded position, the trajectory point is automatically recorded when the displacement difference is greater than the threshold. This method avoids redundant storage of invalid points and improves the spatiotemporal resolution and real-time performance of trajectory data.

[0026] 2. Employing a segmented storage and automatic naming mechanism, the system automatically numbers files using the file index track_. <n>The .txt file manages trajectory segments, enabling rapid retrieval, loading, and reuse of trajectories across multiple tasks or scenarios. Compared to traditional single-file continuous recording methods, this invention offers greater flexibility and scalability, facilitating the creation of a trajectory database for subsequent scheduling or learning.

[0027] 3. This mechanism has been implemented in actual systems through the service interfaces of the system global planner ROS, such as / Start_save and / Start_load. It can be linked with the host computer task system to achieve automatic segmented recording by task, region, or time. Therefore, robots can automatically accumulate trajectory data in complex park or indoor environments, enabling the reuse of empirical paths and reducing redundant planning calculations.

[0028] Second, an adaptive sliding window trajectory filtering and smoothing optimization algorithm is proposed to improve path continuity and navigation stability. 1. This invention designs a trajectory smoothing algorithm based on adaptive window weighted moving average. A weighted average is applied to the position data within a local window, and the window width is dynamically adjusted by the local position variance, thereby achieving adaptive optimization by increasing smoothing in noisy regions and reducing computation in stable regions.

[0029] 2. Quaternion alignment with the same sign and normalized weighted averaging are used for the direction data to effectively eliminate abrupt changes in direction. This method mathematically preserves the continuity of the rotation group, ensuring the smoothness of the path direction.

[0030] 3. Actual tests show that after adopting the smoothing algorithm of this invention, the jitter amplitude between trajectory points is significantly reduced compared to the original data, the directional control error during robot execution is significantly reduced, and the stability and control accuracy of navigation are significantly improved. III. A trajectory merging and global path generation mechanism is proposed to enhance the continuity and robustness of path planning. 1. This invention innovatively combines the fusion of multiple historical trajectories with the invocation of the global path planner. The system calls the global planning service / move_base / make_plan of the system's global planner ROS to calculate the connection path between adjacent trajectories; if the planning is successful, the planning result is used for splicing to ensure the legality and environmental adaptability of the path.

[0031] 2. When the system's global planner ROS fails due to map updates or environmental changes, the system automatically falls back to the interpolation connection mechanism: linear interpolation is used for position and spherical linear interpolation (Slerp) is used for direction.

[0032] 3. The merged path undergoes global filtering again to ensure a smooth transition at the joints, forming an executable fused path. This fused navigation method significantly improves the robustness and global availability of connecting multiple path segments, making it particularly suitable for large parks or multi-story inspection scenarios.

[0033] Fourth, it possesses excellent system visualization and interactivity, enhancing the usability of the project. 1. The system's global planner ROS visualization interface publishes trajectory points, trajectory lines, and endpoint markers through visualization_msgs::Marker, enabling real-time visualization of the path.

[0034] 2. Users can intuitively see the trajectory quality, paragraph distribution, and merging effect, which facilitates debugging and verification.

[0035] 3. A multi-functional system global planner (ROS) service interface, including ` / merge_path`, ` / if_clear`, and ` / load_all_tracks`, is provided to control the entire process of track recording, saving, loading, merging, and clearing. This design improves the system's interactivity and maintainability. Attached Figure Description

[0036] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0037] Figure 1 This is a flowchart of the method in an embodiment.

[0038] Figure 2 The following is a detailed flowchart of step S2 in the implementation method.

[0039] Figure 3 The following is a detailed flowchart of step S3 in the implementation method.

[0040] Figure 4 The following is a detailed flowchart of step S4 in the implementation method.

[0041] Figure 5 The output image of the trajectory recording terminal.

[0042] Figure 6 This is a screenshot of the contents of the txt document saved after the trajectory recording is completed.

[0043] Figure 7 For track_ <n>The output of the file named .txt and saved to the save_dir folder is shown in the image.

[0044] Figure 8 This is a global view of the recorded trajectory in rviz.

[0045] Figure 9 This is a partial view of the recorded trajectory in rviz. Detailed Implementation

[0046] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, embodiments of this invention, and should not be construed as limiting the invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention. In the description of this invention, it should be understood that the terminology used is for descriptive purposes only and should not be construed as indicating or implying relative importance.

[0047] ROS (Robot Operating System) is a flexible framework designed to help developers create complex robotic applications. It provides a suite of tools, libraries, and conventions for writing, building, and running robot software. ROS offers a structured approach to managing hardware abstractions, device drivers, common functions, communication middleware, and algorithm software. ROS provides a variety of powerful tools and packages, making the recording and processing of robot trajectories more flexible and scalable. Developers can choose appropriate tools and algorithms to implement trajectory recording functionality based on specific needs. Furthermore, ROS provides various visualization and debugging tools, such as RViz and rqt_plot, to help developers monitor and analyze robot trajectory data in real time, facilitating algorithm debugging and optimization. ROS can also run on various operating systems, including Ubuntu, Windows, and macOS, meaning robot trajectory recording can be implemented on different hardware platforms, improving system flexibility and portability.

[0048] like Figure 1 As shown, the present invention provides a method for recording and fusing the complete trajectory of an autonomous mobile robot, which may include: S1, using TF as the source of position and direction.

[0049] S2. Real-time recording of trajectory points based on position and orientation differences.

[0050] In a preferred embodiment, step S2 specifically includes the following steps: S21. Calculate the current position Compared to the last recorded position The displacement difference is calculated using the following formula:

[0051] S22. When the displacement difference ∆ is greater than the threshold, record a trajectory point.

[0052] S23. Segmented recording is adopted. When recording starts, the current segment buffer is cleared. When recording ends, the segment is filtered, automatically named, and persisted as a file track_. <n>The file is .txt, and a track_index is maintained in the file directory to prevent overwriting. Figure 5 , Figure 6 The image shows the output result of the trajectory recording terminal in this embodiment.

[0053] S3. Window filtering and quaternion direction smoothing are applied to the saved or loaded trajectory to reduce noise and jitter and enhance path continuity.

[0054] In a preferred embodiment, step S3 includes position smoothing, adaptive window and orientation (quaternion) smoothing, and the specific execution process includes the following steps: S31. Before saving the trajectory segment, apply a sliding window weighted average to the trajectory position. The specific formula is as follows:

[0055] In the formula, For trajectory indexing, For local standard deviation, For fixed windows.

[0056] S32. The window width is adaptively determined by the local position variance to enhance smoothness in jittery areas. The specific formula is as follows:

[0057] In the formula, For fixed windows, Indicates restriction on between, This is an empirical coefficient. For trajectory indexing, The threshold is... The standard deviation is calculated using a short window.

[0058] S33, Perform directional (quaternion) smoothing, because and Equivalently, the average of quaternions cannot be directly averaged over the components; therefore, we first align the signs: if but Then perform a weighted sum and normalization, as shown in the following formula:

[0059] S4. Merge multiple historical trajectories in sequence, generate connection segments through the system global planner ROS, and if the planning fails, use linear interpolation combined with quaternion interpolation to backtrack, generate a continuous and executable global path and publish it.

[0060] In a preferred embodiment, step S4 specifically includes the following steps: S41. Read each segment in the trajectory library in sequence.

[0061] S42. For any two adjacent segments, the endpoints and The system global planner ROS is invoked to attempt to generate connection path C.

[0062] S43. Apply a set of independent connection path validity thresholds: number of points, point spacing, turning angle / curvature, and cost map conflict to determine whether C is directly acceptable. If the number of points and shape of the connection path C formed by the system global planner ROS meet the validity thresholds, then the connection path C is inserted into the merged path; otherwise, linear interpolation is used for the position and quaternary interpolation is used for the direction. The quaternary interpolation used is spherical linear interpolation.

[0063] The linear interpolation formula used for the position is:

[0064] In the formula, , For the two endpoints, It is the spatial position of the robot at time t during trajectory interpolation, and the parameter is... ; Direction is achieved using spherical linear interpolation (SLEP) (quaternion). , ,parameter ,make ): , In the formula, This represents spherical linear interpolation, where the quaternion for interpolation is... , ,make ,parameter For the starting attitude quaternion and the end point The inner product of directional quaternions is negative. Take − first To ensure the shortest angle interpolation; S44. Perform global filtering again on the merged overall path to eliminate seam protrusions and improve smoothness.

[0065] S45. Publish the final path to / global_path and save it as merged_track.txt for reproduction and offline analysis.

[0066] S5. Publish visual markers: trajectory lines (visualization_msgs::Marker::LINE_STRIP), endpoint balls (SPHERE), and text labels (TEXT_VIEW_FACING, which is extensible), for online debugging and abnormal segment location, such as... Figure 8 , Figure 9 The image shows the recorded trajectory displayed in rviz in this embodiment.

[0067] This invention provides a complete trajectory recording and fusion navigation method for autonomous mobile robots, applicable to navigation tasks in complex environments. It utilizes a trajectory recording and fusion navigation method within the ROS framework, a global system planner. By combining the fusion of multiple historical trajectories with calls to the global path planner, it enhances path continuity, robustness, and global availability, making it particularly suitable for large-scale park or multi-story inspection scenarios.

[0068] The following is a specific embodiment of the present invention to illustrate a method for recording and fusing the complete trajectory of an autonomous mobile robot. In this embodiment, under the scenario of autonomous robot movement, the method completes dynamic trajectory recording, adaptive smoothing optimization, and multi-segment trajectory fusion navigation under the global planner ROS framework.

[0069] The first step is to start the system, read the parameters save_store_flies and threshold, and scan save_dir to initialize track_index.

[0070] The second step involves an external or programmatic trigger to Start_record (either the / Start_record topic or the / Start_save service). The system then begins calling Get_tf_change("map","base_footprint") within the main loop (10Hz) and pressing... Determine the record point, append it to path.poses, and simultaneously publish a marker.

[0071] The third step involves the external trigger ending / Start_save. The system then calls FilterTrajectory(...) (or an adaptive version) to smooth the current segment, automatically using track_ <n>Name and save the .txt file to save_dir. Also, publish the endpoint marker.

[0072] Real-time recording and threshold judgment are implemented in the loop `Hq_record_control()`: The current position and orientation are obtained using `Get_tf_change("map", "base_footprint")`, then `diff.norm()>threshold` is used to determine whether to record, and the `geometry_msgs::PoseStamped` object is appended to `path.poses`, while a visual marker is published. `Get_tf_change("map", "base_footprint");` now_pose.x() = x; now_pose.y() = y; diff = last_pose - now_pose; if (diff.norm()>threshold) { geometry_msgs::PoseStamped pose; pose.pose.position.x = x; pose.pose.position.y = y; pose.pose.orientation.x = orx; / / Quaternion components path.poses.push_back(pose); last_pose = now_pose; } Automatic naming and index management are implemented in the constructor: when the program starts, it scans the filenames under save_dir and tracks_\<num\> The .txt file initializes track_index to the next available number to avoid name duplication (implemented using std::filesystem combined with regular expression matching).

[0073] The fourth step involves constructing a global path. The ` / merge_path` service is invoked. The system reads all track files in filename order as `all_paths` and attempts to call `CallGlobalPlanner(last_end, this_start, connect_path)` on adjacent segments. If successful, the merged segment is inserted into the planned path; otherwise, linear interpolation combined with quaternion interpolation is used for connection. After merging, `ApplyFilterToPath` is called for global smoothing, and the merged track is published to ` / global_path`, while `merged_track.txt` is saved.

[0074] In the existing code, `PublishMergedTrack()` reads the `.txt` file under `save_dir`, sorts it by filename, parses it into `nav_msgs::Path` and puts it into `all_paths`, then calls `CallGlobalPlanner(last_end, this_start, connect_path)` on adjacent segments; if the planner succeeds and the number of connected segment points returned is >= 3, then the connected segment is added to `merged_raw`; finally, `ApplyFilterToPath()` is called on `merged_raw` to publish ` / global_path`, and it is saved as `merged_track.txt`. `ros::ServiceClient client = nh.serviceClient`<nav_msgs::GetPlan> (" / move_base / make_plan"); srv.request.start = start; srv.request.goal = goal; srv.request.tolerance = 0.2; if (client.call(srv)&&!srv.response.plan.poses.empty()) { result_path = srv.response.plan; return true; } else { return false; } On the one hand, this invention proposes a method for recording and fusing the complete trajectory of an autonomous mobile robot. By combining the fusion of multiple historical trajectories with the invocation of a global path planner, a smooth transition at the trajectory splicing point is achieved, forming an executable fusion path, which improves the robustness and global availability of connecting multiple path segments.

[0075] On the other hand, this invention proposes a method for recording and fusing the complete trajectory of an autonomous mobile robot. Compared with the existing methods for reusing and fusing historical trajectories, this method can dynamically record trajectories in real time, perform adaptive smoothing optimization, and fuse multi-segment trajectories for navigation, effectively improving trajectory reuse efficiency and enhancing path continuity and navigation stability.

[0076] The present invention provides a method for recording and fusing the complete trajectory of an autonomous mobile robot, which has at least the following beneficial effects: First, it achieves dynamic recording and adaptive storage of trajectories, improving trajectory reuse efficiency. 1. This invention employs a dynamic trajectory recording strategy based on Euclidean distance thresholds, which calculates the current position. Compared to the last recorded position The displacement difference is calculated, and when the displacement difference ∆>threshold, the trajectory point is automatically recorded. This method avoids redundant storage of invalid points and improves the spatiotemporal resolution and real-time performance of the trajectory data.

[0077] 2. Employing a segmented storage and automatic naming mechanism, the system automatically numbers files using the file index track_. <n>The .txt file manages trajectory segments, enabling rapid retrieval, loading, and reuse of trajectories across multiple tasks or scenarios. Compared to traditional single-file continuous recording methods, this invention offers greater flexibility and scalability, facilitating the creation of a trajectory database for subsequent scheduling or learning.

[0078] 3. This mechanism has been implemented in actual systems through the service interfaces of the system global planner ROS, such as / Start_save and / Start_load. It can be linked with the host computer task system to achieve automatic segmented recording by task, region, or time. Therefore, robots can automatically accumulate trajectory data in complex park or indoor environments, enabling the reuse of empirical paths and reducing redundant planning calculations.

[0079] Second, an adaptive sliding window trajectory filtering and smoothing optimization algorithm is proposed to improve path continuity and navigation stability. 1. This invention designs a trajectory smoothing algorithm based on adaptive window weighted moving average. The position data is processed using a weighted average within a local window: Weight The window width is determined by the local position variance. Dynamic adjustment: This achieves adaptive optimization by increasing smoothness in noisy regions and reducing computation in stable regions.

[0080] 2. Quaternion alignment and normalized weighted averaging are applied to the direction data to effectively eliminate abrupt changes in direction. Mathematically, this method preserves the rotation group. The continuity ensures the smoothness of the path direction.

[0081] 3. Actual tests show that after adopting the smoothing algorithm of this invention, the jitter amplitude between trajectory points is significantly reduced compared to the original data, the directional control error during robot execution is significantly reduced, and the stability and control accuracy of navigation are significantly improved. III. A trajectory merging and global path generation mechanism is proposed to enhance the continuity and robustness of path planning. 1. This invention innovatively combines the fusion of multiple historical trajectories with the invocation of the global path planner. The system calls the global planning service / move_base / make_plan of the system's global planner ROS to calculate the connection path between adjacent trajectories; if the planning is successful, the planning result is used for splicing to ensure the legality and environmental adaptability of the path.

[0082] 2. When the system's global planner ROS fails due to map updates or environmental changes, the system automatically falls back to the interpolation join mechanism: Position is determined using linear interpolation:

[0083] In the formula, , For the two endpoints, It is the spatial position of the robot at time t during trajectory interpolation, and the parameter is... , Direction is determined using spherical linear interpolation: , In the formula, This represents spherical linear interpolation, where the quaternion for interpolation is... , ,make ,parameter For the starting attitude quaternion and the end point The inner product of directional quaternions is negative. Take − first To ensure the shortest angle interpolation. 3. The merged path undergoes global filtering again to ensure a smooth transition at the splicing points, forming an executable fused path. This fused navigation method significantly improves the robustness and global availability of connecting multiple path segments, and is especially suitable for large parks or multi-story inspection scenarios.

[0084] Fourth, it possesses excellent system visualization and interactivity, enhancing the usability of the project. 1. The system's global planner ROS visualization interface publishes trajectory points, trajectory lines, and endpoint markers through visualization_msgs::Marker, enabling real-time visualization of the path.

[0085] 2. Users can intuitively see the trajectory quality, paragraph distribution, and merging effect, which facilitates debugging and verification.

[0086] 3. A multi-functional system global planner (ROS) service interface, including ` / merge_path`, ` / if_clear`, and ` / load_all_tracks`, is provided to control the entire process of track recording, saving, loading, merging, and clearing. This design improves the system's interactivity and maintainability.

[0087] The following describes an autonomous mobile robot trajectory recording and fusion navigation system provided by the present invention. The autonomous mobile robot trajectory recording and fusion navigation system described below can be referred to in correspondence with the autonomous mobile robot trajectory recording and fusion navigation method described above.

[0088] The present invention provides an autonomous mobile robot trajectory recording and fusion navigation system, which may include: The dynamic recording and adaptive storage module for trajectories is used to: record trajectory points, automatically name them, and form a trajectory library; The trajectory filtering and smoothing optimization module is used to: apply window filtering and quaternion directional smoothing to saved or loaded trajectories to reduce noise and jitter and enhance path continuity; The trajectory merging and global path generation module is used to merge multiple historical trajectories in sequence to generate connecting segments.

[0089] On the other hand, the present invention also provides a computer program product, the computer program product comprising a computer program, the computer program being able to be stored on a non-transitory computer-readable storage medium, and the computer program being executed by a processor, enabling the computer to perform the following steps: Using TF as the source of position and orientation, the current position and orientation are collected in real time during the running loop; By calculating the current position Compared to the last recorded position The displacement difference is calculated using the following formula:

[0090] When the displacement difference ∆ is greater than the threshold, a trajectory point is recorded; Segmented recording is used. When recording begins, the current segment buffer is cleared. When recording ends, the segment is filtered, automatically named, and persisted as a file named track_. <n>The file is .txt, and a track_index is maintained in the file directory to prevent overwriting; Before saving the trajectory segment, a sliding window weighted average is applied to the trajectory position. The specific formula is as follows:

[0091] In the formula, For trajectory indexing, For local standard deviation, For fixed windows; The window width is adaptively determined by the local position variance to enhance smoothness in jittery regions. The specific formula is as follows:

[0092] In the formula, For fixed windows, Indicates restriction on between, This is an empirical coefficient. For trajectory indexing, The threshold is... For local standard deviation, calculation is performed using a short window. Perform directional (quaternion) smoothing, because and Equivalently, the average of quaternions cannot be directly averaged over the components; therefore, we first align the signs: if but Then perform a weighted sum and normalization, as shown in the following formula:

[0093] Read each segment from the trajectory library in sequence; For any two adjacent segments endpoints and The system global planner ROS is invoked to generate the connection path C; A set of independent connection path validity thresholds—number of points, point spacing, turning angle / curvature, and cost map conflict—are applied to determine whether path C is directly acceptable. If the number of points and shape of connection path C formed by the system global planner ROS meet the validity thresholds, then connection path C is inserted into the merged path; otherwise, linear interpolation is used for the position and quaternary interpolation is used for the direction. The quaternary interpolation used is spherical linear interpolation.

[0094] The linear interpolation formula used for the position is:

[0095] In the formula, , For the two endpoints, It is the spatial position of the robot at time t during trajectory interpolation, and the parameter is... , Direction is determined using spherical linear interpolation: , In the formula, This represents spherical linear interpolation, where the quaternion for interpolation is... , ,make ,parameter For the starting attitude quaternion and the end point The inner product of directional quaternions is negative. Take − first To ensure the shortest angle interpolation; The merged overall path is then subjected to another global filter to eliminate seam spikes and improve smoothness; Publish the final path to / global_path and save it as merged_track.txt for reproduction and offline analysis.

[0096] In another aspect, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, performs the following steps: Using TF as the source of position and orientation, the current position and orientation are collected in real time during the running loop; By calculating the current position Compared to the last recorded position The displacement difference is calculated using the following formula:

[0097] When the displacement difference ∆ is greater than the threshold, a trajectory point is recorded; Segmented recording is used. When recording begins, the current segment buffer is cleared. When recording ends, the segment is filtered, automatically named, and persisted as a file named track_. <n>The file is .txt, and a track_index is maintained in the file directory to prevent overwriting; Before saving the trajectory segment, a sliding window weighted average is applied to the trajectory position. The specific formula is as follows:

[0098] In the formula, For trajectory indexing, For local standard deviation, For fixed windows; The window width is adaptively determined by the local position variance to enhance smoothness in jittery regions. The specific formula is as follows:

[0099] In the formula, For fixed windows, Indicates restriction on between, This is an empirical coefficient. For trajectory indexing, The threshold is... For local standard deviation, calculation is performed using a short window. Perform directional (quaternion) smoothing, because and Equivalently, the average of quaternions cannot be directly averaged over the components; therefore, we first align the signs: if but Then perform a weighted sum and normalization, as shown in the following formula:

[0100] Read each segment from the trajectory library in sequence; For any two adjacent segments endpoints and The system global planner ROS is invoked to generate the connection path C; When the system's global planner fails due to map updates or environmental changes, the system automatically falls back to the interpolation join mechanism, where linear interpolation is used for location:

[0101] In the formula, , For the two endpoints, It is the spatial position of the robot at time t during trajectory interpolation, and the parameter is... , Direction is determined using spherical linear interpolation: , In the formula, This represents spherical linear interpolation, where the quaternion for interpolation is... , ,make ,parameter For the starting attitude quaternion and the end point The inner product of directional quaternions is negative. Take − first To ensure the shortest angle interpolation; The merged overall path is then subjected to another global filter to eliminate seam spikes and improve smoothness; Publish the final path to / global_path and save it as merged_track.txt for reproduction and offline analysis.

[0102] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without any creative effort.

[0103] Through the above description of the embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus necessary general-purpose hardware platforms, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solutions, in essence or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in the various embodiments or some parts of the embodiments.

[0104] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.< / n> < / n> < / n> < / n> < / n> < / n> < / n> < / n>

Claims

1. An autonomous mobile robot complete trajectory recording and fusion navigation method, characterized in that, The method comprises: a trajectory dynamic recording and storage mechanism, supporting trajectory segment recording start and stop; a trajectory filtering and smoothing optimization method, trajectory segment recording is followed by smoothing optimization; a trajectory merging and global path generation mechanism, connecting segments are generated by merging multiple historical trajectories in sequence; the trajectory merging and global path generation mechanism is characterized in that, multiple historical trajectories are merged in sequence, and connecting segments are generated by the system global planner ROS; if generation fails, linear interpolation combined with quaternion interpolation is used to back off, and a continuous and executable global path is generated and published. 2.The autonomous mobile robot complete trajectory recording and fusion navigation method according to claim 1, characterized in that, the trajectory merging and global path generation mechanism comprises: Reading each section in the track library in order, the end points of any two adjacent sections With , the system global planner ROS generates a connection path C.

3. The autonomous mobile robot complete trajectory recording and fusion navigation method according to claim 2, characterized in that, a set of independent connection path validity thresholds: point number, point spacing, corner / curvature, cost map conflict, are applied to determine whether C can be directly accepted; if the point number and shape of the connecting path C formed by the system global planner ROS meet the validity threshold, the connecting path C is inserted into the merged path; if not, linear interpolation is used for position and spherical linear interpolation is used for direction. 4.The autonomous mobile robot complete trajectory recording and fusion navigation method of claim 3, wherein, The specific formula is as follows: Positions are interpolated linearly: In the formula, , is two endpoints, is the spatial position corresponding to the moment t in the trajectory interpolation process of the robot, and the parameter ; The direction adopts spherical linear interpolation: ， where denotes the spherical linear interpolation, the interpolated quaternion is , , let , the parameter , the inner product of the start pose quaternion and the end direction quaternion is negative first take− to ensure the shortest angle interpolation. 5.The autonomous mobile robot complete trajectory recording and fusion navigation method according to claims 1-4, characterized in that, the trajectory dynamic recording and storage mechanism comprises: TF is used as the position and direction source; whether to record the current position and direction is determined according to the displacement threshold: when the displacement difference Δ between the current position and the last recorded point is greater than the threshold threshold, the position and quaternion direction of a trajectory point are recorded.

6. The autonomous mobile robot complete trajectory recording and fusion navigation method according to claim 5, characterized in that, the threshold threshold is 0.16 m-0.25 m, which is read by the parameter server in the constructor. 7.The autonomous mobile robot complete trajectory recording and fusion navigation method according to claims 1-6, characterized in that, the trajectory filtering and smoothing optimization method comprises: position smoothing: sliding window weighted average is applied to the trajectory position before saving the trajectory segment; adaptive window: the window width is adaptively determined by the local position variance to enhance the smoothness in the jitter area, and the specific formula is as follows: wherein is a fixed window, denotes a restriction to , is an empirical coefficient, is a track index, is a threshold value threshold, is a local standard deviation, calculated over a short window.

8. The autonomous mobile robot trajectory recording and fused navigation system of claims 1-7, wherein, The method comprises: a trajectory dynamic recording and adaptive storage module for recording trajectory points, automatically naming and forming a trajectory library; a trajectory filtering and smoothing optimization module for applying window filtering and quaternion direction smoothing to the saved or loaded trajectory to reduce noise and jitter and enhance path continuity; a trajectory merging and global path generation module for merging multiple historical trajectories in sequence to generate connecting segments. 9.A computer device, comprising a memory and a processor, wherein the memory stores a computer program, and the computer device is configured to perform the method according to any one of claims 1-8 when the computer program is executed by the processor. The processor executes the computer program to realize the autonomous mobile robot complete trajectory recording and fusion navigation method according to any one of claims 1-7.

10. A computer-readable storage medium having stored thereon a computer program, characterized in that, The computer program is executed by the processor to realize the autonomous mobile robot complete trajectory recording and fusion navigation method according to any one of claims 1-7.

Images