High-precision mecanum wheel vehicle trajectory planning control system

Abstract

The application discloses a high-precision track planning control system for a wheel-on-rail vehicle and relates to the technical field of track planning. The number of visible satellites is acquired, and the switching frequency of positioning calculation states is obtained. In the time advancing process, the position coordinate change amplitude of the wheel-on-rail vehicle is recorded synchronously, and the position coordinate offset corresponding to the switching moment of the positioning calculation state is identified point by point to form a dynamic record set containing the position coordinate change amplitude and the positioning calculation state switching information. The application perceives the positioning calculation state and the satellite change and reconstructs the position coordinate offset to form a transition track with consistent direction, so that the control input is free from the interference of sudden data and the continuity and smoothness of track tracking of the wheel-on-rail vehicle are improved. Meanwhile, the control output is gradually converged through progressive compression of the heading adjustment, the reverse adjustment fluctuation is suppressed, the running impact is reduced, and the track keeping capability and the overall running stability are enhanced.

Description

Technical Field

[0001] This application relates to the field of trajectory planning technology, specifically to a high-precision trajectory planning and control system for a wheelbarrow. Background Technology

[0002] The trajectory planning and control of the trolley based on RTK (Real-Time Dynamic Differential Positioning) technology is essentially a closed-loop trajectory execution mechanism that couples high-precision differential positioning with the motion control of the mobile carrier. The specific implementation process is as follows: First, a real-time differential positioning link is established between a portable RTK base station and the vehicle-mounted differential receiving unit, so that the trolley can continuously acquire absolute position information at the centimeter to millimeter level during its movement.

[0003] In this process, the acquired geographic coordinates are converted into a Cartesian coordinate system with the test target as the reference origin and true north as the unified reference, thus completing a unified expression of spatial location. Subsequently, based on the arbitrary shape trajectory pre-planned by the user, the coordinates of the current real-time location and the corresponding coordinates of the target trajectory are compared point by point as time progresses, forming a continuous spatial deviation.

[0004] Further dynamic adjustment calculations (such as proportional, integral, and derivative adjustment) are performed on the deviation, mapping the position deviation into control commands for the heading and speed of the wheelbarrow. During continuous iteration, the actual trajectory of the wheelbarrow gradually approaches the planned trajectory, and a stable closed-loop control effect is formed during the deviation convergence process, thereby achieving high-precision tracking control of complex trajectories under conditions without fixed guide rails.

[0005] During the execution of the McDonnell Douglas vehicle's trajectory, when the vehicle is in dynamic operation, the number of visible satellites can change rapidly within a short period due to changes in environmental occlusion, fluctuations in electromagnetic interference, and abrupt changes in satellite geometry. During this process, the positioning calculation state will frequently switch between high-precision differential calculation and low-precision single-point calculation, leading to discontinuous jumps in the position calculation results and causing sudden shifts in the output position coordinates compared to the previous moment.

[0006] When the control system performs real-time trajectory matching on the location data, it may misjudge the jump as a real trajectory deviation. As a result, it will continuously output directional correction commands during the time process, which will cause the control commands to continuously reverse and gradually amplify, eventually causing the vehicle's trajectory to oscillate and deviate or even deviate from the predetermined path.

[0007] The information disclosed in the background section is only intended to enhance the understanding of the background of this disclosure, and therefore may include information that does not constitute prior art known to those skilled in the art. Summary of the Invention

[0008] The purpose of this application is to provide a high-precision trajectory planning and control system for a wheelbarrow to solve the problems mentioned in the background art.

[0009] To achieve the above objectives, this application provides the following technical solution: a high-precision trolley trajectory planning and control system, including a satellite state perception module, a trajectory stretching and splicing module, a trajectory direction reconstruction module, a deviation source replacement module, and a heading rhythm compression module;

[0010] Satellite Status Awareness Module: Acquires changes in the number of visible satellites and the frequency of positioning solution status switching. Simultaneously records the magnitude of position coordinate changes of the vehicle during the time progression and marks the position coordinate offset corresponding to the moment of positioning solution status switching point by point, forming a dynamic record set containing position coordinate change magnitude and positioning solution status switching information.

[0011] The trajectory stretching and stitching module performs stretching and alignment on the position coordinate change amplitude data in the dynamic record set before and after the time, and stitches the abnormal position coordinate offset corresponding to the moment of position calculation state switching with the previous continuous position coordinate changes across time periods to obtain continuously evolving trajectory segments.

[0012] Trajectory Direction Reconstruction Module: Performs direction reconstruction on the position coordinate sequence in the continuously evolving trajectory segment, and gradually pulls back the segment corresponding to the abnormal position coordinate offset according to the previous position coordinate change trend to obtain a transition trajectory with consistent direction.

[0013] Deviation Source Replacement Module: Based on the transition trajectory, the deviation source in the control input is replaced, the jump position in the original position coordinates is converted into the position coordinates corresponding to the transition trajectory, and the direction of the trend of the previous position coordinate change is kept consistent.

[0014] The heading rhythm compression module uses the position coordinates corresponding to the replaced transition trajectory to perform progressive compression of the heading adjustment, controlling the heading adjustment output to gradually converge with the transition trajectory. During continuous operation, it suppresses the tendency of reverse adjustment amplification, thereby achieving stable operation of the McDonnell Douglas trajectory planning and control process.

[0015] Preferably, a dynamic record set containing information on the magnitude of changes in position coordinates and the switching of positioning solution status is formed. The specific steps are as follows:

[0016] The number of visible satellites and the positioning solution status are obtained, and the position coordinates of the wheat wheelbarrow are recorded every moment. The changes in the number of visible satellites, the positioning solution status, and the changes in the position coordinates of the wheat wheelbarrow are bound together with a unified timestamp.

[0017] The position coordinate change range is obtained by point-by-point differential analysis of the position coordinates of the vehicle based on the unified timestamp binding result. The position coordinate change range is then recorded in correspondence with the changes in the number of visible satellites and the positioning solution status.

[0018] To determine the timing of the positioning solution state switching behavior, the system performs segment-by-segment scanning and extracts the changes in the number of visible satellites and the magnitude of changes in their position coordinates within the corresponding time range to form local data segments.

[0019] By using local data fragments, the magnitude of position coordinate changes is analyzed point by point and abnormal position coordinate offsets are identified. The information on changes in the number of visible satellites, the frequency of positioning solution state switching, the magnitude of position coordinate changes, and the anomaly markers are integrated to form a dynamic record set.

[0020] Preferably, the continuous evolution trajectory segment is obtained, and the specific steps are as follows:

[0021] Expand the range of position coordinate changes in the dynamic record set, and combine the corresponding number of visible satellites and the positioning solution state switching information to select the continuous position coordinate change segments before and after the positioning solution state switching node;

[0022] The time nodes of the preceding continuous position coordinate change segments are extended and the subsequent change segments are divided into the same time segments to maintain the consistency of the correspondence between the number of visible satellites and the positioning solution status switching information.

[0023] Based on the time division results, the abnormal position coordinate offset segments are aligned point by point, and the position coordinate change magnitudes are arranged in order according to the trend of previous continuous position coordinate changes.

[0024] By utilizing the alignment processing results to perform cross-time period stitching and maintaining the correspondence between the number of visible satellites and the positioning solution status switching information, continuously evolving trajectory segments are obtained.

[0025] Preferably, when arranging the position coordinate change amplitudes sequentially according to the trend of previous continuous position coordinate changes, the position coordinate change amplitude at the end of the previous continuous position coordinate change segment is used as the reference starting point, and the abnormal position coordinate offset segment change amplitude data is read point by point along the time progression direction, and arranged continuously according to the change direction of the previous time node.

[0026] Preferably, the transition trajectory with consistent direction is obtained through the following steps:

[0027] The position coordinates of the wheat wheelbarrow in the continuously evolving trajectory segment are obtained and abnormal position coordinate offset segments are identified. The trend of the previous position coordinate change is extracted to form the direction change path and arranged in time order.

[0028] Based on the direction change path, the abnormal position coordinate offset segment is pulled back point by point. The current position coordinate is gradually adjusted according to the change trend of the previous position coordinate, while maintaining the correspondence between the satellite visible number change information and the positioning solution status switching information.

[0029] The results of the point-by-point pullback process are sequentially connected, and the coordinates of the connection points are linked to obtain a transition trajectory with consistent direction.

[0030] Preferably, when performing point-by-point pullback processing on the abnormal position coordinate offset segment, the position coordinate of the last time node of the previous position coordinate change trend is used as the pullback starting point. The position coordinate of the abnormal position coordinate offset segment is read point by point along the time progression direction, and the adjustment is made step by step according to the direction path recorded by the previous position coordinate change trend, so that the position changes of adjacent time nodes present a continuous transition relationship.

[0031] Preferably, the sources of deviation in the control input are replaced based on the transition trajectory, and the specific steps are as follows:

[0032] The transition trajectory position coordinates and the original position coordinates are simultaneously unfolded and arranged in chronological order. The time nodes of abnormal position coordinate offsets are extracted and a time mapping relationship between the transition trajectory position coordinates and the original position coordinates is established.

[0033] Based on the time mapping relationship, the source of position deviation in the control input is replaced time-by-time node, and the deviation value corresponding to the abnormal position coordinate offset is replaced with the deviation value formed by the transition trajectory position coordinate while maintaining the consistency of the time node order.

[0034] The replacement processing results are formed into a continuous control input data stream, and the change in the position coordinates of the transition trajectory is used as the directional constraint to replace the source of deviation in the control input.

[0035] Preferably, when performing time-node-by-time replacement processing on the source of position deviation in the control input according to the time mapping relationship, the deviation value corresponding to the abnormal position coordinate offset is deleted, and the position coordinate of the transition trajectory is substituted to form the position deviation, so that the source of deviation in the control input continues along the direction of change of the position coordinate of the transition trajectory.

[0036] Preferably, the heading adjustment is performed with gradual compression, controlling the heading adjustment output to gradually converge with the transition trajectory. The specific steps are as follows:

[0037] Extract the position coordinates corresponding to the transition trajectory after replacement and unfold them in time sequence to form a continuous chain of directional changes and record the directional change information between each time node;

[0038] The heading adjustment is decomposed point by point using the direction change chain, and the direction change is distributed to multiple consecutive time nodes and adjusted according to the trend of the previous position coordinate change.

[0039] The decomposition results are continuously integrated to form a heading adjustment output, which gradually converges with the transition trajectory, thus achieving stable operation of the vehicle trajectory planning and control process.

[0040] Preferably, the heading adjustment is decomposed point by point using a continuous chain of directional changes. Specifically, this includes: decomposing the directional changes between adjacent time nodes point by point, distributing the directional changes to multiple consecutive time nodes, and adjusting the directional changes at each time node according to the trend of the previous position coordinate changes.

[0041] The technical effects and advantages provided by this application in the above technical solution are as follows:

[0042] This application dynamically senses the changing rhythm of satellite visibility and the switching behavior of positioning calculation states, and continuously reconstructs position coordinate offsets, transforming the original abrupt changes in positioning results on the time axis into a transitional trajectory with consistent direction. During the trajectory planning and control of the trolley, the control input no longer directly relies on position coordinates with abrupt changes, but responds based on the continuously evolving transitional trajectory. This effectively reduces the interference of instantaneous positioning anomalies on trajectory judgment, maintains a continuous and consistent directional change during motion, improves the smoothness of the trajectory tracking process, and enables the trolley to maintain a stable trajectory execution state even in complex environments.

[0043] After forming the transition trajectory, this application applies progressive compression to the heading adjustment, causing the control output to gradually converge along the transition trajectory and suppressing the reverse adjustment trend during continuous operation. In the trajectory planning and control process of the trolley, the control response changes from frequent fluctuations to continuous changes, avoiding operational instability caused by repeated direction adjustments. Simultaneously, it reduces the impact during execution, enhancing the trajectory-holding capability under long-term operating conditions, thereby improving the trolley's continuous operation capability and overall control stability in dynamic environments. Attached Figure Description

[0044] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments recorded in this application. For those skilled in the art, other drawings can be obtained based on these drawings.

[0045] Figure 1 This is a schematic diagram of the modules in this application.

[0046] Figure 2 A flowchart for obtaining a continuous evolution trajectory segment for this application.

[0047] Figure 3 A flowchart for obtaining a transition trajectory with consistent direction for this application.

[0048] Figure 4 This is a flowchart illustrating how the present application replaces the sources of deviation in the control input based on the transition trajectory. Detailed Implementation

[0049] Exemplary embodiments will now be described more fully with reference to the accompanying drawings. However, these exemplary embodiments can be implemented in many forms and should not be construed as limited to the examples set forth herein; rather, they are provided so that the description of this disclosure will be more complete and fully convey the concept of the exemplary embodiments to those skilled in the art.

[0050] This application provides, as follows: Figures 1 to 4 The high-precision trajectory planning and control system for the trolley shown includes a satellite state perception module, a trajectory stretching and splicing module, a trajectory direction reconstruction module, a deviation source replacement module, and a heading rhythm compression module.

[0051] Satellite Status Awareness Module: Acquires changes in the number of visible satellites and the frequency of positioning solution status switching. Simultaneously records the magnitude of position coordinate changes of the vehicle during the time progression and marks the position coordinate offset corresponding to the moment of positioning solution status switching point by point, forming a dynamic record set containing information on the magnitude of position coordinate changes and positioning solution status switching.

[0052] The positioning solution status refers to the type of solution result output by the positioning solution algorithm at each moment in the RTK positioning process. It is used to reflect the current positioning accuracy and solution reliability status, and usually includes different status types such as fixed solution, floating-point solution and single-point solution. Among them, fixed solution means that the carrier phase ambiguity has been determined, corresponding to centimeter-level accuracy; floating-point solution means that the ambiguity is not completely determined and the accuracy is in a transitional state; single-point solution means that no differential correction has been performed and it only relies on the original satellite observation data, with relatively low accuracy.

[0053] The positioning solution status can be obtained directly by the vehicle-mounted positioning receiving unit in each positioning epoch. It is usually provided synchronously with the location coordinate data in the form of a status identifier field, thereby realizing the real-time characterization of the positioning quality at each time point.

[0054] A dynamic record set containing information on changes in position coordinates and switching of positioning solution states is generated. The specific steps are as follows:

[0055] During the operation of the vehicle, the status of external satellite signals is tracked in real time using a continuous time-progression method. The number of satellites participating in the positioning calculation is read hourly by hour through the on-board positioning receiving unit, and the number of satellites at adjacent time points is compared one by one to obtain the change in the number of visible satellites. At the same time, the solution status in the positioning calculation output is recorded synchronously. The fixed solution, floating-point solution and single-point solution in the positioning calculation status are unfolded point by point on the time axis, and a complete state change trajectory is established in chronological order.

[0056] The specific changes in the number of visible satellites are described as follows:

[0057] The number of satellites participating in the positioning calculation at adjacent time points is differentially calculated. The number of satellites at the current time is subtracted from the number of satellites at the previous time to obtain the change value of the number of satellites. The sign and magnitude of the change value are recorded to reflect the state of the number of satellites increasing, decreasing or remaining unchanged. At the same time, the change value is arranged point by point in chronological order to form a discrete sequence describing the change of the number of visible satellites over time, which is used to characterize the dynamic fluctuation characteristics of the satellite signal environment in the continuous time process.

[0058] In this process, a unified timestamp is used as a benchmark to bind and record the number of visible satellites, the positioning solution status, and the corresponding geographic coordinates at each moment. Then, the geographic coordinates of consecutive moments are processed by point-by-point difference, and the position coordinates at the current moment are compared with the position coordinates at the previous moment to obtain the position coordinate change range within each time interval. The position coordinate change range is then mapped one-to-one with the satellite visible number change information and the positioning solution status at the same moment, thus forming a multi-dimensional data record that is synchronously unfolded on the same time axis. This ensures that each time node contains complete information on the changes in the number of visible satellites, the positioning solution status, and the position coordinate change range, providing a continuous and detailed data source for subsequent processing.

[0059] The magnitude of position coordinate change refers to the magnitude of the two-dimensional position coordinate difference vector between adjacent time nodes. It is obtained by subtracting the two-dimensional position coordinates of the current time from those of the previous time, and then calculating the length of this difference vector in the plane coordinate system to obtain a single scalar form of position change. This scalar reflects the actual displacement of the vehicle within adjacent time intervals and serves as a measure of the intensity of position change in a continuous time series. It is used to synchronize with the changes in the number of visible satellites and the positioning solution status.

[0060] After completing the above multidimensional data recording, the change process of the positioning solution state on the time axis is scanned segment by segment. For each time node, it is checked whether the positioning solution state of the adjacent time before and after it has changed. When it is detected that the positioning solution state has changed from a fixed solution to a floating-point solution or a single-point solution, or from a floating-point solution to a fixed solution or a single-point solution, the corresponding time node is determined as the positioning solution state switching time. Multiple consecutive time nodes are selected before and after this time as the center, and the corresponding satellite visible number change data and position coordinate change amplitude data are extracted within this time range.

[0061] During the extraction process, the type of state change at the moment of location solution state switching and the corresponding trend of satellite number change are recorded synchronously. Each location coordinate change amplitude data within this time range is assigned identification information related to the location solution state switching, so that each state switching corresponds to a continuous context data. At the same time, the time order, changes in the number of visible satellites, and changes in location coordinate amplitude within this context data are kept consistent, thus forming a local data segment with the location solution state switching as the core, providing complete context support for subsequent anomaly identification.

[0062] After acquiring the aforementioned local data segment containing the location calculation state switching information, the position coordinate change amplitude within the segment is analyzed point by point. Taking the location calculation state switching time as the center, the position coordinate change amplitude at that time is compared with the continuous change amplitude at several previous times. The continuous evolution of the change amplitude over time is observed. When the current position coordinate change amplitude deviates suddenly from the previous continuous change trend, the position coordinate change is identified as an abnormal position coordinate offset, and the time node is marked separately.

[0063] Sudden deviation refers to a discontinuous jump in the change of the current time point's position coordinates relative to the trend formed by the previous several consecutive time points. The determination method is as follows:

[0064] The change in the current time point's position coordinates is compared with the average or continuous range of changes in previous consecutive time points. A sudden deviation is identified when the current change exceeds the upper limit of this continuous range and surpasses a preset threshold, or when the change direction and magnitude do not maintain a continuous progressive relationship with the previous trend. This preset threshold can be determined by the statistical range of changes in previous consecutive time points, ensuring that only abnormal abrupt changes deviating from the original trend are identified.

[0065] The preset change threshold is not a fixed value, but is dynamically determined based on the continuous position coordinate change amplitude data before the positioning calculation state switch. Specifically, before the positioning calculation state switch node, the position coordinate change amplitude corresponding to the continuous time nodes is selected as normal motion reference data. The change amplitude range in this reference data is statistically analyzed to obtain the position coordinate change fluctuation range of the vehicle in continuous operation. The allowable position coordinate change deviation range is determined based on this fluctuation range. Among them, the average change level of the position coordinate change amplitude of the preceding continuous time nodes is used as the basic reference. The change difference of each time node relative to this average change level is accumulated and statistically analyzed. The threshold benchmark is determined based on the maximum change deviation within the continuous change interval, and a preset proportion of safety margin is added to form the preset change threshold.

[0066] When the change in the position coordinates of the current time node exceeds the maximum change range of the preceding continuous change interval, and at the same time exceeds the preset change threshold determined based on the change range of the preceding continuous position coordinates, the change in the position coordinates of the current time node is deemed to have deviated from the normal motion change range, thus being identified as a sudden deviation.

[0067] In this way, the preset change threshold can be dynamically generated based on the continuous position change characteristics during the actual operation of the vehicle, without relying on fixed empirical parameters, and can adapt to position coordinate changes under different operating speeds, different motion trajectories, and different satellite signal environments.

[0068] The safety margin of the preset ratio refers to the allowable deviation range added to avoid misjudgment caused by minor fluctuations, positioning measurement errors, and environmental signal disturbances when determining the normal range of change based on the range of changes in the coordinates of the preceding continuous positions.

[0069] Specifically, firstly, the actual fluctuation range is obtained based on the continuous position coordinate change amplitude before the positioning calculation state switch, and the maximum change deviation within this range is determined. Then, the maximum change deviation is expanded according to a pre-set ratio, and the expanded additional deviation is superimposed on the threshold benchmark as a safety margin, so that the preset change threshold covers non-abnormal changes that may occur during normal movement.

[0070] Only when the change in position coordinates exceeds the threshold range including the safety margin is it determined that it has deviated from the normal motion change range, thereby reducing the error abnormality identification caused by instantaneous measurement error or normal motion disturbance.

[0071] This safety margin is not a fixed value, but is adjusted proportionally according to the fluctuation characteristics of the previous continuous position coordinate changes in order to adapt to the position change characteristics under different operating conditions.

[0072] After completing the center time marker, the analysis continues along the time progression direction to examine the magnitude of position coordinate changes at subsequent times, gradually expanding the temporal propagation range of the abnormal changes and uniformly identifying multiple time nodes belonging to the same abnormal change process. Simultaneously, the abnormal position coordinate offset is associated with changes in the number of visible satellites and the switching information of the positioning solution status within the corresponding time period, so that each abnormal position coordinate offset not only has time and location information, but also information on the satellite environment changes and solution status changes that triggered the offset, thus forming a position coordinate abnormal change structure with a clear source on the time axis.

[0073] After completing the point-by-point identification of all abnormal location coordinate offsets, the multidimensional data over the entire time range is uniformly integrated and processed. The information on changes in the number of visible satellites, the frequency of switching of positioning solution status, the magnitude of changes in location coordinates at each time node, and the corresponding abnormal location coordinate offsets are reorganized in chronological order. During the integration process, the correspondence between various types of data is kept consistent, and the chronological order is not disrupted.

[0074] During this integration process, the data at each time point is organized into a complete record unit containing the number of visible satellites, the positioning calculation status, the magnitude of position coordinate changes, and anomaly indicators. These units are then arranged sequentially according to the time progression, forming a continuously unfolding dynamic record set. This dynamic record set not only fully reflects the position coordinate changes of the vehicle during operation but also clearly identifies the abnormal position coordinate offset corresponding to the moment the positioning calculation status changes. Furthermore, it establishes a direct correlation between the abnormal changes and the changes in the number of visible satellites and the positioning calculation status at the time of their occurrence. This allows subsequent processing to directly utilize the identification information in this dynamic record set to perform targeted processing on the abnormal position coordinates.

[0075] The trajectory stretching and stitching module performs stretching and alignment on the position coordinate change data in the dynamic record set before and after the change. It stitches the abnormal position coordinate offset corresponding to the moment of position solution state switching with the previous continuous position coordinate changes across time periods to obtain a continuously evolving trajectory segment.

[0076] The specific steps to obtain a continuously evolving trajectory segment are as follows:

[0077] The position coordinate change range of all positions in the dynamic record set is expanded point by point in chronological order. The position coordinate change range, the number of visible satellites, and the positioning solution status switching information corresponding to each time node are arranged one by one. In this arrangement process, the time node in which the positioning solution status is switched is used as the core reference point. The position coordinate change range of multiple consecutive time nodes before the core reference point is selected as the preceding consecutive position coordinate change segment. At the same time, the position coordinate change range of multiple consecutive time nodes after the core reference point is selected as the subsequent change segment.

[0078] The setting of multiple consecutive time nodes can be determined based on the time window length and sampling frequency. That is, taking the moment of state switching in the positioning calculation as the center, several time nodes within a continuous time range are selected forward according to a fixed time span as the preceding continuous position coordinate change segment, and several time nodes within the same or corresponding time span are selected backward as the subsequent change segment. The time window length can be determined according to the running speed of the wheelbarrow and the sampling period, so that the selected time nodes can completely cover the position change characteristics in the normal continuous movement process, thereby ensuring that the preceding segment can reflect a stable continuous change trend, and the subsequent segment can reflect the changes after the state switching, and maintain a continuous and equally spaced distribution on the time axis to meet the needs of subsequent alignment and splicing processing.

[0079] After selecting the preceding and following segments, each time node in the preceding continuous position coordinate change segment is extended point by point. The originally closely distributed time nodes are re-divided according to a fixed time interval, so that the time interval between adjacent time nodes is evenly spaced, while keeping the position coordinate change amplitude value of each time node unchanged.

[0080] When processing subsequent change segments, the time nodes are expanded according to the same time division method, so that the preceding continuous position coordinate change segments and the subsequent change segments form a consistent interval structure in terms of time distribution. Throughout the process, the correlation between the number of visible satellites and the positioning solution status switching information and the corresponding time nodes is kept in place, thereby forming a set of position coordinate change amplitude data that is redistributed on the time axis. This disperses the abnormal position coordinate offsets that were originally concentrated in a short period of time to multiple consecutive time nodes, providing an operable time space for subsequent alignment processing.

[0081] After the time nodes are redistributed, the segments of previous continuous position coordinate changes and the segments of abnormal position coordinate offsets are aligned point by point. First, the position coordinate change amplitude of the last time node in the previous continuous position coordinate change segment is used as the reference starting point. Starting from this reference starting point, the change amplitude data of the segment of abnormal position coordinate offset is read point by point along the time progression direction. The change amplitude is compared with the previous continuous change trend to ensure that the change amplitude of the first time node in the abnormal position coordinate offset segment is consistent with the change amplitude at the end of the previous continuous change segment in the direction of change.

[0082] After aligning the first time node, the subsequent time nodes in the abnormal position coordinate offset section are processed point by point. The position coordinate change range of each time node is arranged in order according to the change direction that has been adjusted in the previous time node, so that the change range of the entire abnormal section forms a continuous progressive relationship in the process of time advancement.

[0083] During the alignment process, the original number of visible satellites and the information on the switching of positioning solution status are retained for each time point, and these information continue to correspond with the adjusted position coordinate change range. This ensures that after the abnormal position coordinate offset segment is aligned, it not only maintains the same trend as the previous continuous position coordinate changes, but also retains all the associated information in the original dynamic record set in terms of data structure.

[0084] After completing the alignment of position coordinate change magnitude, the aligned abnormal position coordinate offset segment is spliced ​​with the preceding continuous position coordinate change segment across time periods. During the splicing process, the preceding continuous position coordinate change segment is first preserved in chronological order, and the aligned abnormal position coordinate offset segment is directly connected at its end, so that the two segments form a continuous connection in the direction of time progression.

[0085] During the connection process, the time nodes at the splicing point are rearranged in order to keep the time intervals before and after the splicing point consistent, thereby avoiding abrupt changes in time intervals at the splicing position. Then, the subsequent change segments are connected point by point to the spliced ​​overall change sequence along the time progression direction, so that the data on the change amplitude of the entire position coordinates form a continuous and complete process on the time axis.

[0086] After the stitching is completed, the overall change process formed by the stitching is uniformly organized so that each time node contains the corresponding position coordinate change range, the number of visible satellites, and the positioning solution state switching information, and the correspondence of these information remains unchanged throughout the time process, thus obtaining a set of continuously evolving trajectory segments. These continuously evolving trajectory segments show continuous change characteristics in the time dimension, while avoiding sudden position coordinate offsets caused by the instantaneous switching of positioning solution states, so that the trajectory evolution process can truly reflect the changing trend of the vehicle during continuous operation.

[0087] Trajectory Direction Reconstruction Module: Performs direction reconstruction on the position coordinate sequence in the continuously evolving trajectory segment, and gradually pulls back the segment corresponding to the abnormal position coordinate offset according to the previous position coordinate change trend to obtain a transition trajectory with consistent direction.

[0088] To obtain a transition trajectory with the same direction, the specific steps are as follows:

[0089] All position coordinates in the continuous evolution trajectory segment are unfolded point by point in chronological order. The two-dimensional position coordinate data corresponding to each time node are arranged sequentially. During the arrangement process, the range of the segment corresponding to the abnormal position coordinate offset is identified. The position coordinates of multiple consecutive time nodes before the segment are used as the previous position coordinate change trend to extract the segment.

[0090] During the extraction process, the position coordinates of at least five consecutive time nodes are selected from the segment, and the spatial displacement direction between adjacent time nodes is recorded point by point. These directions are arranged sequentially according to time to form a continuous path of directional change, which is then used as the sole reference for subsequent directional reconstruction. At the same time, each time node in the segment corresponding to the abnormal position coordinate offset is independently identified, ensuring that all position coordinates in the segment remain continuously arranged on the time axis but can be extracted individually during processing. Throughout the extraction and identification process, the original time order of each time node is maintained, while ensuring that each position coordinate still corresponds to its original satellite visibility change information and positioning solution status switching information. This forms a complete position coordinate data structure that includes the trend of previous position coordinate changes and the abnormal position coordinate offset segment, providing a continuous and clear processing object for subsequent step-by-step pullback processing.

[0091] After extracting the trend of previous position coordinate changes and identifying the abnormal position coordinate offset segments, a point-by-point pullback process is performed on the position coordinates in the abnormal position coordinate offset segments. In this process, the time node immediately adjacent to the end of the previous position coordinate change trend is taken as the starting point, and the position coordinates of the starting point are taken as the pullback starting point. The spatial position of this point is taken as the initial reference position for the entire pullback process. Subsequently, each position coordinate in the abnormal position coordinate offset segment is read sequentially along the time progression direction. The position coordinates of the current time node are compared with the position coordinates of the previous time node, and the current coordinates are gradually adjusted according to the direction path recorded in the previous position coordinate change trend, so that the position coordinates of the current time node are consistent with the previous position coordinate change trend in the spatial displacement direction.

[0092] In the specific processing, each abnormal location coordinate is moved in segments along the preceding direction path, decomposing the sudden offset that originally occurred at a single time node into multiple continuous small changes, so that the positional changes between adjacent time nodes present a gradual transition relationship. During the pullback process, the adjustment of each time node is based on its original time sequence, without rearranging the time nodes. At the same time, the information on changes in the number of visible satellites and the information on the switching of positioning solution status are kept to correspond with the adjusted location coordinates, so that the pulled-back data still has complete environmental and status information. Thus, the abnormal location coordinate offset is gradually resolved and transformed into a continuous change process as time progresses.

[0093] After completing the point-by-point pullback processing of the abnormal position coordinate offset segments, the entire position coordinate data is uniformly integrated. The preceding position coordinate change trend segments, the abnormal position coordinate offset segments that have undergone pullback processing, and the subsequent position coordinate segments that have not undergone abnormal changes are connected sequentially in chronological order. During the connection process, the original order of the preceding position coordinate change trend segments is maintained. Then, the abnormal position coordinate offset segments that have undergone pullback processing are directly connected to their ends, so that the starting position of the segment is consistent with the ending position of the preceding segment in the direction of spatial change. Subsequently, the subsequent position coordinate segments are connected point by point along the time progression direction, so that the entire position coordinate data forms a complete and continuous change trajectory on the time axis.

[0094] During the connection process, the coordinates of adjacent positions before and after the connection point are sequentially connected to ensure that the direction of position change at the connection point does not change abruptly, thereby ensuring the spatial continuity of the overall trajectory. Finally, a transition trajectory with a consistent direction is formed. This transition trajectory maintains continuous change characteristics during the time progression, while eliminating sudden changes in direction caused by abnormal position coordinate offsets. This allows the trajectory to stably reflect the actual movement trend of the wheelbarrow during operation and provides a continuous and consistent trajectory basis for subsequent control input adjustments.

[0095] Deviation Source Replacement Module: Based on the transition trajectory, the deviation source in the control input is replaced, the jump position in the original position coordinates is converted into the position coordinates corresponding to the transition trajectory, and the direction of the trend of the previous position coordinate change is kept consistent.

[0096] The source of deviation in the control input is replaced based on the transition trajectory. The jump positions in the original position coordinates are converted into the position coordinates corresponding to the transition trajectory. The specific steps are as follows:

[0097] After forming a transition trajectory with consistent direction, all position coordinate data generated by the control input are unfolded point by point in chronological order. The original position coordinates corresponding to each time node are synchronously arranged with the corresponding time node position coordinates in the transition trajectory, and the spatial position relationship between the two is compared one by one during the arrangement process.

[0098] During the comparison process, time nodes that have been identified as abnormal position coordinate offsets in the original position coordinates are extracted point by point, and the specific positions of these time nodes in the entire time axis are recorded. At the same time, the time positions that completely correspond to these time nodes are found in the transition trajectory, thereby establishing a one-to-one time mapping relationship.

[0099] After establishing the mapping relationship, the location data corresponding to each abnormal time node in the original position coordinates is marked as the data to be replaced, and the location coordinates of the same time node in the transition trajectory are marked as the replacement source data. In this process, the arrangement order of all time nodes remains unchanged, while ensuring that each time node still corresponds to its original satellite visible quantity change information and positioning solution state switching information. In this way, the point-to-point correspondence between the original position coordinates and the transition trajectory position coordinates is completed before the control input is formed, so that the subsequent replacement process can be carried out point by point under a unified time reference.

[0100] After completing the point-to-point correspondence construction, the source of position deviation involved in the control input is replaced time-by-time node. During the time progression, the control input data of each time node is read sequentially, the position deviation calculated based on the original position coordinates in that time node is extracted separately, and it is determined whether the deviation comes from the abnormal position coordinate offset segment.

[0101] For time nodes that belong to the abnormal position coordinate offset section, delete the deviation value corresponding to its original position coordinates, and substitute the position coordinates corresponding to the time node in the transition trajectory to reconstruct the position deviation of the time node, so that the deviation comes entirely from the position coordinate change in the transition trajectory; for time nodes that are not identified as abnormal position coordinate offset, keep the deviation corresponding to the original position coordinates unchanged, so that the normal section and the replacement section are naturally connected on the time axis.

[0102] Throughout the replacement process, each time point is processed point by point in the manner described above, ensuring that the replacement operation proceeds continuously in the direction of time progression. At the same time, the order of time points before and after the replacement is kept completely consistent. After the replacement is completed, all deviation values ​​are made to be consistent with the direction of change of the transition trajectory in the spatial direction, so that the source of deviation in the control input gradually transitions from the original position coordinates to the position coordinates of the transition trajectory.

[0103] After replacing the sources of deviation at all time points, the replaced control input data is continuously processed. The deviation values ​​of all time points are arranged in chronological order to form a continuously changing control input data stream. During the processing, the deviation changes between adjacent time points are sequentially connected to ensure that the deviation directions between adjacent time points remain consistent.

[0104] In this process, the trend of the preceding position coordinate changes is used as an overall directional constraint. The direction of the deviation changes at each time node after replacement is unified, so that the entire control input continues in the same direction of change throughout the time process without any reversal of direction. At the same time, the position coordinate changes of the transition trajectory are continuously maintained throughout the entire control input during continuous operation, so that the change process of the control input is entirely based on the transition trajectory. This eliminates the sudden impact of position jumps in the original position coordinates on the control input, and ensures that the control input maintains continuous, stable and consistent directional change characteristics during the time process. This ensures that the vehicle can run smoothly according to the trend of the preceding position coordinate changes during trajectory execution without directional fluctuations.

[0105] The heading rhythm compression module uses the position coordinates corresponding to the replaced transition trajectory to perform progressive compression of the heading adjustment, controlling the heading adjustment output to gradually converge with the transition trajectory. During continuous operation, it suppresses the tendency of reverse adjustment amplification, thereby achieving stable operation of the McDonnell Douglas trajectory planning and control process.

[0106] The heading adjustment is gradually compressed, and the heading adjustment output is gradually converged with the transition trajectory. The specific steps are as follows:

[0107] After completing the position coordinate replacement processing based on the transition trajectory, the position coordinates corresponding to the replaced transition trajectory are unfolded point by point in the order of time progression. During the unfolding process, the position coordinates of each time node are continuously read. At the same time, the spatial change direction between each time node is extracted point by point in combination with the change trend of the previous position coordinates, so that each time node has a clear directional change relationship.

[0108] Based on this, the position coordinates of the current time node are sequentially associated with the position coordinates of the previous time node to form a continuous chain of directional changes. This chain of directional changes is used as a reference path for heading adjustment. During this process, the order of the time nodes is not changed, so that all position coordinates are arranged sequentially according to the original time progression relationship.

[0109] Subsequently, for the position coordinate changes over the entire time range, the magnitude of directional changes between each time node is recorded point by point, and a continuous directional change distribution is formed on the time axis. This provides complete reference data for subsequent heading adjustment processing, enabling the heading adjustment process to be directly based on the position coordinates corresponding to the replacement transition trajectory.

[0110] After obtaining the distribution of continuous directional changes, the heading adjustment is progressively compressed. During the time progression, the directional change information corresponding to each time node is read sequentially, the directional changes between adjacent time nodes are decomposed point by point, and the directional changes that were originally concentrated in a few time nodes are distributed to multiple consecutive time nodes, so that the directional changes present a gradually unfolding change form on the time axis.

[0111] In this process, the trend of the change of the previous position coordinates is used as the overall directional constraint. The directional change of each time node is adjusted to continue the trend of the change of the previous position coordinates. At the same time, the change amplitude is gradually reduced so that the directional change amplitude between adjacent time nodes presents a gradually decreasing arrangement relationship.

[0112] During continuous processing, the same compression is performed at each time point, so that the heading adjustment gradually changes from rapid changes to gradual changes as time progresses. Throughout the entire processing, the correspondence between the position coordinates and the direction change corresponding to the transition trajectory after replacement processing remains unchanged, so that the changes in heading adjustment always rely on the transition trajectory.

[0113] After the gradual compression of the heading adjustment is completed, the processed heading adjustment output is continuously integrated in chronological order, so that the heading adjustment output of each time node is in a one-to-one correspondence with the corresponding transition trajectory position coordinates. During the integration process, the heading adjustment outputs of adjacent time nodes are sequentially connected, so that the heading adjustment remains continuously changing and does not reverse direction as time progresses.

[0114] Meanwhile, during continuous operation, the position coordinates corresponding to the transition trajectory after replacement are used as the only reference path, so that the heading adjustment output always converges gradually along this path. At each time node, the direction of the previous position coordinate change trend is kept consistent, thereby suppressing the reverse adjustment amplification trend throughout the entire operation, preventing repeated adjustments in the heading adjustment process, and ultimately achieving stable operation of the McDonnell Douglas trajectory planning and control process, so that the trajectory execution maintains a continuous, stable and consistent change state during the time process.

[0115] The foregoing has only described certain exemplary embodiments of this application by way of illustration. Undoubtedly, those skilled in the art can modify the described embodiments in various ways without departing from the spirit and scope of this application. Therefore, the foregoing drawings and descriptions are illustrative in nature and should not be construed as limiting the scope of protection of the claims of this application.

Claims

1. A high-precision trajectory planning and control system for a wheelbarrow, characterized in that, It includes a satellite status awareness module, a trajectory stretching and stitching module, a trajectory direction reconstruction module, a deviation source replacement module, and a heading rhythm compression module; Satellite Status Awareness Module: Acquires changes in the number of visible satellites and the frequency of positioning solution status switching. Simultaneously records the magnitude of position coordinate changes of the vehicle during the time progression and marks the position coordinate offset corresponding to the moment of positioning solution status switching point by point, forming a dynamic record set containing position coordinate change magnitude and positioning solution status switching information. The trajectory stretching and stitching module performs stretching and alignment on the position coordinate change amplitude data in the dynamic record set before and after the time, and stitches the abnormal position coordinate offset corresponding to the moment of position calculation state switching with the previous continuous position coordinate changes across time periods to obtain continuously evolving trajectory segments. Trajectory Direction Reconstruction Module: Performs direction reconstruction on the position coordinate sequence in the continuously evolving trajectory segment, and gradually pulls back the segment corresponding to the abnormal position coordinate offset according to the previous position coordinate change trend to obtain a transition trajectory with consistent direction. Deviation Source Replacement Module: Based on the transition trajectory, the deviation sources in the control input are replaced, and the jump positions in the original position coordinates are converted into the position coordinates corresponding to the transition trajectory; Heading rhythm compression module: Using the position coordinates corresponding to the transition trajectory after replacement processing, the heading adjustment is progressively compressed, and the heading adjustment output is controlled to gradually converge with the transition trajectory.

2. The high-precision trajectory planning and control system for the wheat-wheeled vehicle according to claim 1, characterized in that, A dynamic record set containing information on changes in position coordinates and switching of positioning solution states is generated. The specific steps are as follows: The changes in the number of visible satellites, the positioning solution status, and the changes in the position coordinates of the vehicle are all linked using a unified timestamp. Based on the unified timestamp binding result, the position coordinates of the vehicle are differentially calculated point by point to obtain the position coordinate change range. The position coordinate change range is then recorded in correspondence with the change in the number of visible satellites and the positioning solution status. The positioning solution state switching behavior is scanned segment by segment to determine the moment of positioning solution state switching, and the changes in the number of visible satellites and the magnitude of changes in position coordinates within the corresponding time range are extracted to form local data segments; By using local data fragments, the magnitude of position coordinate changes is analyzed point by point and abnormal position coordinate offsets are identified. The information on changes in the number of visible satellites, the frequency of positioning solution state switching, the magnitude of position coordinate changes, and anomaly indicators are integrated to form a dynamic record set.

3. The high-precision trajectory planning and control system for the wheat-wheeled vehicle according to claim 1, characterized in that, The specific steps to obtain a continuously evolving trajectory segment are as follows: Expand the range of position coordinate changes in the dynamic record set, and combine the corresponding number of visible satellites and the positioning solution state switching information to select the continuous position coordinate change segments before and after the positioning solution state switching node; Extend the time nodes of the preceding continuous position coordinate change segment, and perform the same time division processing on the subsequent change segment; Based on the time division results, the abnormal position coordinate offset segments are aligned point by point. Specifically, the position coordinate change amplitudes are arranged in order according to the trend of previous continuous position coordinate changes. By utilizing the alignment processing results to perform cross-time period stitching and maintaining the correspondence between the number of visible satellites and the positioning solution status switching information, continuously evolving trajectory segments are obtained.

4. The high-precision trajectory planning and control system for the wheat-wheeled vehicle according to claim 3, characterized in that, When arranging the magnitude of position coordinate changes in sequence according to the trend of previous continuous position coordinate changes, the magnitude of position coordinate changes at the end of the previous continuous position coordinate change segment is used as the reference starting point. The magnitude of changes in abnormal position coordinate offset segments is read point by point along the time progression direction and arranged continuously according to the change direction of the previous time node.

5. The high-precision trajectory planning and control system for the wheat-wheeled vehicle according to claim 3, characterized in that, To obtain a transition trajectory with the same direction, the specific steps are as follows: The position coordinates of the wheat wheelbarrow in the continuously evolving trajectory segment are obtained and abnormal position coordinate offset segments are identified. The trend of the change of the previous position coordinates is extracted to form the path of directional change. Based on the direction change path, the abnormal position coordinate offset segment is pulled back point by point, and the current position coordinate is gradually adjusted according to the change trend of the previous position coordinate. The results of the point-by-point pullback process are sequentially connected, and the coordinates of the connection points are linked to obtain a transition trajectory with consistent direction.

6. The high-precision trajectory planning and control system for a wheelbarrow according to claim 5, characterized in that, When performing point-by-point pullback processing on the abnormal position coordinate offset section, the position coordinate of the last time node of the previous position coordinate change trend is used as the pullback starting point, and the position coordinate of the abnormal position coordinate offset section is read point by point along the time advancement direction.

7. The high-precision trajectory planning and control system for a wheelbarrow according to claim 5, characterized in that, The specific steps for replacing the sources of deviation in the control input based on the transition trajectory are as follows: Obtain the transition trajectory position coordinates and the original position coordinates, extract the time nodes of abnormal position coordinate offsets, and establish the time mapping relationship between the transition trajectory position coordinates and the original position coordinates; Based on the time mapping relationship, the source of position deviation in the control input is replaced node by node, and the deviation value corresponding to the abnormal position coordinate offset is replaced with the deviation value formed by the position coordinate of the transition trajectory. The replacement processing results are formed into a continuous control input data stream, and the source of deviation in the control input is replaced by using the change in the position coordinates of the transition trajectory as the directional constraint.

8. The high-precision trajectory planning and control system for a wheelbarrow according to claim 7, characterized in that, When performing time-node-by-time replacement processing on the source of position deviation in the control input based on the time mapping relationship, the deviation value corresponding to the abnormal position coordinate offset is deleted, and the position coordinate of the transition trajectory is substituted to form the position deviation, so that the source of deviation in the control input continues along the direction of change of the position coordinate of the transition trajectory.

9. The high-precision trajectory planning and control system for a wheelbarrow according to claim 7, characterized in that, The heading adjustment is gradually compressed, and the heading adjustment output is gradually converged with the transition trajectory. The specific steps are as follows: Extract the position coordinates corresponding to the transition trajectory after replacement processing to form a continuous chain of directional changes; By using a continuous chain of directional changes to decompose the heading adjustment point by point, the directional changes are distributed to multiple consecutive time nodes; The decomposition results are continuously integrated to form a heading adjustment output, which gradually converges with the transition trajectory.

10. The high-precision trajectory planning and control system for a wheelbarrow according to claim 9, characterized in that, The heading adjustment is decomposed point by point using a continuous chain of directional changes. Specifically, this includes: decomposing the directional changes between adjacent time nodes point by point, distributing the directional changes to multiple consecutive time nodes, and adjusting the directional changes at each time node according to the trend of the previous position coordinate changes.

Images