Target object pose determination method and device, electronic equipment and storage medium

Abstract

The invention relates to the technical field of intelligent navigation, and provides a target object pose determination method and device, electronic equipment and a storage medium. The method comprises the following steps: acquiring pose data acquired by at least two pose estimation sensors of a target object; determining an initial pose result of the target object according to the pose data acquired by the at least two pose estimation sensors; target working conditions of the target object are determined, to-be-adjusted parameters of the initial pose result are determined based on the target working conditions, and different target working conditions correspond to different to-be-adjusted parameters; and adjusting the initial pose result based on the to-be-adjusted parameter to obtain a target pose result. The problem that the navigation pose result is not accurate enough is solved.

Description

Technical Field

[0001] The present application relates to the field of intelligent navigation technology, and in particular to a method, device, electronic device and storage medium for determining the position and posture of a target object. Background Art

[0002] Traditional navigation algorithms based on multimodal data such as vision, inertial, satellite navigation, and wheel odometers are mainly represented by simultaneous localization and mapping (SLAM) of vision, which solves the optimal fusion pose result through graph optimization or filtering algorithms. The complementarity of multimodal sensors can improve the robustness and accuracy of the positioning system. In traditional multi-source fusion positioning algorithms, time synchronization and spatial calibration are required for different sensors to ensure the synchronization and availability of multi-source data. However, in the application of existing navigation technologies, different sensors are affected by different environments, resulting in inaccurate navigation pose results. Summary of the invention

[0003] In view of this, the present application provides a method, device, electronic device and storage medium for determining the position and posture of a target object to solve the problem of inaccurate position and posture results in the prior art.

[0004] In a first aspect of the present application, a method for determining a target object's posture is provided, comprising: obtaining posture data acquired by at least two posture estimation sensors of the target object; determining an initial posture result of the target object based on the posture data acquired by the at least two posture estimation sensors; determining a target operating condition of the target object, and determining parameters to be adjusted for the initial posture result based on the target operating condition, different target operating conditions corresponding to different parameters to be adjusted; and adjusting the initial posture result based on the parameters to be adjusted to obtain a target posture result.

[0005] In a second aspect of the present application, a device for determining a target object's posture is provided, comprising: an acquisition module, configured to acquire posture data acquired by at least two posture estimation sensors of the target object; an initial posture determination module, configured to determine an initial posture result of the target object based on the posture data acquired by at least two posture estimation sensors; an optimization module, configured to determine a target operating condition of the target object, and determine parameters to be adjusted for the initial posture result based on the target operating condition, wherein different target operating conditions correspond to different parameters to be adjusted; and a target posture determination module, configured to adjust the initial posture result based on the parameters to be adjusted to obtain a target posture result.

[0006] According to a third aspect of the present application, an electronic device is provided, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the above method when executing the computer program.

[0007] According to a fourth aspect of the present application, a storage medium is provided, which stores a computer program, and when the computer program is executed by a processor, the steps of the above method are implemented.

[0008] At least one of the above technical solutions adopted in this application can achieve the following beneficial effects:

[0009] The initial pose result of the target object is determined by acquiring pose data acquired by at least two pose estimation sensors of the target object, and based on the pose data acquired by the at least two pose estimation sensors, the confidence levels of different pose estimation sensors may be different under working conditions corresponding to different scenes. Therefore, using the same initial pose result estimation method may not achieve the most accurate pose estimation. Therefore, the target working condition of the target object is further determined, and the parameters to be adjusted in the initial pose result are determined based on the target working condition. Different target working conditions correspond to different parameters to be adjusted. After determining the parameters to be adjusted, the initial pose result is optimized according to the parameters to be adjusted. In this way, the initial pose result can be dynamically adjusted according to the scene where the target object is located to obtain an accurate target pose result. In this way, a preliminary pose estimation is performed on the pose data obtained by each pose estimation sensor to obtain an initial pose result. Further, based on the target working condition corresponding to the scene in which the target object is located, the pose data with high confidence under the target working condition or the pose data selected based on other methods can be selected to further adjust the initial pose result, and the final accurate target pose result is determined in combination with the initial pose result before adjustment. This solves the problem that different scenes have different impacts on different pose sensors and improves the accuracy of the pose results. BRIEF DESCRIPTION OF THE DRAWINGS

[0010] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0011] Figure 1 It is a flow chart of a method for determining the position and posture of a target object provided in an embodiment of the present application;

[0012] Figure 2 It is a flowchart of another method for determining the position and posture of a target object provided in an embodiment of the present application;

[0013] Figure 3 It is a structural schematic diagram of a device for determining the position and posture of a target object provided in an embodiment of the present application;

[0014] Figure 4 It is a structural schematic diagram of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0015] In the following description, specific details such as specific system structures, technologies, etc. are provided for the purpose of illustration rather than limitation, so as to provide a thorough understanding of the embodiments of the present application. However, it should be clear to those skilled in the art that the present application may also be implemented in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted to prevent unnecessary details from obstructing the description of the present application.

[0016] Integrated navigation can be understood as a navigation method that uses a variety of navigation technologies in combination. It improves the accuracy and reliability of the navigation system through the fusion technology of multimodal data. It usually combines different navigation methods such as the Global Navigation Satellite System (GNSS) and the Inertial Navigation System (INS) to overcome the limitations of a single navigation system.

[0017] In the traditional combined navigation application process, the navigation algorithm is usually based on multimodal data such as vision, inertia, satellite navigation, wheel odometer, etc., mainly represented by visual SLAM technology, and the optimal pose result after fusion is calculated through graph optimization or filtering algorithm. The complementarity of multimodal sensors can improve the robustness and accuracy of the positioning system. In the traditional multi-source fusion positioning algorithm, time synchronization and spatial calibration are required for different sensors to ensure the synchronization and availability of multi-source data. However, there are the following technical problems in the application of traditional combined navigation algorithms:

[0018] First, multimodal data requires strict time synchronization before use, and the calibration and transformation of spatial relationships between multi-sensor coordinate systems are complex;

[0019] Second, the visual SLAM algorithm has high requirements for the frequency of video data. When using it, the two frames of images need to have a common view relationship, which leads to a large amount of algorithm calculation and high requirements for computing power.

[0020] Third, different environments have different impacts on different sensors. When the algorithm uses different sensors, it is difficult to dynamically adjust the confidence of each sensor under different conditions.

[0021] In summary, the existing technical methods have the problem that multimodal data requires strict time synchronization and spatial calibration, has high requirements for video data and computing power, the sensors are affected by the environment, and it is difficult to dynamically adjust the confidence of each sensor, resulting in inaccurate final navigation posture results.

[0022] Therefore, the present application proposes a method for determining the position and posture of a target object, by calibrating each sensor in time and space, processing and predicting the data of each sensor through Kalman filtering to obtain an initial position and posture result, and adjusting the initial position and posture result by identifying the scene conditions to obtain the final position and posture result.

[0023] A method and device for determining the position and posture of a target object according to an embodiment of the present application will be described in detail below with reference to the accompanying drawings.

[0024] Figure 1 It is a flow chart of a method for determining the position and posture of a target object provided in an embodiment of the present application.

[0025] like Figure 1 As shown, the method for determining the position and posture of the target object includes:

[0026] S101, obtaining posture data acquired by at least two posture estimation sensors of a target object.

[0027] First of all, it should be understood that combined navigation technology can be applied to scenarios such as aviation, aerospace, navigation, drones, robots, etc. In the field of autonomous driving, vehicle-mounted combined navigation is also widely used in passenger cars and commercial vehicles, which will not be expanded here.

[0028] Therefore, the target object may be aviation, aerospace, navigation, drones, robots, vehicles, etc. To facilitate the understanding of the implementation of the solution, the embodiment of this solution is explained below with the vehicle as the target object.

[0029] Furthermore, the pose estimation sensor can be understood as a sensor used to obtain pose data and determine the position and pose of the target object in three-dimensional space. In this solution, the pose estimation sensor may include a monocular camera, an inertial measurement unit (IMU), GNSS, and a wheel odometer, etc., which are used to obtain pose data and use the pose data for subsequent pose result calculations. Among them, the IMU can measure the three-axis acceleration of the target object {a t} and the three-axis angular velocity {ω t}, GNSS can measure satellite navigation data {GNSS t}, the wheel speed odometer can obtain the wheel speed odometer data {Odo t}, etc., and then each pose data is used to determine the initial pose result, where t represents the time stamp.

[0030] In the specific implementation process, each pose estimation sensor can be installed on a platform, such as a vehicle. Since the timestamps of data collected by different sensors may not be consistent, for example, IMU data may be collected at a high frequency (such as 100 Hz or higher), while GNSS usually outputs data at a lower frequency (such as 1 Hz or 10 Hz). Therefore, in order to make these sensors work together, these sensor data can be connected to the same processing unit to align their timestamps, thereby achieving time synchronization of each pose data and ensuring that valid data from all pose estimation sensors can be obtained at the same time point.

[0031] Furthermore, each pose estimation sensor may be installed at a different position and orientation on the platform, that is, different sensors may have position offsets and angle deviations at different times. For example, the GNSS is on the top of the platform and the wheel odometer is installed on the wheel. Therefore, each pose estimation sensor can be spatially calibrated, and the pose data can be unified into a coordinate system, which can be called the reference coordinate system. In this way, all pose data are transferred to the same coordinate system for fusion. Of course, the above-mentioned spatial calibration process can also be performed in the above-mentioned data processing unit, so that the requirements for strict time synchronization and spatial calibration of multimodal data can be met.

[0032] S102: Determine an initial pose result of the target object according to pose data acquired by at least two pose estimation sensors.

[0033] Specifically, after acquiring the pose data and calibrating it in time and space, the initial pose result of the target object can be determined based on the pose data. In the process of determining the initial pose result, the Kalman filter can be used to process each pose data to obtain a more accurate initial pose result.

[0034] Specifically, the three-axis acceleration {a t}、Three-axis angular velocity {ω t}data and satellite navigation data{G t} for fusion.

[0035] First, construct the system state equation, and determine the Kalman filter continuous time error state equation.

[0036]

[0037] Where t can be expressed as time, F(t) can represent the transfer matrix of the system error state, G(t) can represent the system noise driving matrix, ω(t) can represent the system noise, δx(t) can represent the system error state vector at time t, and δx(t) includes the navigation state error and the sensor error, which can be expressed as:

[0038]

[0039] Among them, δr n It can be expressed as the position error vector, δv n can represent the velocity error vector, φ can represent the attitude error vector, and b g , b a can represent the angular gyro and accelerometer bias error vectors, s g 、s a They can be expressed as the scale factor errors of the gyroscope and accelerometer, respectively, where the gyroscope and accelerometer are both included in the inertial measurement unit.

[0040] Among them, the system dynamic matrix F(t) is expressed as:

[0041]

[0042] It describes the evolution of the system state over time, including the dynamic relationship between position, velocity, attitude and sensor error. It also includes the dynamic relationship between the error terms of each state.

[0043] The noise coefficient matrix G(t) can be expressed as:

[0044]

[0045] ω(t) white noise is the system noise, which can be expressed as:

[0046]

[0047] In the actual implementation process, the state equation can also be discretized so that at discrete time steps t k The discretized system state equation is:

[0048] δx k =Φ k / k-1 δx k +ω k-1

[0049] In the formula

[0050] The measurement equation is:

[0051] z k =H k δx k +v k

[0052] Where z k It can be t k The observation vector at time, H k can be the observation matrix, vk Can be used to measure noise. k and v k is an uncorrelated zero-mean white noise sequence, that is, ω k ~N(0,Q k ), v k ~N(0,R k ), Q k is the system state noise variance matrix, R k is the measurement noise variance matrix.

[0053] The Kalman filter prediction update process is:

[0054]

[0055] P k / k-1 It can represent the current time t k The predicted value of the covariance matrix of the state optimal estimate, P k-1 t k-1 The optimal estimate of the covariance matrix at that moment.

[0056] The Kalman filter measurement update process is:

[0057]

[0058] use Compensate for the inertial position information and calculate the position result of the combined navigation {KF t}.

[0059]

[0060] Where x INS,t is the current time t k Using the three-axis acceleration {a t} and the three-axis angular velocity {ω t The above steps are the principle of Kalman filtering and will not be explained in detail here.

[0061] In this way, the pose data can be fused through the above-mentioned Kalman filter to obtain the pose result of the combined navigation, that is, the initial pose result. However, since the pose data obtained by the pose estimation sensor may be affected by the environment, the initial pose result is not accurate. Therefore, the initial pose result can be adjusted based on the pose data obtained by the pose estimation sensor with higher confidence under the current target condition based on the scene condition to obtain an accurate final pose result.

[0062] S103, determining the target working condition of the target object, and determining the parameters to be adjusted of the initial posture result based on the target working condition, where different target working conditions correspond to different parameters to be adjusted.

[0063] Specifically, the confidence levels of different pose sensors of the target object under different working conditions are different. For example, when the vehicle is driving in a straight line, it exhibits strong inertia at the trajectory level. At this time, the confidence level of the satellite navigation data is higher than that of other pose sensors such as the wheel speed odometer data. Therefore, it is suitable to optimize the initial pose result by predicting the pose at the next moment. Another example is that the vehicle has many turns in working conditions such as a disk or basement. At this time, the confidence level of the inertial measurement unit is higher than that of other pose sensors. Therefore, the wheel speed kinematic model is suitable to calculate the heading angle change and the current heading angle, and the current heading angle is used to optimize the initial pose result.

[0064] Therefore, after determining the initial pose result, the current target working condition of the target object can be determined. The target working condition can represent the scene working condition corresponding to the working scene of the target object, or the system working state of the target object. There is no limitation here. This scheme takes the scene working condition as an example. After determining the target working condition of the target object, the parameters to be adjusted of the initial pose result can be determined based on the target working condition, which will not be expanded here.

[0065] S104, adjusting the initial posture result based on the parameter to be adjusted to obtain a target posture result.

[0066] Specifically, after determining the parameters to be adjusted, a suitable optimization method can be selected based on the characteristics of the parameters to be adjusted to adjust the initial pose result to obtain the target pose result.

[0067] According to the technical solution provided in the above embodiment, the posture data obtained by multiple posture estimation sensors can be effectively integrated, and appropriate parameters to be adjusted can be selected according to different target working conditions to adjust the initial posture result to obtain an accurate and reliable target posture result. This solves the problem in the prior art that sensors are affected by the environment, making it difficult to dynamically adjust the confidence of each sensor, resulting in inaccurate posture results.

[0068] In some embodiments, determining the target working condition of the target object includes: acquiring video data captured by a shooting device of the target object, performing feature extraction on the current video data to obtain target video features; determining the scene in which the target object is located according to the target video features, and determining the target working condition of the target object based on the scene in which the target object is located.

[0069] Specifically, the video data of the environment described by the current target object can be obtained through a shooting device, and the shooting device can be a monocular camera, a binocular camera, a wide-angle camera, etc., which is not limited here, as long as it can provide environmental information and perform accurate feature extraction. It should be understood that the shooting device can also be regarded as a posture sensor. Therefore, after the shooting device obtains the video data, the video data can be transferred to the same output processing unit for processing, but it is not necessary to strictly time-calibrate the video data with other posture data, so that the video data can reflect the scene of the target object in a timely manner. However, for the accuracy of the result, it can be strictly synchronized with other posture data during spatial calibration.

[0070] Furthermore, in order to solve the problem of high requirements on video data and computing power in the prior art, low-frequency monocular video data can be obtained. t}, and extract HOG features, SIFT features and LBP features from them. The HOG feature is Histogram of Oriented Gradients, which can be used to capture the shape information of the object and describe the edge and gradient direction, denoted as {HOG t,i}; SIFT feature is Scale-Invariant Feature Transform, which can capture the key points in the video frame and is used to identify stable feature points, denoted as {SIFT t,j}; LBP features are Local Binary Patterns, which can capture texture information and are used to identify surface texture differences, denoted as {LBP t,k}.

[0071] Next, the three extracted features can be serially fused to obtain the final feature (i.e., the target video feature) and recorded as {Feature t,m The feature data is used to train the support vector machine algorithm (SVM) with different kernel functions, and then soft voting is performed based on the classification probability to determine the final classification result of the scene where the target object is located {DrivingScene t}, and then the target working condition of the target object can be determined according to the scene in which the target object is located.

[0072] Specifically, HOG features are extracted from low-frequency monocular video data, and Gamma correction is used to grayscale the image:

[0073] Gray=0.3*R+0.59*G+0.11*B

[0074] I(x,y)=I(x,y) gamma

[0075] In the formula, R, G, and B represent the values ​​of the red, green, and blue color channels of each pixel in the image, respectively. In the grayscale processing, the weights assigned to red light, green light, and blue light are: 0.3, 0.59, and 0.11, respectively. After weight distribution, the final grayscale value Gray is obtained. I(x, y) represents the grayscale value at the coordinate (x, y) in the image. In image feature extraction, x and y represent the pixel coordinates on the image plane, which can be understood as x is the horizontal coordinate of the pixel, y is the vertical coordinate of the pixel, and gamma is the compression coefficient. By adjusting this coefficient, the purpose of adjusting the image contrast, suppressing noise interference, and reducing the impact of light changes can be achieved. Then calculate the gradient value, gradient amplitude, and gradient direction of each pixel according to the formula:

[0076] G x (x,y)=I(x+1,y)-I(x-1,y)

[0077] G y (x,y)=I(x,y+1)-I(x,y-1)

[0078]

[0079] Where I(x+1,y) represents the pixel value at position (x+1,y) in the image, I(x-1,y) represents the pixel value at position (x-1,y) in the image, I(x,y+1) represents the pixel value at position (x,y+1) in the image, I(x,y-1) represents the pixel value at position (x,y-1) in the image, and G x (x,y) and G y (x, y) are the horizontal and vertical gradient values ​​of the image at the coordinate (x, y), G(x, y) is the gradient amplitude, α(x, y) is the gradient direction, and the final HOG feature {HOG t,i}, t is the time, i is the dimension of HOG feature.

[0080] To extract SIFT features from low-frequency monocular video data, we can obtain them by using Gaussian difference pyramids with different σ:

[0081]

[0082] This formula represents a two-dimensional Gaussian function, which is used to generate Gaussian blur. G1(x, y) represents the value of the pixel value at point (x, y) after Gaussian blur. σ represents the standard deviation of the Gaussian function. Represents the kernel of the Gaussian function, finds the extreme point in the Gaussian difference pyramid space, and obtains the SIFT feature of the current moment through the descriptor of the key point {SIFT t,j}, t is the time, j is the dimension of SIFT features.

[0083] To extract LBP features from low-frequency monocular video data, we can set P sampling points in a circular area with a radius of R. The coordinate calculation formula of each sampling point is:

[0084]

[0085] Where P is the total number of sampling points, p represents the pth sampling point of the total sampling points, and the LBP feature {LBP t,k}, t is time, k is the dimension of LBP feature.

[0086] Afterwards, the three extracted features can be fused to obtain the final {Feature t,m}.

[0087] Feature t =[HOG t,1 ,HOG t,2 ,...,HOG t,i ,0,...,0] 1×m +[0,...,0,SIFT t,1 ,SIFT t,2 ,...,SIFT t,j ,0,...,0] 1×m +[0,...,0,LBP t,1 ,LBP t,2 ,...,LBP t,k ] 1×m

[0088] Wherein, t is time, i, j, and k are the dimensions of the three features respectively, and m is the dimension of the Feature obtained after serializing the features, i.e., the target video feature, m=i+j+k.

[0089] After obtaining the target video features, the scene where the target object is located can be judged and the target working condition can be determined.

[0090] It should be understood that after determining the scene where the target object is located, the target working condition corresponding to the scene where the current target object is located can be obtained by the correspondence between the scene and the working condition. For example, the basement scene corresponds to the working condition with more turns and less straight driving, and the high-speed scene corresponds to the working condition with less turns and more straight driving. Of course, the determination of the target working condition can also be obtained directly by analyzing the target video features without scene analysis, which is not limited here.

[0091] According to the technical solution provided in the above embodiment, features of different dimensions can be extracted from low-frequency video data, and the multi-dimensional features can be fused to obtain target video features. The scene in which the target object is located can be determined based on the target video features, and then the target working condition of the target object can be determined based on the scene in which the target object is located. This can solve the problem of high frequency requirements for video data in the prior art.

[0092] In some embodiments, determining the scene in which the target object is located based on the target video features includes: performing scene prediction on the target video features respectively through different scene prediction models to obtain at least one predicted scene output by each scene prediction model, and an initial probability corresponding to each predicted scene; determining the target probability corresponding to each predicted scene based on the initial probability of each predicted scene in the different scene prediction models; and determining the scene in which the target object is located based on the target probability corresponding to each predicted scene.

[0093] Specifically, the scene prediction model can be used to predict the target video features, that is, the SVM algorithm with different kernel functions, and the training feature {Feature t,m}, and used for real-time scene prediction, soft voting based on classification probability, to determine the final classification result of the scene {DrivingScene t}.

[0094] Real scene classification problems are usually difficult to classify using linear space. To solve nonlinear classification problems, we can find the classification hyperplane in high-dimensional space through mapping. The expression of the classification hyperplane is:

[0095] f(x)=ω T φ(x)+b

[0096] Where φ(x) can represent the function used for feature mapping, the feature vector of sample x in the new space, and ω and b can represent the hyperplane parameters. At this time, the problem to be optimized is:

[0097]

[0098] Where y n is the sample classification, and n is the number of samples used for training. In solving the above optimization problem, it is necessary to calculate the sample x i and x j The inner product on the new space, φ(x i ) T φ(x j ). To simplify the calculation, the kernel function is introduced:

[0099] K(x i ,x j )=φ(x i )T φ(x j )

[0100] Commonly used kernel functions include polynomial kernel and Gaussian kernel. The polynomial kernel formula is: K(x i ,x j )=(x i T x j +1) d , where d≥1 is the degree of the polynomial.

[0101] Its Gaussian kernel formula is:

[0102]

[0103] The target video features can be processed through different scene prediction models, i.e. different kernel functions, to obtain the classification results and probabilities of different kernel functions, i.e. the obtained prediction scenes and the initial probabilities corresponding to each prediction scene. The initial probabilities are used as weights for soft voting to determine the final scene category of the video data at time t:

[0104]

[0105]

[0106] In the formula, the prediction category is {c1,c1,...,c s}, s is the number of scene categories, f is the number of SVM models with different kernel functions, It represents the probability that the qth SVM model judges it as scene s. The target probability corresponding to each predicted scene is obtained after weighted averaging. At this time, the class with the largest target probability can be regarded as the final scene category, that is, the scene where the target object is located.

[0107] According to the technical solution provided in the above embodiment, the scene in which the target object is located can be predicted by SVM algorithms with different kernel functions, the scene in which the target object is located can be obtained, and the target working condition of the target object can be judged based on the scene in which the target object is located. Then, based on the target working condition, the parameters to be adjusted can be determined to optimize the initial posture result.

[0108] In other embodiments, determining the parameters to be adjusted of the initial posture result based on the target working condition includes: obtaining the correspondence between the preset working condition and the parameters; matching the target working condition with the correspondence between the preset working condition and the parameters, and determining the parameters to be adjusted corresponding to the target working condition.

[0109] Specifically, the target working condition of the target object can be judged by the correspondence between the preset working condition and the parameters, and the corresponding parameters to be adjusted can be determined based on the target working condition. That is, an optimization approach can be selected to adjust the initial posture result under different working conditions. Of course, this does not mean that the parameters to be adjusted are inaccurate.

[0110] Among them, the parameters to be adjusted can be understood as parameters that can be used to optimize the initial posture results under the target working conditions. For example, under working conditions with many turns and slow speeds, the confidence of the predicted results of the heading angle can be appropriately increased. Therefore, the initial posture can be optimized by the heading angle. At this time, the heading angle is the parameter to be adjusted. Of course, it should be noted that the parameters to be adjusted can also be regarded as specific parameters that need to be corrected. For example, under certain working conditions, the errors of certain posture sensors are large, that is, the accuracy of the posture data is not enough, so the corresponding algorithm can be set to optimize the posture data, which will not be explained in detail here.

[0111] Furthermore, the correspondence between the preset working conditions and parameters can be obtained based on the summary of historical data and scene analysis, or through algorithm training or manual definition, where the working conditions can be actual scenes or scene characteristics. For example, if the scene is a basement, the working conditions are many turns and slow speed. At this time, the corresponding parameter to be adjusted can be the heading angle. Therefore, when the current scene is identified as a basement through the target video features, the current working conditions and the parameters to be adjusted can be automatically obtained. Of course, in the specific application process, the scene can be not classified, but the working conditions of the target object can be directly identified, and the parameters to be adjusted can be determined based on the corresponding working conditions.

[0112] It should also be noted that, in actual application, the parameters to be adjusted under the target working conditions can also be analyzed through algorithms, which is not limited to the correspondence between the preset working conditions and the parameters, and is not limited here.

[0113] According to the technical solution provided in the above embodiment, the corresponding parameters to be adjusted under the target working condition can be determined through the correspondence between the preset working condition and the parameters. Through the predefined mapping rules, the adjustment requirements under different working conditions can be dynamically adapted, thereby optimizing the accuracy of the posture results of navigation positioning.

[0114] In some embodiments, the initial pose result is adjusted based on the parameter to be adjusted to obtain a target pose result, including: calculating the target parameter corresponding to the parameter to be adjusted, and adjusting the initial pose result based on the target parameter to obtain an adjusted pose result; constraining the target pose result according to the adjusted pose result and the initial pose result to obtain the target pose result.

[0115] Specifically, the process of adjusting the initial pose result to obtain the target pose result can include calculating the target parameters corresponding to the parameters to be adjusted, that is, by using the target parameters that are more suitable for the target working conditions, and integrating the target parameters into the initial pose result after obtaining the target parameters to obtain the adjusted pose result, that is, the adjusted pose result. The adjusted pose result and the initial pose result can be further used to constrain the target pose result to obtain the target pose result. In actual application, the parameters to be adjusted can include all pose data related to the pose result, such as position information, heading angle, speed, etc. According to different working conditions, the corresponding pose estimation sensor can be selected to regard it as a pose estimation sensor with high confidence, and the initial pose result is optimized using the pose data obtained by the pose estimation sensor with high confidence, which can be understood as increasing the weight of the corresponding pose data in the initial pose result.

[0116] It should be understood that the high confidence mentioned here does not mean a specific confidence level, but rather a posture estimation sensor that is determined based on the adaptability of the working characteristics of the target object under different working conditions, and it is considered to have a high confidence level. For example, in a scenario with many turns, the accuracy of calculating the heading angle through the wheel speed kinematic model when predicting the posture is higher than the accuracy of predicting the position information at the next moment. In this case, the confidence level of the wheel speed odometer can be considered to be higher than the confidence level of the satellite navigation data. Similarly, there is not a limit to one posture estimation sensor with high confidence, depending on the specific situation. For example, assuming that the posture results include speed, position, heading angle, etc., and the confidence level of the speed and position is high and the prediction results are accurate under the corresponding working conditions, then after the initial posture results are obtained based on the Kalman filter, the initial posture results can be optimized based on the speed and position. There is no limitation here.

[0117] After obtaining the target parameters, the initial pose result can be adjusted based on the target parameters to obtain the adjusted pose result. The adjusted pose result and the initial pose result can be used to constrain the target pose result to find a balance between the initial pose result and the adjusted pose result to ensure the accuracy and rationality of the target pose result.

[0118] According to the technical solution provided in the above embodiment, the target parameters corresponding to the parameters to be adjusted can be calculated, and the initial pose results can be adjusted based on the target parameters. Then, the constraint method can be used to balance the adjusted pose results and the initial pose results to ensure that the final target pose results achieve the best effect between accuracy and stability.

[0119] In some embodiments, the parameter to be adjusted includes a heading angle, a target parameter corresponding to the parameter to be adjusted is calculated, and based on the target parameter, the initial posture result is adjusted to obtain an adjusted posture result, including: calculating the current heading angle of the target object according to the wheel speed odometer data of the target object, the current heading angle represents the target parameter, and the posture data includes the wheel speed odometer data; replacing the heading angle in the initial posture result with the current heading angle to obtain an updated initial posture result; calculating the posture transformation matrix of the target object according to the updated initial posture result, and using the posture transformation matrix to determine the adjusted posture result corresponding to the current heading angle.

[0120] Specifically, when the target working condition is a situation with many turns and a large difference in left and right wheel speeds, the heading angle change can be calculated using the wheel speed kinematic model, and the current heading angle of the target object can be calculated using the heading angle change. The initial posture result can be optimized based on the current heading angle to obtain the adjusted posture result.

[0121] Specifically, when the vehicle is turning, the angular velocity of the rear wheel trajectory is the same. Without considering the sensor error, according to the wheel speed kinematic model, when the vehicle turns left, the travel distances of the left and right wheels are respectively:

[0122]

[0123]

[0124] Where v r and v l are the wheel speeds of the right and left rear wheels of the vehicle, respectively, ΔYaw t is the heading angle change at discrete time t, r is the turning radius of the rear wheel center point during the turning process, and l is the distance between the rear wheels of the vehicle. The heading angle change can be calculated by the rear wheel speed:

[0125]

[0126] According to the final heading angle {Yaw t-1}, the current heading angle {OdoYaw t}:

[0127] {OdoYaw t}={Yaw t-1}+ΔYaw t

[0128] Furthermore, the initial pose result {KF t} and the current heading angle {OdoYaw t} for fusion.

[0129] Specifically, the initial pose result {KF t} is transformed into Euler angles, expressed as The current heading angle {OdoYaw t} for Euler angles Replace the updated initial pose result:

[0130] Δθ b(t-1)b(t) =[φ θ OdoYaw t ],

[0131] When the heading changes to a small angle, the attitude transformation matrix at time t relative to time t-1 can be calculated by the wheel speed The calculation can be simplified:

[0132]

[0133] The same algorithm of combined navigation is used to output the position and velocity at time t, that is, by calculating the initial pose result combined with the attitude transformation matrix, the adjusted pose result corresponding to the heading angle is calculated and recorded as {OdoAVP t}.

[0134] Furthermore, the adjusted pose result corresponding to the heading angle and the initial pose result can be compared with the target pose result {AVP t}Constraints:

[0135]

[0136] Where {OdoAVP t}, {KF t} and {AVP t} are all the estimated attitude, speed, and position at time t, that is They are pitch angle, roll angle, heading angle, east speed, north speed, sky speed, latitude, longitude, and altitude. t} is the target pose result.

[0137] According to the technical solution provided in the above embodiment, by calculating the current heading angle, the initial posture result is updated based on the current heading angle to obtain an adjusted posture result. In this way, the initial posture result can be adjusted based on the prediction of the heading angle change under conditions with more turns and fewer straight lines, and the confidence of the corresponding sensor under different conditions can be dynamically adjusted to obtain a more accurate target posture result.

[0138] In some embodiments, the parameter to be adjusted also includes position information, the target parameter corresponding to the parameter to be adjusted is calculated, and based on the target parameter, the initial posture result is adjusted to obtain an adjusted posture result, including: calculating the position information of the target object at the next moment according to the position data of the target object in the target time period, the posture data includes position data, and the position information at the next moment represents the target parameter; replacing the position information in the initial posture result with the position information at the next moment, and obtaining an adjusted posture result corresponding to the position information.

[0139] Specifically, if the target working condition is high speed and few turns, such as when the vehicle is in a tunnel or high-speed scene, the vehicle exhibits strong inertia at the trajectory level, so the position information at the next moment can be predicted and the final pose result can be output through graph optimization. When predicting the position information at the next moment, a nonlinear fitting method can be used for prediction.

[0140] Specifically, nonlinear fitting with time t as the independent variable can be used. For example, the model parameters are iterated using the position data 15 seconds before the current moment.

[0141] Lat(τ)=α0+α1(τ-t)+α2(τ-t) 2 +α3(τ-t) 3

[0142] Lon(τ)=β0+β1(τ-t)+β2(τ-t) 2 +β3(τ-t) 3

[0143] High(τ)=γ0+γ1(τ-t)+γ2(τ-t) 2 +γ3(τ-t) 3

[0144] Where Lat(τ) represents the dimensional fitting result, α0α1α2α3 can represent the coefficient of the dimensional fitting polynomial, Lon(τ) represents the light fitting result, β0β1β2β3 can represent the coefficient of the light fitting polynomial, High(τ) represents the high fitting result, γ0γ1γ2γ3 can represent the coefficient of the high fitting polynomial, τ represents the time variable, the range can be within 15s before the current moment, τ∈[t-15,t], t can represent the current moment, for the trained model, let τ=t+1, predict the position information of the next moment, and the predicted position information of the next moment is recorded as {FittingPos t}, including location information of latitude, altitude and latitude, and it should be understood that the location data can be obtained based on GNSS.

[0145] Then, the position information at the next moment can be used to correct the initial pose result {KF t}, the adjusted pose result corresponding to the position information can be obtained and recorded as {FittingAVP t}:

[0146]

[0147] Furthermore, the adjusted pose and initial pose results corresponding to the position information can be used to constrain the target pose results:

[0148]

[0149] Similarly, where {FittingAVP t}, {KF t} and {AVP t} are all the estimated attitude, speed, and position at time t, that is They are pitch angle, roll angle, heading angle, east speed, north speed, sky speed, latitude, longitude, and altitude. The calculated {AVP t} is the target pose result.

[0150] According to the technical solution provided in the above embodiment, when the parameter to be adjusted is position information, the position information at the next moment can be predicted by a nonlinear fitting method, and the predicted position information and the initial pose result can be used to constrain the target pose result. In this way, the initial pose result is adjusted by the parameter to be adjusted, and the further obtained target pose result is made more accurate.

[0151] Figure 2 This is a flow chart of another method for determining the position and posture of a target object provided in an embodiment of the present application. Figure 2 This application is further described.

[0152] First, the corresponding data can be obtained through various sensors, namely low-frequency vision, satellite navigation, inertial measurement unit, and wheel odometer. After obtaining the corresponding data, the corresponding posture data of satellite navigation, inertial measurement unit and wheel odometer can be strictly time-calibrated, and the three can be non-strictly time-synchronized with low-frequency vision (i.e., video data). Because the acquisition frame rate of low-frequency vision is low, there may be only a few frames per second. In order to reduce the computing power demand for video data in this solution, strict time calibration with other posture data with higher frequencies is not required.

[0153] Then, HOG features, SIFT features, and LBP features can be extracted and fused from the video data to obtain the target video features. The target video features can be used to perform scene recognition using SVM algorithms with different kernel functions to obtain scene classification results, such as determining whether the scene is a basement.

[0154] When the scene classification result is an underground garage, it means that the target object is in a scene with many turns and a slow speed. The current heading angle can be calculated based on the wheel speed kinematic model, and the initial pose result output by the combined navigation based on the Kalman filter and the current heading angle can be fused based on graph optimization to obtain the final target pose result.

[0155] When the scene is classified as a tunnel, the target object is considered to be in a scene with few turns and fast speed. The position information at the next moment can be obtained based on nonlinear fitting, and the position information at the next moment and the initial pose result can be fused based on graph optimization to obtain the target pose result.

[0156] In this way, a multi-modal data-assisted combined navigation algorithm that predicts scenes through each posture data and adaptively selects the corresponding optimization method solves the problems in traditional visual SLAM that multiple sensors require strict time synchronization and spatial calibration, high-frequency video data requires a lot of computation, and requires dynamic adjustment of sensor confidence. It realizes the judgment of driving scenes with low-frequency visual data, provides different constraints for the combined algorithm according to the scene, and does not require strict time and space synchronization between vision and other sensors, thereby reducing the amount of computation and improving the positioning accuracy of combined navigation.

[0157] All the above optional technical solutions can be arbitrarily combined to form optional embodiments of the present application, which will not be described one by one here.

[0158] The following is an embodiment of the device of the present application, which can be used to execute the embodiment of the method of the present application. For details not disclosed in the embodiment of the device of the present application, please refer to the embodiment of the method of the present application.

[0159] Figure 3 Schematic diagram of a device for determining the position and posture of a target object provided in an embodiment of the present application. Figure 3 As shown, the device for determining the position and posture of the target object includes:

[0160] An acquisition module 301 is configured to acquire posture data acquired by at least two posture estimation sensors of a target object;

[0161] The initial posture determination module 302 is configured to determine an initial posture result of the target object according to the posture data acquired by at least two posture estimation sensors;

[0162] The optimization module 303 is configured to determine a target working condition of the target object, and determine a parameter to be adjusted of the initial posture result based on the target working condition, where different target working conditions correspond to different parameters to be adjusted;

[0163] The target posture determination module 304 is configured to adjust the initial posture result based on the parameter to be adjusted to obtain a target posture result.

[0164] In some embodiments, the target posture determination module 304 is specifically used to calculate the target parameters corresponding to the parameters to be adjusted, and based on the target parameters, adjust the initial posture results to obtain the adjusted posture results; based on the adjusted posture results and the initial posture results, constrain the target posture results to obtain the target posture results.

[0165] In some embodiments, the target posture determination module 304 is specifically used to calculate the current heading angle of the target object based on the wheel speed odometer data of the target object, the current heading angle represents the target parameter, and the posture data includes the wheel speed odometer data; replace the heading angle in the initial posture result with the current heading angle to obtain an updated initial posture result; calculate the posture transformation matrix of the target object based on the updated initial posture result, and use the posture transformation matrix to determine the adjusted posture result corresponding to the current heading angle.

[0166] In some embodiments, the target posture determination module 304 is specifically used to calculate the position information of the target object at the next moment based on the position data of the target object in the target time period, the posture data includes the position data, and the position information at the next moment represents the target parameters; the position information in the initial posture result is replaced with the position information at the next moment to obtain an adjusted posture result corresponding to the position information.

[0167] In some embodiments, the optimization module 303 is specifically used to obtain video data captured by a shooting device of the target object, perform feature extraction on the current video data, and obtain target video features; determine the scene in which the target object is located according to the target video features, and determine the target working condition of the target object based on the scene in which the target object is located.

[0168] In some embodiments, the optimization module 303 is specifically used to perform scene prediction on the target video features through different scene prediction models, obtain at least one predicted scene output by each scene prediction model, and the initial probability corresponding to each predicted scene; determine the target probability corresponding to each predicted scene according to the initial probability of each predicted scene in different scene prediction models; determine the scene in which the target object is located according to the target probability corresponding to each predicted scene.

[0169] In some embodiments, the optimization module 303 is specifically used to obtain the correspondence between the preset working conditions and parameters; match the target working condition with the correspondence between the preset working conditions and parameters, and determine the parameters to be adjusted corresponding to the target working condition.

[0170] It should be understood that the size of the serial numbers of the steps in the above embodiments 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 application.

[0171] Figure 4 Schematic diagram of an electronic device 4 provided in an embodiment of the present application. Figure 4 As shown, the electronic device 4 of this embodiment includes: a processor 401, a memory 402, and a computer program 403 stored in the memory 402 and executable on the processor 401. When the processor 401 executes the computer program 403, the steps in the above-mentioned method embodiments are implemented. Alternatively, when the processor 401 executes the computer program 403, the functions of the modules / units in the above-mentioned device embodiments are implemented.

[0172] The electronic device 4 may be a desktop computer, a notebook, a PDA, a cloud server, or other electronic device. The electronic device 4 may include, but is not limited to, a processor 401 and a memory 402. Those skilled in the art will appreciate that Figure 4 The electronic device 4 is merely an example and does not limit the electronic device 4 , and may include more or less components than those shown in the figure, or different components.

[0173] The processor 401 may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc.

[0174] The memory 402 may be an internal storage unit of the electronic device 4, for example, a hard disk or memory of the electronic device 4. The memory 402 may also be an external storage device of the electronic device 4, for example, a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card, etc. equipped on the electronic device 4. The memory 402 may also include both an internal storage unit and an external storage device of the electronic device 4. The memory 402 is used to store computer programs and other programs and data required by the electronic device.

[0175] Those skilled in the art can clearly understand that for the convenience and simplicity of description, only the division of the above-mentioned functional units and modules is used as an example. In actual applications, the above-mentioned functions can be assigned to different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiments can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The above-mentioned integrated unit can be implemented in the form of hardware or in the form of software functional units.

[0176] If the integrated module / unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a storage medium. Based on this understanding, the present application implements all or part of the process in the above-mentioned embodiment method, and can also be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a storage medium, and the computer program can implement the steps of the above-mentioned various method embodiments when executed by the processor. The computer program may include computer program code, which may be in source code form, object code form, executable file or some intermediate form. Computer-readable media may include: any entity or device capable of carrying computer program code, recording medium, U disk, mobile hard disk, disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), electric carrier signal, telecommunication signal and software distribution medium, etc.

[0177] The above embodiments are only used to illustrate the technical solutions of the present application, rather than to limit them. Although the present application has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. These modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present application, and should all be included in the protection scope of the present application.

Claims

1. A method for determining the position and posture of a target object, characterized in that: include: Acquire pose data acquired by at least two pose estimation sensors of the target object; Determining an initial pose result of the target object according to the pose data acquired by at least two of the pose estimation sensors; Determining a target working condition of the target object, and determining a parameter to be adjusted of the initial posture result based on the target working condition, wherein different target working conditions correspond to different parameters to be adjusted; The initial pose result is adjusted based on the parameter to be adjusted to obtain a target pose result.

2. The method according to claim 1, characterized in that: The adjusting the initial pose result based on the parameter to be adjusted to obtain a target pose result includes: Calculating a target parameter corresponding to the parameter to be adjusted, and adjusting the initial posture result based on the target parameter to obtain an adjusted posture result; According to the adjusted posture result and the initial posture result, the target posture result is constrained to obtain the target posture result.

3. The method according to claim 2, characterized in that The parameter to be adjusted includes a heading angle, and the calculating a target parameter corresponding to the parameter to be adjusted, and adjusting the initial posture result based on the target parameter to obtain an adjusted posture result, includes: Calculating a current heading angle of the target object according to the wheel speed odometer data of the target object, wherein the current heading angle represents the target parameter, and the posture data includes the wheel speed odometer data; Replacing the heading angle in the initial pose result with the current heading angle to obtain an updated initial pose result; According to the updated initial pose result, a pose transformation matrix of the target object is calculated, and the pose transformation matrix is ​​used to determine an adjusted pose result corresponding to the current heading angle.

4. The method according to claim 2, characterized in that: The parameter to be adjusted also includes position information, and the calculating of the target parameter corresponding to the parameter to be adjusted, and adjusting the initial posture result based on the target parameter to obtain the adjusted posture result, includes: Calculate the position information of the target object at a next moment according to the position data of the target object in a target time period, the position data including the position data, and the position information at the next moment represents the target parameter; The position information in the initial pose result is replaced with the position information at the next moment to obtain an adjusted pose result corresponding to the position information.

5. The method according to claim 1, characterized in that Determining the target operating condition of the target object includes: Acquire video data captured by a camera of the target object, perform feature extraction on the current video data, and obtain target video features; The scene in which the target object is located is determined according to the target video feature, and the target working condition of the target object is determined based on the scene in which the target object is located.

6. The method according to claim 5, characterized in that The determining the scene where the target object is located according to the target video feature includes: Performing scene prediction on the target video features respectively through different scene prediction models, obtaining at least one predicted scene output by each of the scene prediction models, and an initial probability corresponding to each predicted scene; Determining a target probability corresponding to each prediction scenario according to the initial probability of each prediction scenario in the different scenario prediction models; According to the target probability corresponding to each predicted scene, the scene where the target object is located is determined.

7. The method according to claim 1, characterized in that The step of determining the parameters to be adjusted of the initial posture result based on the target working condition includes: Obtain the corresponding relationship between the preset working conditions and parameters; The target operating condition is matched with the corresponding relationship between the preset operating condition and the parameter to determine the parameter to be adjusted corresponding to the target operating condition.

8. A device for determining the position and posture of a target object, characterized in that: include: An acquisition module is configured to acquire posture data acquired by at least two posture estimation sensors of the target object; An initial posture determination module is configured to determine an initial posture result of the target object according to the posture data acquired by at least two posture estimation sensors; An optimization module is configured to determine a target working condition of the target object, and determine a parameter to be adjusted of the initial posture result based on the target working condition, wherein different target working conditions correspond to different parameters to be adjusted; The target posture determination module is configured to adjust the initial posture result based on the parameter to be adjusted to obtain a target posture result.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 7 are implemented.

10. A storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

Images