A multimodal perception and decision-making integrated system for mobile robots

Abstract

This invention relates to the field of mobile robot control technology, and more particularly to an integrated multimodal perception and decision-making system for mobile robots. The invention proposes the following scheme: by jointly smoothing state estimation from multi-source perception data, a state version graph containing temporal and version evolution relationships is constructed. Under action locking window constraints, an action prefix index structure is established by combining historical action sequences. A restricted search is performed on the state version graph to generate an updated action sequence with minimal difference from the historical action structure. This application can suppress unstable changes in event triggering and state machine transitions when backtracking corrections occur in the smoothed state estimation results, maintaining the causal consistency and reproducibility of the action sequence, and improving the operational stability and decision-making reliability of the mobile robot under complex working conditions.

Description

Technical Field

[0001] This invention relates to the field of mobile robot control technology, and in particular to a multimodal perception and decision-making integrated system for mobile robots. Background Technology

[0002] With the widespread application of mobile robots in scenarios such as inspection, handling, and task execution, the requirements for positioning accuracy and decision-making stability during their operation are constantly increasing. Current technologies typically estimate the pose and motion state of mobile robots by fusing multimodal perception information such as vision, inertial sensing, and laser point clouds, and use the state estimation results as input for event triggering and state machine transitions in the decision-making module. However, when using a smoothing optimization method for state estimation, the state results may be backtracked and updated in different output cycles, leading to inconsistent state judgments within the same time period.

[0003] In the aforementioned situations, existing decision-making methods often directly generate or update action sequences based on the current state estimation results. When the state estimation results change, the action sequence is adjusted accordingly, which can easily lead to problems such as changes in the event triggering order, state machine transition backtracking, or repeated triggering. This results in mobile robots exhibiting defects such as disjointed actions, unstable stage transitions, and difficulty in reproducing operational results. Furthermore, existing technologies lack a unified constraint mechanism for the irreversibility of issued actions. When state backtracking updates occur, it is difficult to ensure consistency between the decision-making logic and the actual execution behavior, affecting the overall reliability and controllability of the system.

[0004] To address the above issues, this application presents a multimodal perception and decision-making integrated system for mobile robots. Summary of the Invention

[0005] The technical problem to be solved by this invention is to address the shortcomings of existing technologies by providing a multimodal perception and decision-making integrated system for mobile robots. By performing joint smoothing state estimation on multi-source perception data, a state version graph containing temporal evolution and version evolution relationships is constructed. Under the constraint of action locking window, an action prefix index structure is established by combining historical action sequences. A restricted search is performed on the state version graph to generate an updated action sequence with the smallest difference from the historical action structure, thus solving the problem of discontinuous actions exhibited by mobile robots during operation.

[0006] To achieve the above objectives, the present invention provides the following technical solution:

[0007] A multimodal perception and decision-making integrated system for a mobile robot, the mobile robot including a perception component, the system comprising: a data preprocessing module, for acquiring perception data collected by the perception component and preprocessing the perception data; a data processing module, for initializing the preprocessed perception data to obtain an initial state for nonlinear optimization, and for performing nonlinear optimization on the initial state using an optimization algorithm to obtain a smoothed state estimation result; and a decision-making module, for receiving the smoothed state estimation result, performing action sequence consistency update processing based on the smoothed state estimation result to obtain an updated action sequence for controlling the mobile robot, and sending the updated action sequence to the actuator of the mobile robot, wherein the action sequence consistency update processing includes constructing a state version graph including time edges and version evolution edges based on the smoothed state estimation result, and performing a restricted search in conjunction with the action prefix index of historical action sequences to select the updated action sequence.

[0008] The sensing data includes raw laser point cloud observation data, image data, and inertial measurement data. The data preprocessing module includes: an inertial data preprocessing unit for performing time synchronization and integration processing on the inertial measurement data to generate inertial constraint information between adjacent time points; an image data preprocessing unit for performing feature extraction and feature association on the image data to generate visual observation information characterizing the motion relationship between adjacent image frames; and a point cloud data preprocessing unit for performing usability screening and quality assessment on the raw laser point cloud observation data to eliminate point cloud observations that do not meet preset observation conditions and generate point cloud observation constraint information.

[0009] The data processing module includes: a data initialization unit, used to determine the initial gravity direction, initial pose, initial velocity, and inertial sensor bias parameters based on inertial constraint information and visual observation information, and generate an initial state; and a data optimization and solution unit, used to perform joint optimization and solution on the initial state to obtain a smooth state estimation result containing multiple timestamp states; wherein, the data initialization unit is configured with initialization logic, and the data optimization and solution unit is configured with optimization logic.

[0010] The initialization logic includes: determining the gravity direction and initial attitude based on the inertial measurement data; calculating the visual inertial odometry initial solution based on the inertial measurement data and the image data, and determining the scale factor, initial velocity, and initial offset parameters of the inertial measurement unit; determining the initial position in the global coordinate system based on the original laser point cloud observation data, and determining the alignment parameters between the local coordinate system and the global coordinate system, wherein the alignment parameters include at least the yaw angle offset parameter and the coordinate origin anchoring parameter, the global coordinate system is constructed through the geographic coordinate system corresponding to the location of the mobile robot, and the local coordinate system is constructed through a local reference coordinate system with the initial pose of the mobile robot as a reference; and combining the initial attitude, the scale factor, the initial velocity, the initial offset parameters, the initial position, and the alignment parameters into a state vectorization to obtain the initial state.

[0011] The optimization logic includes: selecting multiple state nodes within a preset sliding window, expanding the initial state into a window state vector, wherein the window state vector includes the pose, velocity, and inertial bias parameters of each state node; constructing a joint optimization objective function based on the inertial constraint information, the visual observation information, and the point cloud observation constraint information, wherein the joint optimization objective function includes a pre-integrated residual term constructed using the inertial constraint information, a reprojection residual term constructed using the visual observation information, and an original observation residual term constructed using the point cloud observation constraint information; performing a consistency check and adaptive weight update on the original observation residual term according to the window state vector, wherein the consistency check includes gating processing based on residual statistics and a preset threshold; and solving the joint optimization objective function through incremental nonlinear optimization to obtain a smooth state estimation result.

[0012] The step of solving the joint optimization objective function through incremental nonlinear optimization includes: perturbing and parameterizing the window state vector, representing the pose, velocity, and inertial bias parameters of each state node as quantities to be updated; in the current iteration, calculating the pre-integration residual term, the reprojection residual term, and the original observation residual term based on the quantities to be updated, and obtaining the Jacobian matrix adapted to the quantities to be updated for each residual term to construct the linearized normal equation; and using the Levenberg-Marquardt algorithm, with the quantities to be updated as the optimization objective, and using the damping factor of the Levenberg-Marquardt algorithm... The normal equation is damped and solved. The Levenberg-Marquardt damping factor is obtained by: constructing a marginal prior in Schul complement form for the state nodes of the sliding window to obtain the corresponding prior factor; calculating the decrease in the joint optimization objective function based on the prior factor; and calculating the damping factor based on the decrease in the joint optimization objective function. The residual calculation, linearization, and solution are repeated until the preset convergence condition is met. The smoothed state estimation result is output based on the optimized update quantity. The preset convergence condition includes that the decrease in the joint optimization objective function is less than a preset change threshold.

[0013] The decision-making module includes: a version state differentiation unit, used to determine a first version state and a second version state, wherein the first version state represents the smoothed state estimation result of the timestamp corresponding to the historical output batch, and the second version state represents the smoothed state estimation result of the current output batch; a graph construction unit, used to construct a state version graph based on the first version state and the second version state, wherein the state version graph includes multiple state nodes and directed edges for characterizing the relationship between state nodes, wherein the state nodes are used to characterize different version states under the same timestamp, and the directed edges include at least: time edges for connecting adjacent timestamp state nodes under the same version, and version evolution edges for connecting different version state nodes under the same timestamp; an action locking unit, used to acquire the historical action sequence of the mobile robot and the execution progress information corresponding to the historical action sequence, and determine an action locking window based on the execution progress information, wherein the action locking window is used to indicate the issued and irrevocable action sub-sequence; and an action update unit, used to determine an updated action sequence based on the action locking window, and issue the updated action sequence to the actuator of the mobile robot; wherein the action update unit is configured with evolution logic.

[0014] The evolution logic includes: constructing an action prefix index structure based on the historical action sequence to record the action prefix of each historical action in the historical action sequence and the corresponding matching relationship; under the constraint that the action subsequence corresponding to the action locking window remains unchanged, performing a restricted search of the action prefix index structure on the state version graph to generate a candidate action sequence, and outputting the candidate action sequence as the updated action sequence.

[0015] The restricted search of the action prefix index structure on the state version graph includes: determining the current state node based on the state reference bound to the last locked action corresponding to the action locking window; initializing the search state, wherein the search state includes the current state node, the current action prefix index pointer, the generated action subsequences, the cumulative sequence difference cost, and version usage description information, wherein the version usage description information includes the number of version switches and the number of times the version evolution edge is used; and generating at least two types of expansion branches starting from the current state node, wherein the two types of expansion branches include: an expansion branch along the time edge, used to expand the current state node to the state node corresponding to the next timestamp while keeping the current version unchanged; and an expansion branch along the version evolution edge, used to expand the current state node to the state node corresponding to the next timestamp while keeping the timestamp unchanged. In the case of upgrading the current state node to another version state node under the same timestamp; for the branch extending along the version evolution edge, perform a version evolution benefit evaluation to determine whether the corresponding version evolution edge is allowed to be included in the candidate extension branch. If included, select and extend the current version evolution edge and time edge; call the action generator to generate action fragments for the selected extension branch, update the action fragments to the generated action subsequence, and perform prefix migration in the action prefix index structure based on the action fragments to update the current action prefix index pointer; update the state node corresponding to the selected extension branch to the current state node, and repeatedly traverse the state version graph until there are no subsequent state nodes, and output the generated action subsequence as a candidate action sequence.

[0016] The evaluation of version evolution benefits includes: determining the version rewriting magnitude index based on the state differences between the state nodes at both ends of the version evolution edge; calculating the predicted action prefix matching gain when using the state node corresponding to the version evolution edge based on the cumulative sequence difference cost; and including the corresponding version evolution edge in the candidate expansion when the action prefix matching gain is greater than the version rewriting magnitude index.

[0017] Compared with the prior art, the beneficial effects of the present invention are:

[0018] This invention uses versioned modeling of multimodal smooth state estimation results and introduces action locking windows and action prefix consistency constraints at the decision layer. This allows the action update process to maintain structural consistency with historical execution behavior while absorbing state backtracking corrections. Compared to existing methods that directly update actions based on the current state, this application effectively avoids problems such as disordered event triggering order, state machine transition backtracking, and frequent reordering of action sequences, enabling the mobile robot to maintain stable and reproducible decision-making behavior during continuous operation. Attached Figure Description

[0019] Other features, objects, and advantages of the invention will become more apparent from the following detailed description of non-limiting embodiments with reference to the accompanying drawings:

[0020] Figure 1 An exemplary application scenario diagram provided for an embodiment of this application;

[0021] Figure 2 This is a schematic diagram of the structure of the mobile robot provided in the embodiments of this application;

[0022] Figure 3 A schematic diagram of a multimodal perception and decision-making integrated system for mobile robots provided in an embodiment of this application;

[0023] Figure 4 A flowchart illustrating an integrated multimodal perception and decision-making method for mobile robots, provided as an embodiment of this application;

[0024] Figure 5 A schematic diagram of the execution flow of the decision module provided in an embodiment of this application;

[0025] Figure 6 A schematic diagram illustrating the principle of version status differentiation provided in this application embodiment;

[0026] Figure 7 This is a schematic diagram of the structure of the state version diagram provided in the embodiments of this application;

[0027] The figure shows the robot body 100, the sensing component 101, and the processor 102. Detailed Implementation

[0028] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments.

[0029] The term "embodiment" as used herein means that a particular feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of this application. The appearance of this phrase in various places throughout the specification does not necessarily refer to the same embodiment, nor is it a separate or alternative embodiment mutually exclusive with other embodiments. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described herein can be combined with other embodiments.

[0030] In the engineering scenarios addressed in this application, mobile robots typically do not operate in a single, stable, and consistently observable ideal environment. Instead, they need to continuously perform tasks such as inspection, material handling, line following, or task execution under conditions of alternating indoor and outdoor environments, alternating occlusion and texture degradation, and fluctuating sensor observation quality depending on the working conditions. To obtain a globally consistent pose reference and suppress long-term cumulative drift, the exemplary technologies commonly employ a state estimation link based on multi-source sensors: complementary constraints are formed by cameras, inertial devices, laser point clouds, or other odometry information, and pose, velocity, and related states are jointly solved within a sliding window through nonlinear optimization over a period of time, thereby outputting a smooth state estimation result. This type of smooth estimation has significant advantages in terms of accuracy, and is particularly suitable for scenarios requiring geometric consistency correction of the trajectory. For example, it can introduce point cloud constraints in open outdoor areas to obtain global anchoring, utilize inertial continuity to maintain short-term stability when visual texture is insufficient, and backtrack to correct historical trajectories after sensor recovery to eliminate drift accumulation.

[0031] However, when using smoothed state estimation results directly as input to the decision-making module, those skilled in the art have long faced an engineering-level deterministic dilemma: the output of smoothed estimation has an inherent property of backtracking variability, meaning that the state at the same timestamp may still be re-optimized and output updated values ​​after subsequent observations are added. In other words, the output of smoothed estimation is not the current pose of a single fixed value, but a window trajectory in batches of outputs, and the same timestamp may correspond to different state results in different output batches. For event triggering and state machine transition logic that relies on threshold judgments, this backtracking variability is amplified into the instability of discrete decision-making near boundary conditions: for example, arrival judgment, boundary crossing judgment, entry / exit region judgment, or stage switching judgment may have been triggered and actions issued in a single run, but after subsequent smoothed backtracking, the trajectory near the same timestamp is corrected, resulting in inconsistencies between the recalculated triggering results and the online execution records; also, under different computing loads and different observation qualities, the iteration rhythm and convergence batches of smoothed solutions are different, causing the state snapshot boundary of the state machine to shift, which in turn leads to the action sequence being unreproducible in repeated experiments. Common approaches in existing engineering implementations include modifying the state machine to only read the latest output at the current moment, introducing a simple hysteresis threshold for event judgment, or resetting the entire state machine when a positioning transition is detected. However, these methods still struggle to eliminate the contradiction between the irreversibility of executed actions and the rewriting of historical states when smoothing estimation involves continuous backtracking updates. On the one hand, once the actuator receives and begins executing the action sequence, the online system cannot roll back the issued instructions. On the other hand, the smoother's backtracking corrections can change the judgment criteria of past moments, leading to the risk of logical backtracking, repeated triggering, or missed triggering in the state machine. This manifests as jitter, hesitation, abnormal stage transitions, or unstable task flow at the motion control level.

[0032] Based on the aforementioned issues, the core concept of this application is not to replace the smoothing estimation itself, but rather to establish a decision consistency strategy oriented towards versioned states, focusing on how the smoothing output is replaced by decisions. The starting point is that since smoothing estimation naturally provides different versions of the same timestamp in the form of output batches, this version difference should be explicitly and structurally expressed. This allows the state machine to update subsequent action sequences with minimal perturbation while constraining the irreversibility of executed actions, rather than recalculating everything after each new snapshot, which could potentially lead to a structural flip of the state machine. This embodiment introduces the concept of an action locking window on the decision side, solidifying the issued and irreversible action subsequences into locking segments, and using these as anchors to organize the smoothing output in a versioned manner: at the same timestamp, the states corresponding to historical output batches and the current output batch are considered different version states, associated through version evolution relationships; states with adjacent timestamps within the same version are associated through temporal continuity. The resulting state version graph depicts both the causal link of time progression and the evolutionary link of backtracking and rewriting. This allows subsequent candidate action generation to no longer be limited to a single path, but to select branches that better match the historical action prefixes on the graph structure formed by the version evolution edge and the time edge. Thus, the consistency update of the action sequence is completed with the criterion of minimizing the difference from the historical action sequence.

[0033] For example, in the transition from an open outdoor area to a shaded area, rapid changes in point cloud observation quality cause the smoother to backtrack and correct the trajectory of the transition section after restoring constraints. Another example is in corridors / factory passages with repetitive visual textures or drastic lighting changes, where short-term degradation of visual constraints causes the state estimate to be reassessed after several seconds. Yet another example is in conditions with slight slippage on the ground or changes in load, where the error distribution between inertia and visual / point cloud constraints changes, causing the smoother to readjust past speed and yaw. In these scenarios, if the state machine directly triggers events based solely on the "latest state," consistency conflicts will inevitably arise where the state is rewritten while the action has already been executed.

[0034] refer to Figure 1 , Figure 1 This is an exemplary application scenario diagram provided for an embodiment of this application.

[0035] exist Figure 1 In the application scenario shown, the mobile robot needs to switch between trackless and tracked paths within the same task flow. The trackless path is typically an open or semi-open environment, where the mobile robot mainly relies on multimodal perception results for autonomous localization and motion control. The tracked path, on the other hand, usually has clear track constraints or structured passage boundaries, limiting the mobile robot's degrees of freedom, and its movement is more of a constrained journey along a predetermined path. This is understandable. Figure 1The tracked and trackless passages shown are only for illustrating different types of operational constraints and do not limit the specific track type, structural dimensions or laying method.

[0036] It is understandable that the perception conditions and motion constraints of a mobile robot will change significantly during the process of moving from a trackless channel to a tracked channel, or vice versa. For example, in a trackless channel, the robot can obtain relatively rich but unstable observations of the external environment, and its state estimation results are highly sensitive to the quality of external sensor observations. In a tracked channel, although the robot is subject to structural constraints, the effectiveness of multimodal perception data may also change due to limited viewing angles, reduced observation redundancy, or occlusion and reflection interference introduced by the track structure. These changes often lead the data processing module to re-optimize historical states within a sliding window, thereby smoothing the state estimation results and making backtracking corrections to the same timestamp between adjacent output batches. When the mobile robot is in Figure 1 When the channel switching boundary is near the boundary shown, the backtracking correction of the smooth state estimation result is very likely to cause the event judgment to change in different output batches, which will lead to the decision logic to bounce back, trigger repeatedly or stage disorder, thus affecting the motion continuity and safety of the mobile robot during the channel switching process.

[0037] refer to Figure 2 , Figure 2 This is a schematic diagram of the structure of a mobile robot provided in an embodiment of this application.

[0038] Figure 2 The illustration shows a mobile robot comprising a robot body 100, a sensing component 101, and a processor 102. The robot body 100 carries various functional components and executes control commands output by the processor 102. In a specific embodiment not shown in the figure, the processor 102 sends control commands to the actuators of the robot body 100, which may include a walking mechanism, a steering mechanism, a braking mechanism, and actuators adapted to specific application scenarios, such as a handling mechanism, a working mechanism, or a detection mechanism. The specific structural form does not constitute a limitation of this application. The sensing component 101 is used to collect sensing data related to the mobile robot's operating state and external environment. The sensing component 101 may include one or more sensors for acquiring pose, motion state, or environmental information, such as a vision sensor, an inertial measurement unit, a laser point cloud module, an odometry system, or other auxiliary sensing units. The processor 102 is used to process the sensing data collected by the sensing component 101.

[0039] Understandable, Figure 2The functional modules shown are only logical divisions. In specific implementations, the processor 102 can be implemented by one or more general-purpose processors, dedicated processors, or combinations thereof; the sensing component 101 can also be flexibly configured according to application requirements. The above structural illustrations are intended to illustrate the functional relationships between the various components of the mobile robot in the embodiments of this application, and do not constitute a limitation on the hardware architecture or module division method.

[0040] In one example, this application provides a multimodal perception and decision-making integrated system for a mobile robot, the system being configured within the processor.

[0041] refer to Figure 3 , Figure 3 This is a schematic diagram of a multimodal perception and decision-making integrated system for mobile robots, provided as an embodiment of this application.

[0042] The system includes: a data preprocessing module, which acquires sensing data collected by the sensing component and preprocesses the sensing data; a data processing module, which initializes the preprocessed sensing data to obtain an initial state for nonlinear optimization, and performs nonlinear optimization on the initial state using an optimization algorithm to obtain a smoothed state estimation result; and a decision module, which receives the smoothed state estimation result, performs action sequence consistency update processing based on the smoothed state estimation result to obtain an updated action sequence for controlling the mobile robot, and sends the updated action sequence to the actuator of the mobile robot. The action sequence consistency update processing includes constructing a state version graph including time edges and version evolution edges based on the smoothed state estimation result, and performing a restricted search using the action prefix index of historical action sequences to select the updated action sequence.

[0043] The sensing data includes raw laser point cloud observation data, image data, and inertial measurement data. The data preprocessing module includes: an inertial data preprocessing unit for performing time synchronization and integration processing on the inertial measurement data to generate inertial constraint information between adjacent time points; an image data preprocessing unit for performing feature extraction and feature association on the image data to generate visual observation information characterizing the motion relationship between adjacent image frames; and a point cloud data preprocessing unit for performing usability screening and quality assessment on the raw laser point cloud observation data to eliminate point cloud observations that do not meet preset observation conditions and generate point cloud observation constraint information.

[0044] The data processing module includes: a data initialization unit, used to determine the initial gravity direction, initial pose, initial velocity, and inertial sensor bias parameters based on inertial constraint information and visual observation information, and generate an initial state; and a data optimization and solution unit, used to perform joint optimization and solution on the initial state to obtain a smooth state estimation result containing multiple timestamp states; wherein, the data initialization unit is configured with initialization logic; and the data optimization and solution unit is configured with optimization logic.

[0045] The decision-making module includes: a version state differentiation unit, used to determine a first version state and a second version state, wherein the first version state represents a smoothed state estimation result of the timestamp corresponding to the historical output batch, and the second version state represents a smoothed state estimation result of the current output batch; a graph construction unit, used to construct a state version graph based on the first version state and the second version state, wherein the state version graph includes multiple state nodes and directed edges for characterizing the relationship between state nodes, wherein the state nodes are used to characterize different version states under the same timestamp, and the directed edges include at least: time edges for connecting adjacent timestamp state nodes under the same version, and version evolution edges for connecting different version state nodes under the same timestamp; an action locking unit, used to acquire historical action sequences and execution progress information corresponding to the historical action sequences, and determine an action locking window based on the execution progress information, wherein the action locking window is used to indicate an action sub-sequence that has been issued and is irreversible; and an action update unit, used to determine an update action sequence based on the action locking window, and issue the update action sequence to the actuator of the mobile robot; wherein the decision-making module also includes a search strategy, the search strategy including evolution logic, and the evolution logic is configured within the action update unit.

[0046] In some optional embodiments, this application also provides a method for integrating multimodal perception and decision-making for mobile robots, referencing... Figure 4 , Figure 4 The methods shown include:

[0047] S1: Acquire the sensing data collected by the sensing component and preprocess the sensing data;

[0048] In this embodiment, the perception data mainly comes from the mobile robot's continuous perception of its own motion state and external environment during actual operation. It may include, but is not limited to, visual observation data, inertial measurement data, positioning-related data, and auxiliary information reflecting the continuity of motion.

[0049] Those skilled in the art will understand that the aforementioned sensing data does not require a fixed combination format; it only needs to be able to characterize the changes in the mobile robot's operating state over time. The preprocessing of the sensing data is mainly used to complete time alignment, outlier suppression, and basic format unification, so as to reduce the impact of differences in scale, frequency, or noise characteristics between different sensing sources on the subsequent estimation process.

[0050] S2: Initialize the preprocessed sensing data to obtain the initial state for nonlinear optimization solution;

[0051] In this embodiment, the initialization process does not simply generate the initial pose value at a certain moment, but rather constructs an initial state guess based on a continuous running data to support the joint optimization solution within the subsequent sliding window.

[0052] S3: Obtain a smooth state estimation result by performing nonlinear optimization on the initial state using an optimization algorithm, and transmit the smooth state estimation result to the decision module;

[0053] In this embodiment, the nonlinear optimization uses the states at multiple time points within a time window as joint variables. By introducing constraints from different sensing data, the initial state is iteratively solved to obtain a smooth state estimation result that balances geometric consistency and temporal continuity.

[0054] It should be noted that the smoothed state estimation results are not a one-time fixed output, but rather a set of results that are backtracked and corrected for historical states in different output batches as running data is continuously input.

[0055] Before delving into the specific technical details of the steps, the embodiments of this application need to be emphasized again.

[0056] Smoothing estimation does not simply output the state value at a single instant, but rather performs joint optimization on a state sequence over a period of time under window constraints. Therefore, its output naturally carries batch and snapshot attributes. The state quantity corresponding to the same timestamp is not necessarily unique. When subsequent observations enter the window and change the constraint assignment, the optimal solution at an earlier time will be revised and reappear in the new output batch. The key issue arising from this is not that the error increases or decreases, but rather that the stability of the state sequence changes: for continuous control, small corrections to state values ​​often only manifest as slight perturbations in the command; however, for event-triggered and state machine transitions, decisions usually depend on thresholds, boundaries, or logical conditions. Small corrections may cross decision boundaries, leading to opposite event conclusions in different batches at the same time. Those skilled in the art will understand that this phenomenon stems from a structural inconsistency between the smooth optimization output form and the discrete decision input assumption. Its essence is a logical conflict between the allowability of history rewriting and the irrevocability of actions, requiring the introduction of time consistency constraints on the decision side that match the smooth output.

[0057] Understandably, this application does not change the solution method for smoothing estimation. For each output batch, in addition to the smoothed state vector itself, a timestamp index and a version identifier are introduced to distinguish the update state of the same timestamp in different batches. Thus, the difference between historical batches and the current batch is no longer implicitly covered, but is explicitly expressed as an evolutionary relationship: the state update at the same timestamp can be regarded as a backtracking evolution from an earlier version to a later version; the states at adjacent timestamps are reflected as temporal continuity within the same version.

[0058] Furthermore, this application equates the stability problem of discrete decision-making to a sequence consistency problem: since historical actions have been partially issued and are irrevocable, subsequent action updates should prioritize maintaining the causal closure of the executed parts. That is, any new candidate action sequence should be prefixed with the locked segment formed by the issued actions, and updated with minimal perturbation on this basis. Minimal perturbation here does not refer to maintaining similar control variables in general, but rather to maintaining the consistency of the action symbol sequence formed by event triggering and state machine transitions; in other words, keeping the discrete structure of which events were triggered and in what order which stage transitions were completed unchanged, with limited repairs only at adjustable boundaries.

[0059] Those skilled in the art will understand that defining the update target as minimizing the difference in the action sequence can simultaneously satisfy two types of constraints in engineering: on the one hand, the locking segment ensures that the issued actions are not rolled back, avoiding inconsistencies with the actual state of the actuator; on the other hand, the principle of minimizing the difference suppresses the overall flipping of decision branches caused by smooth backtracking, so that the same segment of running data can still obtain a reproducible state machine transition trajectory under different computational loads.

[0060] Next, we will further elaborate on the technical aspects of the preprocessing method in this application.

[0061] In one example, the sensing data includes raw laser point cloud observation data, image data, and inertial measurement data.

[0062] In another example, preprocessing the perceived data includes:

[0063] The inertial measurement data is time-synchronized and integrated to generate inertial constraint information between adjacent time points;

[0064] Specifically, since inertial measurement units typically output data at a much higher frequency than other sensors, their timestamp accuracy and sampling continuity have a decisive impact on subsequent state estimation. In this embodiment, a method based on hardware timestamps or software time alignment can be used to map inertial data and other sensing data to a unified time axis. Based on this, inertial data between adjacent time points are integrated or pre-integrated to generate inertial constraint information reflecting attitude changes, velocity changes, and displacement changes over a short period of time.

[0065] Feature extraction and feature association are performed on the image data to generate visual observation information that characterizes the motion relationship between adjacent image frames;

[0066] Specifically, existing feature extraction and matching techniques, such as local feature methods based on corners, spots, or descriptors, can be used to extract feature points with stable geometric properties from each frame of the image. Subsequently, feature correspondences can be established between adjacent image frames through feature association or tracking algorithms, thereby forming visual observation information to describe the relative motion of the camera.

[0067] The original laser point cloud observation data is subjected to availability screening and quality assessment to eliminate point cloud observations that do not meet the preset observation conditions and generate point cloud observation constraint information.

[0068] In this embodiment, the raw data can be filtered based on observation conditions commonly used in the prior art. For example, observations that do not meet the preset observation conditions can be eliminated or downweighted based on indicators such as signal-to-noise ratio, observation continuity, or epoch consistency. At the same time, corresponding point cloud observation constraint information is generated for the retained observations to reflect their contribution to state estimation.

[0069] Next, we will further elaborate on the technical content of the initialization method in this application.

[0070] Understandably, the purpose of the initialization process is to provide a self-consistent starting solution in terms of both physical meaning and geometric constraints for subsequent nonlinear optimization. This avoids situations where non-convergence or convergence to an unreal solution occurs when joint optimization is directly entered before multimodal constraints are fully established. Those skilled in the art will understand that multi-source sensing data often suffers from unknown scale, misaligned coordinate systems, and uncalibrated bias parameters in the initial stage. Without reasonable initialization to constrain these uncertainties, subsequent smoothing state estimation will struggle to produce stable and interpretable results. Therefore, this embodiment constructs the initial state in stages and by physical quantities, ensuring that various types of sensing data satisfy basic physical consistency before entering optimization.

[0071] In one example, the initialization of the preprocessed sensory data includes:

[0072] The direction of gravity is determined based on the inertial measurement data, and the initial attitude is determined.

[0073] Based on the inertial measurement data and the image data, calculate the initial solution for visual inertial odometry, and determine the scale factor, initial velocity, and initial bias parameters of the inertial measurement unit;

[0074] Specifically, when determining the direction of gravity and the initial attitude, the characteristic of acceleration measurements in inertial measurement data, which primarily reflect gravitational acceleration under short-term stationary or quasi-stationary conditions, can be utilized. Time averaging or low-pass filtering of the acceleration observations can be performed to estimate the direction of the gravity vector in the sensor coordinate system. Those skilled in the art will understand that by aligning this gravity vector with the known gravity direction in the global coordinate system, estimates of the initial pitch and roll angles can be obtained. Based on this, the yaw angle can be temporarily uncertain or assigned an initial value, reserving degrees of freedom for subsequent correction using point cloud or visual constraints. When calculating the initial solution for visual inertial odometry, given that the direction of gravity is known, inertial measurement data and image data can be jointly utilized. Existing visual inertial initialization techniques can be used to estimate the scale factor, initial velocity, and initial bias parameters of the inertial measurement unit. For example, a scale-free relative motion trajectory can be obtained first based on the temporal changes of image features. Then, by combining the integral constraints of inertial measurement in the time dimension, the scale factor can be solved, and the velocity state at each moment can be estimated simultaneously. At the same time, by analyzing the residual distribution between inertial measurement and visual motion, the zero bias of the gyroscope and accelerometer can be initially estimated. Those skilled in the art will understand that the above process does not require high-precision results; its main purpose is to provide initial values ​​of correct magnitude and consistent sign for subsequent nonlinear optimization, thereby avoiding degradation of the optimization in the scale or velocity direction.

[0075] The initial position in the global coordinate system is determined based on the original observation data of the laser point cloud, and the alignment parameters between the local coordinate system and the global coordinate system are determined. The alignment parameters include at least the yaw angle offset parameter and the coordinate origin anchoring parameter. The global coordinate system is constructed using the geographic coordinate system corresponding to the location of the mobile robot, and the local coordinate system is constructed using a local reference coordinate system with the initial pose of the mobile robot as a reference.

[0076] Specifically, the global coordinate system originates from the Earth reference frame used by navigation systems. When calculating the receiver position using raw laser point cloud observations, the process essentially involves solving for the receiver's three-dimensional coordinates and clock errors within a fixed Earth reference coordinate system. In engineering implementations, two main types of global coordinate representations are commonly used: one is the Earth-centered Earth-fixed coordinate system (ECEF), which uses the Earth's center of mass as its origin and remains fixed with the Earth's rotation; the other is a local tangent coordinate system (e.g., ENU, East-North-Up), which uses a reference point in the receiver's region as its origin and establishes three orthogonal axes—east, north, and celestial—at that point.

[0077] In this embodiment, the initial geographic coordinates (latitude, longitude, and altitude) are first calculated based on the original laser point cloud observations, and this location is used as a reference point. Then, an ENU coordinate system is established at this reference point, approximating small-scale movements near the Earth's surface as displacement changes within a three-dimensional Cartesian coordinate system. The theoretical basis for this approach is the Earth ellipsoid model and the local tangent approximation: within the typical operating radius of a mobile robot, the ENU can be considered as a linear space approximately parallel to the ground, allowing subsequent optimization to handle positional errors in Euclidean space without directly performing nonlinear geographic calculations on the surface coordinates. Those skilled in the art will understand that if the application scope is larger, the ECEF can also be directly used as the global coordinate system, or a known rotation matrix can be used to convert between the ENU and ECEF.

[0078] Furthermore, the local coordinate system originates from the way vision / inertial odometry represents robot motion. Vision and inertial data can provide high-frequency, short-term stable relative motion information, but their output is usually defined in a "reference coordinate system at startup": that is, the origin is taken as the moment the mobile robot begins initialization, and the initial attitude direction at that time is taken as the coordinate axis direction. This coordinate system can be called the local world system or the local reference coordinate system.

[0079] The initial state is obtained by combining the initial attitude, the scale factor, the initial velocity, the initial bias parameter, the initial position, and the alignment parameter into a state vector.

[0080] Next, we will further elaborate on the technical content of the optimization algorithm in this application.

[0081] In one example, the step of obtaining a smooth state estimation result by performing nonlinear optimization on the initial state using an optimization algorithm includes:

[0082] S3.1: Select multiple state nodes within a preset sliding window, and expand the initial state into a window state vector, wherein the window state vector includes the pose, velocity, and inertial bias parameters of each state node;

[0083] Specifically, the introduction of the sliding window corresponds to the balance between computational complexity and utilization of historical information when implementing nonlinear smoothing estimation in engineering applications: if only the current time is considered, it is difficult to utilize the observation redundancy across timeframes to suppress noise and short-term degradation; if the entire time domain is considered, the computational complexity accumulates over time and is difficult to run in real time. Therefore, using a sliding window of a preset length restricts the optimization variables to the most recent time domain, enabling state estimation to both utilize multiple frames of observations to form a constraint loop and keep the solution scale within a real-time processing range.

[0084] In this embodiment, the multiple state nodes are organized chronologically. Each node includes at least position, attitude, velocity, and zero-bias parameters of the inertial measurement unit. Attitude can be represented by quaternions or rotation vectors, and velocity is represented by a three-dimensional velocity vector in a local or global coordinate system. Zero bias includes gyro zero bias and acceleration zero bias. The window state vector is constructed as follows: the initial state of the first node is used as the starting point of the window. Subsequently, for each new node entering the window, its state variables are appended to the end of the window state vector. To ensure the observability and numerical stability of the variables, the attitude variables can be optimized using a parameterization method with minimum perturbation increment. That is, during the optimization process, the small increment of attitude is updated instead of directly replacing the complete attitude representation. To facilitate subsequent marginalization and factor construction, each state node stores its index relationship and observation association relationship with its neighboring nodes in memory. For example, it stores the inertial pre-integration segment number in which the node participates, the visual feature observation set number in which it participates, and the available point cloud observation set number.

[0085] S3.2: Construct a joint optimization objective function based on the inertial constraint information, the visual observation information, and the point cloud observation constraint information, wherein the joint optimization objective function includes a pre-integration residual term constructed through the inertial constraint information, a reprojection residual term constructed through the visual observation information, and an original observation residual term constructed through the point cloud observation constraint information;

[0086] Specifically, the joint optimization objective function needs to unify constraints from different physical sources into the same error metric framework, so that the constraints of each observation on the state variables can complement each other rather than conflict during the iterative solution process. Inertial constraints provide high-frequency short-time continuity, visual constraints provide relative geometric consistency, and the original point cloud observations provide external anchoring for global position and velocity directions.

[0087] In this embodiment, the construction of the inertial pre-integration residual term takes the pre-integration result between two adjacent state nodes as input. This pre-integration result is output from the previous stage preprocessing and includes relative rotation increment, relative velocity increment, and relative displacement increment, carrying uncertainty information obtained from noise propagation. In the objective function, the pre-integration residual is used to constrain the attitude, velocity, and displacement changes of adjacent nodes to be consistent with the pre-integration prediction. At the same time, the gyroscope zero bias and acceleration zero bias are used as estimable parameters in the residual calculation to gradually correct the bias drift within the window. The construction of the visual reprojection residual term takes feature observation as input. Feature observation is obtained by feature extraction and association, usually represented as the pixel coordinates of a certain feature in several frames. In the objective function, the three-dimensional feature points can be treated as explicit or implicit variables. When using explicit variables, the three-dimensional coordinates of the feature points are maintained within the window and participate in the optimization together with the camera extrinsic parameters. When using implicit variables, inverse depth parameterization can be used and feature points can be eliminated in each iteration to control the size of the optimization variables. The point cloud raw observation residual term takes the raw observations such as pseudorange, Doppler or carrier phase as input, and directly imposes constraints on receiver position, velocity and receiver clock error and clock drift related variables in the objective function.

[0088] S3.3: Perform consistency checks and adaptive weight updates on the original observation residuals based on the window state vector. The consistency check includes gating processing based on residual statistics and preset thresholds.

[0089] In this embodiment, the consistency check can be organized in three layers: observation, point cloud, and epoch. The observation check is based on the residual statistics of a single pseudorange or Doppler observation. The residual statistics can be normalized residuals, which are the residuals divided by the standard deviation or weight parameter corresponding to the observation, to eliminate scale differences caused by different point cloud elevation angles and different signal-to-noise ratios. When the normalized residuals exceed a preset threshold, gating processing is performed on the observation. Gating processing can be manifested as removing the observation or reducing its weight to a preset minimum value. Point cloud testing is used to identify situations where the same point cloud exhibits persistent anomalies over a short period. It involves counting the number of times the point cloud is gated or the mean residual drift within a sliding window. When the persistent anomaly condition is met, the participation of that point cloud is temporarily frozen until consistency is restored over several consecutive epochs. Epoch testing is used to identify overall GNSS observation degradation at a given moment. It involves counting the number of effective observations, the dispersion of the residual distribution, or the consistency pass rate at that epoch. When the overall quality of that epoch falls below a threshold, all point cloud observations at that epoch are uniformly downweighted to avoid excessive perturbation of the window solution by concentrated anomalies in a single epoch. Adaptive weight updates can be implemented using a piecewise function: maintaining the basic weights when consistency is good, smoothly reducing the weights according to the residual magnitude when consistency is marginal, and triggering gating and setting the weights to minimum when consistency is poor.

[0090] S3.4: Solve the joint optimization objective function by incremental nonlinear optimization to obtain the smooth state estimation result;

[0091] Specifically, the joint objective function is typically nonlinear, and the dimension of the window state vector increases significantly with the number of nodes and features. Solving iteratively from scratch directly introduces a high computational burden and impacts real-time performance. Furthermore, the sliding window requires continuous updates to the solution as new data arrives. If a complete reconstruction and full solution are performed with each window slide, computational costs accumulate over time, leading to latency fluctuations. Therefore, incremental nonlinear optimization is employed, updating only the new variables and factors based on the previous solution and reusing existing linearization information. This maintains solution quality while reducing recalculation costs for each iteration. The Levenberg-Marquardt iteration adaptively switches between Gaussian-Newton and gradient descent strategies through a damping strategy, ensuring convergence stability even with large initial value deviations or changing observation conditions.

[0092] In one example, solving the joint optimization objective function through incremental nonlinear optimization includes:

[0093] The window state vector is perturbed and parameterized, and the pose, velocity and inertial bias parameters of each state node are represented as quantities to be updated.

[0094] In the current iteration, the pre-integrated residual term, the reprojection residual term, and the original observation residual term are calculated based on the quantity to be updated, and the Jacobian matrix adapted to the quantity to be updated is obtained for each residual term to construct the linearized normal equation.

[0095] The Levenberg-Marquardt algorithm is employed, with the quantity to be updated as the optimization objective. The normal equation is damped and solved using the damping factor of the Levenberg-Marquardt algorithm. Obtaining the Levenberg-Marquardt damping factor involves: constructing a marginalized prior in Schul complement form for the state nodes of the sliding window, obtaining the corresponding prior factor; calculating the decrease in the joint optimization objective function based on the prior factor; and calculating the damping factor based on the decrease in the joint optimization objective function. The residual calculation, linearization, and solution are repeated until a preset convergence condition is met. Finally, a smoothed state estimation result is output based on the optimized quantity to be updated.

[0096] The preset convergence condition includes the decrease in the joint optimization objective function being less than a preset change threshold.

[0097] It is understood that the Levenberg-Marquardt algorithm is mainly used in the state estimation process of this application. Those skilled in the art can reproduce the aforementioned steps according to the specific implementation of the Levenberg-Marquardt algorithm, and this application will not elaborate on them here.

[0098] It's important to note that in sliding window smoothing estimation, the observation constraints upon which the joint optimization objective function relies do not only apply to the state nodes at the "current moment," but rather form a temporal correlation across multiple moments: inertial pre-integration naturally connects adjacent nodes, visual features often form long-chain constraints across multiple frames, and point cloud observations provide external anchoring for multiple nodes within the window. As new data arrives, the window needs to slide forward to control the computational scale, at which point the earliest state node is removed from the window. If the removed node and its associated observation factors are directly deleted, the historical information carried by the residual terms within the window will be truncated, resulting in the estimated solution being more prone to accumulation and drift over long periods, and lacking historical constraint support during the perception degradation phase, leading to instability in the solution. Conversely, if all historical nodes are retained for a long time, the variable dimensionality increases over time, and the solution complexity continuously rises, making it difficult to meet real-time requirements.

[0099] The principle of edge-mapping is precisely to address this contradiction: while removing the variable that slides out of the window, the constraint information between it and the remaining variables in the window is compressed and preserved in the form of equivalent priors, so that the window size can be controlled and historical information can be preserved at the same time.

[0100] In this embodiment, the core operation of constructing the marginalization prior is to perform variable elimination on the linearized normal equation. Specifically, at the current iteration point, all residual factors related to the slip-out node are linearized, and the obtained linearization information is summarized into the corresponding normal equation. Then, the optimization variables are divided into variables to be marginalized (i.e., the state node of the slip-out window and its local variables that are only related to the node, such as the deactivated feature point parameters) and retained variables (i.e., the state node still in the window and its related variables).

[0101] Under this partitioning, the normal equation can be represented in blocks as a coupling relationship between two types of variables. By performing Schur complement elimination on the marginalized variables, the equation that originally depended on both types of variables can be equivalently transformed into an equation that only depends on the retained variables. The result is that a prior constraint term is added to the retained variables, which is numerically equivalent to the combined constraint of the slip-out node and its historical observations on the variables within the current window. Subsequently, this prior constraint term is added to the factor set of the current window as a prior factor, and participates in the solution in subsequent iterations along with factors such as inertia, vision, and point cloud, thereby ensuring that historical information continues to influence the solution within the window through the prior factor.

[0102] For example, the following numerical example is provided to illustrate how to solve for the smooth state estimation result using an optimization algorithm. This example is only used to explain the computational link and dimensional relationship; the selected parameters and values ​​are illustrative and do not represent actual calibration results or engineering recommendations.

[0103] In this example, the sliding window is configured to contain 6 state nodes, with node intervals corresponding to a keyframe rhythm of approximately 0.2 seconds, allowing the window to cover a temporal range of approximately 1.0 second. The state variables of each state node include 3D position, attitude, 3D velocity, and the gyro and acceleration zero biases of the inertial measurement unit (IMU). Therefore, the window state vector is organized as a set of multi-node states concatenated in chronological order. Within the window, adjacent nodes are connected by inertial pre-integration constraints. Visual observations provide reprojection constraints across several nodes, while the original point cloud observations provide pseudorange and Doppler constraints at each node. To better reflect weight differences, the noise level corresponding to inertial pre-integration is set to the typical IMU level, the visual reprojection error is set to the sub-pixel to multi-pixel range in pixel units, the equivalent noise of point cloud pseudorange observations is set to the meter level, and the equivalent noise of Doppler observations is set to meters per second. Therefore, the joint optimization objective function is numerically represented as a weighted superposition of the three types of residuals after normalization of their respective noise models, so that observations of different dimensions can work together in the same solution process to update the state.

[0104] In one iteration, perturbation parameterization represents the state update of each node as a set of small increments, where the attitude increments are expressed using Lie algebra perturbations to keep the attitude updates within the effective domain of the rotation group. Based on this perturbation form, the residuals and corresponding Jacobians are calculated for all factors within the window, forming the linearized normal equation. For example, when the average residual of visual reprojection decreases from about 2.5 pixels to about 1.4 pixels in a certain iteration, the residual statistics corresponding to inertial pre-integration remain within the expected range, while some point cloud pseudorange residuals become too large (e.g., the normalized residual exceeds the threshold), gating and weighting are applied to these point cloud observations to reduce their contribution to the normal equation in that iteration, thereby preventing them from pulling the solution in a direction inconsistent with both inertial and visual approaches. Subsequently, a Levenberg-Marquardt damping strategy is employed to adjust the normal equations and solve for the incremental update: when the objective function decreases significantly, the damping factor is reduced to approximate the Gauss-Newton step size; when the objective function does not decrease significantly or shows an upward trend, the damping factor is increased to tend towards a more conservative update. After several iterations (illustrated as 3 to 8 iterations), when the change in the objective function decreases to within a preset change threshold, for example, if the change in the normalized objective function between two adjacent iterations is lower than a certain small threshold, then the convergence condition is satisfied, and the smoothed state estimation result corresponding to that window is output.

[0105] When the window slides forward and introduces the 7th state node, the 1st state node slides out of the window. To avoid directly discarding the inertial, visual, and point cloud constraints associated with the 1st node, this example performs marginalization processing on the slid-out node, compressing the constraints between it and the remaining nodes in the window into an equivalent prior factor, and attaching this prior factor to the earliest retained node in the current window.

[0106] Next, refer to Figure 5 , Figure 5 This is a schematic diagram of the execution flow of the decision module provided in an embodiment of this application.

[0107] Understandable, Figure 5 The method shown is applied to the decision-making module of a mobile robot.

[0108] A1: Based on the timestamp of the smoothed state estimation result of the current output batch, determine the first version state and the second version state, wherein the first version state represents the smoothed state estimation result of the timestamp corresponding to the historical output batch, and the second version state represents the smoothed state estimation result of the current output batch.

[0109] A2: Construct a state version diagram based on the first version state and the second version state;

[0110] A3: Obtain historical action sequences and execution progress information corresponding to the historical action sequences, and determine action lock windows based on the execution progress information, wherein the action lock windows are used to indicate action sub-sequences that have been issued and cannot be undone;

[0111] A4: Determine the updated action sequence based on the action lock window, and send the updated action sequence to the actuator of the mobile robot.

[0112] It should be noted that the timestamps mentioned in this application do not simply correspond to the sensor sampling time or a single physical point in time, but rather to the time indexes of the state nodes included in the smoothed state estimation results of a certain output batch. In other words, the timestamps in this application have a clear output semantic: when the data processing module outputs a set of smoothed state estimation results in the current output batch, each state node in that output batch carries a timestamp identifier to indicate the time position corresponding to that state within the window; if there is no smoothed state estimation result in a certain historical output batch that completely corresponds to a certain timestamp of the current output batch, then when constructing the state version diagram, the historical state under that timestamp can be regarded as a missing state, and it can be filled in by inheriting the most recent available state to ensure the integrity and consistency of the state version diagram on the time axis. To maintain as much state continuity as possible, the historical output batches in this application can be set as the previous output of the current output batch.

[0113] refer to Figure 6 , Figure 6 This is a schematic diagram illustrating the principle of version status differentiation provided in the embodiments of this application.

[0114] Figure 6 The current output batch includes four state node time indices: timestamp 1, timestamp 2, timestamp 3, and timestamp 4. The smoothed state estimation results corresponding to the four state node time indices are used as the second version state.

[0115] Figure 6 The historical output batch is further shown to include the time indexes of four state nodes: timestamp 0, timestamp 1, timestamp 2, and timestamp 3. Timestamp 0 does not belong to the current output batch, so the corresponding smoothed state estimation result is discarded. The smoothed state estimation results corresponding to timestamp 1, timestamp 2, and timestamp 3 are used to fill in the most recent available state, and the placeholder result of timestamp 4 corresponding to the current output batch is obtained, thus obtaining the first version state.

[0116] In one example, the state version graph includes multiple state nodes and directed edges for representing the relationships between state nodes. The state nodes are used to represent different version states at the same timestamp, and the directed edges include at least: time edges for connecting adjacent timestamp state nodes at the same version, and version evolution edges for connecting different version state nodes at the same timestamp.

[0117] Understandably, constructing a state version graph is not about creating abstract data records, but rather transforming the "smoothed state estimation results output in batches" into a searchable, traversable, and structured graph capable of supporting subsequent restricted searches. Specifically, before constructing the state version graph, the output of the data processing module needs to be uniformly batch-encapsulated. Each output batch can be defined as a snapshot record, which at least includes: a batch identifier, a set of timestamps covered by the batch, and a set of state estimation entries indexed by timestamps. Each state estimation entry at least includes state variables such as timestamp, pose, velocity, and inertial bias, and optionally includes residual statistics, covariance, or observation visibility labels for subsequent filtering or cost calculation.

[0118] To ensure comparability across batches, this embodiment employs a timestamp-aligned indexing strategy: the timestamp set in the current output batch is considered the baseline set, and historical output batches are mapped to the same set during graph construction. If a historical batch lacks a state estimation entry corresponding to a certain timestamp, that timestamp is considered missing within the historical batch, and a placeholder entry is generated according to a preset completion strategy, allowing subsequent node generation and edge connection to occur on a unified timeline. The completion strategy can employ nearest neighbor inheritance, i.e., finding the nearest existing state entry in the historical batch to that timestamp, copying it, and marking it as a completion node; or it can employ an empty node approach, i.e., generating only a node identifier without filling in values ​​and marking it as unusable for cost calculation. Both methods can achieve graph structure integrity, and the specific choice can be determined based on implementation preferences.

[0119] In practical implementation, a node index table can be established, and the timestamp set of the current output batch can be traversed. For each timestamp, the status entries of the historical output batch and the current output batch at that timestamp are read respectively. When a historical entry exists, a historical version node is generated; when a historical entry is missing, a supplementary version node is generated, and a flag is written to the node to distinguish it from the real node and the supplementary node. At the same time, the current version node is generated. In addition to storing the state vector, the node object can also store the batch to which the node belongs, the window position index corresponding to the node, and a summary of the observation set that the node participates in, so as to be quickly retrieved when expanding along the edges laterally. To avoid duplicate node construction, the node index table performs an existence check before insertion. If a node with the same key already exists, its numeric field and metadata field are updated, and its edge connection relationship is preserved or updated incrementally according to the update rules.

[0120] Furthermore, the generation of directed edges requires separate processing of time edges and version evolution edges. Time edges are used to connect nodes with adjacent timestamps under the same version, so their generation is based on the version as the main index. Version evolution edges are used to connect nodes with different versions under the same timestamp, so their generation is based on the timestamp as the main index: for each timestamp, obtain the historical version nodes and the current version nodes under that timestamp, and establish a version evolution edge from the historical version nodes to the current version nodes.

[0121] refer to Figure 7 , Figure 7 This is a structural schematic diagram of the state version diagram provided in the embodiments of this application.

[0122] Figure 7 by Figure 6 The output batch relationship shown is used as an example for illustration. Solid lines represent time edges, and dashed lines represent version evolution edges, to intuitively illustrate the dual relationship between the smooth state estimation results in the time dimension and the output batch dimension.

[0123] like Figure 7 As shown, the state version diagram can be structurally divided into two layers: the upper layer corresponds to the state nodes under the current output batch, and the lower layer corresponds to the state nodes under the historical output batches. In a specific implementation not shown in the diagram, each state node is associated with a unique timestamp, representing the smoothed state estimation result at that timestamp. Taking "Current State Node 1" to "Current State Node 4" in the upper layer as an example, these nodes are connected by solid lines in chronological order, forming a continuous temporal link, used to characterize the continuity of the smoothed state estimation result in the current output batch over time.

[0124] Next, we will further elaborate on the technical content of the motion-locked window method in this application.

[0125] Understandably, during the actual operation of a mobile robot, the decision-making module does not generate and issue a complete sequence of actions all at once. Instead, it continuously outputs actions or action segments online. Once these actions are issued to the actuator, they begin execution at the physical level, and there is usually no mechanism for rollback or cancellation. For example, once motion control commands have been sent to the drive unit, the mechanical structure has begun to move or turn, and the working device has entered the working state, even if the subsequent state estimation results change, it is impossible to require the actuator to revert to the previous state. Therefore, from the perspective of system consistency, the decision-making module must logically distinguish between historical actions that have already taken effect and are irreversible, and future actions that have not yet been issued but can be replanned based on the new state. Otherwise, when there is a backtracking correction in the smoothing of state estimation, a disconnect between the decision-making logic and the actual execution can easily occur.

[0126] The action locking window in this application is used to explicitly model irreversibility at the action sequence level. The action locking window can be viewed as an interval on the timeline or action sequence index axis, used to identify the portion of the historical action sequence that has been issued and entered the execution phase. In this embodiment, the determination of the action locking window is not based solely on the current moment, but rather on a comprehensive consideration of factors such as execution latency, communication latency, and the minimum response cycle of the actuator. For example, based on the control cycle and buffer depth of the actuator, the termination position of the locking window can be set several action steps before the last action that has been confirmed to be executed, to avoid boundary actions being mistakenly judged as modifiable when their execution status is unclear.

[0127] Next, we will further elaborate on the technical content of the method in this application regarding updating the action sequence.

[0128] Understandably, in mobile robot systems with smooth state estimation capabilities, state updates are not predicated on the assumption that the current state is uniquely correct, but rather on gradual correction based on observations. Especially when using sliding window smoothing optimization, the state estimation result given by the current output batch is essentially a stage-optimal solution for multiple states within the window, and its value may still be adjusted retrospectively in subsequent output batches due to the introduction of new observations. If the state evaluation result of the current output batch is directly used as the sole basis for decision-making, and subsequent actions are completely replaced, then during the continuous sliding of the window and the gradual strengthening of constraints, the action sequence is prone to frequent structural changes due to state fine-tuning. This change may only manifest as parameter jitter at the continuous control level, but at the event triggering and state machine transition levels, it is often amplified into problems such as premature, delayed, or even repeated stage switching, thus disrupting the continuity of the operation.

[0129] This application does not employ a strategy of recalculating all actions based on the current batch state when updating action sequences. Instead, it treats action updates as a gradual correction process constrained by history. In this process, historical action sequences are not simply considered outdated results, but rather incorporated into the update logic as established facts that have already had a physical impact. After boundary-limiting irreversible actions through action locking windows, the generation of updated action sequences no longer aims for complete consistency with the estimated values ​​of the current state, but rather for maximum structural consistency with historical action sequences while satisfying the constraints of the current state. This consistency is reflected in the preservation of event triggering order, state machine transition paths, and key behavioral stages, ensuring that backtracking corrections of state estimates are not directly amplified into repeated jumps at the action level.

[0130] In one example, determining the update action sequence based on the action lock window includes:

[0131] A4.1: Construct an action prefix index structure based on the historical action sequence, wherein the action prefix index structure is used to record the action prefix of each historical action in the historical action sequence and the corresponding matching relationship;

[0132] Specifically, in this type of problem, the historical action sequence is not a simple old plan, but rather the actual execution trajectory carrying event triggers and state machine transitions. Some of these actions have already been issued and entered an irreversible phase on the executor side. Since the smoothed state estimation output batch may backtrack and update the state at the same timestamp, directly regenerating actions based on the current batch state can easily lead to a rearrangement of the action structure within adjacent update cycles, causing deviations between the event trigger order, stage switching positions, and historical execution facts. Therefore, transforming the historical action sequence into a searchable and matchable prefix structure allows subsequent candidate actions to be guided by the historically traversed action structure during generation, reducing unnecessary branch deviations while keeping the constraint locking window unchanged.

[0133] In this embodiment, the action prefix index structure can be implemented using a prefix tree, a directed prefix graph, or an equivalent hash prefix table. The index object is not the continuous numerical value of the original control quantity, but rather a sequence of action symbols after the action has been discretized and encoded. Each action symbol at least includes an action type identifier representing event triggering and state machine transitions, such as entering a state, exiting a state, triggering an event, or maintaining a control mode. It may also optionally carry parameter labels related to the action, such as speed level, curvature level, target point number, or constraint mode number. For continuous parameters, a quantized bucketing method is used to generate parameter labels, ensuring that cases with the same action structure but slightly different parameters are still considered matchable branches at the index level, thus avoiding excessive fragmentation of the index structure due to parameter perturbations. During index construction, actions are inserted sequentially according to the chronological order of the historical action sequence. For each inserted action symbol, the action sequence number range, the corresponding timestamp range, and the set of subsequent actions for that prefix are recorded on the index node, forming a prefix-successor matching table. To facilitate quick determination of the matching length in the subsequent search phase, each index node can also store pointers to parent and child nodes, as well as a hash index for fast jumps, so that a matching branch can be found in constant time given an action symbol.

[0134] A4.2: Under the constraint that the action subsequence corresponding to the action locking window remains unchanged, perform a restricted search of the action prefix index structure on the state version graph to generate a candidate action sequence, and output the candidate action sequence as the updated action sequence.

[0135] Specifically, the state version graph provides information on multiple versions of the state at the same timestamp and the continuous structure across time. However, if only one-way progress is made, the generation of candidate actions will still be constrained by the assumption that the current version is unique, and it cannot absorb version differences caused by backtracking optimization. If all version combinations are traversed on the graph without constraints, the candidate space will expand rapidly and easily generate branches that differ significantly from the historical action structure, making the update action sequence lack stability in adjacent cycles. Therefore, introducing the action prefix index structure into the graph search process makes the graph expansion constrained not only by state reachability but also by the matching relationship of the historical action structure. This can limit candidate generation to graph paths that are consistent with or can be aligned with the historical action prefix at low cost, so that candidate actions can both absorb backtracking corrections by utilizing version evolution edges and avoid introducing irrelevant state machine branches due to version switching.

[0136] In one example, a restricted search of the action prefix index structure is performed on the state version graph, including:

[0137] A4.2.1: Determine the current state node by the state reference bound to the last locking action corresponding to the action locking window;

[0138] Specifically, the restricted search on the state version graph does not start from arbitrary nodes, but needs to maintain the same reference point as the actual actions that have occurred in the executor. Otherwise, even if the candidate action sequence is reachable on the graph, it may not be consistent with the end state of the already issued actions, causing subsequent actions to be unable to connect in terms of time alignment and state machine context. The state reference corresponding to the last locked action in the action locking window precisely carries the state semantics to which the executed action depends. Using it as the search anchor point can limit the generation of candidate sequences to the same causal link as the executed actions, avoiding the drift of the action structure caused by "restarting from any time node of the current batch".

[0139] A4.2.2: Initialize the search state, where each search state includes the current state node, the current action prefix index pointer, the generated action subsequence, the cumulative sequence difference cost, and the version usage description information. The version usage description information includes the number of version switching and the number of times the version evolution edge is used. When the search state is initialized, the current state node corresponds to the starting state node, the current action prefix index pointer corresponds to the prefix index of the starting state node, the generated action subsequence is an empty set, the cumulative sequence difference cost is set to zero, and the version usage description information is set to zero.

[0140] Specifically, restricted search needs to establish a synchronous relationship between graph path selection and action sequence alignment: where one goes on the graph determines the available state evolution information, and the extent to which an action is generated determines the matching position with historical actions and the acceptable range of subsequent editing and patching. If path expansion is only performed on the graph without carrying action prefix index pointers and difference costs, it is impossible to suppress branches that deviate significantly from the historical action structure in a timely manner during the expansion process; if only actions are carried without version usage information, the abuse of version evolution edges cannot be limited, resulting in frequent switching of candidate sequences at the version level and a lack of interpretability.

[0141] In this embodiment, the starting state node during search state initialization is the current state node determined in A4.2.1. The starting action prefix index is not simply set to the root node of the historical action sequence, but rather located to the prefix node corresponding to the action at the end of the locked window. Specifically, the index pointer is gradually migrated in the action prefix index structure according to the action sub-sequence within the locked window until the node corresponding to the end of the locked window is reached. This node serves as the starting prefix node, and its subsequent branch set corresponds to the feasible continuation of historical actions after the locked window. The generated action sub-sequence is set to an empty set during initialization, but a locked action prefix summary can be stored separately in the search state for concatenating the locked action sub-sequence with the subsequently generated sub-sequence to form a complete candidate action sequence during the candidate output stage. The cumulative sequence difference cost is set to zero, and the number of version switching times and the number of times version evolution edges are used are set to zero. At the same time, a time advancement counter and a termination counter can be initialized to control the candidate action sequence generation length and the search stopping condition, respectively.

[0142] A4.2.3: Starting from the current state node, at least two types of extension branches are generated. The two types of extension branches include: an extension branch along the time edge, which is used to extend the current state node to the state node corresponding to the next timestamp while keeping the current version unchanged; and an extension branch along the version evolution edge, which is used to upgrade the current state node to another version state node at the same timestamp while keeping the timestamp unchanged.

[0143] Specifically, the state version graph is constructed to include both time edges and version evolution edges because smooth state estimation has two different evolutionary mechanisms at the output batch level: time advancement and backtracking updates. Advancing along the time edge corresponds to continuous states within the same version, while switching along the version evolution edge corresponds to the state being updated at the same timestamp. Dividing the expansion branch into time-based expansion and version-based expansion allows the search to clearly distinguish at each step whether to advance into the future or select a more suitable version for the current moment, thus enabling controlled trade-offs between the requirements for action consistency and state correction.

[0144] In this embodiment, the generation of branches extending along time edges is constrained by the version to which the current state node belongs, prioritizing the search for the next timestamp node under the same version as a candidate for extension. The generation of branches extending along version evolution edges is constrained by the current timestamp remaining unchanged, searching for version evolution edges pointing to the updated version under that timestamp.

[0145] A4.2.4: For the branch expansion along the version evolution edge, perform a version evolution benefit evaluation to determine whether the corresponding version evolution edge is allowed to be included in the candidate expansion branch. If included, select and expand the current version evolution edge and time edge.

[0146] Specifically, the introduction of version evolution edges means that the same timestamp can be switched between different versions. If the switching is frequent without selection, the candidate action sequence is prone to structural instability: the action logic changes repeatedly due to different version selections within the same period of time, resulting in a lack of continuous baseline for the state machine transition path.

[0147] In one example, version evolution benefit evaluation is performed to determine whether the corresponding version evolution edge can be included in the candidate extension, including:

[0148] The version rewrite magnitude index is determined based on the state differences between the state nodes at both ends of the version evolution edge.

[0149] The action prefix matching gain predicted when using the state node corresponding to the version evolution edge is calculated based on the cumulative sequence difference cost.

[0150] When the action prefix matching gain is greater than the version rewrite magnitude index, the corresponding version evolution edge is included in the candidate expansion.

[0151] In this embodiment, the version evolution benefit assessment includes a comprehensive judgment of at least two types of quantities: one is the version rewrite magnitude description quantity, which is used to characterize the magnitude of change in state in pose, velocity, or bias dimensions after switching from the current version node to the upgraded version node; this magnitude of change can be judged by a combination of threshold values ​​for displacement difference, pose difference, and velocity difference, and can be supplemented by changes in residual statistics or observation quality labels in the node metadata. The other is the action prefix matching gain description quantity, which is used to characterize the likelihood that the action generator is more likely to generate action symbols that match the historical action prefix index structure in the next few steps if the upgraded version node is used to continue the search. To achieve this gain assessment, a small look-ahead step can be performed without actually extending the time edge: based on the local state characteristics of the upgraded version node (e.g., whether it has entered a certain region, whether it meets a certain trigger condition, the current velocity range and curvature range), one or more sets of expected action types are generated and matched with the set of subsequent action types of the node where the action prefix index pointer is located; the more matching items, the higher the probability that the historical action can be maintained after the version switch. If the action prefix matching gain is insufficient to cover the structural disturbance risk caused by the version rewrite, then the evolution edge of that version will not be included; if the matching gain can support the maintenance of the action structure after the locking window, then it will be included and enter the candidate expansion queue.

[0152] Furthermore, after the version evolution benefit assessment is completed, whether to include a version evolution edge only addresses the question of whether a version can be switched; it does not address which edge should be followed for the current step, given that switching is allowed. Since both time-based and version evolution edges exist as optional expansion paths under the same search state, the search process may still experience unstable fluctuations between these two types of paths without further selection rules, thus affecting the structural coherence of the candidate action sequence. Therefore, in this embodiment, a selection expansion mechanism is further introduced after the version evolution benefit assessment to determine whether the current expansion step should prioritize using a time-based edge or a version evolution edge, given that including version evolution edges is allowed.

[0153] Specifically, if the version evolution benefit assessment fails—that is, when the action prefix matching gain is insufficient to cover the version rewrite magnitude—the version evolution edge is explicitly excluded from the candidate expansion branch. In this case, the search only retains the expansion path along the time edge. Logically, this approach is equivalent to maintaining the current version state unchanged and continuing to advance the state node along the time axis, keeping the candidate action sequence stable at the version level. Future actions are only gradually corrected during subsequent time advancement through action prefix alignment and difference cost control. If the version evolution benefit assessment passes—that is, when the version evolution edge is included in the candidate expansion branch—both the time edge and the version evolution edge become optional expansion paths. In this case, this embodiment does not adopt a simple simultaneous expansion or random selection strategy, but rather selects and expands both types of edges based on a phased priority principle. The core principle is that at the same timestamp, the version evolution edge is used to complete the alignment of state semantics, while the time edge is used to complete the advancement of the state in the time dimension. The two are not functionally equivalent and should therefore play different roles at different stages.

[0154] In this embodiment, when the action prefix index pointer corresponding to the current search state is still near the end of the locked window or still within the high-confidence continuation range of the historical action structure, the version evolution edge is preferentially selected for expansion. The continuity of the action structure is highly required at this stage, and the state correction introduced by the version evolution edge can correct the state semantics at the same timestamp without advancing time, making it more likely to generate action fragments consistent with the historical action prefix when advancing along the time edge. In other words, the version evolution edge at this time plays the role of "aligning semantics first, then advancing time," which helps to establish a stable action continuation baseline in the early stages after the locked window.

[0155] Furthermore, when the search state has progressed several steps along the time edge, and the current action prefix index pointer has migrated into the low-confidence or modifiable range of the historical action structure, the time edge is prioritized for expansion, while the version evolution edge is used as a secondary choice. At this point, the action sequence has entered the adjustable range, and continuing to advance along the time edge can quickly cover the length of subsequent actions, avoiding repeated version switching at the same timestamp that could lead to search stagnation or candidate branch expansion. The version evolution edge is only reactivated as a corrective expansion at this stage when there is a significant action prefix mismatch or a rapid accumulation of difference costs, to realign the state semantics.

[0156] It should be noted that the number of steps can be determined by those skilled in the art through extensive experimentation, and in some optional specific implementations, the number of steps is theoretically not less than 3.

[0157] A4.2.5: Call the action generator to generate an action fragment for the selected extension branch, update the action fragment to the generated action subsequence, and perform prefix migration in the action prefix index structure based on the action fragment to update the current action prefix index pointer;

[0158] Specifically, graph expansion only determines "the path along which the state evolves," but updating the action sequence needs to fall at the "action symbol sequence" level and be aligned with the historical action structure. Therefore, an action generator is needed to map state changes to action fragments and simultaneously advance the matching position in the action prefix index structure. If the action generator and prefix migration are executed separately, it is difficult to backtrack or repair in time when mismatches occur, causing candidate branches to be found to be misaligned only after expanding a long path, resulting in increased search overhead and unstable candidate quality. Therefore, binding action fragment generation, action subsequence update, and prefix migration into the same expansion step ensures that every movement on the graph is immediately reflected as a change in matchability at the action level, facilitating immediate suppression or limited repair of deviating branches.

[0159] In this embodiment, the action generator can output action fragments based on event triggering and state machine transition logic. Each action fragment includes at least an action type identifier and optional parameter labels. The generation of the action type identifier can be based on a combination of the current state node and the expanded state node. For example: when the region relationship changes from "not entered" to "entered," an entry event trigger action is generated; when the speed range crosses a certain threshold and meets the continuity condition, a speed stage transition action is generated; when the state machine stage dependency is met and a preset condition is reached, a stage switching action is generated; when no trigger condition is met, a hold action or a continuous control mode action is generated. After an action fragment is generated, it is appended to the end of the generated action subsequence. Then, starting from the prefix index pointer, prefix migration is performed using a matching rule that prioritizes action type identifiers and secondarily parameter labels. If a branch of the same type exists and the parameter labels match or are tolerable, the migration proceeds to that child node. If only the action type matches but the parameter labels do not match, a "weak parameter match" migration is performed according to the preset parameter tolerance rule, and the parameter replacement cost is added to the accumulated difference cost. If the action type does not match, a restricted alignment operation is triggered. Within a limited number of editing steps, an attempt is made to insert a historical action, delete a generated action, or replace it with the nearest action type. The new prefix index pointer position is determined based on the alignment result with the lowest cost. At the same time, the editing operations that occur during the alignment process are recorded in the search state for subsequent difference cost accumulation and candidate interpretation.

[0160] A4.2.6: Update the state node corresponding to the selected extension branch to the current state node, repeatedly traverse the state version graph, and output the generated action subsequence as candidate action sequence.

[0161] Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments within the scope of the present invention.

Claims

1. A multimodal perception and decision-making integrated system for mobile robots, characterized in that, The mobile robot includes a sensing component, and the system includes: The data preprocessing module acquires the sensing data collected by the sensing component and preprocesses the sensing data. The data processing module is used to initialize the preprocessed sensing data to obtain an initial state for nonlinear optimization solution, and to obtain a smooth state estimation result by performing nonlinear optimization solution on the initial state through optimization algorithm; The decision module is used to receive the smoothed state estimation result, perform action sequence consistency update processing based on the smoothed state estimation result, obtain an updated action sequence for controlling the mobile robot, and send the updated action sequence to the actuator of the mobile robot. The action sequence consistency update process includes constructing a state version graph including time edges and version evolution edges based on the smooth state estimation results, and performing a restricted search in combination with the action prefix index of historical action sequences to select the updated action sequence; The decision-making module includes: The version status differentiation unit is used to determine the first version status and the second version status, wherein the first version status represents the smoothed state estimation result of the timestamp corresponding to the historical output batch, and the second version status represents the smoothed state estimation result of the current output batch. The graph construction unit is used to construct a state version graph based on the first version state and the second version state. The state version graph includes multiple state nodes and directed edges for representing the relationship between state nodes. The state nodes are used to represent different version states under the same timestamp. The directed edges include at least: time edges for connecting adjacent timestamp state nodes under the same version, and version evolution edges for connecting different version state nodes under the same timestamp. An action locking unit is used to acquire the historical action sequence of a mobile robot and the execution progress information corresponding to the historical action sequence, and to determine an action locking window based on the execution progress information, wherein the action locking window is used to indicate an action sub-sequence that has been issued and is irrevocable. The action update unit determines the update action sequence based on the action locking window and sends the update action sequence to the actuator of the mobile robot. The action update unit is configured with evolution logic, which includes: An action prefix index structure is constructed based on the historical action sequence to record the action prefix of each historical action in the historical action sequence and the corresponding matching relationship. Under the constraint that the action subsequence corresponding to the action locking window remains unchanged, a restricted search of the action prefix index structure is performed on the state version graph to generate a candidate action sequence, and the candidate action sequence is output as the updated action sequence.

2. The multimodal perception and decision-making integrated system for mobile robots according to claim 1, characterized in that, The sensing data includes raw laser point cloud observation data, image data, and inertial measurement data; The data preprocessing module includes: An inertial data preprocessing unit is used to perform time synchronization and integration processing on the inertial measurement data to generate inertial constraint information between adjacent time points; An image data preprocessing unit is used to extract and associate features from the image data to generate visual observation information that characterizes the motion relationship between adjacent image frames. The point cloud data preprocessing unit performs usability screening and quality assessment on the raw laser point cloud observation data to eliminate point cloud observations that do not meet the preset observation conditions and generate point cloud observation constraint information.

3. The multimodal perception and decision-making integrated system for mobile robots according to claim 2, characterized in that, The data processing module includes: The data initialization unit is used to determine the initial gravity direction, initial pose, initial velocity, and inertial sensor bias parameters based on inertial constraint information and visual observation information, and to generate the initial state. The data optimization and solution unit is used to perform joint optimization and solution on the initial state to obtain a smooth state estimation result containing multiple timestamp states.

4. The multimodal perception and decision-making integrated system for mobile robots according to claim 3, characterized in that, The data initialization unit is configured with initialization logic. The initialization logic includes: The direction of gravity is determined based on the inertial measurement data, and the initial attitude is determined. Based on the inertial measurement data and the image data, calculate the initial solution for visual inertial odometry, and determine the scale factor, initial velocity, and initial bias parameters of the inertial measurement unit; The initial position in the global coordinate system is determined based on the original observation data of the laser point cloud, and the alignment parameters between the local coordinate system and the global coordinate system are determined. The alignment parameters include at least the yaw angle offset parameter and the coordinate origin anchoring parameter. The global coordinate system is constructed using the geographic coordinate system corresponding to the location of the mobile robot, and the local coordinate system is constructed using a local reference coordinate system with the initial pose of the mobile robot as a reference. The initial state is obtained by combining the initial attitude, the scale factor, the initial velocity, the initial bias parameter, the initial position, and the alignment parameter into a state vector.

5. The multimodal perception and decision-making integrated system for mobile robots according to claim 3, characterized in that, The data optimization solution unit is configured with optimization logic. The optimization logic includes: Multiple state nodes are selected within a preset sliding window, and the initial state is expanded into a window state vector, wherein the window state vector includes the pose, velocity, and inertial bias parameters of each state node. A joint optimization objective function is constructed based on the inertial constraint information, the visual observation information, and the point cloud observation constraint information. The joint optimization objective function includes a pre-integration residual term constructed using the inertial constraint information, a reprojection residual term constructed using the visual observation information, and an original observation residual term constructed using the point cloud observation constraint information. The original observation residuals are subjected to consistency checks and adaptive weight updates based on the window state vector. The consistency check includes gating processing based on residual statistics and preset thresholds. The joint optimization objective function is solved by incremental nonlinear optimization to obtain the smooth state estimation result.

6. The multimodal perception and decision-making integrated system for mobile robots according to claim 5, characterized in that, The step of solving the joint optimization objective function through incremental nonlinear optimization includes: The window state vector is perturbed and parameterized, and the pose, velocity and inertial bias parameters of each state node are represented as quantities to be updated. In the current iteration, the pre-integrated residual term, the reprojection residual term, and the original observation residual term are calculated based on the quantity to be updated, and the Jacobian matrix adapted to the quantity to be updated is obtained for each residual term to construct the linearized normal equation. The Levenberg-Marquardt algorithm is adopted, with the quantity to be updated as the optimization objective. The normal equation is damped and solved by the damping factor of the Levenberg-Marquardt algorithm. The damping factor of the Levenberg-Marquardt algorithm is obtained by: constructing a marginal prior in the form of Schur complement for the state nodes of the sliding window, obtaining the corresponding prior factor, calculating the decrease of the joint optimization objective function based on the prior factor, and calculating the damping factor based on the decrease of the joint optimization objective function. Repeatedly perform residual calculation, linearization, and solution until the preset convergence condition is met. Output the smooth state estimation result based on the optimized quantity to be updated. The preset convergence condition includes that the decrease in the joint optimization objective function is less than a preset change threshold.

7. The multimodal perception and decision-making integrated system for mobile robots according to claim 1, characterized in that, The restricted search of the action prefix index structure on the state version graph includes: The current state node is determined by the state reference bound to the last locking action corresponding to the action locking window; Initialize the search state, which includes the current state node, the current action prefix index pointer, the generated action subsequences, the cumulative sequence difference cost, and the version usage description information, which includes the number of version switching times and the number of version evolution edge usage times. Starting from the current state node, at least two types of extension branches are generated. The two types of extension branches include: an extension branch along the time edge, which is used to extend the current state node to the state node corresponding to the next timestamp while keeping the current version unchanged; and an extension branch along the version evolution edge, which is used to upgrade the current state node to another version state node at the same timestamp while keeping the timestamp unchanged. For the branch extended along the version evolution edge, the version evolution benefit is evaluated to determine whether the corresponding version evolution edge is allowed to be included in the candidate extension branch. If included, the current version evolution edge and time edge are selected for extension. The action generator is invoked to generate an action fragment for the selected extension branch. The action fragment is then updated to the generated action subsequence. Based on the action fragment, a prefix migration is performed in the action prefix index structure to update the current action prefix index pointer. Update the state node corresponding to the selected extension branch to the current state node, repeat the traversal of the state version graph until there are no subsequent state nodes, and output the generated action subsequence as a candidate action sequence.

8. The multimodal perception and decision-making integrated system for mobile robots according to claim 7, characterized in that, The evaluation of version evolution benefits includes: The version rewrite magnitude index is determined based on the state differences between the state nodes at both ends of the version evolution edge. The action prefix matching gain predicted when using the state node corresponding to the version evolution edge is calculated based on the cumulative sequence difference cost. When the action prefix matching gain is greater than the version rewrite magnitude index, the corresponding version evolution edge is included in the candidate expansion branch.

Images