Visual positioning method based on prior target position and image detection
By integrating high-precision point cloud maps with online image detection, a nonlinear optimization positioning method is developed to solve the positioning problem in GNSS signal-constrained environments. This method achieves high-precision, low-cost, and robust positioning results, making it suitable for autonomous driving and mobile robots.
Patent Information
- Application Number
- CN202511413577.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-09-29
- Publication Date
- 2026-01-20
AI Technical Summary
Existing positioning technologies suffer from insufficient positioning accuracy and robustness due to factors such as signal obstruction in environments like tunnels and urban canyons, cumulative errors in INS, high costs and susceptibility to severe weather in LiDAR, susceptibility to environmental interference in pure visual SLAM, and the lack of globally consistent high-precision prior map references or deep fusion of recognition results in existing landmark or semantic feature-assisted positioning methods.
By fusing the prior target location in the offline high-precision point cloud map with the online image detection results, a nonlinear optimization problem is constructed to solve the optimal pose of the vehicle in the current frame. A deep learning target detection model is used to identify static targets, and a reprojection error optimization model is combined for localization.
It significantly improves positioning accuracy and robustness, reduces hardware costs, adapts to different complex environments, and meets the real-time positioning needs of autonomous driving and mobile robots.
Smart Images
Figure CN121363947A_ABST
Abstract
Description
TECHNICAL FIELD
[0001] The present application relates to the technical field of computer vision, robot navigation and automatic driving, and particularly relates to a visual positioning method based on prior target position and image detection. BACKGROUND
[0002] High-precision pose estimation is the key to reliable navigation and decision-making for autonomous vehicles and mobile robots. The current mainstream positioning technologies include Global Navigation Satellite System (GNSS), Inertial Navigation System (INS), Light Detection and Ranging (LiDAR), and Visual Simultaneous Localization and Mapping (Visual SLAM). However, GNSS signals are easily blocked in environments such as tunnels and urban canyons; INS has cumulative errors; LiDAR is costly and affected by bad weather; and pure visual SLAM is easily affected by environmental light changes and dynamic objects, and has problems of scale ambiguity and cumulative drift in long-term operation.
[0003] In the prior art, there are methods that assist positioning by recognizing road signs or semantic features, but they usually lack a globally consistent high-precision prior map as a reference, or fail to deeply integrate discrete recognition results through an optimization framework, resulting in insufficient positioning accuracy and robustness.
[0004] Therefore, there is an urgent need for a solution that can utilize the prior information of a pre-constructed accurate map and combine real-time perception results to achieve stable and accurate positioning through optimization calculation. SUMMARY
[0005] In view of the deficiencies of the related prior art, the present application provides a visual positioning method based on prior target position and image detection, which aims to overcome the deficiencies of GNSS signal blocking in tunnel, urban canyon and other environments, INS cumulative error, high cost of LiDAR and its impact on bad weather, and pure visual SLAM being easily affected by environmental interference, and the lack of a globally consistent high-precision prior map reference or the failure to deeply integrate recognition results in existing road sign or semantic feature assisted positioning methods. By fusing the target prior position in the offline constructed high-precision point cloud map and the online image detection result, a nonlinear optimization problem is constructed, and the optimal pose of the vehicle in the current frame is solved, thereby effectively solving the positioning problem in GNSS failure environment and significantly improving the positioning accuracy and robustness.
[0006] To achieve the above purpose, the present application provides a visual positioning method based on prior target position and image detection, comprising the following steps:
[0007] Step S1: offline mapping and target position calibration A collection vehicle equipped with a laser radar and / or a multi-camera is used to run a simultaneous localization and mapping (SLAM) algorithm in a region to be positioned, and a globally consistent point cloud map is constructed. In the point cloud map, the accurate 3D coordinates of static targets available for positioning in a global map (Map) coordinate system are labeled by manual or automatic identification.
[0008] Step S2: online visual recognition and target position query When a vehicle is running online, images are collected in real time by a vehicle-mounted camera. A pre-trained deep learning target detection model is used to process the current images, and static targets labeled in step S1 are identified in the field of view. For each successfully identified target, the accurate 3D coordinates of the target in the Map coordinate system are queried and obtained from the prior map database established in step S1 according to the category information of the target.
[0009] Step S3: constructing a re-projection error optimization model For each target identified in step S2, the following operations are performed in turn:
[0010] S3.1: coordinate transformation, the 3D coordinates of the target in the Map coordinate system are transformed to the camera coordinate system through the current estimated vehicle pose (including position parameters and attitude parameters) and the fixed external parameters (including translation parameters and rotation parameters) between the camera and the vehicle, to obtain the 3D coordinates of the target in the camera coordinate system;
[0011] S3.2: projection calculation, the 3D coordinates of the target in the camera coordinate system are projected to the image pixel plane using the intrinsic matrix of the camera to obtain the theoretical projection pixel coordinates of the target;
[0012] S3.3: error calculation, the deviation between the theoretical projection pixel coordinates of the target and the actual pixel coordinates of the center point or specific key point of the target detection box in step S2 is calculated to form a re-projection error term;
[0013] Step S4: pose optimization The re-projection error terms of all targets in step S3 are added to construct a total objective function. The initial value of the vehicle pose is used as the starting point for optimization, and a nonlinear optimization algorithm is used to minimize the total objective function to obtain the optimal pose of the current frame of the vehicle.
[0014] Further, in step S1, the point cloud map is a dense point cloud map or a semi-dense point cloud map; the static targets include traffic signs, street lamps, building corners, and objects with specific shapes; and the accurate 3D coordinates of the target in the Map coordinate system are denoted as P mapi , where P mapi = (X mi , Y mi , Z mi ), i represents the i-th target, Xmi , Y mi , Z mi are respectively coordinate values of the i-th target in the X-axis, Y-axis, and Z-axis directions of the Map coordinate system.
[0015] Further, in step S2, the deep learning target detection model is a YOLO model or a FasterR-CNN model; and the accurate 3D position of the target in the Map coordinate system obtained in step S2 is denoted as P cami , and i represents the i-th target.
[0016] Further, in step S3.1, the specific process of the coordinate transformation is: transforming the 3D coordinates of the target in the Map coordinate system to coordinates in the camera coordinate system, denoted as P cami , and the transformation relationship satisfies the formula: P cami = R map2cam P mapi + T map2cam .
[0017] wherein R map2cam is a rotation matrix from the Map coordinate system to the camera coordinate system, and is an orthogonal matrix of 3x3 dimensions; T map2cam is a translation vector from the Map coordinate system to the camera coordinate system; and P mapi is the 3D coordinates of the target in the Map coordinate system.
[0018] The R map2cam and T map2cam are solved by the following method:
[0019] (a) solving the rotation matrix R body2map and the translation matrix T body2map from the vehicle body coordinate system to the Map coordinate system: the calculation of R body2map satisfies the formula:
[0020]
[0021] the calculation of T body2map satisfies the formula: T body2map = [x, y, z] T .
[0022] wherein yaw is the yaw angle of the vehicle body pose, pitch is the pitch angle of the vehicle body pose, and roll is the roll angle of the vehicle body pose; x, y, and z are respectively the position parameters of the vehicle body pose in the X-axis, Y-axis, and Z-axis directions of the Map coordinate system.
[0023] (b) solving the rotation matrix R cam2body and the translation matrix T cam2body from the camera coordinate system to the vehicle body coordinate system: R cam2bodyThe calculation satisfies the formula:
[0024]
[0025] T cam2body The calculation satisfies the formula: T cam2body =[tx,ty,tz] T ;
[0026] Where rz, ry, and rx are the rotation parameters in the Z, Y, and X axes of the fixed external parameters between the camera and the vehicle body, respectively; tx, ty, and tz are the translation parameters in the X, Y, and Z axes of the fixed external parameters between the camera and the vehicle body, respectively; (c) Calculate R map2cam and T map2cam :R map2cam The calculation satisfies the formula: in For R cam2body The transpose of the matrix, For R body2map The transpose of T; cam2body The calculation satisfies the formula:
[0027] Furthermore, in step S3.2, the specific process of projection calculation is as follows: the 3D coordinates of the target in the camera coordinate system are denoted as P. cami And P cami =(X ci ,Y ci Z ci ), where X ci ,Y ci Z ci These are the coordinates of the i-th target in the X, Y, and Z directions in the camera coordinate system, respectively.
[0028] The theoretical projected pixel coordinates of the target are as follows in Let x be the x-coordinate of the theoretically projected pixel. The ordinate of the theoretically projected pixel;
[0029] The projection relationship satisfies the following formula:
[0030] u proj_i =(fx×X) ci / Z ci )+cx
[0031] v proj_i =(fy×Y) ci / Z ci )+cy
[0032] Wherein, fx, fy are the focal length of the X-axis, Y-axis direction of the camera respectively, cx, cy are the coordinate value of the camera optical center in the image pixel plane X-axis, Y-axis direction respectively, and fx, fy, cx, cy are all obtained by camera intrinsic calibration.
[0033] Further, in step S3.3, the error is the Euclidean distance, and the corresponding re-projection error term is denoted as e i , and the calculation of e i satisfies the formula:
[0034] e i =(u -proj_i -u -obs_i ) 2 +(v -proj_i -v -obs_i ) 2 ;
[0035] Wherein, u -proj_i , v -proj_i are the horizontal coordinate and vertical coordinate of the theoretical projection pixels of the i-th target; u -obs_i , v -obs_i are the horizontal coordinate and vertical coordinate of the actual pixels of the center point or specific key point of the i-th target detection frame in step S2.
[0036] Further, in step S4, the total target function is denoted as E(T -vehicle ), and the construction of E(T -vehicle ) satisfies the formula:
[0037] Wherein, E(T -vehicle ) is the vehicle body pose to be optimized, k is the total number of targets successfully identified in step S2, e i is the re-projection error term of the i-th target; the nonlinear optimization algorithm is Levenberg-Marquardt algorithm, and the minimization of the total target function is realized through Google's CeresSolver optimization library; the initial value of the vehicle body pose is derived from the inertial navigation system (INS), the wheel speed meter or the positioning result of the last frame; the optimal vehicle body pose is denoted as T -vehicle_ptimal .
[0038] Further, in step S4, when performing pose optimization, the initial value or constraint condition provided by the global satellite navigation system (GNSS) or the inertial navigation system (INS) is also combined to form a multi-sensor fusion pose optimization system.
[0039] The above technical scheme is adopted in the present application, and at least the following beneficial effects are achieved:
[0040] In the application, the prior position of the static target in the high-precision point cloud map constructed offline is fused with the online image detection result to provide a globally consistent absolute position reference for positioning, and the accumulated error and scale ambiguity problems of the SLAM technology are effectively avoided
[0041] The SLAM technology has accumulated error and scale ambiguity problems, and even in the scene where the environment light changes greatly, the texture is sparse, or there is dynamic object interference, as long as a small amount of calibrated static target can be identified, the precise positioning of the vehicle can be realized, and the positioning accuracy and robustness are significantly improved.
[0042] In the application, the positioning process mainly depends on the vehicle-mounted camera and the prior map constructed offline, and does not need to rely on high-cost sensors such as high-line laser radar, so that the hardware cost of the positioning system is greatly reduced, the dependence on expensive equipment is reduced, and the technology is more conducive to popularization and application in the field of automatic driving and mobile robots.
[0043] In the application, good flexibility is provided, the initial value or constraint condition provided by other sensors such as the global satellite navigation system (GNSS) and the inertial navigation system (INS) can be combined, a multi-sensor fusion pose optimization system can be constructed, the positioning requirements in different complex environments such as tunnels, urban canyons and bad weather can be adapted, and the environmental adaptability and reliability of the system are further improved.
[0044] In the application, the CeresSolver optimization library of Google is used to realize the pose optimization calculation, the optimization library has high and stable calculation performance, can automatically complete the Jacobian matrix calculation and iterative update of the objective function on the pose variable, can solve the optimal pose of the current frame of the vehicle in a short time, meets the core requirements of automatic driving vehicles and mobile robots on positioning real-time, and ensures the timely response of navigation and decision. BRIEF DESCRIPTION OF DRAWINGS
[0045] In order to more clearly illustrate the technical solutions in the embodiments of the application or the prior art, the drawings needed to be used in the embodiments or the prior art description will be briefly introduced. Obviously, the drawings in the following description are only some embodiments of the application, and other drawings can be obtained by those skilled in the art without creative labor.
[0046] Figure 1 is a flowchart of the visual positioning method of the application;
[0047] Figure 2 is a schematic diagram of the system of the application;
[0048] Figure 3 is a schematic diagram of coordinate transformation and re-projection error of the application;
[0049] Figure 4is a Ceres solver optimization process block diagram of the present application. DETAILED DESCRIPTION
[0050] The exemplary embodiments will be described in detail herein with reference to the attached drawings. In the following description, like reference numerals refer to like elements, unless the context clearly dictates otherwise. The following description of exemplary embodiments is not representative of all possible embodiments consistent with the present application. Rather, it is merely an example of apparatus and methods consistent with some aspects of the present application as detailed in the appended claims.
[0051] As Figure 1 shown, the present embodiment provides a visual positioning method based on prior target position and image detection, comprising the following steps:
[0052] Step S1: offline mapping and target position calibration. A collection vehicle equipped with a laser radar and / or a multi-camera runs a simultaneous localization and mapping (SLAM) algorithm in the area to be positioned, and constructs a globally consistent point cloud map. In the point cloud map, the accurate 3D coordinates of static targets available for positioning in the global map (Map) coordinate system are labeled by manual or automatic identification.
[0053] Step S2: online visual recognition and target position query. When the vehicle is running online, real-time images are collected by the vehicle-mounted camera. A pre-trained deep learning target detection model is used to process the current image, and the static targets labeled in step S1 are identified in the field of view. For each successfully identified target, its accurate 3D coordinates in the Map coordinate system are queried and obtained from the prior map database established in step S1 according to its category information.
[0054] Step S3: constructing a re-projection error optimization model. For each target identified in step S2, the following operations are performed in turn:
[0055] S3.1: coordinate transformation. The 3D coordinates of the target in the Map coordinate system are transformed to the camera coordinate system by the current estimated vehicle pose (including position parameters and attitude parameters) and the fixed external parameters (including translation parameters and rotation parameters) between the camera and the vehicle, to obtain the 3D coordinates of the target in the camera coordinate system.
[0056] S3.2: projection calculation. The 3D coordinates of the target in the camera coordinate system are projected to the image pixel plane using the intrinsic matrix of the camera to obtain the theoretical projection pixel coordinates of the target.
[0057] S3.3: error calculation. The deviation between the theoretical projection pixel coordinates of the target and the actual pixel coordinates of the center point or specific key point of the target detection box in step S2 is calculated to form a re-projection error term.
[0058] Step S4: The pose optimization accumulates the re-projection error terms of all targets in step S3, constructs an overall objective function; takes the initial value of the vehicle body pose as the optimization starting point, minimizes the overall objective function through a nonlinear optimization algorithm, and solves to obtain the optimal pose of the current frame of the vehicle.
[0059] As an implementation form, in step S1 of the embodiment, the point cloud map is a dense point cloud map or a semi-dense point cloud map; the static target includes a traffic sign, a street lamp, a building corner, and an object with a specific shape; and the accurate 3D coordinates of the target in the Map coordinate system are denoted as P mapi , i represents the ith target, X mapi , Y mi , and Z mi are the coordinate values of the ith target in the X-axis, Y-axis, and Z-axis directions of the Map coordinate system, respectively. mi mi mi mi
[0060] As an implementation form, in step S2 of the embodiment, the deep learning target detection model is a YOLO model or a FasterR-CNN model; and the accurate 3D position of the target in the Map coordinate system obtained by the query in step S2 is denoted as P cami , i represents the ith target.
[0061] As an implementation form, in step S3.1 of the embodiment, the specific process of the coordinate transformation is: the 3D coordinates of the target in the Map coordinate system are transformed into the coordinates in the camera coordinate system, denoted as P cami , and the transformation relationship satisfies the formula: P cami =R map2cam P mapi +T map2cam .
[0062] , R map2cam is a rotation matrix from the Map coordinate system to the camera coordinate system, and is an orthogonal matrix with a dimension of 3x3; T map2cam is a translation vector from the Map coordinate system to the camera coordinate system; and P mapi is the 3D coordinates of the target in the Map coordinate system.
[0063] The R map2cam and T map2cam are solved by the following method:
[0064] (a) solving the rotation matrix R body2map and the translation matrix T body2map from the vehicle body coordinate system to the Map coordinate system: R body2map The calculation satisfies the formula:
[0065]
[0066] T body2map The calculation satisfies the formula: T body2map =[x,y,z] T ;
[0067] Where yaw is the yaw angle of the vehicle pose, pitch is the pitch angle of the vehicle pose, and roll is the roll angle of the vehicle pose; x, y, and z are the position parameters of the vehicle pose in the X-axis, Y-axis, and Z-axis directions in the Map coordinate system, respectively.
[0068] (b) Solve for the rotation matrix R from the camera coordinate system to the vehicle coordinate system. cam2body Translation matrix T cam2body :R cam2body The calculation satisfies the formula:
[0069]
[0070] T cam2body The calculation satisfies the formula: T cam2body =[tx,ty,tz] T ;
[0071] Where rz, ry, and rx are the rotation parameters in the Z, Y, and X axes of the fixed external parameters between the camera and the vehicle body, respectively; tx, ty, and tz are the translation parameters in the X, Y, and Z axes of the fixed external parameters between the camera and the vehicle body, respectively; (c) Calculate R map2cam and T map2cam :R map2cam The calculation satisfies the formula: in For R cam2body The transpose of the matrix, For R body2map The transpose of T; cam2body The calculation satisfies the formula:
[0072] As one implementation method, in step S3.2 of this embodiment, the specific process of projection calculation is as follows: the 3D coordinates of the target in the camera coordinate system are P. cami And P cami =(X ci ,Y ci Z ci ), where X ci ,Y ci Z ci These are the coordinates of the i-th target in the X, Y, and Z directions in the camera coordinate system, respectively.
[0073] The theoretical projection pixel coordinate of the target is denoted as wherein is the horizontal coordinate of the theoretical projection pixel, is the vertical coordinate of the theoretical projection pixel.
[0074] The projection relationship satisfies the following formula:
[0075] u proj_i =(fx×X ci / Z ci )+cx
[0076] v proj_i =(fy×Y ci / Z ci )+cy
[0077] wherein fx and fy are the focal lengths of the X axis and Y axis of the camera respectively, and cx and cy are the coordinate values of the optical center of the camera on the X axis and Y axis of the image pixel plane respectively, and fx, fy, cx and cy are obtained by camera intrinsic calibration.
[0078] As an implementation manner, in step S3.3, the error is the Euclidean distance, and the corresponding re-projection error term is denoted as e i , and the calculation of e i satisfies the formula:
[0079] e i =(u -proj_i -u -obs_i ) 2 +(v -proj_i -v -obs_i ) 2 ;
[0080] wherein u -proj_i and v -proj_i are the horizontal and vertical coordinates of the theoretical projection pixel of the i-th target respectively; and u -obs_i and v -obs_i are the horizontal and vertical coordinates of the actual pixel of the center point or specific key point of the i-th target detection frame in step S2.
[0081] As an implementation manner, in step S4, the total target function is denoted as E(T -vehicle ), and the construction of E(T -vehicle ) satisfies the formula:
[0082] wherein E(T -vehicle ) is the vehicle body pose to be optimized, k is the total number of targets successfully recognized in step S2, and e ireprojection error term for the ith target;the nonlinear optimization algorithm is a Levenberg-Marquardt algorithm, and minimization of the overall objective function is realized through a CeresSolver optimization library of Google;the initial value of the vehicle body pose is derived from an inertial navigation system (INS), a wheel speed meter, or a positioning result of a previous frame;the optimal vehicle body pose is recorded as T -vehicle_ptimal .
[0083] The visual positioning based on the prior target position and image detection is realized. The method fully fuses the global consistency of the high-precision prior map and the environment perception ability of real-time image detection, effectively solves the positioning problem in the GNSS signal limited scene, and has significant advantages in positioning accuracy, robustness, hardware cost and real-time performance, thereby providing strong technical support for reliable navigation of autonomous vehicles and mobile robots.
[0084] In order to further detail the embodiment, a specific implementation example is provided as follows:
[0085] In the embodiment, the target prior position in the high-precision point cloud map constructed offline and the online image detection result are fused, a nonlinear optimization problem is constructed, and the optimal pose of the vehicle in the current frame is solved, so that the positioning problem in the GNSS failure environment is effectively solved, and the positioning accuracy and robustness are significantly improved.
[0086] In order to achieve the above object, the application adopts the following technical scheme:
[0087] Step S1: offline mapping and target position calibration
[0088] A global consistent dense or semi-dense point cloud map (i.e. global map) is constructed by using a collection vehicle equipped with a laser radar and / or a multi-camera to run a SLAM (simultaneous localization and mapping) algorithm in a region to be positioned. In the map, the accurate 3D coordinates Pmapi=(Xmi,Ymi,Zmi) of the static targets (such as traffic signs, street lamps, building corners, objects with specific shapes, etc.) that can be used for positioning in the subsequent process in the global map coordinate system (i.e. Map coordinate system) are manually calibrated or calibrated through an automatic recognition algorithm. Wherein i is the ith target.
[0089] Step S2: online visual recognition and target position query
[0090] When the vehicle is online, the images are collected in real time by the on-board camera. The current image is processed by using a pre-trained deep learning target detection model (such as YOLO, FasterR-CNN, etc.), and the static target calibrated in step S1 in the field of view is identified. For each successfully identified target i, according to its category and other information, the accurate 3D position P_map_i in the Map coordinate system is obtained from the prior map database established in step S1.
[0091] Step S3: Constructing a re-projection error optimization model
[0092] For each target i identified in step S2, the following sub-steps are performed:
[0093] S3.1: Coordinate transformation
[0094] The point P_map_i of the target in the Map coordinate system is rotated by the current estimated vehicle pose T_vehicl e (to-be-optimized variable, including position (x, y, z) and attitude (roll, pitch, yaw)) and the fixed external parameter T_cam between the camera and the vehicle (including translation (tx, ty, tz) and rotation (rx, ry, rz)), and the rotation direction is ZYX. Transform to the camera coordinate system to obtain P_cami, denoted as:
[0095] P_cami = R map2cam P_map_i + T map2cam (1)
[0096] Wherein R is the rotation matrix from the camera coordinate system to the map coordinate system, which is a 3*3 orthogonal matrix, and T is the translation vector from the camera coordinate system to the map coordinate system.
[0097] The solving process of (1) is as follows:
[0098] Solve the rotation matrix R_body2map and the translation matrix
[0099] T_body2map, wherein:
[0100]
[0101] T body2map = [x, y, z] T
[0102] Solve the rotation matrix R_cam2body and the translation matrix
[0103] T_cam2body, wherein:
[0104]
[0105] T body2map = [tx, ty, tz] T
[0106] Calculate the projection matrix and translation matrix from the map coordinate system to the camera coordinate system:
[0107]
[0108] Thus, the point Pmapi of the target in the Map coordinate system is converted into the coordinate Pcami in the camera coordinate system, that is, formula (1).
[0109] S3.2: Projection calculation: use the intrinsic matrix K of the camera to project the 3D point
[0110] Pcami = (Xci, Yci, Zci) in the camera coordinate system to the image pixel plane to obtain its theoretical projection pixel coordinates (u_proj_i, v_proj_i).
[0111] The projection formula is:
[0112] u_proj_i = (fx * Xci / Zci) + cx
[0113] v_proj_i = (fy * Yci / Zci) + cy
[0114] Where (fx, fy) is the focal length of the camera, (cx, cy) is the optical center of the camera, and both are parameters obtained by camera intrinsic calibration.
[0115] S3.3: Error calculation:
[0116] Calculate the Euclidean distance between the theoretical projection coordinates (u_proj_i, v_proj_i) of the target and the actual pixel coordinates (u_obs_i, v_obs_i) of the center point or specific key point of the target detection frame in step S2, which is a re-projection error term ei:
[0117] e i = (u_proj_i - u_obs_i) 2 + (v_proj_i - v_obs_i) 2
[0118] Step S4: Pose optimization based on CeresSolver
[0119] Add the re-projection error terms constructed for all recognized targets in step S3 to form the overall objective function:
[0120]
[0121] The objective function E is minimized using Google's CeresSolver optimization library. The current pose estimate of the vehicle T_vehicle (which can come from IMU, wheel odometry, or the localization result of the previous frame) is used as the initial value for optimization.
[0122] CeresSolver utilizes a gradient-based nonlinear optimization algorithm (such as Levenberg-Marquardt algorithm) to automatically compute the Jacobian matrix of the objective function with respect to the pose variables (x, y, z, roll, pitch, yaw), and iteratively updates to finally solve the optimal vehicle pose T_vehicle_optimal that minimizes the total re-projection error.
[0123] The benefits of the present application are:
[0124] High precision and robustness: The high-precision map constructed offline provides absolute position prior, avoiding the cumulative error and scale ambiguity problem of pure visual SLAM. Even in an environment with sparse texture or dramatic changes in lighting, as long as a few known targets can be detected, accurate positioning can be achieved.
[0125] Low cost: Mainly relying on cameras and prior maps, reducing the dependence on expensive sensors (such as high-line laser radar).
[0126] Flexibility: This method can be combined with the initial value or constraint provided by other sensors such as GNSS and IMU to form a more powerful multi-sensor fusion system.
[0127] Computational efficiency: CeresSolver optimization library is efficient and stable, and can realize real-time pose optimization calculation, meeting the real-time requirements of autonomous driving and robot systems.
[0128] For example, Figure 2As shown, the flow of the visual positioning method based on prior target position and image detection of the present application is divided into the following links: first, the offline stage, sequentially performing the operations of "SLAM mapping", "calibration target in the map", and "generating prior map database", completing the construction of a globally consistent point cloud map, the calibration of the 3D coordinates of the locatable static target in the global map (Map) coordinate system, and generating a prior map database to provide prior reference for subsequent online positioning; then entering the online stage, sequentially carrying out the operations of "vehicle-mounted camera capturing image", "deep learning model detecting target", and "querying database to obtain target Map coordinate", realizing real-time image collection by the vehicle-mounted camera, identifying the calibrated static target in the field of view through the deep learning target detection model, and then querying and obtaining the 3D coordinates of the target in the Map coordinate system from the prior map database, establishing the association between online perception and prior information; then entering the core processing link, performing the operation of "constructing re-projection error", constructing a re-projection error model based on the actual pixel information of the online identified target and the prior Map coordinate; then entering the optimization process, minimizing the error through the "Ceres optimizer", using the CeresSolver optimization library to perform nonlinear optimization on the re-projection error model, and iteratively solving the vehicle pose that minimizes the error; finally, in the output result link, performing the operation of "outputting the optimal vehicle pose", obtaining the optimal positioning result of the current frame vehicle.
[0129] As shown in Figure 3 , the calculation process of the re-projection error is as follows: taking the map point P map in the prior map as the starting point, first converting it to the vehicle coordinate system through the coordinate transformation relationship of body2map; then, converting the point in the vehicle coordinate system to the camera coordinate system through the coordinate transformation relationship of cam2body; then, converting the point in the camera coordinate system to the pixel coordinate system using the camera intrinsic matrix to obtain the theoretical projection pixel coordinates; finally, comparing the theoretical projection pixel coordinates with the actual pixel coordinates of the detection box center obtained by online target detection, calculating the projection error between the two, and providing the error term basis for the subsequent optimization of the vehicle pose.
[0130] As shown in Figure 4As shown, the process of the Ceres optimizer for vehicle body pose optimization is as follows: after the process is started from "start", firstly, the "initial pose" is obtained (the initial value can be derived from an inertial navigation system, a wheel speed meter or a positioning result of a previous frame); based on the initial pose, the operation of "calculating re-projection error" is performed to obtain the total re-projection error of all identified targets under the current pose; then, the "Ceres optimizer (calculating Jacobian, updating state)" link is entered, the Ceres optimization library automatically calculates the Jacobian matrix of the objective function (the total re-projection error) with respect to the pose variable, and iteratively updates the vehicle body pose state according to the matrix; then, the "whether converged" judgment step is performed, if the optimization does not converge, the above process is repeated by returning to the "calculating re-projection error" step, if the convergence condition is met, the "output optimal pose" step is entered, and finally the optimal pose of the current frame of the vehicle is obtained.
[0131] Although the embodiments of the present application have been shown and described above, it should be understood that the above embodiments are exemplary and should not be construed as limiting the present application, and those of ordinary skill in the art can make changes, modifications, replacements and variations to the above embodiments within the scope of the present application.
Claims
1. A method of visual positioning based on prior target position and image detection, characterized in that, Comprise the following steps: Step S1: offline mapping and target position calibration Use the acquisition vehicle equipped with laser radar and / or multi-camera to run the simultaneous localization and mapping (SLAM) algorithm in the area to be positioned, and construct a globally consistent point cloud map; In the point cloud map, the accurate 3D coordinates of the static targets that can be used for positioning in the global map (Map) coordinate system are labeled by manual or automatic identification; Step S2: online visual recognition and target position query When the vehicle is running online, real-time image acquisition is performed by the vehicle-mounted camera; The pre-trained deep learning target detection model is used to process the current image, and the static targets labeled in step S1 in the field of view are identified; For each successfully identified target, the accurate 3D coordinates of the target in the Map coordinate system are queried and obtained from the prior map database established in step S1 according to the category information of the target; Step S3: Construct a re-projection error optimization model for each target identified in step S2, and perform the following operations in turn: S3.1: Coordinate transformation, transform the 3D coordinates of the target in the Map coordinate system to the camera coordinate system through the current estimated vehicle pose and the fixed external parameters between the camera and the vehicle, and obtain the 3D coordinates of the target in the camera coordinate system; S3.2: Projection calculation, use the intrinsic matrix of the camera to project the 3D coordinates of the target in the camera coordinate system to the image pixel plane to obtain the theoretical projection pixel coordinates of the target; S3.3: Error calculation, calculate the deviation between the theoretical projection pixel coordinates of the target and the actual pixel coordinates of the center point or specific key point of the target detection box in step S2 to form a re-projection error term; Step S4: pose optimization Add all the re-projection error terms of the targets in step S3 to construct the overall objective function; Take the initial value of the vehicle pose as the starting point of optimization, and minimize the overall objective function by a nonlinear optimization algorithm to obtain the optimal pose of the current frame of the vehicle.
2. The visual positioning method based on prior target position and image detection according to claim 1, characterized in that, In step S1, the point cloud map is a dense point cloud map or a semi-dense point cloud map; the static target includes a traffic sign, a street lamp, a building corner, and a specific shape object; and the accurate 3D coordinate of the target in a Map coordinate system is marked as P mapi , wherein P mapi = (X mi , Y mi , Z mi ), i represents the i-th target, X mi , Y mi , and Z mi are coordinate values of the i-th target in the X-axis, Y-axis, and Z-axis directions of the Map coordinate system, respectively.
3. The visual positioning method based on prior target position and image detection according to claim 1, characterized in that, In step S2, the deep learning target detection model is a YOLO model or a Faster R-CNN model; and the precise 3D position of the target in the Map coordinate system obtained in step S2 is denoted as P cami , where i represents the i-th target.
4. The visual positioning method based on prior target position and image detection according to claim 1, characterized in that, In step S3.1, the specific process of the coordinate transformation is: transforming the 3D coordinates of the target in the Map coordinate system to coordinates in the camera coordinate system, denoted as P cami , and the transformation relationship satisfies the formula: P cami = R map2cam P mapi + T map2cam ; wherein R map2cam is a rotation matrix from the Map coordinate system to the camera coordinate system, and is an orthogonal matrix of 3x3 dimension; T map2cam is a translation vector from the Map coordinate system to the camera coordinate system; P mapi is the 3D coordinate of the target in the Map coordinate system; The R map2cam and T map2cam Solved by: (a) Solve for the rotation matrix R from the vehicle coordinate system to the Map coordinate system. body2map Translation matrix T body2map :R body2map The calculation satisfies the formula: T body2map The calculation satisfies the formula: T body2map = [x, y, z] T ; Wherein, yaw is the yaw angle of the vehicle pose, pitch is the pitch angle of the vehicle pose, and roll is the roll angle of the vehicle pose; x, y, z are the position parameters of the vehicle pose in the X, Y, Z axes of the Map coordinate system, respectively; (b) Solve for the rotation matrix R from the camera coordinate system to the vehicle coordinate system. cam2body Translation matrix T cam2body :R cam2body The calculation satisfies the formula: T cam2body The calculation satisfies the formula: T cam2body = [tx, ty, tz] T ; Wherein, rz, ry, rx are the Z, Y, X axis direction rotation parameters of the fixed external parameters between the camera and the vehicle; tx, ty, tz are the X, Y, Z axis direction translation parameters of the fixed external parameters between the camera and the vehicle; (c) Calculate R map2cam and T map2cam :R map2cam The calculation satisfies the formula: in For R cam2body The transpose of the matrix, For R body2map The transpose of T; cam2body The calculation satisfies the formula:
5. The visual positioning method based on prior target location and image detection according to claim 1, characterized in that, In step S3.2, the specific process of the projection calculation is as follows: the 3D coordinates of the target in the camera coordinate system are marked as P cami , and P cami =(X ci ,Y ci ,Z ci ), wherein X ci , Y ci , and Z ci are the coordinate values of the i-th target in the X-axis, Y-axis, and Z-axis directions of the camera coordinate system, respectively. The theoretical projected pixel coordinates of the target are denoted as wherein is the horizontal coordinate of the theoretical projected pixel, is the vertical coordinate of the theoretical projected pixel; The projection relationship satisfies the following formula: u proj_i = (fx x X ci / Z ci )+cx v proj_i = (fy x Y ci / Z ci )+ cy Wherein, fx, fy are the X, Y axis direction focal lengths of the camera, cx, cy are the X, Y axis direction coordinate values of the camera optical center in the image pixel plane, and fx, fy, cx, cy are obtained by camera intrinsic calibration.
6. The visual positioning method based on prior target position and image detection according to claim 1, characterized in that, In step S3.3, the error is the Euclidean distance, and the corresponding re-projection error term is denoted as e i , and the calculation of e i satisfies the formula: e i = (u -proj_i - u -obs_i ) 2 + (v -proj_i - v -obs_i ) 2 ; wherein u -proj_i , v -proj_i are the theoretical projected pixel horizontal coordinate, vertical coordinate of the i-th target, respectively; u -obs_i , v -obs_i are the actual pixel horizontal coordinate, vertical coordinate of the center point or the specific key point of the i-th target detection frame in step S2, respectively.
7. The visual positioning method based on prior target location and image detection according to claim 1, characterized in that, In step S4, the overall target function is denoted as E(T -vehicle ), and the construction of E(T -vehicle ) satisfies the formula: where E(T -vehicle ) is the pose of the vehicle body to be optimized, k is the total number of targets successfully recognized in step S2, e i is the re-projection error term of the i-th target; the non-linear optimization algorithm is Levenberg-Marquardt algorithm, and the minimization of the overall objective function is realized through Google's CeresSolver optimization library; the initial value of the pose of the vehicle body is derived from an inertial navigation system (INS), a wheel speed meter or the positioning result of the previous frame; and the optimal pose of the vehicle body is recorded as T -vehicle-ptimal .
8. The visual positioning method based on prior target position and image detection according to claim 1, characterized in that, In step S4, the initial value or constraint condition provided by the global satellite navigation system (GNSS) or inertial navigation system (INS) is also combined to form a multi-sensor fusion pose optimization system.