Pose estimation optimization method and device, positioning equipment, program product and medium

Abstract

The invention provides a pose estimation optimization method and device, positioning equipment, a program product and a medium, and relates to the technical field of positioning. The method comprises the following steps: acquiring observation information corresponding to a target object, wherein the observation information comprises inertial measurement unit (IMU) data, visual data and ultra wide band (UWB) data; based on a sliding window optimization mode, according to the observation information, constructing at least one piece of constraint information corresponding to the pose estimation task; and according to the at least one piece of constraint information, optimizing a model corresponding to the pose estimation task until the model converges. According to the scheme, the problems that the pose estimation precision of an existing positioning technology is poor and the existing positioning technology needs to depend on anchor point prior position information are solved.

Description

Technical Field

[0001] The present invention relates to the field of positioning technology, and in particular to a posture estimation optimization method, device, positioning equipment, program product and medium. Background Art

[0002] Simultaneous Localization and Mapping (SLAM) positioning technology is an interdisciplinary field that includes computer vision, numerical analysis and optimization, and sensor fusion. This technology aims to enable intelligent agents to accurately locate and build environmental maps in unknown environments based on observation data from visual-inertial and other sensors, combined with related technologies such as computer vision and optimization, thereby providing technical support for tasks such as autonomous driving, exploration, and rescue.

[0003] However, existing positioning technologies have problems such as poor pose estimation accuracy and reliance on prior position information of anchor points (which requires manual measurement or online calibration). Summary of the invention

[0004] The purpose of the present invention is to provide a posture estimation optimization method, device, positioning equipment, program product and medium, which solve the problems of poor posture estimation accuracy and reliance on anchor point prior position information in existing positioning technologies.

[0005] To achieve the above object, an embodiment of the present invention provides a posture estimation optimization method, comprising:

[0006] Acquire observation information corresponding to the target object, the observation information including: Inertial Measurement Unit (IMU) data, visual data and Ultra-Wideband (UWB) data;

[0007] Based on the sliding window optimization method, construct at least one constraint information corresponding to the pose estimation task according to the observation information;

[0008] According to the at least one constraint information, the model corresponding to the pose estimation task is optimized until the model converges.

[0009] Optionally, the at least one constraint information corresponding to the pose estimation task is constructed based on the observation information in a sliding window optimization manner, including:

[0010] Performing data alignment on the IMU data, the visual data, and the UWB data to obtain target observation information;

[0011] The at least one constraint information is constructed according to the target observation information.

[0012] Optionally, the performing data alignment on the IMU data, the visual data, and the UWB data to obtain target observation information includes at least one of the following:

[0013] Convert the visual data and the UWB data to an IMU coordinate system;

[0014] The IMU propagation posture is determined according to the IMU data, and the time difference between the UWB data and the visual data is compensated according to the IMU propagation posture.

[0015] Optionally, optimizing the model corresponding to the pose estimation task according to the at least one constraint information until the model converges includes:

[0016] Based on a nonlinear least squares optimization method, constructing a loss function of the model according to the at least one constraint information;

[0017] Based on the observation information and the loss function, the model is optimized until the model converges.

[0018] Optionally, the constraint information includes UWB inter-frame constraint information, and the loss function includes a target error term defined according to the UWB inter-frame constraint information;

[0019] The optimizing the model based on the observation information and the loss function until the model converges includes:

[0020] Simplifying the target error term in the loss function to obtain a target loss function;

[0021] Based on the observation information and the target loss function, the model is optimized until the model converges.

[0022] Optionally, simplifying the target error term in the loss function to obtain a target loss function includes:

[0023] Based on the Mahalanobis distance, the target error term is normalized to obtain the target function corresponding to the target error term;

[0024] For the covariance matrix in the objective function, determining the covariance of the UWB data based on geometric approximation;

[0025] Determining a simplified target error term according to the covariance of the UWB data;

[0026] The target loss function is obtained according to the simplified target error term.

[0027] To achieve the above object, an embodiment of the present invention provides a posture estimation optimization device, comprising:

[0028] A data acquisition module is used to acquire observation information corresponding to the target object, wherein the observation information includes: inertial measurement unit IMU data, visual data and ultra-wideband UWB data;

[0029] A constraint construction module, used to construct at least one constraint information corresponding to the pose estimation task according to the observation information based on a sliding window optimization method;

[0030] A model optimization module is used to optimize the model corresponding to the pose estimation task according to the at least one constraint information until the model converges.

[0031] Optionally, the constraint building module includes:

[0032] A data alignment submodule, used to align the IMU data, the visual data and the UWB data to obtain target observation information;

[0033] The constraint construction submodule is used to construct the at least one constraint information according to the target observation information.

[0034] Optionally, the data alignment submodule includes:

[0035] A data conversion unit, used for converting the visual data and the UWB data into an IMU coordinate system;

[0036] A time compensation unit is used to determine an IMU propagation posture according to the IMU data, and to compensate for a time difference between the UWB data and the visual data according to the IMU propagation posture.

[0037] Optionally, the model optimization module includes:

[0038] A function construction submodule, used for constructing a loss function of the model according to the at least one constraint information based on a nonlinear least squares optimization method;

[0039] The model optimization submodule is used to optimize the model based on the observation information and the loss function until the model converges.

[0040] Optionally, the constraint information includes UWB inter-frame constraint information, the loss function includes a target error term defined according to the UWB inter-frame constraint information; and the model optimization submodule includes:

[0041] A simplification processing unit, used to simplify the target error term in the loss function to obtain a target loss function;

[0042] A model optimization unit is used to optimize the model based on the observation information and the target loss function until the model converges.

[0043] Optionally, the simplified processing unit includes:

[0044] A standardization subunit, used for standardizing the target error term based on the Mahalanobis distance to obtain an objective function corresponding to the target error term;

[0045] A first processing subunit, configured to determine the covariance of the UWB data based on geometric approximation with respect to the covariance matrix in the objective function;

[0046] A second processing subunit, configured to determine a simplified target error term according to the covariance of the UWB data;

[0047] The third processing subunit is used to obtain the target loss function according to the simplified target error term.

[0048] To achieve the above object, an embodiment of the present invention provides a positioning device, including a processor and a transceiver, wherein the processor is used to:

[0049] Acquire observation information corresponding to the target object, the observation information including: inertial measurement unit IMU data, visual data and ultra-wideband UWB data;

[0050] Based on the sliding window optimization method, construct at least one constraint information corresponding to the pose estimation task according to the observation information;

[0051] According to the at least one constraint information, the model corresponding to the pose estimation task is optimized until the model converges.

[0052] Optionally, when constructing at least one constraint information corresponding to the pose estimation task based on the observation information in the sliding window optimization mode, it is specifically used to:

[0053] Performing data alignment on the IMU data, the visual data, and the UWB data to obtain target observation information;

[0054] The at least one constraint information is constructed according to the target observation information.

[0055] Optionally, when the processor performs data alignment on the IMU data, the visual data, and the UWB data to obtain target observation information, the processor is specifically configured to perform at least one of the following:

[0056] Convert the visual data and the UWB data to an IMU coordinate system;

[0057] The IMU propagation posture is determined according to the IMU data, and the time difference between the UWB data and the visual data is compensated according to the IMU propagation posture.

[0058] Optionally, the processor optimizes the model corresponding to the pose estimation task according to the at least one constraint information until the model converges, specifically for:

[0059] Based on a nonlinear least squares optimization method, constructing a loss function of the model according to the at least one constraint information;

[0060] Based on the observation information and the loss function, the model is optimized until the model converges.

[0061] Optionally, the constraint information includes UWB inter-frame constraint information, and the loss function includes a target error term defined according to the UWB inter-frame constraint information;

[0062] The processor optimizes the model based on the observation information and the loss function until the model converges, specifically for:

[0063] Simplifying the target error term in the loss function to obtain a target loss function;

[0064] Based on the observation information and the target loss function, the model is optimized until the model converges.

[0065] Optionally, when simplifying the target error term in the loss function to obtain a target loss function, the processor is specifically configured to:

[0066] Based on the Mahalanobis distance, the target error term is normalized to obtain the target function corresponding to the target error term;

[0067] For the covariance matrix in the objective function, determining the covariance of the UWB data based on geometric approximation;

[0068] Determining a simplified target error term according to the covariance of the UWB data;

[0069] The target loss function is obtained according to the simplified target error term.

[0070] To achieve the above-mentioned purpose, an embodiment of the present invention provides a positioning device, including a transceiver, a processor, a memory, and a program or instruction stored in the memory and executable on the processor; when the processor executes the program or instruction, the above-mentioned posture estimation optimization method is implemented.

[0071] In order to achieve the above-mentioned purpose, a computer program product is provided, comprising computer instructions, which, when executed by a processor, implement the steps of the above-mentioned posture estimation optimization method.

[0072] To achieve the above objectives, an embodiment of the present invention provides a readable storage medium having a program or instruction stored thereon, which, when executed by a processor, implements the steps in the above-mentioned posture estimation optimization method.

[0073] The beneficial effects of the above technical solution of the present invention are as follows:

[0074] The method of the embodiment of the present invention, after obtaining the observation information corresponding to the target object, constructs at least one constraint information corresponding to the pose estimation task based on the observation information based on the sliding window optimization method, wherein the observation information includes: IMU data, visual data and UWB data, and then optimizes the model corresponding to the pose estimation task based on the at least one constraint information until the model converges. In this way, based on the sliding window optimization method, by adding UWB data as constraint information, the estimation accuracy of the model for the pose of the target object can be improved without relying on prior information. BRIEF DESCRIPTION OF THE DRAWINGS

[0075] Figure 1 Flow chart of a method for optimizing posture estimation according to an embodiment of the present invention;

[0076] Figure 2 A schematic diagram of data alignment according to an embodiment of the present invention;

[0077] Figure 3-1 A schematic diagram of random point simulation results under an angle combination of an embodiment of the present invention;

[0078] Figure 3-2 A schematic diagram of random point simulation results under another angle combination according to an embodiment of the present invention;

[0079] Figure 3-3 A schematic diagram of random point simulation results under another angle combination according to an embodiment of the present invention;

[0080] Figure 3-4 This is a schematic diagram of random point simulation results under another angle combination of an embodiment of the present invention;

[0081] Figure 4 Schematic diagram of the structure of the posture estimation optimization system according to an embodiment of the present invention;

[0082] Figure 5 1 is a structural diagram of a posture estimation and optimization device according to an embodiment of the present invention;

[0083] Figure 6 is a structural diagram of a positioning device according to an embodiment of the present invention;

[0084] Figure 7 It is a structural diagram of a positioning device according to another embodiment of the present invention. DETAILED DESCRIPTION

[0085] In order to make the technical problems, technical solutions and advantages to be solved by the present invention more clear, a detailed description will be given below with reference to the accompanying drawings and specific embodiments.

[0086] It should be understood that the references to "one embodiment" or "an embodiment" throughout the specification mean that the specific features, structures, or characteristics associated with the embodiment are included in at least one embodiment of the present invention. Therefore, the references to "in one embodiment" or "in an embodiment" appearing throughout the specification do not necessarily refer to the same embodiment. In addition, these specific features, structures, or characteristics may be combined in any suitable manner in one or more embodiments.

[0087] In various embodiments of the present invention, it should be understood that the size of the serial numbers of the following processes does not mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present invention.

[0088] Additionally, the terms "system" and "network" are often used interchangeably herein.

[0089] In the embodiments provided in the present application, it should be understood that "B corresponding to A" means that B is associated with A, and B can be determined according to A. However, it should also be understood that determining B according to A does not mean determining B only according to A, and B can also be determined according to A and / or other information.

[0090] Related technology introduction:

[0091] Simultaneous localization and mapping technologies are designed to enable intelligent agents to obtain unknown information about themselves in unfamiliar environments and model the environment in order to better perform downstream tasks such as autonomous driving, three-dimensional reconstruction, and environmental exploration. These technologies are widely used in various scenarios in life and industrial production.

[0092] Simultaneous localization and mapping SLAM technology consists of two parts: real-time localization and mapping. Among them, localization technology refers to the intelligent body obtaining its own position and posture in an unfamiliar environment. In automotive applications, localization includes two displacement degrees of freedom and one rotational degree of freedom; in exploration tasks in drones or some complex environments, localization includes three displacement degrees of freedom and three rotational degrees of freedom. Depending on the task and sensor type, the coordinate system where the positioning result is located is also different. For example, in autonomous driving applications, the coordinate system of the car and the navigation map are unified, which is a global coordinate system; while in automatic parking applications, only a local coordinate system is required. Mapping technology refers to the intelligent body building a map of the surrounding environment during movement. Common map forms include raster maps, point cloud maps, and topological maps. These types of maps have their own advantages and disadvantages. In short, raster maps are simple and direct, easy to plan paths, but require a large storage space; point cloud maps are more accurate, but the computational complexity is higher; topological maps only save key environmental information and lack detailed geometric information.

[0093] At present, with the rapid development of various industries such as robots, automobiles, smart homes, virtual reality (VR), and augmented reality (AR), positioning technology is facing higher requirements. In order to better meet and achieve these requirements, various new sensors and different sensor fusion methods are constantly being proposed.

[0094] The visual-inertial scheme uses the visual perception of the environment to detect feature points in the image, and calculates the transformation between two frames of images through epipolar geometry, combined with the pre-integration of the inertial device (IMU) to achieve high-precision pose estimation. Commonly used visual SLAM multi-sensor fusion schemes include: filter-based methods and optimization-based methods. Among them, the filter-based method only uses the information of the previous moment and the current moment to estimate the current state, so it has a faster running speed, but when the accelerometer and gyroscope have deviations or drifts, it will cause the deviation of the pose estimation to grow exponentially, resulting in a significant decrease in the accuracy of the pose estimation; the optimization-based method is usually based on a sliding window, using sensor observations and state information over a period of time in the past, so it has a higher positioning accuracy, but a slower speed. Both types of visual SLAM schemes have drift when running for a long time, and in visually restricted environments such as insufficient lighting and single texture, the SLAM positioning system may be difficult to work normally.

[0095] To solve these problems, the mainstream method is to eliminate long-term drift by introducing some global sensors such as the Global Navigation Satellite System (GNSS). However, in a GNSS-denied environment, using high-precision wireless measurement sensors such as UWB to constrain the drift of IMU and vision to achieve high-precision pose estimation is a more effective solution.

[0096] In the currently more common SLAM multi-sensor fusion solutions based on UWB sensors, usually only the ranging capability of UWB sensors is used, combined with known anchor point positions to build constraints to reduce drift. These solutions use anchor point positions and current position information to build constraints. Filter-based and optimization-based methods use similar methods to constrain the position part of the positioning output, and cannot constrain rotation.

[0097] In addition, the current UWB fusion solution includes two stages: anchor point initialization and normal operation. In the anchor point initialization stage, the anchor point position is obtained manually or automatically. The method of automatically obtaining the anchor point position is one of the main research contents of the current UWB fusion solution. The solutions for obtaining the anchor point position in the existing technology mainly include:

[0098] Solution 1: First, initialize the anchor point, and then use the collected UWB data and anchor point position to build constraints. In the anchor point initialization stage, the system first uses the acceleration information output by the IMU to determine whether it has enough movement; if so, the system saves the pose and UWB measurement data output at the current moment. After collecting enough data, the graph optimization method is used to estimate the position coordinates of the anchor point to complete the anchor point initialization. However, since this solution requires collecting enough pose-UWB measurement data at the beginning of the system operation, the pose output at the beginning of the system operation without UWB participation is not accurate enough, which can easily lead to the failure of anchor point initialization, and then make it difficult for the entire system to operate normally.

[0099] Solution 2: Use the target detection method to determine the position of the UWB anchor point in the image. By collecting several frames of images containing the anchor point position, the system can use epipolar geometry to calculate the position of the anchor point in the image coordinate system, and then complete the anchor point initialization. Since the camera may not be able to capture the anchor point when the system is running, this solution sets the number of anchor points to a form that can be dynamically adjusted according to the initialization results, which to a certain extent avoids the problem of the entire system being difficult to run due to the failure of anchor point initialization. However, this solution requires the system to run the anchor point in the camera screen, which is not flexible enough. In some scenarios, the system may not be able to observe the anchor point, which makes it difficult to integrate the information brought by the UWB sensor into the system. In addition, the computational complexity of the target detection model is generally large, and it is difficult to deploy on low-computing systems that need to run in real time.

[0100] like Figure 1 As shown, a posture estimation optimization method according to an embodiment of the present invention includes:

[0101] Step 101, obtaining observation information corresponding to the target object, wherein the observation information includes: inertial measurement unit IMU data, visual data and ultra-wideband UWB data.

[0102] Here, the observation information can be collected from sensors or other data sources. Among them, IMU data refers to the data collected by the inertial measurement unit, mainly including acceleration, angular velocity, temperature and other data; visual data is the information collected by visual sensors (such as cameras); UWB data refers to data collected using ultra-wideband technology (such as data generated or processed by UWB sensors), mainly including position data, velocity data, angle data (such as pitch angle and azimuth angle) and distance information. Here, a UWB sensor that can measure the distance and angle of the anchor point can be used to collect UWB data.

[0103] Step 102: construct at least one constraint information corresponding to the pose estimation task based on the observation information based on the sliding window optimization method.

[0104] In this step, the observation information (such as the current position data, rotation and UWB distance, angle measurement, etc. of the target object) can be used to construct constraint information to realize the constraint of its own rotation information. In this way, the UWB distance and angle measurement of vision, inertia and any number of anchor points are integrated in a tightly coupled manner, without relying on the accurate position information of the prior anchor points, and without the need to perform anchor point estimation during the operation of the entire system. The positioning system can be constrained in all six degrees of freedom (including three displacement degrees of freedom and three rotation degrees of freedom), and a more accurate UWB distance measurement-angle measurement positioning can be obtained, which solves the problem of determining the anchor point position in the existing UWB-SLAM solution.

[0105] Step 103: Optimize the model corresponding to the pose estimation task according to the at least one constraint information until the model converges.

[0106] In this embodiment, after obtaining the observation information corresponding to the target object, at least one constraint information corresponding to the pose estimation task is constructed based on the observation information based on the sliding window optimization method, wherein the observation information includes: IMU data, visual data and UWB data, and then, based on the at least one constraint information, the model corresponding to the pose estimation task is optimized until the model converges. In this way, based on the sliding window optimization method, by adding UWB data as constraint information, the estimation accuracy of the model for the pose of the target object can be improved without relying on prior information.

[0107] In some embodiments, step 102, constructing at least one constraint information corresponding to the pose estimation task based on the observation information in a sliding window optimization manner, includes:

[0108] Step 1021, align the IMU data, the visual data and the UWB data to obtain target observation information.

[0109] It should be noted that since the IMU data, visual data and UWB data come from different data sources, more effective analysis and integration can be performed by aligning these data in step 1021. Among them, the UWB data includes distance, pitch angle and azimuth angle, etc.

[0110] Step 1022: construct the at least one constraint information according to the target observation information.

[0111] In a specific optional embodiment, step 1021, aligning the IMU data, the visual data, and the UWB data to obtain target observation information, includes at least one of the following:

[0112] (1) Convert the visual data and the UWB data into the IMU coordinate system.

[0113] Here, a sliding window optimization of length N can be used to couple all measurements. The complete window state variable is used express, Includes: n IMU state variables (i.e. IMU data: ), visual state (i.e. visual data: ) and UWB status (i.e. UWB data: ), It can be expressed as:

[0114]

[0115] Among them, the IMU state variable can be expressed as:

[0116]

[0117] Visual states can be represented as:

[0118]

[0119] The UWB state can be expressed as:

[0120]

[0121] in, Indicates t k The position of the IMU in the world coordinate system at this moment; Indicates t k The speed of the IMU in the world coordinate system at this moment; Indicates t k The rotation of IMU in the world coordinate system at this moment; b a Indicates the accelerometer bias of the IMU; b g represents the gyroscope bias of the IMU; m is the total number of visual feature points in the window; l is the inverse depth of the visual feature points; Represents the transformation from camera to IMU coordinate system; Represents the transformation from UWB sensor to IMU coordinate system.

[0122] (2) Determine an IMU propagation posture based on the IMU data, and compensate for a time difference between the UWB data and the visual data based on the IMU propagation posture.

[0123] It should be noted that the common time compensation method is to perform linear spherical interpolation on the rotation part of the posture between two frames, and perform second-order interpolation on the displacement when the speed is considerable, but this method is not sufficient for rotation compensation. In addition, this type of compensation method requires the participation of the posture of the previous and next frames. In order to use this method, constraints need to be established between 3 or 4 frames, resulting in a large amount of computing resources being consumed in interpolating the posture.

[0124] In an embodiment of the present invention, considering that there is almost no drift in the posture propagated by the IMU in a short time, the posture propagated by the IMU (i.e., the IMU propagated posture) is used to compensate for the motion, and this compensation value is used as part of the residual, thereby achieving compensation for the posture in each calculation of the UWB error.

[0125] Specifically, Figure 2 As shown, first, the nearest UWB data is matched for each image frame (ie, each frame image in the visual data).c ,t u Taking the image (i.e., visual data) and UWB data at the moment as an example, find the distance t c ,t u The IMU propagation pose at the latest moment is recorded as and Then the transformation relationship between them can be expressed as: in, Indicates the compensation value; Indicates the distance t c The most recent IMU propagated pose; Indicates the distance t u The IMU propagated pose at the most recent time; SE(3) represents a vector space for rotation. To compensate for the time difference between UWB data and image frames (ie, visual data).

[0126] like Figure 2 As shown in the figure, the black arrow represents the camera (image frame, i.e., visual data), the blue arrow represents the imu (i.e., IMU data), the green arrow represents the uwb (i.e., UWB data), the red box represents rejected, and the blue box represents the imu-rate pose (i.e., IMU propagation pose). Since the frequency of UWB data is greater than the image frame rate and is not time-aligned, some UWB data frames will be discarded (such as the UWB data in the red box), and the UWB data closest to the image frame may be earlier or later than the image frame, but the two situations are consistent in mathematical processing.

[0127] In an embodiment of the present invention, under the framework of a visual-inertial navigation system (VINS), the IMU propagated posture can be used to compensate for the UWB time difference, thereby achieving time alignment of the data, and further enabling the processing and fusion of multiple sensor data in a complex environment.

[0128] In some embodiments, step 103, optimizing the model corresponding to the pose estimation task according to the at least one constraint information until the model converges, includes:

[0129] Step 1031, based on a nonlinear least squares optimization method, construct a loss function of the model according to the at least one constraint information.

[0130] In a specific embodiment, the loss function of the model can be expressed as:

[0131]

[0132] Among them, ev denotes the visual reprojection error; C denotes the set of all landmarks that appear in the i-th and j-th frames; is the residual of the pre-integration constructed using all IMU data between the k-th frame and the k+1-th frame; e p denotes the prior residual from marginalization; denotes the UWB inter-frame constraint residual; m denotes the anchor point number; i and j denote any two frames in the window.

[0133] In this way, the pose between any two frames (the i-th frame and the j-th frame) in the sliding window is constrained, and through marginalization, this constraint of the UWB data on the pose in the window is converted into prior information (e p ), which participates in the next optimization, and can eliminate the potential long-term drift of the sensor.

[0134] It should be noted that here, only the UWB residuals that satisfy i < j are added to save computing resources. The Huber loss function ρ(·) is used for the visual residuals to reduce the influence caused by outliers.

[0135] Step 1032, optimize the model based on the observation information and the loss function until the model converges.

[0136] It should be noted that the optimal state estimation can be obtained by minimizing this loss function. That is, by adjusting the model parameters to find a set of parameters that minimize the difference (i.e., the loss) between the model prediction and the actual data. Specifically, the ceres optimizer can be used to solve this non-linear least squares problem.

[0137] In some embodiments, the constraint information includes UWB inter-frame constraint information, and the loss function includes a target error term defined according to the UWB inter-frame constraint information; Step 1032, the optimizing the model based on the observation information and the loss function until the model converges includes:

[0138] (1) Simplify the target error term in the loss function to obtain a target loss function.

[0139] It should be noted that the target error term is the error term in the loss function defined according to the UWB inter-frame constraint information. By simplifying the target error term, the derivation can be simplified, the computational complexity can be reduced, and it helps to accelerate the model optimization process.

[0140] Specifically, in the embodiments of the present invention, a UWB sensor capable of measuring the distance and angle of an anchor point can be used. Since this type of UWB sensor only requires a set of measurement data (i.e., the distance r, pitch angle θ, and azimuth angle of a single anchor point The position of the anchor point in the UWB sensor coordinate system can be calculated by using the data obtained by measurement. Therefore, each anchor point can be regarded as a feature point that can be stably tracked, and the UWB inter-frame constraint error can be defined in a similar way to the reprojection error. Here, without considering the time difference between the vision and UWB sensors, the target error term can be defined as:

[0141]

[0142] in, The rectangular coordinate system representation of the UWB measurement value of the i-th frame; The rectangular coordinate system represents the j-th frame UWB measurement value; R represents rotation, t represents displacement, and R and t are state variables; Represents the inverse of the rotation from the UWB coordinate system to the body coordinate system; represents the inverse of the rotation from the body coordinate system to the world coordinate system of the jth frame; Represents the rotation from the body coordinate system to the world coordinate system of the i-th frame; Indicates the rotation from the UWB coordinate system to the body coordinate system; Represents the displacement from the body coordinate system to the world coordinate system of the i-th frame; Represents the displacement from the body coordinate system to the world coordinate system in the jth frame.

[0143] In some specific embodiments, simplifying the target error term in the loss function to obtain the target loss function includes:

[0144] (1) Based on the Mahalanobis distance, the target error term is standardized to obtain the target function corresponding to the target error term.

[0145] Here, the objective function corresponding to the target error term obtained based on the Mahalanobis distance can be expressed as:

[0146]

[0147] Where P represents the covariance matrix of e, which can be expressed as The covariance matrix (P0) is used instead to simplify the derivation.

[0148] (2) Determine the covariance of the UWB data based on geometric approximation for the covariance matrix in the objective function.

[0149] It should be noted that when using normal distribution to describe the results of range measurement (distance measurement) and angle measurement (angle measurement), calculating the error covariance matrix of these measurement results may become very complicated and difficult. In order to calculate the covariance matrix, one method in the prior art is to use the Von Mises distribution to calculate the covariance matrix of θ, φ corresponding to the rectangular coordinate system on the unit sphere, and then further derive the complete covariance matrix; another method is to directly use the covariance operation rules to calculate the elements of the covariance matrix one by one. However, both methods require a large number of sine and cosine operations and are not suitable for real-time systems.

[0150] In order to solve this problem, the embodiment of the present invention adopts a covariance calculation method that first calculates the simple case and then performs a linear transformation, which effectively simplifies the calculation complexity and achieves good performance in actual tests. Specifically, assuming that a certain measurement value The corresponding rectangular coordinates are It is known that their standard deviations are: n r 、n θ and The calculation steps are as follows: First, The angle part is approximated at , and the arc length is used instead of the chord length, and the standard deviations of θ and φ here are: and Thus, the covariance matrix in a simple case (that is, the covariance of UWB data) is obtained, which is expressed as follows:

[0151]

[0152] Where P0 represents the covariance of UWB data, r represents the distance, θ represents the pitch angle; Indicates the azimuth.

[0153] Then, rotate the vector (0,0,0) to Corresponding shaft And the angle (θ) can be expressed as:

[0154]

[0155] Finally, using Rodriguez’s formula, we convert the above axis (n) and angle (θ) into a rotation matrix R c , then the final covariance matrix can be expressed as: Using the orthogonality of the rotation matrix, the inverse of the covariance matrix can be directly obtained:

[0156] (3) Determine a simplified target error term based on the covariance of the UWB data.

[0157] like Figure 3-1 to Figure 3-4 As shown in FIG. 1 , the simulation results of random points under different angle combinations are shown. Among them, the red points are random points directly generated according to the angle and distance standard deviation, and the blue points are random points directly generated after the covariance is calculated using a simplified method in the embodiment of the present invention. , the random points obtained by simulation and the random points generated after calculating the covariance are represented by red and blue respectively. It can be seen that the two are only slightly different in the distribution direction, which is almost negligible in the actual operation. Therefore, the embodiment of the present invention can effectively simplify the calculation while ensuring the calculation accuracy, thereby improving the model optimization speed. In the actual optimization, the ceres optimizer only accepts the least squares optimization, that is, min(e T e), therefore, the error term (target error term) using the Mahalanobis distance needs to be inversely covariance matrix (i.e., P -1 ) to do LLT decomposition, that is, P -1 =LL T , and then according to the objective function corresponding to the target error term (i.e. ), we can get:

[0158]

[0159] It should be noted that when performing LLT decomposition of the covariance matrix, only the diagonal matrix P -1 The square root of the diagonal elements of can be taken without the need for additional matrix decomposition calculations.

[0160] Finally, the formula of the original inter-frame constraint error (that is, the target error term) is updated to the simplified target error term:

[0161]

[0162] in,

[0163]

[0164] Among them, R represents rotation and p represents displacement; Indicates time-compensated Indicates time-compensated Indicates time-compensated Indicates time-compensated Indicates the time compensation from UWB frame to camera frame.

[0165] (4) Obtain the target loss function based on the simplified target error term.

[0166] (ii) optimizing the model based on the observation information and the target loss function until the model converges.

[0167] like Figure 4 As shown, the pose estimation optimization system of the embodiment of the present invention can execute the pose estimation optimization method of the embodiment of the present invention. Among them, visual data, IMU data and UWB data are sensor inputs (also known as sensor inputs), Kalman filter, VI-UWB optimization marginalization and time alignment are UWB-aided components, and feature detection tracking, pre-integration and propagation are vi-only components. The system does not rely on the prior position information of the anchor point and can be plug-and-play integrated in the back-end fusion processing of the mainstream visual inertial measurement system (Visual-Inertial Odometry, VIO). In the VINS framework, first, the UWB data is smoothed by a Kalman filter and then converted to a rectangular coordinate system, and combined with the pose information (imu-rate pose) output by the high frame rate IMU, the interframe transform between the UWB frame (i.e., UWB data) and the visual frame (i.e., visual data) is calculated; finally, the transform and the UWB data are added to the sliding window, and together with the visual constraint, the IMU pre-integration constraint, and the prior constraint, the back-end bundle adjustment (BA) is optimized to obtain the camera rate pose output (cam-rate pose). In this way, the UWB ranging-angle measurement constraints are integrated into the VINS framework, achieving high-precision, high-robust real-time pose estimation.

[0168] The pose estimation optimization method of this embodiment is based on visual-inertial-UWB constraints and can achieve high-precision and high-robust pose estimation of the intelligent body without being restricted by the number and position of anchor points. It can be applied to various technical fields such as robot navigation guidance control and smart transportation autonomous driving.

[0169] like Figure 5 As shown, a posture estimation and optimization device according to an embodiment of the present invention includes:

[0170] The data acquisition module 510 is used to acquire observation information corresponding to the target object, wherein the observation information includes: inertial measurement unit IMU data, visual data and ultra-wideband UWB data;

[0171] A constraint construction module 520, configured to construct at least one constraint information corresponding to the pose estimation task according to the observation information based on a sliding window optimization method;

[0172] The model optimization module 530 is used to optimize the model corresponding to the pose estimation task according to the at least one constraint information until the model converges.

[0173] In this embodiment, after obtaining the observation information corresponding to the target object, at least one constraint information corresponding to the pose estimation task is constructed based on the observation information based on the sliding window optimization method, wherein the observation information includes: IMU data, visual data and UWB data, and then, based on the at least one constraint information, the model corresponding to the pose estimation task is optimized until the model converges. In this way, based on the sliding window optimization method, by adding UWB data as constraint information, the estimation accuracy of the model for the pose of the target object can be improved without relying on prior information.

[0174] Optionally, the constraint building module 520 includes:

[0175] A data alignment submodule, used to align the IMU data, the visual data and the UWB data to obtain target observation information;

[0176] The constraint construction submodule is used to construct the at least one constraint information according to the target observation information.

[0177] Optionally, the data alignment submodule includes:

[0178] A data conversion unit, used for converting the visual data and the UWB data into an IMU coordinate system;

[0179] A time compensation unit is used to determine an IMU propagation posture according to the IMU data, and to compensate for a time difference between the UWB data and the visual data according to the IMU propagation posture.

[0180] Optionally, the model optimization module 530 includes:

[0181] A function construction submodule, used for constructing a loss function of the model according to the at least one constraint information based on a nonlinear least squares optimization method;

[0182] The model optimization submodule is used to optimize the model based on the observation information and the loss function until the model converges.

[0183] Optionally, the constraint information includes UWB inter-frame constraint information, the loss function includes a target error term defined according to the UWB inter-frame constraint information; and the model optimization submodule includes:

[0184] A simplification processing unit, used to simplify the target error term in the loss function to obtain a target loss function;

[0185] A model optimization unit is used to optimize the model based on the observation information and the target loss function until the model converges.

[0186] Optionally, the simplified processing unit includes:

[0187] A standardization subunit, used for standardizing the target error term based on the Mahalanobis distance to obtain an objective function corresponding to the target error term;

[0188] A first processing subunit, configured to determine the covariance of the UWB data based on geometric approximation with respect to the covariance matrix in the objective function;

[0189] A second processing subunit, configured to determine a simplified target error term according to the covariance of the UWB data;

[0190] The third processing subunit is used to obtain the target loss function according to the simplified target error term.

[0191] It should be noted here that the above-mentioned posture estimation and optimization device provided in the embodiment of the present invention can implement all the method steps implemented in the above-mentioned posture estimation and optimization method embodiment, and can achieve the same technical effect. The parts and beneficial effects of this embodiment that are the same as the method embodiment will not be described in detail here.

[0192] like Figure 6 As shown, a positioning device 600 according to an embodiment of the present invention includes a processor 610 and a transceiver 620, wherein the processor is used to:

[0193] Acquire observation information corresponding to the target object, the observation information including: inertial measurement unit IMU data, visual data and ultra-wideband UWB data;

[0194] Based on the sliding window optimization method, construct at least one constraint information corresponding to the pose estimation task according to the observation information;

[0195] According to the at least one constraint information, the model corresponding to the pose estimation task is optimized until the model converges.

[0196] In this embodiment, after obtaining the observation information corresponding to the target object, at least one constraint information corresponding to the pose estimation task is constructed based on the observation information based on the sliding window optimization method, wherein the observation information includes: IMU data, visual data and UWB data, and then, based on the at least one constraint information, the model corresponding to the pose estimation task is optimized until the model converges. In this way, based on the sliding window optimization method, by adding UWB data as constraint information, the estimation accuracy of the model for the pose of the target object can be improved without relying on prior information.

[0197] Optionally, when constructing at least one constraint information corresponding to the pose estimation task based on the observation information in the sliding window optimization mode, it is specifically used to:

[0198] Performing data alignment on the IMU data, the visual data, and the UWB data to obtain target observation information;

[0199] The at least one constraint information is constructed according to the target observation information.

[0200] Optionally, when the processor performs data alignment on the IMU data, the visual data, and the UWB data to obtain target observation information, the processor is specifically configured to perform at least one of the following:

[0201] Convert the visual data and the UWB data to an IMU coordinate system;

[0202] The IMU propagation posture is determined according to the IMU data, and the time difference between the UWB data and the visual data is compensated according to the IMU propagation posture.

[0203] Optionally, the processor optimizes the model corresponding to the pose estimation task according to the at least one constraint information until the model converges, specifically for:

[0204] Based on a nonlinear least squares optimization method, constructing a loss function of the model according to the at least one constraint information;

[0205] Based on the observation information and the loss function, the model is optimized until the model converges.

[0206] Optionally, the constraint information includes UWB inter-frame constraint information, and the loss function includes a target error term defined according to the UWB inter-frame constraint information;

[0207] The processor optimizes the model based on the observation information and the loss function until the model converges, specifically for:

[0208] Simplifying the target error term in the loss function to obtain a target loss function;

[0209] Based on the observation information and the target loss function, the model is optimized until the model converges.

[0210] Optionally, when simplifying the target error term in the loss function to obtain a target loss function, the processor is specifically configured to:

[0211] Based on the Mahalanobis distance, the target error term is normalized to obtain the target function corresponding to the target error term;

[0212] For the covariance matrix in the objective function, determining the covariance of the UWB data based on geometric approximation;

[0213] Determining a simplified target error term according to the covariance of the UWB data;

[0214] The target loss function is obtained according to the simplified target error term.

[0215] It should be noted here that the above-mentioned positioning device provided by the embodiment of the present invention can implement all the method steps implemented by the embodiment of the posture estimation optimization method, and can achieve the same technical effect. The parts and beneficial effects that are the same as the method embodiment in this embodiment will not be described in detail here.

[0216] A positioning device according to another embodiment of the present invention, such as Figure 7 As shown, it includes a transceiver 710, a processor 700, a memory 720, and a program or instruction stored in the memory 720 and executable on the processor 700; when the processor 700 executes the program or instruction, the above-mentioned posture estimation optimization method is implemented.

[0217] The transceiver 710 is used to receive and send data under the control of the processor 700.

[0218] Among them, Figure 7In the embodiment, the bus architecture may include any number of interconnected buses and bridges, specifically one or more processors represented by processor 700 and various circuits of memory represented by memory 720 are linked together. The bus architecture may also link together various other circuits such as peripherals, voltage regulators, and power management circuits, which are well known in the art and are therefore not further described herein. The bus interface provides an interface. The transceiver 710 may be a plurality of components, namely, a transmitter and a receiver, providing a unit for communicating with various other devices on a transmission medium. For different positioning devices, the user interface 730 may also be an interface capable of externally and internally connecting required devices, and the connected devices include but are not limited to a keypad, a display, a speaker, a microphone, a joystick, and the like.

[0219] The processor 700 is responsible for managing the bus architecture and general processing, and the memory 720 can store data used by the processor 700 when performing operations.

[0220] The present application also provides a computer program product, including computer instructions, which, when executed by a processor, implement the above Figure 1 The various processes of the method embodiment shown can achieve the same technical effect, and will not be described again here to avoid repetition.

[0221] A computer-readable storage medium according to an embodiment of the present invention stores a program or instruction thereon, and when the program or instruction is executed by a processor, the steps in the pose estimation optimization method described above are implemented, and the same technical effect can be achieved. To avoid repetition, it is not repeated here. The computer-readable storage medium is, for example, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0222] It should be further explained that the terminals described in this specification include but are not limited to smart phones, tablet computers, etc., and many of the functional components described are called modules in order to more particularly emphasize the independence of their implementation methods.

[0223] In the embodiment of the present invention, module can be implemented with software so that it can be executed by various types of processors. For example, an executable code module of an identification can include one or more physical or logical blocks of computer instructions, for example, it can be constructed as an object, process or function. Nevertheless, the executable code of the identified module does not need to be physically located together, but can include different instructions stored in different positions, and when these instructions are logically combined together, it constitutes a module and realizes the specified purpose of the module.

[0224] In fact, executable code module can be a single instruction or many instructions, and can even be distributed on a plurality of different code segments, distributed among different programs, and distributed across a plurality of memory devices. Similarly, operating data can be identified in the module, and can be implemented and organized in the data structure of any appropriate type according to any appropriate form. The operating data can be collected as a single data set, or can be distributed in different locations (including on different storage devices), and can only be present on a system or network as an electronic signal at least in part.

[0225] When a module can be implemented by software, considering the level of existing hardware technology, a person skilled in the art can build a corresponding hardware circuit to implement the corresponding function of the module that can be implemented by software without considering the cost. The hardware circuit includes a conventional very large scale integration (VLSI) circuit or gate array and existing semiconductors such as logic chips, transistors, or other discrete components. The module can also be implemented by a programmable hardware device, such as a field programmable gate array, a programmable array logic, a programmable logic device, etc.

[0226] The above exemplary embodiments are described with reference to the accompanying drawings, and many different forms and embodiments are feasible without departing from the spirit and teachings of the present invention. Therefore, the present invention should not be constructed as a limitation of the exemplary embodiments proposed herein. More specifically, these exemplary embodiments are provided so that the present invention will be perfect and complete, and the scope of the present invention will be conveyed to those who are familiar with the technology. In these figures, the component sizes and relative sizes may be exaggerated for clarity. The terms used here are only based on the purpose of describing specific exemplary embodiments and are not intended to be limiting. As used herein, unless the text clearly indicates otherwise, the singular forms "one", "an" and "the" are intended to include these multiple forms. It will be further understood that the terms "including" and / or "comprising" when used in this specification indicate the presence of the features, integers, steps, operations, components and / or components, but do not exclude the presence or increase of one or more other features, integers, steps, operations, components, components and / or their groups. Unless otherwise indicated, when stated, a range of values ​​includes the upper and lower limits of that range and any subranges therebetween.

[0227] The above is a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A pose estimation optimization method, characterized in that: include: Acquire observation information corresponding to the target object, the observation information including: inertial measurement unit IMU data, visual data and ultra-wideband UWB data; Based on the sliding window optimization method, construct at least one constraint information corresponding to the pose estimation task according to the observation information; According to the at least one constraint information, the model corresponding to the pose estimation task is optimized until the model converges.

2. The method according to claim 1, characterized in that The method of constructing at least one constraint information corresponding to the pose estimation task based on the observation information based on the sliding window optimization method includes: Performing data alignment on the IMU data, the visual data, and the UWB data to obtain target observation information; The at least one constraint information is constructed according to the target observation information.

3. The method according to claim 2, characterized in that The step of aligning the IMU data, the visual data, and the UWB data to obtain target observation information includes at least one of the following: Convert the visual data and the UWB data to an IMU coordinate system; The IMU propagation posture is determined according to the IMU data, and the time difference between the UWB data and the visual data is compensated according to the IMU propagation posture.

4. The method according to claim 1, characterized in that: The optimizing the model corresponding to the pose estimation task according to the at least one constraint information until the model converges includes: Based on a nonlinear least squares optimization method, constructing a loss function of the model according to the at least one constraint information; Based on the observation information and the loss function, the model is optimized until the model converges.

5. The method according to claim 4, characterized in that The constraint information includes UWB inter-frame constraint information, and the loss function includes a target error term defined according to the UWB inter-frame constraint information; The optimizing the model based on the observation information and the loss function until the model converges includes: Simplifying the target error term in the loss function to obtain a target loss function; Based on the observation information and the target loss function, the model is optimized until the model converges.

6. The method according to claim 5, characterized in that The step of simplifying the target error term in the loss function to obtain a target loss function includes: Based on the Mahalanobis distance, the target error term is normalized to obtain the target function corresponding to the target error term; For the covariance matrix in the objective function, determining the covariance of the UWB data based on geometric approximation; Determining a simplified target error term according to the covariance of the UWB data; The target loss function is obtained according to the simplified target error term.

7. A posture estimation and optimization device, characterized in that: include: A data acquisition module is used to acquire observation information corresponding to the target object, wherein the observation information includes: inertial measurement unit IMU data, visual data and ultra-wideband UWB data; A constraint construction module, used to construct at least one constraint information corresponding to the pose estimation task according to the observation information based on a sliding window optimization method; A model optimization module is used to optimize the model corresponding to the pose estimation task according to the at least one constraint information until the model converges.

8. A positioning device, characterized in that: include: transceivers and processors; The processor is used to: Acquire observation information corresponding to the target object, the observation information including: inertial measurement unit IMU data, visual data and ultra-wideband UWB data; Based on the sliding window optimization method, construct at least one constraint information corresponding to the pose estimation task according to the observation information; According to the at least one constraint information, the model corresponding to the pose estimation task is optimized until the model converges.

9. A positioning device, comprising: A transceiver, a processor, a memory, and a program or instruction stored in the memory and executable on the processor; characterized in that when the processor executes the program or instruction, the posture estimation optimization method as described in any one of claims 1-6 is implemented.

10. A computer program product, characterized in that It includes computer instructions, which, when executed by a processor, implement the steps of the pose estimation optimization method as described in any one of claims 1 to 6.

11. A readable storage medium having a program or instruction stored thereon, characterized in that: When the program or instruction is executed by the processor, the steps in the pose estimation optimization method as described in any one of claims 1 to 6 are implemented.

Images