Search

CN-121977564-A - Real-time pose estimation method and system based on multiple feature points PnP

CN121977564ACN 121977564 ACN121977564 ACN 121977564ACN-121977564-A

Abstract

The invention relates to the technical field of vision and automatic navigation, and discloses a real-time pose estimation method and a real-time pose estimation system based on multiple feature points PnP, wherein the method extracts multiple feature points and evaluates quality according to the structural characteristics of a two-dimensional code to obtain a feature point set; the method comprises the steps of constructing a pose optimization objective function to obtain a pose optimization mathematical model, obtaining a historical pose sequence and motion sensor data to predict an initial pose, carrying out iterative solution by adopting a self-adaptive trust domain optimization algorithm to obtain an optimal camera pose and quality assessment, and merging and converting a coordinate system by adopting a self-adaptive unscented Kalman filter to obtain a global pose and confidence of a vehicle. The invention can realize high-precision real-time pose estimation in a complex environment and improve the positioning reliability and robustness of the automatic guiding transport vehicle.

Inventors

  • LIU JIE

Assignees

  • 贵州轻工职业大学

Dates

Publication Date
20260505
Application Date
20260123

Claims (10)

  1. 1. The real-time pose estimation method based on the multi-feature point PnP is characterized by comprising the following steps of: S1, extracting a plurality of characteristic points of a two-dimensional code according to the structural characteristics of the two-dimensional code, and evaluating the quality of the characteristic points to obtain a characteristic point set; s2, constructing a pose-optimized objective function based on the feature point set to obtain a pose-optimized mathematical model; S3, acquiring a historical pose sequence and motion sensor data, and predicting an initial pose to obtain initial pose estimation; S4, carrying out iterative solution by adopting an optimization algorithm based on the pose optimization mathematical model and the initial pose estimation to obtain the optimal camera pose and quality estimation; And S5, based on the optimal camera pose and quality evaluation, adopting self-adaptive unscented Kalman filtering to fuse and convert a coordinate system, and obtaining the global pose and the confidence of the vehicle.
  2. 2. The real-time pose estimation method based on multi-feature PnP of claim 1, wherein said S1 comprises: extracting multi-type feature point pixel coordinates of a two-dimensional code, wherein the multi-type feature point pixel coordinates comprise four corner point pixel coordinates and center point pixel coordinates of three positioning graphs, and correcting the graph center point pixel coordinates; Calculating an image quality index of each feature point, wherein the image quality index comprises neighborhood image gradient strength, image contrast, a boundary penalty factor and a corner response function value; Calculating the quality weight of the feature points by integrating a plurality of quality indexes, calculating the comprehensive quality score by adopting a weighted fusion method, and screening the feature points with qualified quality according to a preset minimum threshold; establishing a two-dimensional code coordinate system and calculating the three-dimensional coordinate of each characteristic point according to the physical size of the two-dimensional code; and constructing a weighted two-dimensional three-dimensional point corresponding relation set, and outputting a two-dimensional pixel coordinate set, a three-dimensional world coordinate set and a quality weight set of the feature points.
  3. 3. The real-time pose estimation method based on multi-feature PnP of claim 1, wherein said S2 comprises: The camera pose is represented by a rotation vector and a translation vector, and the rotation vector is converted into a rotation matrix through a Rodrigues formula; establishing a projection relation from a three-dimensional coordinate to a pixel coordinate based on a pinhole imaging model; calculating the reprojection error of each feature point, and defining a reprojection error vector as the difference between the actual coordinate and the theoretical coordinate; processing abnormal values by using a self-adaptive double-threshold dynamic loss function, and dynamically adjusting loss characteristics according to the characteristic point quality weight and the error distribution by adopting a three-section design; Combining the feature point quality weight and the geometric constraint to construct a weighted objective function, and introducing rectangular constraint, distance constraint and collineation constraint; and calculating jacobian matrix and hessian matrix approximations of the pose parameters of the objective function.
  4. 4. The real-time pose estimation method based on multi-feature PnP of claim 1, wherein said S3 comprises: selecting a fast PnP algorithm, zero-order prediction or multi-source information fusion prediction according to the length of the historical pose sequence; Based on the motion sensor data, performing kinematic prediction, calculating vehicle displacement and corner change by utilizing wheel speed and steering angle data, and converting the vehicle displacement and corner change into camera pose change quantity; Identifying a vehicle motion mode, selecting a corresponding prediction strategy, and carrying out weighted fusion by adopting a multi-model fusion mechanism; Carrying out multi-level rationality inspection on the predicted pose, including kinematic constraint inspection, historical prediction error consistency inspection and scene feature consistency inspection; And adopting a confidence evaluation model of multi-factor fusion to calculate the comprehensive confidence score.
  5. 5. The real-time pose estimation method based on multi-feature PnP of claim 1, wherein said S4 comprises: calculating the reprojection errors and the weighted robust loss of all the characteristic points under the current pose; Parallel computing a jacobian matrix and assembling an optimization equation, and distributing feature points to a plurality of computing threads for parallel computing; solving an increment equation by adopting a subspace trust domain method, and solving a trust domain sub-problem in a subspace formed by stretching in a gradient direction and a Newton direction; adopting gain ratio self-adaption to adjust the radius of the trust zone, and determining whether to accept updating according to the ratio of the actual descent quantity and the predicted descent quantity; checking convergence conditions and determining whether to continue iteration, wherein the convergence conditions comprise parameter updating quantity norms, target function change rates and iteration times; and calculating quality evaluation indexes of the optimization result, including objective function values, iteration times and comprehensive quality scores.
  6. 6. The real-time pose estimation method based on multi-feature PnP of claim 1, wherein said S5 comprises: the camera is installed with an external parameter to convert the coordinates of the camera to the vehicle, and then the two-dimensional code is converted to a global coordinate system through the global pose of the two-dimensional code; Establishing a state space model of the self-adaptive unscented Kalman filtering, defining a state vector comprising position, attitude and speed, and establishing a nonlinear state transfer equation and an observation equation; A prediction step of unscented Kalman filtering is executed, and a Sigma point set is generated by unscented transformation to perform state prediction; and (3) performing an updating step of unscented Kalman filtering and outlier detection, adopting a Markov distance to perform outlier detection and using a robust updating strategy, calculating comprehensive confidence coefficient based on information entropy and outputting global pose, speed estimation and pose uncertainty of the vehicle.
  7. 7. The real-time pose estimation method based on multi-feature point PnP according to claim 3, wherein said adaptive dual-threshold dynamic loss function adopts a three-segment design: A first segment that multiplies the quality enhancement factor by a squaring loss when the scalar error is less than or equal to a first threshold; the second section, when the scalar error is larger than the first threshold value and smaller than or equal to the second threshold value, adopting a cubic polynomial interpolation function to realize smooth transition, and solving a polynomial coefficient through a constraint equation with continuous function values and continuous derivatives; third, multiplying the quality attenuation factor by a logarithmic loss function when the scalar error is greater than a second threshold; the first threshold is calculated according to the weighted median of all the characteristic point re-projection errors, and the second threshold is obtained by adding a weighted standard deviation of a preset multiple to the first threshold.
  8. 8. The method for estimating real-time pose based on multi-feature PnP according to claim 4, wherein said identifying a vehicle motion pattern and selecting a corresponding prediction strategy comprises: Extracting motion characteristic vectors based on the historical pose sequences, wherein the motion characteristic vectors comprise speed change rate characteristics, steering angle change characteristics, motion track curvature characteristics and speed size characteristics; Inputting the feature vector into a motion mode classifier to perform mode recognition, wherein the motion mode comprises uniform linear motion, acceleration linear motion, deceleration linear motion, uniform turning motion and variable speed turning motion; selecting a corresponding prediction strategy according to the identified motion mode, adopting a first-order linear prediction model for uniform linear motion, adopting a second-order polynomial prediction model for acceleration or deceleration linear motion, adopting an arc track prediction model for uniform turning motion, adopting an adaptive Kalman prediction model for variable turning motion, and carrying out weighted fusion on prediction results according to probability distribution of each mode.
  9. 9. The method for estimating real-time pose based on multi-feature PnP of claim 6, wherein said calculating an integrated confidence level based on information entropy comprises: Calculating the confidence coefficient of observation quality, using the comprehensive quality score of the optimized result, calculating the uncertainty confidence coefficient, normalizing the pose uncertainty, calculating the information entropy and mapping the information entropy to the confidence coefficient, calculating the detection confidence coefficient of an abnormal value, calculating the consistency confidence coefficient of a filter according to the relation between the Mahalanobis distance and a preset abnormal value detection threshold value, and calculating the deviation between the normalized square sum of the information and a theoretical expected value; and fusing the four confidence degrees by using weighted geometric average to obtain comprehensive confidence degrees, and introducing a confidence degree lower bound protection mechanism and time sequence consistency test.
  10. 10. A real-time pose estimation system based on multiple feature points PnP for performing the steps of a real-time pose estimation method based on multiple feature points PnP according to any of claims 1-9, comprising: The feature point extraction module is used for extracting multi-feature points of the two-dimensional code according to the structural characteristics of the two-dimensional code and evaluating the quality of the feature points to obtain a feature point set; The objective function construction module is used for constructing an objective function for pose optimization based on the feature point set to obtain a pose optimization mathematical model; the initial pose prediction module is used for acquiring a historical pose sequence and motion sensor data, predicting an initial pose and obtaining initial pose estimation; The pose optimization solving module is used for carrying out iterative solving by adopting an optimization algorithm based on the pose optimization mathematical model and the initial pose estimation to obtain the optimal camera pose and quality estimation; And the pose fusion conversion module is used for fusing and converting a coordinate system by adopting a self-adaptive unscented Kalman filter based on the optimal camera pose and quality evaluation to obtain the global pose and the confidence of the vehicle.

Description

Real-time pose estimation method and system based on multiple feature points PnP Technical Field The invention relates to the technical field of vision and automatic navigation, in particular to a real-time pose estimation method and system based on multiple feature points PnP. Background Automatic Guided Vehicles (AGVs) are widely used in the fields of intelligent manufacturing and logistics storage, and accurate pose estimation is a key technology for the AGVs to realize autonomous navigation. The visual-based positioning method solves the pose of the camera by identifying the two-dimensional code mark in the environment and utilizing the PnP algorithm, and has the advantages of low cost and flexible deployment. However, in the prior art, pose calculation is usually performed by using only four corner points of a two-dimensional code, the positioning accuracy is insufficient due to the limited number of characteristic points, and the problem of degradation of the detection quality of the characteristic points easily occurs in complex environments such as illumination change and motion blur. The existing PnP pose estimation method is insufficient in robustness when processing abnormal feature points, and the traditional RANSAC method can remove abnormal values, but is low in calculation efficiency and possible to lose effective information. Meanwhile, the existing method lacks of quantitative evaluation of the quality of the feature points, and cannot carry out weighted optimization according to the reliability of the feature points, so that the low-quality feature points have negative influence on pose estimation. In addition, the single-frame pose estimation result is easily affected by image noise and detection errors, a time sequence information fusion mechanism is lacked, and the historical pose and motion sensor data cannot be fully utilized to improve the estimation precision and stability. Therefore, a real-time pose estimation method capable of fully utilizing multi-feature point information of a two-dimensional code, having feature point quality evaluation capability, having robustness to abnormal values and being capable of fusing multi-source information is needed, so that the technical problems of insufficient positioning precision, poor robustness and lack of time sequence fusion in the prior art are solved, and the requirement of high-precision real-time positioning of an AGV in a complex environment is met. Disclosure of Invention The invention provides a real-time pose estimation method and a real-time pose estimation system based on multiple feature points PnP, which solve the technical problems of insufficient positioning accuracy, lack of feature point quality estimation and insufficient time sequence information fusion in the related technology. The invention provides a real-time pose estimation method based on multiple feature points PnP, which comprises the following steps: S1, extracting a plurality of characteristic points of a two-dimensional code according to the structural characteristics of the two-dimensional code, and evaluating the quality of the characteristic points to obtain a characteristic point set; s2, constructing a pose-optimized objective function based on the feature point set to obtain a pose-optimized mathematical model; S3, acquiring a historical pose sequence and motion sensor data, and predicting an initial pose to obtain initial pose estimation; S4, carrying out iterative solution by adopting an optimization algorithm based on the pose optimization mathematical model and the initial pose estimation to obtain the optimal camera pose and quality estimation; And S5, based on the optimal camera pose and quality evaluation, adopting self-adaptive unscented Kalman filtering to fuse and convert a coordinate system, and obtaining the global pose and the confidence of the vehicle. In a preferred embodiment, the S1 includes: extracting multi-type feature point pixel coordinates of a two-dimensional code, wherein the multi-type feature point pixel coordinates comprise four corner point pixel coordinates and center point pixel coordinates of three positioning graphs, and correcting the graph center point pixel coordinates; Calculating an image quality index of each feature point, wherein the image quality index comprises neighborhood image gradient strength, image contrast, a boundary penalty factor and a corner response function value; Calculating the quality weight of the feature points by integrating a plurality of quality indexes, calculating the comprehensive quality score by adopting a weighted fusion method, and screening the feature points with qualified quality according to a preset minimum threshold; establishing a two-dimensional code coordinate system and calculating the three-dimensional coordinate of each characteristic point according to the physical size of the two-dimensional code; and constructing a weighted two-dimensional three-dimensional point corr