Positioning method and device, electronic equipment and storage medium
By integrating the inertia measurement unit and image acquisition unit on autonomous driving vehicles, combining timestamp matching and factor graph optimization library, the problem of vehicle positioning in a GPS-free environment is solved, and a high-precision positioning effect is achieved.
Patent Information
- Application Number
- CN202411542896.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-10-31
- Publication Date
- 2025-02-14
AI Technical Summary
In an environment without GPS signals, how to accurately locate autonomous vehicles has become an urgent problem.
By loading the inertial measurement unit and the image acquisition unit on the movable device, the acceleration and angular velocity data of the inertial measurement unit and the characteristic point data of the image acquisition unit are used to perform timestamp matching, and the initial state vector and current position of the device are determined, and The factor graph optimization library optimizes the loop loop and improves positioning accuracy.
It realizes accurate positioning of autonomous vehicles in a GPS-free environment, improves the accuracy of fusion positioning, avoids error accumulation, and enhances the stability of the positioning system.
Smart Images
Figure CN119437238A_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the field of positioning technology, and in particular to a positioning method and device, an electronic device, and a storage medium. Background Art
[0002] With the development of autonomous driving technology, how to ensure the accurate positioning of autonomous driving vehicles has become a research hotspot. In the existing technology, the Global Positioning System (GPS) is usually used for positioning, but in some environments (for example, basements and tunnels), the GPS signal is weak or even non-existent. Therefore, how to accurately position an autonomous driving vehicle in an environment without a GPS signal has become an urgent problem to be solved. Summary of the invention
[0003] In order to solve the above problems, the embodiments of the present application provide a positioning method and device, an electronic device, and a storage medium.
[0004] In a first aspect, an embodiment of the present application provides a positioning method, which is applied to a movable device, and the movable device is equipped with an inertial measurement unit and an image acquisition unit. The method includes: determining an initial state vector of the movable device; acquiring data measured by the inertial measurement unit and data collected by the image acquisition unit, and timestamping the data measured by the inertial measurement unit and the data collected by the image acquisition unit to obtain first data and second data, the first data including accelerations and angular velocities at multiple moments, and the second data including feature points corresponding to each of the multiple moments; determining a first posture of the movable device at the current moment based on the first data and the initial state vector; determining a second posture of the movable device at the current moment based on the first posture and the second data; determining the current posture of the movable device based on the first posture and the second posture.
[0005] In combination with the first aspect, based on the first posture and the second posture, the current posture of the movable device is determined, including: determining the distance between the first posture and the second posture; if the distance is less than or equal to a preset threshold, determining the second posture as the current posture; if the distance is greater than the preset threshold, determining the first posture as the current posture; wherein the distance includes Mahalanobis distance or Euclidean distance.
[0006] In combination with the first aspect, after determining the current posture of the movable device based on the first posture and the second posture, the method also includes: placing the current posture, the current moment, and the feature points corresponding to the current moment into a sliding window, the sliding window including multiple historical postures arranged in chronological order, the historical moments corresponding to the historical postures, and the feature points corresponding to the historical moments; when the detection conditions are met at the current moment, traversing the historical postures in the active window, determining the historical postures whose position difference with the current posture is within a preset threshold range as the target historical posture, and constructing a loop based on the target historical posture and the current posture; using the factor graph optimization library to optimize and solve the loop to obtain the optimized current posture.
[0007] In combination with the first aspect, based on the first pose and the second data, the second pose of the movable device at the current moment is determined, including: based on the first pose, the coordinates of the feature points corresponding to the current moment in the first coordinate system, and the coordinates of the feature points corresponding to the current moment in the second coordinate system at the moment before the current moment, a residual observation equation is constructed, the first coordinate system corresponds to the coordinate system of the movable device at the current moment, and the second coordinate system corresponds to the coordinate system of the movable device at the initial moment; the residual observation equation is filtered and iterated to optimize the first pose, and when a certain number of iterations is reached or the overall convergence is reached, the optimized first pose is determined as the second pose.
[0008] In combination with the first aspect, before determining the second posture of the movable device at the current moment based on the first posture and the second data, the method also includes: obtaining data measured by a wheel speed meter of the movable device state, and timestamp matching the data measured by the wheel speed meter with the data measured by the inertial measurement unit to obtain third data; and using the third data to perform observation correction on the first posture.
[0009] In combination with the first aspect, the initial state vector includes at least one of the initial position, initial attitude and initial velocity of the movable device; based on the first data and the initial state vector, the first attitude of the movable device at the current moment is determined, including: determining the bias of the inertial measurement unit; based on the initial position, initial attitude, initial velocity, acceleration and angular velocity at multiple moments in the first data, and the bias, determining the first attitude of the movable device at the current moment.
[0010] In combination with the first aspect, determining the bias of the inertial measurement unit includes: after the movable device is started and the speed is zero, determining multiple accelerations and multiple angular velocities of the inertial measurement unit in different frames; and determining an average value of the multiple accelerations and / or an average value of the multiple angular velocities as the bias of the inertial measurement unit.
[0011] In a second aspect, an embodiment of the present application provides a positioning device, which is applied to a movable device, and the movable device is equipped with an inertial measurement unit and an image acquisition unit. The positioning device includes: a determination module and an acquisition module; the determination module is used to determine the initial state vector of the movable device; the acquisition module is used to acquire data collected by the inertial measurement unit and the data collected by the image acquisition unit, and perform timestamp matching to obtain first data and second data, the first data including accelerations and angular velocities at multiple moments, and the second data including feature points corresponding to each of the multiple moments; the determination module is also used to determine the first posture of the movable device at the current moment based on the first data and the initial state vector; the determination module is also used to determine the second posture of the movable device at the current moment based on the first posture and the second data; the determination module is also used to determine the current posture of the movable device based on the first posture and the second posture.
[0012] In a third aspect, an embodiment of the present application provides an electronic device, comprising: a processor; a memory, the memory being connected to the processor, the memory being used to store a computer program, and the computer program implementing the above-mentioned positioning method when executed by the processor.
[0013] In a fourth aspect, an embodiment of the present application provides a storage medium having a computer program stored thereon. When the computer program is executed by a processor, the above-mentioned positioning method is implemented.
[0014] In a fifth aspect, an embodiment of the present application provides a computer program product, including computer program instructions, which implement the above-mentioned positioning method when executed by a processor.
[0015] Through the above technical solution, the first data of the inertial measurement unit is used as a prediction to determine the first pose, the second data of the image acquisition unit is used as an observation, the first pose is optimized to obtain the second pose, and a tightly coupled optimization method is established through the first data and the second data. Compared with the loose coupling method, the tightly coupled method realizes the mutual constraint between the sensor data, thereby improving the accuracy of fusion positioning. In addition, after obtaining the second pose, it is necessary to judge the second pose to avoid the second pose error being large, affecting the stability of the positioning system in subsequent iterations, and reducing the positioning accuracy. If the distance between the second pose and the first pose is greater than the preset threshold, it is determined that the second pose error is large, the second pose is discarded, and the first pose is used as the current pose. If the distance between the second pose and the first pose is less than or equal to the preset threshold, the second pose is more accurate, and the second pose is the pose optimized for the first pose, so the second pose is used as the current pose, thereby improving the positioning accuracy. BRIEF DESCRIPTION OF THE DRAWINGS
[0016] Figure 1 It is a flowchart of a method for determining the current position of a movable device provided in an embodiment of the present application.
[0017] Figure 2 It is a flowchart of a method for determining the first position of a movable device at the current moment provided by an embodiment of the present application.
[0018] Figure 3 It is a flowchart of a method for determining a second posture of a movable device at a current moment provided by an embodiment of the present application.
[0019] Figure 4 It is a flowchart of a method for determining the current position of a movable device provided in an embodiment of the present application.
[0020] Figure 5 It is a flowchart of a method for determining the current position of a movable device provided in another embodiment of the present application.
[0021] Figure 6 It is a schematic diagram of a sliding window provided in an embodiment of the present application.
[0022] Figure 7 It is a schematic diagram of the structure of a loop provided in an embodiment of the present application.
[0023] Figure 8 It is a structural schematic diagram of a positioning device provided in one embodiment of the present application.
[0024] Fig. 9 It is a schematic diagram of the structure of an electronic device provided in one embodiment of the present application. DETAILED DESCRIPTION
[0025] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.
[0026] The precise positioning of autonomous vehicles is crucial for the driving of autonomous vehicles. Since GPS signals are easily interfered with, various sensors (such as wheel speed meters, inertial measurement units (IMUs), vehicle surround cameras, etc.) are installed on the vehicle to sense the surrounding environment and rely on the sensed information for positioning. The inventors found that IMUs have zero bias and random walk noise, and long-term integration will result in cumulative errors; in addition, the vehicle surround camera obtains the posture through feature point matching, however, there are matching errors in the matching process, and the matching of the next frame requires the posterior posture information of the previous frame as the prior information for the matching of the next frame, which results in the transmission of errors. The long-distance and long-term operation of the algorithm will cause the error to become larger and larger, resulting in the inability to accurately position.
[0027] In the face of the above technical problems, an embodiment of the present application provides a positioning method, which is applied to a mobile device, and the mobile device is equipped with an inertial measurement unit and an image acquisition unit. The method includes: determining the initial state vector of the mobile device; obtaining data measured by the inertial measurement unit and data collected by the image acquisition unit, and matching the data measured by the inertial measurement unit and the data collected by the image acquisition unit by time stamp to obtain first data and second data, the first data including acceleration and angular velocity at multiple moments, and the second data including feature points corresponding to each of the multiple moments; based on the first data and the initial state vector, determining the first position of the mobile device at the current moment; based on the first position and the second data, determining the second position of the mobile device at the current moment; based on the first position and the second position, determining the current position of the mobile device. In the embodiment of the present application, the first data of the inertial measurement unit is used as a prediction to determine the first position, the second data of the image acquisition unit is used as an observation, the first position is optimized to obtain the second position, and based on the first position and the second position, the current position of the mobile device is determined to improve the positioning accuracy.
[0028] Figure 1 1 is a flow chart of a method for determining the current position of a movable device provided in an embodiment of the present application. The method is applied to a movable device, and the movable device is equipped with an inertial measurement unit and an image acquisition unit. Optionally, the movable device includes an autonomous driving vehicle, an intelligent robot, an intelligent aircraft, etc. Figure 1 As shown, the method includes the following steps.
[0029] Step S110, determining an initial state vector of the movable device.
[0030] In the embodiment of the present application, optionally, the starting position of the movable device is determined as the location of the global coordinate system of the entire positioning system, that is, the starting position of the movable device is the origin of the global coordinate system. The initial state vector is the state vector of the movable device at the starting position (i.e., the initial moment). The initial state vector includes at least one of the initial position, initial attitude, and initial speed of the movable device. The starting position of the movable device is determined as the initial position, the orientation of the movable device at the starting position is determined as the initial attitude, and the speed of the movable device at the starting position is determined as the initial speed. Optionally, the initial position and initial attitude are set to 0. Since the movable device is stationary at the starting point, the initial speed is also 0. It should be noted that the position and attitude of the movable device are collectively referred to as the posture of the movable device, so the posture includes 3 positional degrees of freedom and 3 directional degrees of freedom, and the directional degrees of freedom are usually represented by pitch angle (Pitch), roll angle (Roll), and yaw angle (Yaw). It can be understood that in other embodiments, the initial position and initial attitude may not be set to 0.
[0031] Step S120, acquiring data measured by the inertial measurement unit and data collected by the image acquisition unit, and performing time stamp matching on the data measured by the inertial measurement unit and the data collected by the image acquisition unit to obtain first data and second data.
[0032] In an embodiment of the present application, the inertial measurement unit includes an accelerometer and a gyroscope, which can measure the acceleration and angular velocity of an object in three directions. The image acquisition unit includes a camera. Due to the different output frequencies of different sensors, the timestamps (timestamps are used to record the output time of the sensor) of the data measured by the inertial measurement unit and the data collected by the image acquisition unit are different. The output frequency of the inertial measurement unit (for example, 100Hz) is greater than the output frequency of the image acquisition unit (for example, 20Hz). Therefore, before data fusion, timestamp matching is required to obtain multiple groups of data measured by the inertial measurement unit (i.e., first data) and data collected by the image acquisition unit (i.e., second data) with similar timestamps. Specifically, multiple moments are determined; at each moment, the acceleration and angular velocity measured by the inertial measurement unit closest to the moment are selected as the first data, and the multiple feature points collected by the image acquisition unit closest to the moment are selected as the second data. Therefore, the first data includes the acceleration and angular velocity of multiple moments, and the second data includes the feature points corresponding to each of the multiple moments.
[0033] Step S130, determining the first position of the movable device at the current moment based on the first data and the initial state vector.
[0034] Alternatively, if Figure 2 As shown, the method for determining the first position of the movable device at the current moment includes the following steps.
[0035] Step S210, determining the bias of the inertial measurement unit.
[0036] When the movable device is stationary, the acceleration and angular velocity output by the inertial measurement unit should be 0, but due to physical circuit and environmental reasons, the acceleration and angular velocity output by the inertial measurement unit are not 0, that is, the data output by the inertial measurement unit contains bias. In order to improve the positioning accuracy of the inertial measurement unit, in the embodiment of the present application, when determining the first posture, the bias error needs to be offset, so it is necessary to determine the bias of the inertial measurement unit. After the movable device (for example, a vehicle) is started, the inertial measurement unit starts to work, and the bias of the inertial measurement unit can be determined by using the period when the movable device has not moved (that is, the speed is 0). Specifically, the method includes: after the movable device is started and the speed is zero, determine multiple accelerations and multiple angular velocities of the inertial measurement unit in different frames; determine the average value of multiple accelerations and / or the average value of angular velocities as the bias of the inertial measurement unit. For example, the average value of the acceleration and / or the average value of the angular velocity in the first 500 frames of data after the inertial measurement unit works is determined as the bias of the inertial measurement unit.
[0037] Optionally, when the iterative extended Kalman filter (IEKF) is used to optimize the posture, there are three key parameters, the process noise covariance matrix (Q matrix), the measurement noise covariance matrix (R matrix) and the state covariance matrix (P matrix), wherein the R matrix and the P matrix can be adjusted manually, and the Q matrix represents an estimate of the random noise in the system model. In an embodiment of the present application, the Q matrix is determined by the data of the inertial measurement unit. Optionally, the variance of the above-mentioned multiple accelerations and / or the variance of the above-mentioned multiple angular velocities are determined as the process noise covariance matrix. The Q matrix does not change during the iteration process of the IEKF, reflecting the confidence in the predicted quantity. It can be understood that this scheme only lists a method for determining the Q matrix, and those skilled in the art can also select other suitable methods for determining the Q matrix according to actual conditions, and the embodiments of the present application are not limited.
[0038] Step S220, determining the first posture of the movable device at the current moment based on the initial position, the initial posture, the initial velocity, the acceleration and angular velocity at multiple moments in the first data, and the bias.
[0039] In the embodiment of the present application, when determining the first pose at any moment, it is necessary to use the first pose at the previous moment as prior information, combined with the acceleration and angular velocity of the inertial measurement unit, to determine the first pose at that moment. Therefore, when determining the first pose at any moment, it is necessary to determine it through multiple iterative operations based on the initial state vector and bias, as well as the acceleration and angular velocity at different moments. Specifically, the first pose is determined by formulas (1)-(3):
[0040] V k =V k-1 +R k-1 (a k -bias-g)Δt (Formula 1)
[0041] R k =R k-1 Exp((ω k -bias))Δt (Formula 2)
[0042]
[0043] Where k represents the current moment, k-1 represents the previous moment, and k≥1; a k represents the acceleration at the current moment, ω k represents the angular velocity at the current moment, a k and ω k It is measured by an inertial measurement unit; bias represents the bias of the inertial measurement unit, g represents the gravitational acceleration, Δt represents the time interval; V k Indicates the speed at the current moment, V k-1 represents the speed at the previous moment, R k-1 Indicates the posture at the last moment, P k-1 Indicates the position at the last moment, R k Indicates the current posture, P k represents the current position, P k and R k That is, the current position, where when k = 1, V k-1 (i.e. V0) represents the initial velocity, R k-1 (i.e. R0) represents the initial posture, P k-1 (ie P0) represents the initial position.
[0044] In the embodiment of the present application, in order to improve the accuracy of the first posture, the speed can be corrected in combination with a wheel speed meter. Optionally, in the embodiment of the present application, the data measured by the wheel speed meter loaded on the movable device is obtained, and the data measured by the wheel speed meter is timestamped and matched with the data measured by the inertial measurement unit to obtain the third data; the first posture is observed and corrected using the third data. Optionally, the third data includes the left rear wheel speed and the right rear wheel speed, and the observed angular velocity and the observed velocity are determined according to the rear wheel differential model, as shown in formula (4) and formula (5):
[0045]
[0046] Among them, ω ’ represents the observed angular velocity, V ’ represents the observed speed, V1 represents the left rear wheel speed, V2 represents the right rear wheel speed, d represents the wheel width between the left rear wheel and the right rear wheel, m1 and m2 represent the wheel speed factors, and the wheel speed factors are calibrated by providing true values through the combined inertial navigation. After determining the observed angular velocity and observed speed, the above speed and angular velocity are observed and corrected (i.e., optimized speed and angular velocity) to correct the first position at the current moment.
[0047] Step S140, determining a second posture of the movable device at the current moment based on the first posture and the second data.
[0048] In an embodiment of the present application, the first data of the inertial measurement unit is used as a prediction to determine the first pose, and the second data of the image acquisition unit is used as an observation to optimize the first pose to obtain the second pose. A tightly coupled optimization method is established through the first data and the second data. Compared with a loosely coupled method, the tightly coupled method realizes mutual constraints between sensor data, thereby improving the accuracy of fusion positioning.
[0049] Alternatively, if Figure 3 As shown, the method for determining the second posture of the movable device at the current moment includes the following steps.
[0050] Step S310, based on the first pose, the coordinates of the feature points corresponding to the current moment in the first coordinate system, and the coordinates of the feature points corresponding to the current moment in the second coordinate system at the moment before the current moment, construct a residual observation equation. The first coordinate system corresponds to the coordinate system of the movable device at the current moment, and the second coordinate system corresponds to the coordinate system of the movable device at the initial moment (i.e., the global coordinate system).
[0051] The first pose includes the position and attitude of the movable device at the current moment, and the first pose can reflect the position change and attitude change of the movable device relative to the global coordinate system (i.e., the starting position). For example, the residual observation equation is shown in formula (6):
[0052] r k,i =|R k U k,i +P k -V k-1,i | Formula (6)
[0053] Among them, i represents the i-th feature point, U k,i represents the coordinates of the i-th feature point k at the current time in the first coordinate system, V k-1,i represents the coordinates of the i-th feature point in the second coordinate system at the previous moment, R k U k,i +P k Indicates that the i-th feature point is transformed to the second coordinate system, r k,i Represents the difference between the i-th feature point at the current moment and the previous moment, that is, the residual.
[0054] In the embodiment of the present application, the first pose at the current moment can be used to transform the feature points in the first coordinate system to the second coordinate system (global coordinate system), that is, the difference between the same feature point at the current moment and the previous moment is determined in the global coordinate system. The smaller the difference, the more accurate the first pose. Therefore, it can be optimized iteratively to make r k,i Minimize to obtain the optimized posture. Specifically, as in step S320.
[0055] Step S320, filtering and iterating the residual observation equation to optimize the first pose, when a certain number of iterations is reached or the overall convergence is reached, the optimized first pose is determined as the second pose.
[0056] In the embodiment of the present application, the iterative extended Kalman filter (IEKF) method is used to optimize the first pose. Since the second data includes multiple feature points, for each feature point, a residual observation equation is established to determine the Jacobian matrix Hi of the residual observation equation relative to the state vector (a matrix composed of the partial derivatives of the residual observation equation to the state vector, where the state vector includes at least one of the position, posture, and velocity at the current moment), thereby obtaining the total Jacobian matrix H. Then the Jacobian matrix H is substituted into the IEKF model for filtering iteration, and when a certain number of iterations is reached or the overall convergence is reached, the optimized first pose is determined as the second pose.
[0057] Generally, if the extracted feature points are relatively accurate, the first pose after IEKF optimization is relatively accurate; if the extracted feature points are inaccurate, the optimized pose after IEKF optimization will have a large deviation. Therefore, after obtaining the second pose, it is necessary to judge the second pose to avoid a large error in the second pose, which will affect the stability of the positioning system in subsequent iterations and reduce the positioning accuracy. Specifically, as shown in step S150.
[0058] Step S150: determining the current posture of the movable device based on the first posture and the second posture.
[0059] In the embodiments of the present application, Figure 4 As shown, the method for determining the current position and posture of the movable device includes the following steps.
[0060] Step S410, determining the distance between the first pose and the second pose. In the embodiment of the present application, the distance includes Mahalanobis distance or Euclidean distance.
[0061] Step S420, determine whether the distance is less than or equal to a preset threshold. In the embodiment of the present application, the preset threshold is a manually set value. When the distance between the first pose and the second pose is less than or equal to the preset threshold, it indicates that the second pose is relatively accurate (that is, the gap between the prediction and the observation is within a reasonable range), and step S430 is executed to determine the second pose as the current pose. When the distance between the first pose and the second pose is greater than the preset threshold, it indicates that the second pose is inaccurate (that is, the gap between the prediction and the observation is too large, and the feature point error is too large), and step S430 is executed to determine the first pose as the current pose.
[0062] In an embodiment of the present application, the first data of the inertial measurement unit is used as a prediction to determine the first pose, the second data of the image acquisition unit is used as an observation, the first pose is optimized to obtain the second pose, and a tightly coupled optimization method is established through the first data and the second data. Compared with the loosely coupled method, the tightly coupled method realizes the mutual constraints between the sensor data, thereby improving the accuracy of fusion positioning. In addition, after obtaining the second pose, it is necessary to judge the second pose to avoid a large error in the second pose, which affects the stability of the positioning system in subsequent iterations and reduces the positioning accuracy. If the distance between the second pose and the first pose is greater than a preset threshold, it is determined that the error of the second pose is large, the second pose is discarded, and the first pose is used as the current pose. If the distance between the second pose and the first pose is less than or equal to the preset threshold, the second pose is more accurate, and the second pose is an optimized pose of the first pose, so the second pose is used as the current pose, thereby improving the positioning accuracy.
[0063] Figure 5 : is a flowchart of a method for determining the current position of a movable device provided by another embodiment of the present application. The method is applied to a movable device, and the movable device is equipped with an inertial measurement unit and an image acquisition unit. Optionally, the movable device includes an autonomous driving vehicle, an intelligent robot, an intelligent aircraft, etc. Figure 5 As shown, the method includes the following steps.
[0064] Step S510, determining an initial state vector of the movable device.
[0065] Step S520: acquiring data measured by the inertial measurement unit and data collected by the image acquisition unit, and performing time stamp matching on the data measured by the inertial measurement unit and the data collected by the image acquisition unit to obtain first data and second data.
[0066] Step S530: determining the first position of the movable device at the current moment based on the first data and the initial state vector.
[0067] Step S540: determining a second posture of the movable device at the current moment based on the first posture and the second data.
[0068] Step S550: determining the current posture of the movable device based on the first posture and the second posture.
[0069] In the embodiment of the present application, the method of steps S510-S550 is the same as the method of steps S110-S150, and will not be repeated here.
[0070] Step S560, putting the current posture, current time, and multiple feature points corresponding to the current time into the sliding window.
[0071] In the embodiment of the present application, the sliding window can be set according to the range of vehicle travel, and the sliding window is represented by a queue of fixed size, such as Figure 6 After determining the position and posture at any moment, the position and posture at that moment, the moment, and the multiple feature points corresponding to the moment are put into the sliding window. Therefore, the sliding window includes multiple historical position and postures arranged in time, the historical moments corresponding to the historical position and postures, and the multiple feature points corresponding to the historical moments.
[0072] In some embodiments, after the current posture, current time, and multiple feature points corresponding to the current time are placed in the sliding window, the loop is detected. It can be understood that in order to reduce the computing load of the computer, in the embodiment of the present application, the loop is detected at a fixed frequency (for example, 1s). If the current moment is a moment when the loop needs to be detected, it is judged that the current moment meets the detection condition; if the current moment is a moment when the loop does not need to be detected, it is judged that the current moment does not meet the detection condition.
[0073] Step S570, when the detection condition is met at the current moment, the historical postures are traversed in the sliding window, and the historical postures whose position difference with the current posture is within a preset threshold range are determined as the target historical postures, and a loop is constructed based on the target historical postures and the current posture.
[0074] In the embodiment of the present application, when the detection condition is met at the current moment, the historical posture is traversed in the sliding window to determine the position difference between the historical posture and the current posture. If the posture difference between the historical posture and the current posture is within a preset threshold range, the historical posture is determined as the target historical posture; then, a local map is generated based on multiple feature points of the target historical posture and multiple feature points of multiple historical postures located between the target historical posture and the current posture in the sliding window; then, the feature points of the current posture are matched with the local map, and the matching results are used as loop edges to form a loop. Figure 7 As shown, No. 3 is the historical pose of the target, No. 9 is the current pose, the feature points of multiple pose points between No. 3 and No. 9 are spliced to form a local map, and the feature points of the pose point No. 9 are spliced with the local map to obtain loop edges and construct a loop.
[0075] Step S580, using the factor graph optimization library to optimize and solve the loop to obtain the optimized current posture.
[0076] After the loop is constructed, the factor graph optimization library (for example, the Gtsam factor graph optimization library) is used to optimize and solve the loop, correct the accumulated error, and obtain the optimized current posture. Using the factor graph optimization library to optimize and solve the loop is a conventional method in the art and will not be repeated in the embodiments of this application.
[0077] In the embodiment of the present application, the accumulated error can be eliminated by the loop detection method, thereby further improving the accuracy of positioning.
[0078] Figure 8 is a structural block diagram of a positioning device provided in an embodiment of the present application. Figure 8 As shown, the positioning device 800 includes a determination module 810 and an acquisition module 820 .
[0079] The determination module 810 is used to determine an initial state vector of the movable device.
[0080] The acquisition module 820 is used to acquire the data collected by the inertial measurement unit and the data collected by the image acquisition unit, and perform timestamp matching to obtain first data and second data, where the first data includes acceleration and angular velocity at multiple moments, and the second data includes feature points corresponding to each of the multiple moments.
[0081] The determination module 810 is further used to determine the first position of the movable device at the current moment based on the first data and the initial state vector;
[0082] The determination module 810 is further configured to determine a second posture of the movable device at a current moment based on the first posture and the second data.
[0083] The determination module 810 is further configured to determine a current posture of the movable device based on the first posture and the second posture.
[0084] Optionally, the determination module 810 is also used to determine the distance between the first posture and the second posture; if the distance is less than or equal to a preset threshold, the second posture is determined as the current posture; if the distance is greater than the preset threshold, the first posture is determined as the current posture, wherein the distance includes Mahalanobis distance or Euclidean distance.
[0085] Optionally, the determination module 810 is also used to put the current posture, the current moment, and multiple feature points corresponding to the current moment into a sliding window, the sliding window including multiple historical postures arranged in chronological order, historical moments corresponding to the historical postures, and multiple feature points corresponding to the historical moments; when the detection condition is met at the current moment, the historical postures are traversed in the sliding window, and the historical postures whose position difference with the current posture is within a preset threshold range are determined as the target historical postures, and a loop is constructed based on the target historical posture and the current posture; the loop is optimized and solved using the factor graph optimization library to obtain the optimized current posture.
[0086] Optionally, the determination module 810 is also used to construct a residual observation equation based on the first pose, the coordinates of the feature point corresponding to the current moment in the first coordinate system, and the coordinates of the feature point corresponding to the current moment in the second coordinate system at the moment before the current moment, the first coordinate system corresponds to the coordinate system of the movable device at the current moment, and the second coordinate system corresponds to the coordinate system of the movable device at the initial moment; the residual observation equation is filtered and iterated to optimize the first pose, and when a certain number of iterations is reached or the overall convergence is achieved, the optimized first pose is determined as the second pose.
[0087] Optionally, the acquisition module 820 is also used to acquire data measured by a wheel speed meter, and timestamp match the data measured by the wheel speed meter with data measured by an inertial measurement unit to obtain third data; and use the third data to perform observation correction on the first posture.
[0088] Optionally, the determination module 810 is also used to determine the bias of the inertial measurement unit; based on the initial position, initial posture, initial velocity, acceleration and angular velocity at multiple moments in the first data, and the bias, determine the first posture of the movable device at the current moment.
[0089] Optionally, the determination module 810 is also used to determine multiple accelerations and multiple angular velocities of the inertial measurement unit in different frames after the movable device is started and the speed is zero; and determine the average value of the multiple accelerations and / or the average value of the multiple angular velocities as the bias of the inertial measurement unit.
[0090] The specific working principle and benefits of the positioning device provided in the embodiment of the present application are similar to the specific working principle and benefits of the positioning method provided in the embodiment of the present application, and will not be repeated here.
[0091] Below, reference Fig. 9 To describe an electronic device according to an embodiment of the present application. Fig. 9 It is a schematic diagram of the structure of an electronic device provided in one embodiment of the present application.
[0092] like Fig. 9 As shown, the electronic device 900 includes one or more processors 901 and a memory 902 .
[0093] The processor 901 may be a central processing unit (CPU) or other forms of processing units having data processing capabilities and / or instruction execution capabilities, and may control other components in the electronic device 900 to perform desired functions.
[0094] The memory 902 may include one or more computer program products, and the computer program product 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, a random access memory (RAM) and / or a cache memory (cache), etc. The non-volatile memory may include, for example, a read-only memory (ROM), a hard disk, a flash memory, etc. One or more computer program instructions may be stored on the computer-readable storage medium, and the processor 901 may run the program instructions to implement the positioning method of each embodiment of the present application described above and / or other desired functions. Various contents such as first data, second data, and third data may also be stored in the computer-readable storage medium.
[0095] In one example, the electronic device 900 may further include: an input device 903 and an output device 904 , and these components are interconnected via a bus system and / or other forms of connection mechanisms (not shown).
[0096] The input device 903 may include, for example, a keyboard, a mouse, etc.
[0097] The output device 904 can output various information to the outside, including the first posture, the second posture, the current posture, etc. The output device 904 can include, for example, a display, a speaker, a printer, a communication network and a remote output device connected thereto, and the like.
[0098] Of course, to simplify, Fig. 9Only some of the components related to the present application in the electronic device 900 are shown, and components such as a bus, an input / output interface, etc. are omitted. In addition, according to specific application situations, the electronic device 900 may also include any other appropriate components.
[0099] In addition to the above methods and devices, an embodiment of the present application may also be a computer program product, which includes computer program instructions, which, when executed by a processor, enable the processor to execute the steps of the positioning method according to various embodiments of the present application described above in this specification.
[0100] The computer program product may be written in any combination of one or more programming languages to write program codes for performing the operations of the embodiments of the present application, including object-oriented programming languages, such as Java, C++, etc., and conventional procedural programming languages, such as "C" language or similar programming languages. The program code may be executed entirely on the user computing device, partially on the user device, as an independent software package, partially on the user computing device and partially on a remote computing device, or entirely on a remote computing device or server.
[0101] In addition, an embodiment of the present application may also be a computer-readable storage medium having computer program instructions stored thereon, which, when executed by a processor, enables the processor to execute the steps of the positioning method according to various embodiments of the present application described above in this specification.
[0102] The computer readable storage medium can adopt any combination of one or more readable media. The readable medium can be a readable signal medium or a readable storage medium. The readable storage medium can include, for example, but is not limited to, a system, device or device of electricity, magnetism, light, electromagnetic, infrared, or semiconductor, or any combination of the above. More specific examples (non-exhaustive list) of readable storage media include: an electrical connection with one or more wires, a portable disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above.
[0103] The basic principles of the present application are described above in conjunction with specific embodiments. However, it should be noted that the advantages, strengths, effects, etc. mentioned in the present application are only examples and not limitations, and it cannot be considered that these advantages, strengths, effects, etc. are required by each embodiment of the present application. In addition, the specific details disclosed above are only for the purpose of illustration and ease of understanding, not for limitation, and the above details do not limit the present application to being implemented by adopting the above specific details.
[0104] The block diagrams of the devices, apparatuses, equipment, and systems involved in this application are only illustrative examples and are not intended to require or imply that they must be connected, arranged, and configured in the manner shown in the block diagram. As will be appreciated by those skilled in the art, these devices, apparatuses, equipment, and systems can be connected, arranged, and configured in any manner. Words such as "including", "comprising", "having", etc. are open words, referring to "including but not limited to", and can be used interchangeably with them. The words "or" and "and" used here refer to the words "and / or" and can be used interchangeably with them, unless the context clearly indicates otherwise. The words "such as" used here refer to the phrase "such as but not limited to", and can be used interchangeably with them.
[0105] It should also be noted that in the apparatus, device and method of the present application, each component or each step can be decomposed and / or recombined. Such decomposition and / or recombination should be regarded as equivalent solutions of the present application.
[0106] The above description of the disclosed aspects is provided to enable any person skilled in the art to make or use the present application. Various modifications to these aspects will be readily apparent to those skilled in the art, and the general principles defined herein may be applied to other aspects without departing from the scope of the present application. Therefore, the present application is not intended to be limited to the aspects shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.
[0107] The above description has been given for the purpose of illustration and description. In addition, this description is not intended to limit the embodiments of the present application to the forms disclosed herein. Although multiple example aspects and embodiments have been discussed above, those skilled in the art will recognize certain variations, modifications, changes, additions and sub-combinations thereof.
Claims
1. A positioning method, characterized in that: Applied to a movable device, the movable device is equipped with an inertial measurement unit and an image acquisition unit, and the method comprises: Determining an initial state vector of the movable device; Acquire data measured by the inertial measurement unit and data collected by the image acquisition unit, and perform timestamp matching on the data measured by the inertial measurement unit and the data collected by the image acquisition unit to obtain first data and second data, wherein the first data includes acceleration and angular velocity at multiple moments, and the second data includes feature points corresponding to each of the multiple moments; Determine a first position of the movable device at a current moment based on the first data and the initial state vector; Determine a second posture of the movable device at the current moment based on the first posture and the second data; Based on the first pose and the second pose, a current pose of the movable device is determined.
2. The method according to claim 1, characterized in that The determining, based on the first posture and the second posture, a current posture of the movable device includes: determining a distance between the first pose and the second pose; If the distance is less than or equal to a preset threshold, determining the second posture as the current posture; If the distance is greater than the preset threshold, determining the first posture as the current posture; The distance includes Mahalanobis distance or Euclidean distance.
3. The method according to claim 1, characterized in that After determining the current posture of the movable device based on the first posture and the second posture, the method further includes: Putting the current posture, the current moment, and the feature point corresponding to the current moment into a sliding window, wherein the sliding window includes a plurality of historical postures arranged in chronological order, the historical moments corresponding to the historical postures, and the feature points corresponding to the historical moments; When the detection condition is met at the current moment, the historical postures are traversed in the sliding window, a historical posture whose position difference with the current posture is within a preset threshold range is determined as a target historical posture, and a loop is constructed according to the target historical posture and the current posture; The loop is optimized and solved using a factor graph optimization library to obtain an optimized current posture.
4. The method according to claim 1, characterized in that: The determining, based on the first posture and the second data, a second posture of the movable device at the current moment, comprises: Constructing a residual observation equation based on the first posture, the coordinates of the feature point corresponding to the current moment in a first coordinate system, and the coordinates of the feature point corresponding to the current moment in a second coordinate system at a moment before the current moment, wherein the first coordinate system corresponds to the coordinate system of the movable device at the current moment, and the second coordinate system corresponds to the coordinate system of the movable device at an initial moment; The residual observation equation is filtered and iterated to optimize the first posture, and when a certain number of iterations is reached or the overall convergence is reached, the optimized first posture is determined as the second posture.
5. The method according to claim 1, characterized in that: Before determining the second posture of the movable device at the current moment based on the first posture and the second data, the method further includes: Acquire data measured by a wheel speed meter loaded on the movable device, and perform timestamp matching between the data measured by the wheel speed meter and the data measured by the inertial measurement unit to obtain third data; The first posture is observed and corrected using the third data.
6. The method according to claim 1, characterized in that The initial state vector includes at least one of an initial position, an initial posture, and an initial speed of the movable device; and determining the first posture of the movable device at the current moment based on the first data and the initial state vector includes: determining a bias of the inertial measurement unit; The first posture of the movable device at the current moment is determined based on the initial position, the initial posture, the initial velocity, the accelerations and angular velocities at the plurality of moments in the first data, and the bias.
7. The method according to claim 1, characterized in that The determining a bias of the inertial measurement unit comprises: After the movable device is started and the speed is zero, determining a plurality of accelerations and a plurality of angular velocities of the inertial measurement unit in different frames; An average value of the plurality of accelerations and / or an average value of the plurality of angular velocities is determined as a bias of the inertial measurement unit.
8. A positioning device, characterized in that: Applied to a movable device, the movable device is equipped with an inertial measurement unit and an image acquisition unit, and the positioning device includes: A determination module, used to determine an initial state vector of the movable device; an acquisition module, configured to acquire the data collected by the inertial measurement unit and the data collected by the image acquisition unit, and perform timestamp matching to obtain first data and second data, wherein the first data includes acceleration and angular velocity at multiple moments, and the second data includes feature points corresponding to each of the multiple moments; The determination module is further used to determine the first position of the movable device at the current moment based on the first data and the initial state vector; The determination module is further used to determine a second posture of the movable device at the current moment based on the first posture and the second data; The determination module is further configured to determine a current posture of the movable device based on the first posture and the second posture.
9. An electronic device, characterized in that: include: processor; A memory, wherein the memory is connected to the processor, and the memory is used to store a computer program, and when the computer program is executed by the processor, the positioning method according to any one of claims 1 to 7 is implemented.
10. A storage medium, characterized in that: The storage medium stores a computer program, and when the computer program is executed by the processor, the positioning method according to any one of claims 1 to 7 is implemented.