Track extrapolation method, storage medium, electronic device, and program product

Abstract

This application provides a trajectory extrapolation method, storage medium, electronic device, and program product, relating to the field of kinematics technology. The trajectory extrapolation method includes: acquiring inertial measurement data collected by an inertial measurement unit and angle data of each wheel recorded by an encoder; calculating first motion state data of the self-moving device based on the inertial measurement data; calculating second motion state data of the self-moving device based on the angle data of each wheel; and fusing the second motion state data and the first motion state data to obtain third motion state data of the self-moving device. This application eliminates systematic biases introduced by errors in the velocity calculation model (such as wheel diameter calibration errors and encoder scale errors), providing high-quality motion state information for downstream tasks such as precise positioning, navigation control, and path tracking of self-moving devices.

Description

Technical Field

[0001] This application relates to the field of kinematics technology, specifically to a trajectory calculation method, storage medium, electronic device, and program product. Background Technology

[0002] Currently, most common trajectory estimation methods are based on Ackermann kinematic models or differential drive models, which are widely used in traditional two-wheel drive or differential drive vehicles. However, trajectory estimation methods designed for chassis with four wheels, each with steering and driving functions, are still relatively few. Summary of the Invention

[0003] In view of this, embodiments of this application provide a trajectory extrapolation method, a storage medium, an electronic device, and a program product.

[0004] In a first aspect, one embodiment of this application provides a trajectory extrapolation method applied to a self-moving device. The self-moving device includes four wheels, each with steering and driving functions. The self-moving device also includes an inertial measurement unit (IMU) and an encoder. The method includes: acquiring inertial measurement data collected by the IMU and angle data of each wheel recorded by the encoder; calculating first motion state data of the self-moving device based on the inertial measurement data; calculating second motion state data of the self-moving device based on the angle data of each wheel; and fusing the second motion state data and the first motion state data to obtain third motion state data of the self-moving device.

[0005] In conjunction with the first aspect, in some implementations of the first aspect, the second motion state data and the first motion state data are fused to obtain the third motion state data of the self-moving device, including: determining a first residual based on the difference between the second motion state data and the first motion state data; determining a second residual based on the first motion state data; determining the product of the difference between the first residual and the second residual and the state correction coefficient as the state error corresponding to the first motion state data; and correcting the first motion state data based on the state error to obtain the third motion state data of the self-moving device.

[0006] In conjunction with the first aspect, in some implementations of the first aspect, before determining the product of the difference between the first residual and the second residual and the state correction coefficient as the state error corresponding to the first motion state data, the method further includes: determining the state error covariance matrix corresponding to the first motion state data, which represents the degree of uncertainty of the first motion state data; determining the state error covariance matrix corresponding to the second motion state data, which represents the degree of uncertainty of the second motion state data; and determining the state correction coefficient based on the state error covariance matrix corresponding to the first motion state data and the state error covariance matrix corresponding to the second motion state data.

[0007] In conjunction with the first aspect, in some implementations of the first aspect, before acquiring the inertial measurement data collected by the inertial measurement unit and the angle data of each wheel recorded by the encoder, the method further includes: after the self-moving device has been stationary for a target duration, if it is determined that the self-moving device is in a stationary state, then an initial value is set for the motion state of the self-moving device so as to recursively deduce the first motion state data of the self-moving device based on the initial value of the motion state.

[0008] In conjunction with the first aspect, in some implementations of the first aspect, the motion state of the self-moving device includes three-dimensional position, three-dimensional velocity, three-dimensional attitude, three-dimensional gyroscope zero bias, three-dimensional accelerator zero bias, and three-dimensional gravity vector; the second motion state data includes three-dimensional displacement change and three-dimensional rotation change; wherein, the three-dimensional displacement change is used to constrain the three-dimensional position, and the three-dimensional rotation change is used to constrain the three-dimensional attitude.

[0009] In conjunction with the first aspect, in some implementations of the first aspect, the angle data includes rotation angle and / or steering angle. Based on the angle data of each wheel, the second motion state data of the self-moving device is calculated, including: calculating the speed of the self-moving device based on the steering angle and rotational speed of each wheel; calculating the displacement change and rotation change of the self-moving device based on the change in the steering angle and rotation angle of each wheel; and calculating the position and attitude of the self-moving device based on the displacement change and rotation change of the self-moving device.

[0010] In conjunction with the first aspect, in some implementations of the first aspect, the speed of the self-moving device, as well as the displacement and rotational changes of the self-moving device, are calculated using a four-wheel drive kinematic model.

[0011] In conjunction with the first aspect, in some implementations of the first aspect, before acquiring the inertial measurement data collected by the inertial measurement unit and the angle data of each wheel recorded by the encoder, the method further includes: determining at least one fusion moment based on the target fusion frequency; determining multiple time periods based on the at least one fusion moment; performing timestamp alignment processing on the inertial measurement data collected by the inertial measurement unit and the angle data of each wheel recorded by the encoder in each time period, so as to determine the third motion state data of the self-moving device in that time period based on the timestamp aligned inertial measurement data and angle data.

[0012] Secondly, one embodiment of this application provides a trajectory prediction device applied to a self-moving device. The self-moving device includes four wheels, which have steering and driving functions. The self-moving device also includes an inertial measurement unit (IMU) and an encoder. The device includes: an acquisition module for acquiring inertial measurement data collected by the IMU and angle data of each wheel recorded by the encoder; a first calculation module for calculating first motion state data of the self-moving device based on the inertial measurement data; a second calculation module for calculating second motion state data of the self-moving device based on the angle data of each wheel; and a fusion module for fusing the second motion state data and the first motion state data to obtain third motion state data of the self-moving device.

[0013] Thirdly, one embodiment of this application provides a computer-readable storage medium storing a computer program for executing the trajectory extrapolation method described in the first aspect.

[0014] Fourthly, one embodiment of this application provides an electronic device, which includes: a processor; a memory for storing processor-executable instructions; the processor is used to execute the trajectory extrapolation method described in the first aspect.

[0015] Fifthly, one embodiment of this application provides a computer program product including instructions that, when executed on an electronic device, cause the electronic device to implement the trajectory extrapolation method described in the first aspect.

[0016] In this embodiment, firstly, the method calculates the second motion state data using the wheel angle data recorded by the encoder. Compared to the indirect path of calculating speed first and then integrating in traditional methods, this method fundamentally avoids the intermediate step of speed estimation, thereby completely eliminating the systematic deviation introduced by speed calculation model errors (such as wheel diameter calibration errors and encoder scale errors). Secondly, the inertial measurement unit provides high-frequency but error-divergent absolute motion measurement, while the encoder provides low-frequency, high-precision relative motion measurement. Then, the encoder data and the calculation results of the inertial measurement unit are fused to finally achieve high-precision and high-reliability motion state estimation. Finally, the third motion state data output by this method combines the advantages of high-frequency inertial updates and accurate relative displacement measurements. It not only provides smooth, high-frequency attitude estimation but also achieves accurate displacement and velocity calculations, providing high-quality motion state information for downstream tasks such as precise positioning, navigation control, and path tracking of self-moving devices. Attached Figure Description

[0017] The above and other objects, features, and advantages of this application will become more apparent from the more detailed description of the embodiments of this application in conjunction with the accompanying drawings. The drawings are provided to further illustrate the embodiments of this application and form part of the specification. They are used together with the embodiments of this application to explain this application and do not constitute a limitation thereof. In the drawings, the same reference numerals generally represent the same components or steps.

[0018] Figure 1 The diagram shown is a schematic flowchart of a trajectory extrapolation method provided in an embodiment of this application.

[0019] Figure 2 The diagram shown is a schematic diagram of a structure for aligning inertial measurement data and wheel angle data according to an embodiment of this application.

[0020] Figure 3 The diagram shown is a schematic diagram of the trajectory simulation device provided in one embodiment of this application.

[0021] Figure 4 The diagram shown is a structural schematic of an electronic device provided in an embodiment of this application. Detailed Implementation

[0022] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0023] Currently, most common trajectory estimation methods are based on Ackermann kinematic models or differential drive models. These methods have been widely used in traditional two-wheel drive or differential drive vehicles and have a high degree of technological maturity. However, trajectory estimation methods designed for chassis with four wheels, each with steering and driving functions, are still relatively few.

[0024] Specifically, four-wheel drive chassis offer superior maneuverability and environmental adaptability compared to traditional differential drive or Ackerman chassis. For instance, four wheels with steering capabilities effectively reduce the turning radius at low speeds, improving handling in narrow or complex environments; four wheels with drive capabilities significantly enhance the traction and stability of self-propelled vehicles, exhibiting superior off-road performance, especially in unstructured terrain. Therefore, this type of chassis has a clear advantage in high-mobility applications and complex terrain conditions.

[0025] However, the kinematic equations corresponding to this type of chassis are highly complex, requiring the establishment of more accurate mathematical models and the development of efficient data fusion algorithms to achieve effective pose estimation and trajectory calculation.

[0026] In view of this, this application aims to provide a trajectory calculation method for four-wheel drive chassis to fully utilize the flexibility and adaptability of such chassis. Specifically, Figure 1 The diagram shown is a schematic flowchart of a trajectory extrapolation method provided in an embodiment of this application. Exemplarily, this method is applied to a self-moving device, which includes four wheels, each wheel having steering and driving functions, and further includes an inertial measurement unit and an encoder.

[0027] The function of an inertial measurement unit (IMU) is to measure and output the three-axis linear acceleration and three-axis angular velocity data of an automatic mobile device in its coordinate system through its integrated three-axis accelerometer and three-axis gyroscope. Its advantages are that it can achieve autonomous measurement without relying on external reference information and has high short-term accuracy. However, its calculation results will diverge over time due to the inherent bias of the sensor (such as zero bias) and noise, causing the integral error to accumulate.

[0028] The function of an encoder is to measure and record the rotation angle or steering angle of each wheel of a self-moving device. Its characteristics include high accuracy in measuring relative displacement and no error accumulation over time, but its data acquisition frequency is low.

[0029] Furthermore, such as Figure 1 As shown, the method includes the following steps.

[0030] Step S110: Obtain the inertial measurement data collected by the inertial measurement unit and the angle data of each wheel recorded by the encoder.

[0031] Optionally, the triaxial linear acceleration and triaxial angular velocity data of the self-moving device in its coordinate system, measured and output by the inertial measurement unit, are used as inertial measurement data to calculate the first motion state data of the self-moving device. The rotation angle or steering angle of each wheel of the self-moving device recorded by the encoder is used as angular data to derive the second motion state data. The rotation angle refers to the radian of the wheel's rotation around its own axis, reflecting the wheel's linear and angular displacement. The steering angle is the deflection angle of the wheel relative to the longitudinal axis of the self-moving device, affecting the steering characteristics of the self-moving device, such as turning radius and trajectory.

[0032] In one implementation, data streams are received from the inertial measurement unit and encoder in real time or periodically via a wired or wireless data communication interface, and necessary signal preprocessing, such as filtering or calibration, is performed to obtain inertial measurement data and angle data for each wheel.

[0033] Step S120: Calculate the first motion state data of the self-moving device based on the inertial measurement data.

[0034] The first motion state data is a set of estimated data derived from inertial measurement data to describe the motion state of the self-moving device.

[0035] In one implementation, the inertial measurement data is first preprocessed, including but not limited to calibration and filtering / denoising. Then, using the preprocessed three-axis angular velocity data, the attitude change of the self-moving device is solved through integration, thereby obtaining the current attitude angles of the self-moving device relative to the global coordinate system or its initial attitude, namely roll, pitch, and yaw angles. Next, based on the current attitude angles, the three-axis linear acceleration data is rotated to the global coordinate system, and the gravitational acceleration component is subtracted to obtain the pure motion acceleration of the self-moving device in the global coordinate system. Finally, a second time integral is performed on the pure motion acceleration to calculate the linear velocity and position of the self-moving device in the global coordinate system. Ultimately, the obtained first motion state data encompasses attitude, velocity, and position.

[0036] It should be noted that due to inherent errors such as sensor noise and zero bias being amplified during the integration process, the velocity and position components in the first motion state data will accumulate errors over time, causing their accuracy to gradually decrease.

[0037] Step S130: Calculate the second motion state data of the self-moving device based on the angle data of each wheel.

[0038] In one implementation, the angle data of each wheel recorded by the encoder is preprocessed, including but not limited to pulse counting conversion and outlier filtering. Then, based on the steering angle of the self-moving device, the linear velocity and angular velocity of each wheel during the calculation cycle are calculated. Based on the wheel's rotation angle and physical radius, the displacement change of each wheel during the calculation cycle is calculated. This information collectively constitutes the second motion state data.

[0039] It should be noted that the second motion state data obtained by this calculation method is essentially a recursive estimation of relative motion. Its advantage lies in the fact that the error does not accumulate over time, resulting in high accuracy over short timescales. However, its disadvantage is that the encoder's data acquisition frequency is relatively low compared to the inertial measurement unit, limiting its ability to capture motion information. In fast or complex dynamic environments, it is prone to losing instantaneous motion details, affecting real-time performance and accuracy.

[0040] Step S140: The second motion state data and the first motion state data are fused to obtain the third motion state data of the self-moving device.

[0041] Optionally, the second motion state data can be used as an observation to perform optimal estimation and correction on the calculated first motion state data, thereby outputting more accurate and reliable third motion state data.

[0042] For example, firstly, the two types of heterogeneous data are synchronized in time and space to ensure they are under the same timestamp and coordinate system. Then, a Kalman filter is used as the fusion algorithm to estimate and correct the cumulative drift error of the velocity and position components in the first motion state data in real time, while also estimating and compensating for sensor errors of the inertial measurement unit (such as gyroscope bias).

[0043] In addition, the fusion process has the ability to detect sensor anomalies (such as wheel slippage causing distortion of the second motion state data). When a significant inconsistency is detected between the second motion state data and the first motion state data, the weight of the abnormal data can be automatically reduced or it can be removed, thereby ensuring the robustness of the third motion state data output.

[0044] In this embodiment, firstly, the method calculates the second motion state data using the wheel angle data recorded by the encoder. Compared to the indirect path of calculating speed first and then integrating in traditional methods, this method fundamentally avoids the intermediate step of speed estimation, thereby completely eliminating the systematic deviation introduced by speed calculation model errors (such as wheel diameter calibration errors and encoder scale errors). Secondly, the inertial measurement unit provides high-frequency but error-divergent absolute motion measurement, while the encoder provides low-frequency, high-precision relative motion measurement. Then, the encoder data and the calculation results of the inertial measurement unit are fused to finally achieve high-precision and high-reliability motion state estimation. Finally, the third motion state data output by this method combines the advantages of high-frequency inertial updates and accurate relative displacement measurements. It not only provides smooth, high-frequency attitude estimation but also achieves accurate displacement and velocity calculations, providing high-quality motion state information for downstream tasks such as precise positioning, navigation control, and path tracking of self-moving devices.

[0045] To ensure the accuracy and timeliness of multi-source heterogeneous sensor data fusion, this application provides an optional implementation method. This method involves dividing the data into fusion time periods and performing time-aligned preprocessing to provide a reliable data foundation for subsequent state fusion calculations. The specific implementation is as follows: Before acquiring the inertial measurement data collected by the inertial measurement unit and the angle data of each wheel recorded by the encoder, at least one fusion time is determined based on the target fusion frequency; based on the at least one fusion time, multiple time periods are determined; and the inertial measurement data collected by the inertial measurement unit and the angle data of each wheel recorded by the encoder within each time period are time-stamp aligned.

[0046] The target fusion frequency refers to the execution frequency of state fusion calculations. Optionally, this frequency is set based on computing resources and the timeliness of state updates. Further, the fusion moment refers to the specific point in time where state fusion calculations need to be performed, determined according to the target fusion frequency. A time period refers to the time window between two adjacent fusion moments, which is the processing unit for inertial measurement data and wheel angle data within that time period.

[0047] Figure 2 The diagram shown is a schematic representation of a structure for aligning inertial measurement data and wheel angle data according to an embodiment of this application. Figure 2As shown, in one implementation, a fusion time sequence is determined based on a preset target fusion frequency (e.g., 100Hz). Then, consecutive time periods are divided using adjacent fusion times as boundaries. For each time period, all inertial measurement data packets output by the inertial measurement unit and all wheel angle data output by the encoder within that time period are cached. Subsequently, timestamp alignment processing is performed on these two types of data, a data preprocessing process that unifies the inertial measurement data and the encoder angle data to the same standard time point. This aims to eliminate time asynchrony problems caused by independent sensor acquisition, different data output frequencies, or transmission delays.

[0048] For example, a uniform time point is set within each time period. Based on the inertial measurement data of its surrounding times, a set of inertial measurement data aligned to this time point is calculated through interpolation. Similarly, for this time point, based on the encoder angle data of surrounding times, a set of wheel angle data aligned to this time point is calculated. Finally, based on these time-aligned data, steps S120 to S140 in the trajectory extrapolation method are executed to obtain more accurate third motion state data corresponding to that time period.

[0049] In this embodiment, firstly, because the raw data output by the inertial measurement unit and encoder has an extremely high frequency, performing fusion calculations immediately for each new data point would generate a huge computational load, posing a challenge to the processor's computing power and the system's real-time performance. Therefore, in this embodiment, by pre-setting a target fusion frequency and determining the fusion time and time period accordingly, the continuous high-frequency data stream can be divided into discrete processing units, allowing fusion calculations to be performed only once within each time period. This significantly reduces the consumption of computing resources and ensures the efficiency and real-time performance of the trajectory extrapolation method.

[0050] Secondly, since the inertial measurement unit (IMU) and the encoder are independent sensors, their data acquisition and transmission timestamps are not synchronized. Directly using the raw data queue will lead to a mismatch between the first motion state data and the second motion state data, introducing fusion errors. Therefore, in this embodiment, by performing timestamp alignment processing on the inertial measurement data and encoder angle data, the time deviation caused by differences in sensor sampling frequencies, data transmission delays, or clock asynchrony is eliminated. This provides the subsequent fusion algorithm with multi-source data inputs that can match on the time axis, reducing the fusion errors introduced by time asynchrony.

[0051] To ensure the accuracy and reliability of the initial reference for the recursion of the first motion state and to avoid the continuous impact of initial errors on subsequent trajectory calculations, this application provides an optional initialization scheme, the specific implementation of which is as follows. Before acquiring the inertial measurement data collected by the inertial measurement unit and the angle data of each wheel recorded by the encoder, after the self-moving device has been stationary for a certain period of time, if it is determined that the self-moving device is in a stationary state, an initial value is set for the motion state of the self-moving device, so that the first motion state data of the self-moving device can be recursively derived based on the initial value of the motion state.

[0052] Optionally, the target static duration refers to a preset time threshold to ensure that the self-moving device is sufficiently stationary, and its length must be sufficient for the output signal of the inertial measurement unit to stabilize.

[0053] In one implementation, after the self-moving device is powered on or needs to be reinitialized, it is first controlled or waited for the self-moving device to remain completely stationary, and inertial measurement data is continuously collected for a target duration (e.g., 5 seconds). Then, it is determined whether the self-moving device is stationary.

[0054] For example, the variance or standard deviation of the accelerometer and gyroscope readings over the target time period is calculated. If the variance of the accelerometer readings remains stable within a fluctuation range close to gravitational acceleration, and the variance of the gyroscope readings is close to zero, the self-moving device is determined to be stationary. Subsequently, an initial value is set for the motion state. Finally, based on this initial value, the subsequent inertial measurement data is integrated to recursively calculate the first motion state data for each time period.

[0055] In this embodiment, by setting the initial value of the motion state when the self-moving device is stationary, a high-precision and low-error starting reference is provided for recursively calculating the first motion state data based on inertial measurement data. This reduces the initial error introduced in subsequent integration calculations due to inaccurate initial attitude, speed and position settings, and improves the accuracy and reliability of the entire trajectory extrapolation process.

[0056] In some embodiments, the motion state of the self-moving device includes three-dimensional position, three-dimensional velocity, three-dimensional attitude, three-dimensional gyroscope zero bias, three-dimensional accelerator zero bias, and three-dimensional gravity vector; the second motion state data includes three-dimensional displacement change and three-dimensional rotation change; wherein, the three-dimensional displacement change is used to constrain the three-dimensional position, and the three-dimensional rotation change is used to constrain the three-dimensional attitude.

[0057] Specifically, three-dimensional position and three-dimensional velocity describe the translational motion of the self-moving device in space; three-dimensional attitude describes the rotational orientation of the self-moving device; three-dimensional gyroscope zero bias and three-dimensional accelerometer zero bias are online estimates of the inherent sensor errors of the inertial measurement unit, used for real-time compensation during the calculation process to improve accuracy; and the three-dimensional gravity vector is used to characterize the magnitude and direction of gravitational acceleration in the navigation coordinate system, providing a benchmark for the decomposition of acceleration data.

[0058] The second motion state data is calculated from the angle data of each wheel recorded by the encoder. Among them, the three-dimensional displacement change refers to the linear displacement increment of the self-moving device in three-dimensional space within adjacent time periods; the three-dimensional rotation change refers to the rotation angle increment of the self-moving device around its own coordinate axis within adjacent time periods.

[0059] In some embodiments, the second motion state data is used as observation information to constrain and update the motion state. Specifically, the three-dimensional displacement change is used as an observation to constrain and correct the three-dimensional position estimate in the motion state, suppressing its divergence. The three-dimensional rotation change is used as an observation to constrain and correct the three-dimensional attitude estimate in the motion state, optimizing its accuracy. In this way, the accurate relative motion information provided by the encoder is integrated into the global state estimation based on the inertial measurement unit, thereby outputting reliable third motion state data.

[0060] In this embodiment, the second motion state data is a six-dimensional observation that includes three-dimensional displacement changes and three-dimensional rotation changes. Compared to the traditional wheel speed observation method that only provides three-dimensional velocity observations, this six-dimensional observation not only provides constraint information on position changes but also provides direct constraint information on attitude changes, thus introducing more effective observation information into the eighteen-dimensional motion state. Furthermore, the increase in observation dimensions significantly improves the observability of the entire state estimation, enabling more state components to be effectively estimated and corrected, thereby enhancing the stability, overall estimation accuracy, and robustness of the state estimation process.

[0061] Furthermore, when the motion state of the self-moving device includes three-dimensional position, three-dimensional velocity, three-dimensional attitude, three-dimensional gyroscope bias, three-dimensional accelerator bias, and three-dimensional gravity vector, the method for setting initial values ​​for its motion state is as follows.

[0062] First, define the state variables as follows: Their positions are represented from top to bottom (p∈R). 3 ), velocity (v∈R) 3 ), attitude (R∈R) 3 ), gyroscope zero bias (b g ∈R 3 ), plus zero bias (b)a ∈R 3 and the gravity vector (g∈R) 3 ).

[0063] In some embodiments, the average gyroscope and accelerometer readings over the duration the self-moving device remains stationary are calculated and denoted as m. gyr m acc Then, the gyroscope mean is recorded as the initial value b of the gyroscope's zero bias. g =m gyr The magnitude of the gravity vector can be considered a fixed value of 9.81, thus yielding the initial value of the gravity vector. The average value was recalculated after removing gravity from the accelerator readings during the resting period. The initial value of zero bias can be obtained. Since the self-moving device is stationary, p = 0, v = 0, and R = I (identity matrix).

[0064] Traditional trajectory estimation methods typically use wheel speed sensor readings combined with a kinematic model of the self-moving device to calculate its linear and angular velocities. These velocities are then integrated over time to obtain the relative displacement and rotation of the self-moving device within a given time interval. However, this method suffers from two problems:

[0065] First, wheel speed is usually not a directly measured physical quantity, but is calculated from the wheel's angle readings recorded by the encoder using a conversion formula (considering the wheel radius). This calculation process may introduce systematic deviations due to factors such as the encoder's own measurement errors, calibration errors of the wheel's physical radius, or slippage between the wheel and the ground, leading to distorted speed estimates.

[0066] Secondly, even if the velocity estimate is accurate, the inherent discretization approximation characteristic of integral as a numerical calculation method will gradually accumulate calculation errors over time, causing the final displacement and rotation estimates to gradually deviate from the true trajectory, thus limiting the long-term accuracy of the overall trajectory estimation.

[0067] Therefore, this application provides a method for calculating the second motion state data of wheels that differs from traditional methods to avoid the aforementioned problems. Its specific implementation is as follows: Angle data includes rotation angle and / or steering angle. Based on the angle data of each wheel, the second motion state data of the self-moving device is calculated, including: calculating the speed of the self-moving device based on the steering angle and rotational speed of each wheel; calculating the displacement change and rotation change of the self-moving device based on the change in the steering angle and rotational angle of each wheel; and calculating the position and attitude of the self-moving device based on the displacement change and rotation change.

[0068] Specifically, for a four-wheel drive chassis, the coordinates of each wheel in the self-moving device in its own coordinate system are defined as follows:

[0069]

[0070] The wheel speeds of the four wheels can be obtained directly from the encoder and are defined as: [v1 v2 v3 v4].

[0071] The steering angles of the four wheels can be obtained directly from the wheel encoder and are defined as: [δ1δ2δ3δ4].

[0072] Then, using the four-wheel drive kinematic model, the speed, displacement change, and rotation change of the self-moving device are calculated.

[0073] Specifically, based on the four-wheel drive kinematic model, the robot's velocity (including linear velocity and angular velocity) can be obtained as follows:

[0074] Among them, v x v represents the linear velocity in the x-direction. y W represents the linear velocity in the y-direction, and ω represents the angular velocity. i for:

[0075] Furthermore, the rotation angles of the four wheels can also be obtained using an encoder, defined as: [δ ' 1δ ' 2δ ' 3δ ' 4]. Therefore, the change in rotation angle of the four wheels over a time interval Δt can be obtained as: [Δδ] ' 1Δδ ' 2Δδ ' 3Δδ ' 4).

[0076] Based on the four-wheel drive kinematic model, the changes in displacement and rotation of the robot over a time interval Δt can be obtained as follows:

[0077] Where Δx represents how much the self-moving device actually moves in the x-direction during the time interval Δt; Δy represents how much the self-moving device actually moves in the x-direction during the time interval Δt; and Δω represents how many degrees the self-moving device actually rotates during the time interval Δt.

[0078] Next, starting from the initial pose, the attitude is continuously updated using the rotational changes, and the displacement changes are converted to the global coordinate system using the latest attitude before the position is updated, thereby achieving continuous estimation of the entire motion trajectory.

[0079] First, this embodiment utilizes a four-wheel-drive kinematic model to calculate the velocity, displacement, and rotational changes of the self-moving device. This accurately describes the unique kinematic coupling relationship between the four-wheel independent steering and the drive chassis, overcoming the limitations of traditional kinematic models. Second, this embodiment directly calculates the displacement and rotational changes of the self-moving device by analyzing the wheel rotation and steering angles. This method fundamentally avoids the intermediate step of velocity estimation, thus completely eliminating the systematic bias introduced by errors in the velocity calculation model (such as wheel diameter calibration errors and encoder calibration errors). Simultaneously, by skipping the integration process of the velocity values, it significantly reduces the cumulative error caused by numerical integration itself, thereby improving the accuracy and stability of relative trajectory estimation at its source. Furthermore, because this scheme reduces unnecessary intermediate calculation steps, it further improves overall computational efficiency, ensuring real-time processing of high-frequency sensor data.

[0080] To make the deduction of the third motion state more accurate and reliable, this application provides an optional data fusion strategy. This strategy can improve the stability and reliability of the trajectory deduction by establishing a residual compensation mechanism to correct the output of the inertial measurement unit in real time. The specific implementation is as follows: Based on the difference between the second motion state data and the first motion state data, a first residual is determined; based on the first motion state data, a second residual is determined; the product of the difference between the first and second residuals and the state correction coefficient is determined as the state error corresponding to the first motion state data; based on the state error, the first motion state data is corrected to obtain the third motion state data of the self-moving device.

[0081] For example, the first residual is calculated as follows:

[0082] Where, p wheel v wheel R wheel These represent the position, velocity, and attitude determined based on the second motion state data, respectively; p pred v pred R and R represent the position, velocity, and attitude determined based on the first motion state data, respectively.

[0083] For example, the second residual is calculated as h(x) pred ), where x preddenoted by , h represents the first motion state data, and represents the observation equation, used to map the first motion state data to the observation space. Here, the observation space refers to a 9-dimensional space constructed by the encoder using a kinematic model. Each point in this space represents a set of measurements of the position, velocity, and attitude of the self-moving device, calculated directly from the wheel motion at a given moment.

[0084] Furthermore, the state error corresponding to the first motion state data is δx = K(zh(x) pred Where K represents the state correction coefficient. Finally, the third motion state data x = x pred +δx.

[0085] In this embodiment, the first and second residuals are comprehensively processed using state correction coefficients to ultimately calculate the optimal estimated state error. Firstly, since the state error is typically small, its dynamic model exhibits higher linearity during filtering. This effectively reduces the estimation bias caused by insufficient linearization approximation in traditional Kalman filtering under nonlinear systems, thereby improving the stability and accuracy of the third motion state data. Secondly, by correcting the first motion state data using the estimated state error, optimal calibration of the first motion state data is achieved, improving the accuracy and reliability of the final output third motion state data.

[0086] To further improve the accuracy and reliability of state correction, this application optimizes the process of determining the state correction coefficients, as specifically described below. First, the state error covariance matrix corresponding to the first motion state data is determined, representing the degree of uncertainty of the first motion state data. Second, the state error covariance matrix corresponding to the second motion state data is determined, representing the degree of uncertainty of the second motion state data. Third, based on the state error covariance matrices corresponding to the first and second motion state data, the state correction coefficients are determined.

[0087] First, define the state error of the first motion state. Then, the equation of motion for this state error is derived as: δx(t+Δt)=Fδx(t)+w. Here, F is the transition matrix, describing how the state error changes with time.

[0088] For example,

[0089] Where w is noise, following a normal distribution w~N(0,Q), Q=diag(0,3,Cov(y) v ), Cov(y R ),Cov(y g ),Cov(ya ),03)∈R 18·18 Therefore, the state error covariance matrix P corresponding to the first motion state data can be obtained. pred =FPF T +Q.

[0090] It is understood that the state error of the first motion state in this embodiment is derived, while the state error of the first motion state in the previous embodiment is obtained by multiplying the difference between the first residual and the second residual with the state correction coefficient. When correcting the third motion state data, the state error obtained by the product is corrected.

[0091] Furthermore, the state error covariance matrix V corresponding to the second motion state data is determined. Finally, the state correction coefficient K = P pred H T HP pred H T +V) -1 .

[0092] in, Let represent the Jacobian matrix of z relative to the state error.

[0093] In some embodiments, while obtaining the third motion state data, the state error covariance matrix corresponding to the first motion state data is updated to facilitate motion state correction in the next stage. Specifically, the update process is P = (I - KH)P. pred .

[0094] In this embodiment, the state error covariance matrix corresponding to the first motion state data and the state error covariance matrix corresponding to the second motion state data each quantify the degree of uncertainty of the first and second motion state data. Furthermore, the state error covariance matrix is ​​not a fixed value, but a dynamic indicator that reflects the reliability of the current data in real time. Based on these two uncertainty measures, a state correction coefficient is further determined. The calculation of this coefficient follows the optimal estimation principle: when the uncertainty of the first motion state data is relatively small, the state correction coefficient is adjusted to make the final result more reliable to the inertial measurement unit's (IMU) estimate; conversely, when the uncertainty of the second motion state data is smaller, the encoder's observations are more reliable for correction, thereby balancing the contributions of the IMU and encoder in different motion states and environments, and improving the estimation accuracy of the third motion state data. Especially when facing complex scenarios (such as instantaneous slippage or sudden noise changes in the IMU), this solution can automatically reduce the influence of unreliable data sources and maintain the stability and accuracy of state estimation.

[0095] The trajectory extrapolation method embodiments of this application have been described in detail above. The trajectory extrapolation apparatus embodiments of this application are described in detail below. It should be understood that the descriptions of the trajectory extrapolation method embodiments correspond to the descriptions of the trajectory extrapolation apparatus embodiments; therefore, any parts not described in detail can be referred to the preceding method embodiments.

[0096] Figure 3 The diagram shown is a schematic representation of a trajectory prediction device according to an embodiment of this application. Exemplarily, this device is applied to a self-moving device, which includes four wheels with steering and driving functions. The self-moving device also includes an inertial measurement unit and an encoder. Figure 3 As shown, the trajectory simulation device 30 provided in this application embodiment includes:

[0097] The acquisition module 310 is used to acquire the inertial measurement data collected by the inertial measurement unit and the angle data of each wheel recorded by the encoder;

[0098] The first calculation module 320 is used to calculate the first motion state data of the self-moving device based on inertial measurement data;

[0099] The second calculation module 330 is used to calculate the second motion state data of the self-moving device based on the angle data of each wheel;

[0100] The fusion module 340 is used to fuse the second motion state data and the first motion state data to obtain the third motion state data of the self-moving device.

[0101] In one embodiment of this application, the fusion module 340 is further configured to: determine a first residual based on the difference between the second motion state data and the first motion state data; determine a second residual based on the first motion state data; determine the product of the difference between the first residual and the second residual and the state correction coefficient as the state error corresponding to the first motion state data; and correct the first motion state data based on the state error to obtain the third motion state data of the self-moving device.

[0102] In one embodiment of this application, the fusion module 340 is further configured to: determine the state error covariance matrix corresponding to the first motion state data, the state error covariance matrix representing the degree of uncertainty of the first motion state data; determine the state error covariance matrix corresponding to the second motion state data, the state error covariance matrix representing the degree of uncertainty of the second motion state data; and determine the state correction coefficient based on the state error covariance matrix corresponding to the first motion state data and the state error covariance matrix corresponding to the second motion state data.

[0103] In one embodiment of this application, the acquisition module 310 is further configured to, after the self-moving device has been stationary for a target duration, if it is determined that the self-moving device is in a stationary state, set an initial value for the motion state of the self-moving device so as to recursively deduce the first motion state data of the self-moving device based on the initial value of the motion state.

[0104] In one embodiment of this application, the motion state of the self-moving device includes three-dimensional position, three-dimensional velocity, three-dimensional attitude, three-dimensional gyroscope zero bias, three-dimensional accelerator zero bias, and three-dimensional gravity vector; the second motion state data includes three-dimensional displacement change and three-dimensional rotation change; wherein, the three-dimensional displacement change is used to constrain the three-dimensional position, and the three-dimensional rotation change is used to constrain the three-dimensional attitude.

[0105] In one embodiment of this application, the angle data includes rotation angle and / or steering angle. The second calculation module 330 is further configured to: calculate the speed of the self-moving device based on the steering angle of each wheel and the rotation speed of each wheel; calculate the displacement change and rotation change of the self-moving device based on the change in the steering angle and rotation angle of each wheel; and calculate the position and attitude of the self-moving device based on the displacement change and rotation change of the self-moving device.

[0106] In one embodiment of this application, the speed of the self-moving device, as well as the displacement and rotational changes of the self-moving device, are calculated using a four-wheel drive kinematic model.

[0107] In one embodiment of this application, the acquisition module 310 is further configured to: determine at least one fusion time based on the target fusion frequency; determine multiple time periods based on the at least one fusion time; and perform timestamp alignment processing on the inertial measurement data collected by the inertial measurement unit and the angle data of each wheel recorded by the encoder in each time period, so as to determine the third motion state data of the self-moving device in that time period based on the inertial measurement data and angle data after timestamp alignment processing.

[0108] Below, for reference Figure 4 This describes an electronic device according to embodiments of the present application. Figure 4 The diagram shown is a structural schematic of an electronic device provided in an exemplary embodiment of this application.

[0109] like Figure 4 As shown, the electronic device 40 includes one or more processors 401 and memory 402.

[0110] The processor 401 may be a central processing unit (CPU) or other form of processing unit with data processing capabilities and / or instruction execution capabilities, and may control other components in the electronic device 40 to perform desired functions.

[0111] The memory 402 may include one or more computer program products, which may include various forms of computer-readable storage media, such as volatile memory and / or non-volatile memory. The volatile memory may include, for example, random access memory (RAM) and / or cache memory. The non-volatile memory may include, for example, read-only memory (ROM), hard disk, flash memory, etc. One or more computer program instructions may be stored on the computer-readable storage medium, and the processor 401 may execute the program instructions to implement the trajectory calculation methods of the various embodiments of this application described above and / or other desired functions.

[0112] In one example, the electronic device 40 may also include an input device 403 and an output device 404, which are interconnected via a bus system and / or other forms of connection mechanism (not shown).

[0113] The input device 403 may include, for example, a keyboard, a mouse, etc.

[0114] The output device 404 can output various information to the outside. The output device 404 may include, for example, a display, a speaker, a printer, and a communication network and its connected remote output devices, etc.

[0115] Of course, for the sake of simplicity, Figure 4 Only some of the components of the electronic device 40 relevant to this application are shown in this illustration; components such as buses, input / output interfaces, etc., are omitted. In addition, the electronic device 40 may include any other suitable components depending on the specific application.

[0116] In addition to the methods and devices described above, embodiments of this application may also be computer program products, which include computer program instructions that, when executed by a processor, cause the processor to perform the steps in the trajectory extrapolation methods according to various embodiments of this application described above.

[0117] The computer program product can be written in any combination of one or more programming languages ​​to perform the operations of the embodiments of this application. The programming languages ​​include object-oriented programming languages ​​such as Java and C++, as well as conventional procedural programming languages ​​such as C or similar languages. The program code can be executed entirely on the user's computing device, partially on the user's computing device, as a standalone software package, partially on the user's computing device and partially on a remote computing device, or entirely on a remote computing device or server.

[0118] Furthermore, embodiments of this application may also be computer-readable storage media storing computer program instructions thereon, which, when executed by a processor, cause the processor to perform the steps in the trajectory extrapolation methods according to various embodiments of this application described above.

[0119] The computer-readable storage medium may be any combination of one or more readable media. A readable medium may be a readable signal medium or a readable storage medium. A readable storage medium may, for example, include, but is not limited to, electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatuses, or devices, or any combination thereof. More specific examples of readable storage media (a non-exhaustive list) include: electrical connections having one or more wires, portable disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fibers, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof.

[0120] The basic principles of this application have been described above with reference to specific embodiments. However, it should be noted that the advantages, benefits, and effects mentioned in this application are merely examples and not limitations, and should not be considered as essential features of each embodiment of this application. Furthermore, the specific details disclosed above are for illustrative and facilitative purposes only, and are not limitations. These details do not limit the application to the necessity of employing the aforementioned specific details for implementation.

[0121] The block diagrams of devices, apparatuses, devices, and systems involved in this application are merely illustrative examples and are not intended to require or imply that they must be connected, arranged, or configured in the manner shown in the block diagrams. As those skilled in the art will recognize, these devices, apparatuses, devices, and systems can be connected, arranged, and configured in any manner. Words such as “comprising,” “including,” “having,” etc., are open-ended terms meaning “including but not limited to,” and are used interchangeably with them. The terms “or” and “and” as used herein refer to the terms “and / or,” and are used interchangeably with them unless the context clearly indicates otherwise. The term “such as” as used herein refers to the phrase “such as but not limited to,” and is used interchangeably with it.

[0122] It should also be noted that in the apparatus, equipment, and methods of this application, the components or steps can be disassembled and / or recombined. These disassemblies and / or recombinations should be considered as equivalent solutions of this application.

[0123] The above description of the disclosed aspects is provided to enable any person skilled in the art to make or use this application. Various modifications to these aspects will be readily apparent to those skilled in the art, and the general principles defined herein can be applied to other aspects without departing from the scope of this application. Therefore, this application is not intended to be limited to the aspects shown herein, but rather to be accorded the widest scope consistent with the principles and novel features disclosed herein.

[0124] The above description has been given for purposes of illustration and description. Furthermore, this description is not intended to limit the embodiments of this application to the forms disclosed herein. Although numerous exemplary aspects and embodiments have been discussed above, those skilled in the art will recognize certain variations, modifications, alterations, additions, and sub-combinations thereof.

Claims

1. A method for trajectory extrapolation, characterized in that, An application to a self-moving device, the self-moving device comprising four wheels having steering and driving functions, and the self-moving device further comprising an inertial measurement unit and an encoder, the method comprising: Acquire the inertial measurement data collected by the inertial measurement unit and the angle data of each wheel recorded by the encoder; Based on the inertial measurement data, the first motion state data of the self-moving device is calculated; Based on the angle data of each wheel, the second motion state data of the self-moving device is calculated; The second motion state data and the first motion state data are fused together to obtain the third motion state data of the self-moving device; The angle data includes rotation angle and / or steering angle. Calculating the second motion state data of the self-moving device based on the angle data of each wheel includes: calculating the speed of the self-moving device based on the steering angle and rotational speed of each wheel; calculating the displacement change and rotation change of the self-moving device based on the change in the steering angle and rotation angle of each wheel; and calculating the position and attitude of the self-moving device based on the displacement change and rotation change.

2. The trajectory extrapolation method according to claim 1, characterized in that, The step of fusing the second motion state data and the first motion state data to obtain the third motion state data of the self-moving device includes: Based on the difference between the second motion state data and the first motion state data, the first residual is determined; Based on the first motion state data, determine the second residual; The product of the difference between the first residual and the second residual and the state correction coefficient is determined as the state error corresponding to the first motion state data. Based on the state error, the first motion state data is corrected to obtain the third motion state data of the self-moving device.

3. The trajectory extrapolation method according to claim 2, characterized in that, Before determining the product of the difference between the first residual and the second residual and the state correction coefficient as the state error corresponding to the first motion state data, the method further includes: Determine the state error covariance matrix corresponding to the first motion state data, wherein the state error covariance matrix represents the degree of uncertainty of the first motion state data; Determine the state error covariance matrix corresponding to the second motion state data, wherein the state error covariance matrix represents the degree of uncertainty of the second motion state data; The state correction coefficients are determined based on the state error covariance matrix corresponding to the first motion state data and the state error covariance matrix corresponding to the second motion state data.

4. The trajectory extrapolation method according to any one of claims 1 to 3, characterized in that, Before acquiring the inertial measurement data collected by the inertial measurement unit and the angle data of each wheel recorded by the encoder, the method further includes: After the self-moving device has been stationary for a target duration, if it is determined that the self-moving device is in a stationary state, an initial value is set for the motion state of the self-moving device so that the first motion state data of the self-moving device can be recursively derived based on the initial value of the motion state.

5. The trajectory extrapolation method according to claim 4, characterized in that, The motion state of the self-moving device includes three-dimensional position, three-dimensional velocity, three-dimensional attitude, three-dimensional gyroscope zero bias, three-dimensional accelerator zero bias, and three-dimensional gravity vector; The second motion state data includes three-dimensional displacement change and three-dimensional rotation change; wherein, the three-dimensional displacement change is used to constrain the three-dimensional position, and the three-dimensional rotation change is used to constrain the three-dimensional attitude.

6. The trajectory extrapolation method according to claim 1, characterized in that, The speed of the self-moving device, as well as the displacement and rotational changes of the self-moving device, are calculated using a four-wheel drive kinematic model.

7. The trajectory extrapolation method according to any one of claims 1 to 3, characterized in that, Before acquiring the inertial measurement data collected by the inertial measurement unit and the angle data of each wheel recorded by the encoder, the method further includes: Determine at least one fusion moment based on the target fusion frequency; Based on the at least one fusion moment, multiple time periods are determined; The inertial measurement data collected by the inertial measurement unit and the angle data of each wheel recorded by the encoder in each time period are timestamped and aligned so as to determine the third motion state data of the self-moving device in that time period based on the inertial measurement data and angle data after the timestamping and alignment process.

8. A computer-readable storage medium, characterized in that, The storage medium stores a computer program for executing the trajectory extrapolation method according to any one of claims 1 to 7.

9. An electronic device, characterized in that, include: processor; Memory used to store the processor's executable instructions; The processor is used to execute the trajectory extrapolation method according to any one of claims 1 to 7.

10. A computer program product, characterized in that, The computer program product includes instructions that, when executed on an electronic device, cause the electronic device to implement the trajectory extrapolation method according to any one of claims 1 to 7.

Images