Vehicle pose determination method and device, electronic equipment and storage medium

Abstract

The application relates to a vehicle pose determination method and device, electronic equipment and a storage medium. The method comprises the following steps: acquiring an initial pose of a vehicle at a current time; acquiring shooting images of reference objects in a preset range of the vehicle collected before the current time, and determining the space coordinates corresponding to each scattered point of the reference objects according to the pixel coordinates of each scattered point of the reference objects in each shooting image; projecting the space coordinates corresponding to each scattered point of the reference objects into a target camera to obtain the projection point coordinates corresponding to each scattered point of the reference objects; comparing the projection point coordinates corresponding to each scattered point of the reference objects with the pixel coordinates of each scattered point of the reference objects acquired by the target camera at the current time, and determining a vehicle pose correction amount; and correcting the initial pose according to the vehicle pose correction amount to obtain a target pose of the vehicle. In this way, the initial pose of the vehicle can be corrected based on the coordinates of each scattered point of the reference objects acquired from the shooting images, so that the accuracy of the vehicle pose is improved.

Description

Technical Field

[0001] This application relates to the field of autonomous driving technology, and in particular to a vehicle pose determination method, device, electronic device and storage medium. Background Technology

[0002] In the process of autonomous driving, vehicle posture is a very important parameter, and its accuracy is directly related to driving safety.

[0003] Currently, vehicle attitude is typically determined using an inertial navigation system (INS), and the accuracy of the Global Positioning System (GPS) signal directly affects the accuracy of the INS. If the GPS signal is interfered with or weakened due to obstruction, the accuracy of the vehicle attitude obtained through the INS will be low. Therefore, improving the accuracy of vehicle attitude has become a pressing technical problem. Summary of the Invention

[0004] This application provides a vehicle pose determination method, apparatus, electronic device, and storage medium to solve the problem of low accuracy of vehicle pose in the prior art when the GPS signal is poor.

[0005] In one aspect, embodiments of this application provide a method for determining vehicle pose, the method comprising:

[0006] Obtain the vehicle's initial pose at the current moment;

[0007] Acquire images of reference objects within a preset range of the vehicle collected before the current time, and determine the spatial coordinates of each scatter point of the reference object based on the pixel coordinates of each scatter point of the reference object in each of the captured images;

[0008] The spatial coordinates corresponding to each scattered point of the reference object are projected onto the target camera to obtain the coordinates of the projected points corresponding to each scattered point of the reference object.

[0009] The coordinates of the projection points corresponding to each scatter point of the reference object are compared with the pixel coordinates of each scatter point of the reference object obtained by the target camera at the current time to determine the vehicle pose correction amount;

[0010] The initial pose is corrected based on the vehicle pose correction amount to obtain the vehicle target pose.

[0011] Optionally, the initial pose includes an initial rotation matrix and an initial translation vector from the vehicle coordinate system to the world coordinate system;

[0012] The step of projecting the spatial coordinates corresponding to each scattered point of the reference object onto the target camera to obtain the projection point coordinates corresponding to each scattered point of the reference object includes:

[0013] Based on the initial rotation matrix, the initial translation vector, and the preset camera intrinsic parameter matrix, the intermediate rotation matrix and intermediate translation vector from the camera coordinate system to the vehicle coordinate system are determined, wherein the camera intrinsic parameter matrix is ​​the intrinsic parameter matrix corresponding to the target camera, and the target camera is any camera on the vehicle;

[0014] Using the initial rotation matrix, the initial translation vector, the camera intrinsic parameter matrix, the intermediate rotation matrix, and the intermediate translation vector, the coordinates of the projection points corresponding to each scattered point of the reference object are calculated.

[0015] Optionally, the formula for calculating the coordinates of the projection points corresponding to each scattered point of the reference object is as follows:

[0016]

[0017] Among them, u proj v represents the x-coordinate of the projected point corresponding to the target scatter point. proj This represents the ordinate of the projected point corresponding to the target scatter point. Let t be the transpose of the initial rotation matrix. wb Represents the initial translation vector. Let t be the transpose of the intermediate rotation matrix. bc Let P represent the intermediate translation vector, K represent the camera intrinsic parameter matrix, and P represent the intermediate translation vector. w The spatial coordinates corresponding to the target scatter point are indicated, where the target scatter point is any scatter point of the reference object in each of the captured images.

[0018] Optionally, the step of comparing the projection point coordinates corresponding to each scattered point of the reference object with the pixel coordinates of each scattered point of the reference object acquired by the target camera at the current time to determine the vehicle pose correction amount includes:

[0019] The coordinates of the projection points corresponding to each scattered point of the reference object are compared with the pixel coordinates of each scattered point of the reference object obtained by the target camera at the current time to determine the observation error of the coordinates of the projection points corresponding to each scattered point of the reference object.

[0020] Based on the observation error, determine the linearization matrix of the observation model;

[0021] The vehicle pose correction amount is determined using a preset extended Kalman filter, the observation error, and the linearization matrix.

[0022] Optionally, the step of comparing the projection point coordinates corresponding to each scattered point of the reference object with the pixel coordinates of each scattered point of the reference object acquired by the target camera at the current time to determine the observation error of the projection point coordinates corresponding to each scattered point of the reference object includes:

[0023] The coordinates of the projection points corresponding to the target scattered points are compared with the pixel coordinates of each scattered point of the reference object acquired by the target camera at the current time. The pixel coordinates of the scattered points that are closest to the projection point coordinates corresponding to the target scattered points are determined from the scattered points acquired by the target camera. The target scattered point is any scattered point of the reference object in each captured image.

[0024] Calculate the distance between the coordinates of the projected point corresponding to the target scatter point and the pixel coordinates of the nearest scatter point, and determine the distance as the observation error of the coordinates of the projected point corresponding to the target scatter point.

[0025] Optionally, determining the vehicle pose correction using a preset extended Kalman filter, the observation error, and the linearization matrix includes:

[0026] The gain of the extended Kalman filter is determined based on the preset extended Kalman filter and the linearization matrix.

[0027] The vehicle pose correction amount is determined based on the gain and the observation error.

[0028] Optionally, the formula for calculating the vehicle pose correction amount is as follows:

[0029] δT wb =Nr;

[0030] Where N = PH T (HPH T +Q) -1 N represents the gain, P represents the covariance of the initial pose, Q represents the zero-mean covariance of the observation error, H represents the linearization matrix, and δT wb This represents the vehicle pose correction amount; r = x proj -x, r represents the observation error, x proj The coordinates of the projection point corresponding to the target scatter point are represented by x, which represents the pixel coordinates of the scatter point acquired by the target camera that is closest to the projection point coordinates of the target scatter point. The target scatter point is any scatter point of the reference object in each of the captured images.

[0031] Optionally, the reference points include lane lines and / or markers;

[0032] The step of correcting the initial pose based on the vehicle pose correction amount to obtain the vehicle target pose includes:

[0033] When the reference point is the lane line, the heading angle and pitch angle in the initial pose are corrected according to the vehicle pose correction amount to obtain the vehicle target pose.

[0034] When the reference point is the benchmark, the heading angle and roll angle in the initial pose are corrected according to the vehicle pose correction amount to obtain the vehicle target pose.

[0035] Optionally, the reference point is a lane line;

[0036] The step of determining the spatial coordinates corresponding to each scattered point of the reference object based on the pixel coordinates of each scattered point of the reference object in each captured image includes:

[0037] Based on the pixel coordinates of each scattered point of the lane line in each captured image, the intermediate coordinates corresponding to each scattered point of the lane line are calculated, wherein the intermediate coordinates are the coordinates of each scattered point of the lane line in the vehicle coordinate system.

[0038] The intermediate coordinates are transformed to the world coordinate system to obtain the spatial coordinates of each scattered point of the lane line.

[0039] Optionally, the reference object is a benchmark;

[0040] The step of determining the spatial coordinates corresponding to each scattered point of the reference object based on the pixel coordinates of each scattered point of the reference object in each captured image includes:

[0041] Obtain the pixel coordinates of each scattered point of the benchmark in two target images, wherein the two target images refer to any two images containing the same benchmark.

[0042] Using the triangulation principle and the pixel coordinates of each point of the benchmark in the two target images, the spatial coordinates corresponding to each point of the benchmark are determined.

[0043] Secondly, embodiments of this application provide a vehicle pose determination device, the device comprising:

[0044] The first acquisition module is used to acquire the initial pose of the vehicle at the current moment;

[0045] The second acquisition module is used to acquire images of reference objects within a preset range of the vehicle collected before the current time, and to determine the spatial coordinates of each scatter point of the reference object based on the pixel coordinates of each scatter point of the reference object in each captured image.

[0046] The projection module is used to project the spatial coordinates corresponding to each scattered point of the reference object onto the target camera to obtain the projection point coordinates corresponding to each scattered point of the reference object.

[0047] The determination module is used to compare the projection point coordinates corresponding to each scattered point of the reference object with the pixel coordinates of each scattered point of the reference object acquired by the target camera at the current time to determine the vehicle pose correction amount;

[0048] The correction module is used to correct the initial pose according to the vehicle pose correction amount to obtain the vehicle target pose.

[0049] Thirdly, embodiments of this application provide an electronic device, including a processor, a communication interface, a memory, and a communication bus, wherein the processor, the communication interface, and the memory communicate with each other through the communication bus;

[0050] Memory, used to store computer programs;

[0051] When a processor executes a program stored in a memory, it implements the steps of the vehicle pose determination method according to any embodiment of the first aspect.

[0052] Fourthly, embodiments of this application provide a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the vehicle pose determination method as described in any embodiment of the first aspect.

[0053] Fifthly, embodiments of this application provide a mobile tool, including the electronic device described in the third aspect.

[0054] In this embodiment, the vehicle's initial pose at the current moment is obtained; images of reference objects within a preset range of the vehicle, captured before the current moment, are obtained, and the spatial coordinates corresponding to each point of the reference object are determined based on the pixel coordinates of each point in the captured images; the spatial coordinates corresponding to each point of the reference object are projected onto a target camera to obtain the projection point coordinates corresponding to each point of the reference object; the projection point coordinates corresponding to each point of the reference object are compared with the pixel coordinates of each point of the reference object acquired by the target camera at the current moment to determine the vehicle pose correction amount; and the initial pose is corrected based on the vehicle pose correction amount to obtain the vehicle target pose. Through this method, the spatial coordinates corresponding to each point of the reference object can be obtained based on the captured images, and the vehicle pose correction amount can be calculated based on the projection point coordinates of the spatial coordinates corresponding to each point of the reference object in the target camera, and then the initial pose can be corrected based on the vehicle pose correction amount. In this way, when the GPS signal is interfered with or poor due to obstruction, the vehicle's initial pose can be corrected based on the coordinates of various scattered points of the reference object obtained from the captured image, thereby improving the accuracy of the vehicle's pose. Attached Figure Description

[0055] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with the invention and, together with the description, serve to explain the principles of the invention.

[0056] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, for those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0057] Figure 1 A flowchart illustrating a vehicle pose determination method provided in an embodiment of this application;

[0058] Figure 2 A schematic diagram illustrating the projection of lane lines from a captured image onto a road surface, provided as an embodiment of this application;

[0059] Figure 3 This is a schematic diagram of the structure of a vehicle pose determination device provided in an embodiment of this application;

[0060] Figure 4 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this application. Detailed Implementation

[0061] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0062] See Figure 1 , Figure 1 This is a flowchart illustrating a vehicle pose determination method provided in an embodiment of this application. Figure 1 As shown, the vehicle pose determination method may include the following steps:

[0063] Step 101: Obtain the initial pose of the vehicle at the current moment.

[0064] Specifically, the initial pose can include the vehicle's position and orientation at the current moment, which can be represented by an initial rotation matrix and an initial translation vector from the vehicle coordinate system to the world coordinate system.

[0065] The initial pose of a vehicle at the current moment can be obtained through the vehicle's own inertial navigation system. That is, the vehicle's initial pose at the current moment is calculated by the vehicle's own inertial navigation system based on the received GPS signals, wheel speed odometer, and information from the inertial measurement unit (IMU) sensor.

[0066] Step 102: Acquire images of reference objects within a preset range of the vehicle collected before the current moment, and determine the spatial coordinates of each point of the reference object based on the pixel coordinates of each point in the captured images.

[0067] Specifically, the aforementioned reference objects may include lane lines on the road, roadside signs, etc. The preset vehicle range can be set according to actual conditions, such as within a range of 10-20 meters. The aforementioned captured images can be obtained by a camera on the vehicle; these captured images refer to multiple frames of the reference objects captured before the current moment, for example, 10 frames captured before the current moment. The aforementioned pixel coordinates refer to the pixel coordinates x = [u, v] of each scattered point of the reference object on the captured image, where u represents the row number of the pixel and v represents the column number of the pixel. The aforementioned spatial coordinates refer to the coordinates of the scattered points in the world coordinate system.

[0068] As an alternative implementation, multiple frames of images of the reference object captured before the current moment can be obtained by a camera on the vehicle. Then, a perceptual vision algorithm is used to identify the reference object from each frame of the image, determine the pixel coordinates of each scattered point of the reference object, and then determine the spatial coordinates corresponding to each scattered point of the reference object based on the pixel coordinates of each scattered point of the reference object in each captured image.

[0069] Step 103: Project the spatial coordinates of each scattered point of the reference object onto the target camera to obtain the projection point coordinates of each scattered point of the reference object.

[0070] Specifically, the target camera can be any camera on the vehicle. The projection point coordinates refer to the pixel coordinates (x, x) of the projected points in the image captured by the target camera, projected from scattered points in the world coordinate system. proj =[u proj v proj ], where u represents the row number of the pixel where the scatter projection point is located, and v represents the column number of the pixel where the scatter projection point is located.

[0071] In this step, after obtaining the spatial coordinates of each scattered point of the reference object in the multi-frame captured images, the obtained spatial coordinates can be projected onto the target camera to obtain the projection point coordinates corresponding to each scattered point of the reference object.

[0072] Step 104: Compare the coordinates of the projection points corresponding to each point of the reference object with the pixel coordinates of each point of the reference object obtained by the target camera at the current moment to determine the vehicle pose correction amount.

[0073] In this step, the coordinates of the projected points corresponding to each point of the reference object can be compared with the pixel coordinates of each point of the reference object acquired by the target camera at the current moment to determine the observation error of the coordinates of the projected points corresponding to each point of the reference object. Then, based on the observation error, the vehicle pose correction amount can be determined.

[0074] Step 105: Correct the initial pose according to the vehicle pose correction amount to obtain the vehicle target pose.

[0075] In this step, parameters such as the heading angle, roll angle, and pitch angle in the initial pose can be corrected according to the vehicle pose correction amount to obtain the target vehicle pose.

[0076] In this embodiment, the spatial coordinates of each scattered point of the reference object can be obtained based on the captured image. Then, based on the projection coordinates of these spatial coordinates onto the target camera, the vehicle pose correction amount is calculated. Finally, the initial pose is corrected based on this correction amount. This allows for correction of the vehicle's initial pose even when GPS signals are interfered with or poorly received due to obstruction. By using the coordinates of the scattered points of the reference object obtained from the captured image, the accuracy of the vehicle pose can be improved.

[0077] Furthermore, the initial pose includes the initial rotation matrix and the initial translation vector from the vehicle coordinate system to the world coordinate system;

[0078] Step 103 above involves projecting the spatial coordinates of each scattered point of the reference object onto the target camera to obtain the coordinates of the projected points corresponding to each scattered point of the reference object. Specifically, this includes:

[0079] Based on the initial rotation matrix, initial translation vector, and preset camera intrinsic parameter matrix, the intermediate rotation matrix and intermediate translation vector from the camera coordinate system to the vehicle coordinate system are determined. The camera intrinsic parameter matrix is ​​the intrinsic parameter matrix corresponding to the target camera, and the target camera is any camera on the vehicle.

[0080] Using the initial rotation matrix, initial translation vector, camera intrinsic parameter matrix, intermediate rotation matrix, and intermediate translation vector, the coordinates of the projection points corresponding to each scattered point of the reference object are calculated.

[0081] In one embodiment, the initial pose of the vehicle at the current moment can be represented by T. wb =[R wb , t wb ] indicates that, among which, R wb Let t represent the initial rotation matrix from the vehicle coordinate system to the world coordinate system. wb This represents the initial translation vector from the vehicle coordinate system to the world coordinate system. Assuming the preset camera intrinsic matrix is ​​K, it can be determined based on the initial rotation matrix R. wb Initial translation vector t wb Using the preset camera intrinsic parameter matrix K, the vehicle pose T, transformed from the camera coordinate system to the vehicle coordinate system, is determined. bc =[R bc , t bc ], where R bc t represents the intermediate rotation matrix from the camera coordinate system to the vehicle coordinate system. bc This represents the intermediate translation vector from the camera coordinate system to the vehicle coordinate system. Then, the initial rotation matrix R can be used... wb Initial translation vector t wb Camera intrinsic parameter matrix K, intermediate rotation matrix R bc and the intermediate translation vector tbc The coordinates of the projection points corresponding to each scattered point of the reference object are calculated.

[0082] By using the above method, the coordinates of the projection points corresponding to each scattered point of the reference object can be obtained. This makes it convenient to determine the vehicle pose correction amount based on the coordinates of the projection points corresponding to each scattered point of the reference object, so as to correct the initial pose of the vehicle at the current moment.

[0083] Furthermore, the formula for calculating the coordinates of the projection points corresponding to each of the scattered points of the above reference object is as follows:

[0084]

[0085] Among them, u proj v represents the x-coordinate of the projected point corresponding to the target scatter point. proj This represents the ordinate of the projected point corresponding to the target scatter point. Let t be the transpose of the initial rotation matrix. wb Represents the initial translation vector. Let t be the transpose of the intermediate rotation matrix. bc Let P represent the intermediate translation vector, K represent the camera intrinsic parameter matrix, and P represent the intermediate translation vector. w This represents the spatial coordinates of the target point, where the target point is any point of the reference object in each captured image.

[0086] In one embodiment, the above formula can be used to obtain the initial rotation matrix R. wb Initial translation vector t wb Camera intrinsic parameter matrix K, intermediate rotation matrix R bc , intermediate translation vector t bc Spatial coordinates P corresponding to the target scatter point w Calculations are performed to obtain the coordinates x of the projected points corresponding to the target scattered points. proj =[u proj v proj Using the same method, the coordinates of the projected points corresponding to each point of the reference object can be calculated. This facilitates the subsequent determination of the vehicle pose correction amount based on the coordinates of the projected points corresponding to each point of the reference object, so as to correct the initial pose of the vehicle at the current moment.

[0087] Further, step 104 above, which compares the coordinates of the projected points corresponding to each scattered point of the reference object with the pixel coordinates of each scattered point of the reference object acquired by the target camera at the current moment to determine the vehicle pose correction amount, specifically includes:

[0088] The coordinates of the projected points corresponding to each scattered point of the reference object are compared with the pixel coordinates of each scattered point of the reference object acquired by the target camera at the current time to determine the observation error of the coordinates of the projected points corresponding to each scattered point of the reference object.

[0089] Based on the observation error, determine the linearization matrix of the observation model;

[0090] The vehicle pose correction is determined using a pre-defined extended Kalman filter, observation error, and linearization matrix.

[0091] Specifically, the aforementioned observation error refers to a matrix composed of the error values ​​of the projection point coordinates corresponding to a certain scattered point of the reference object. The linearization matrix of the aforementioned observation model can be represented by H = [H δR H δp ] indicates that,

[0092]

[0093]

[0094]

[0095]

[0096]

[0097] Where H represents the linearization matrix of the observation model, That is, the linearization matrix of the observation model is a 2x6 matrix, H δR and H δp Each is a 2x3 matrix, x proj P represents the coordinates of the projected points corresponding to the target scattered points. c This represents the coordinates of the target points transformed into the camera coordinate system [X]. c Y c Z c ], R wb Let t represent the initial rotation matrix. wb Let f represent the initial translation vector. x and f y Let be the parameters in the camera intrinsic parameter matrix, where δ represents a correction parameter, and ()^ represents the antisymmetric operation on the matrix.

[0098] In one embodiment, the coordinates of the projection points corresponding to each scattered point of the reference object can be compared with the pixel coordinates of each scattered point of the reference object acquired by the target camera at the current time to determine the observation error of the projection point coordinates corresponding to each scattered point of the reference object. Then, based on the observation error, the linearization matrix of the observation model is determined. Finally, the vehicle pose correction amount is determined using a preset extended Kalman filter, the observation error, and the linearization matrix.

[0099] It should be noted that there is a one-to-one correspondence between the observation error and the linearization matrix of the observation model. For example, suppose 10 frames of images of the reference object were captured before the current moment, with 50 scattered points in each frame, resulting in a total of 500 scattered points. Assume that the target camera captures 80 scattered points in the image at the current moment. Then, each of these 500 scattered points needs to be compared with these 80 scattered points. From these 80 scattered points, the point closest to each of the 500 scattered points is found, thus determining the observation error corresponding to each of the 500 scattered points. In this way, 500 observation errors and 500 linearization matrices corresponding to the 500 scattered points can be obtained.

[0100] In this embodiment, the coordinates of the projection points corresponding to each point of the reference object can be compared with the pixel coordinates of each point of the reference object obtained by the target camera at the current moment to accurately determine the vehicle pose correction amount, thereby facilitating the subsequent correction of the vehicle's initial pose based on the vehicle pose correction amount.

[0101] Further, the above steps involve comparing the coordinates of the projected points corresponding to each scattered point of the reference object with the pixel coordinates of each scattered point of the reference object acquired by the target camera at the current moment to determine the observation error of the coordinates of the projected points corresponding to each scattered point of the reference object, including:

[0102] The coordinates of the projection points corresponding to the target scattered points are compared with the pixel coordinates of each scattered point of the reference object acquired by the target camera at the current time. The pixel coordinates of the scattered points acquired by the target camera are determined from the scattered points that are closest to the coordinates of the projection points corresponding to the target scattered points. The target scattered point is any scattered point of the reference object in each captured image.

[0103] Calculate the distance between the coordinates of the projected point corresponding to the target scatter point and the pixel coordinates of the nearest scatter point, and determine the distance as the observation error of the coordinates of the projected point corresponding to the target scatter point.

[0104] In one embodiment, when determining the observation error of the projection point coordinates corresponding to the target scattered points of the reference object, the projection point coordinates corresponding to the target scattered points can be compared with the pixel coordinates of each scattered point of the reference object acquired by the target camera at the current time. The pixel coordinates of the scattered points acquired by the target camera that are closest to the projection point coordinates corresponding to the target scattered points are determined. Then, the distance between the projection point coordinates corresponding to the target scattered points and the pixel coordinates of the nearest scattered point is calculated, and this distance is determined as the observation error of the projection point coordinates corresponding to the target scattered points. For example, assuming the projection point coordinates corresponding to the target scattered points are represented by x... proj Let x represent the pixel coordinates of the nearest scatter point, then the observation error can be expressed by the formula r = x. proj-x indicates that the distance between the projection point coordinates of each scatter point and the pixel coordinates of its nearest scatter point can be used as an observation error to assist the inertial navigation system on the vehicle in updating the vehicle's pose.

[0105] Furthermore, the above steps, using a preset extended Kalman filter, observation error, and linearization matrix, determine the vehicle pose correction amount, including:

[0106] The gain of the extended Kalman filter is determined based on the preset extended Kalman filter and linearization matrix.

[0107] The vehicle pose correction amount is determined based on the gain and observation error.

[0108] In one embodiment, after determining the linearization matrix of the observation model, the vehicle pose correction can be determined using a preset extended Kalman filter, observation error, and linearization matrix. Specifically, it is assumed that the covariance of the initial pose given by the inertial navigation system is... That is, we can get:

[0109]

[0110] Assume that the observation errors of each scatter point of the reference object follow a zero mean and covariance. The Gaussian distribution can be obtained.

[0111]

[0112] At this point, the gain of the Extended Kalman Filter (EKF) can be obtained using its solution formula. Then, based on the gain and observation error, the vehicle pose correction can be determined. This allows for accurate determination of the vehicle pose correction.

[0113] Furthermore, the formula for calculating the vehicle pose correction is as follows:

[0114] δT wb =Nr;

[0115] Where N = PH T (HPH T +Q) -1 N represents the gain of the extended Kalman filter, P∈R 6x6 P represents the covariance of the initial pose, Q∈R 2x2 Q represents the zero-mean covariance of the observation error, H represents the linearization matrix, and δT wb ∈R 6x1 The vehicle pose correction amount is represented by δR. wb and δt wbComposition, that is r = x proj -x, r represents the observation error, x proj denoted by x, which represents the coordinates of the projection point corresponding to the target scatter point. x represents the pixel coordinates of the scatter points acquired by the target camera that are closest to the coordinates of the projection point corresponding to the target scatter point. The target scatter point is any scatter point of the reference object in each captured image.

[0116] In one embodiment, the vehicle pose correction amount δT can be accurately determined using the above formula. wb At this point, the vehicle pose correction amount δT can be calculated. wb Superimposed on the original pose T wb The target vehicle pose is obtained. Vehicle target pose This can be expressed using the following formula:

[0117]

[0118]

[0119]

[0120] in, This represents the rotation matrix after correction from the vehicle coordinate system to the world coordinate system. Let represent the translation vector corrected from the vehicle coordinate system to the world coordinate system, and Exp represent the exponential mapping from the Lie algebra so(3) to the special orthogonal group SO(3).

[0121] In this embodiment, the vehicle pose correction amount can be accurately calculated, and then the initial pose can be corrected based on the vehicle pose correction amount. In this way, when the GPS signal is interfered with or is poor due to obstruction, the initial pose of the vehicle can be corrected based on the coordinates of various scattered points of the reference object obtained from the captured image, thereby improving the accuracy of the vehicle pose.

[0122] Furthermore, reference points include lane lines and / or markers;

[0123] The initial pose is corrected based on the vehicle pose correction amount to obtain the target vehicle pose, including:

[0124] With lane lines as the reference, the heading and pitch angles in the initial pose are corrected according to the vehicle pose correction amount to obtain the vehicle target pose.

[0125] With the reference object as the benchmark, the heading angle and roll angle in the initial pose are corrected according to the vehicle pose correction amount to obtain the vehicle target pose.

[0126] In one embodiment, the reference point can be at least one of lane lines and markers. Since the points on the lane lines and / or markers have almost no effect on the residuals (i.e., observation errors) when moving along the direction of the lane lines and / or markers, the lane lines and / or markers only affect the initial pose T. wb =[R wb , t wb Some components in the matrix [H] have a corrective effect. Since the marker is almost vertical to the ground, it provides good correction for the yaw angle and some correction for the roll angle. Because the lane lines are located on the road surface and almost parallel to the vehicle, they provide some correction for the yaw and pitch angles. Therefore, when using lane lines to correct the vehicle's attitude, feedback correction can be applied to yaw and pitch, i.e., by modifying the 2x3 matrix H. aR The first column is set to zero to remove interference with roll. When using a benchmark to correct the vehicle's attitude, feedback corrections can be made to yaw and roll, i.e., the 2x3 matrix H is modified accordingly. δR The second column is set to zero to remove interference with the pitch. It should be noted that in a 2x3 matrix H... δR In the diagram, the elements in columns 1 through 3 represent the roll component, pitch component, and yaw component, respectively.

[0127] In this embodiment, semantic information (including lane lines and / or markers) provided by pure visual perception can be used to correct the vehicle's attitude based on the inertial navigation system, thereby improving the robustness and accuracy of the vehicle's localization.

[0128] Furthermore, the reference point is the lane line;

[0129] Step 102 above, based on the pixel coordinates of each scattered point of the reference object in each captured image, determines the spatial coordinates corresponding to each scattered point of the reference object, including:

[0130] Based on the pixel coordinates of each scattered point of the lane line in each captured image, the intermediate coordinates corresponding to each scattered point of the lane line are calculated. The intermediate coordinates are the coordinates of each scattered point of the lane line in the vehicle coordinate system.

[0131] Transform the intermediate coordinates to the world coordinate system to obtain the spatial coordinates of each scattered point of the lane line.

[0132] Specifically, the above intermediate coordinates are the coordinates of each point on the lane line in the vehicle coordinate system.

[0133] In one embodiment, when the reference object is a lane line, the intermediate coordinates corresponding to each scattered point of the lane line can be calculated first based on the pixel coordinates of each scattered point of the lane line in each captured image. Then, the intermediate coordinates are transformed to the world coordinate system to obtain the spatial coordinates corresponding to each scattered point of the lane line. Specifically, lane line-assisted positioning can use a camera to acquire captured images. Based on the vehicle's position and attitude provided by the inertial navigation system, and on the assumption that the vehicle is basically driving close to the road and that the road surface within the vehicle's length is basically a plane (except for extreme cases, which is basically consistent with reality), the lane lines in the captured images can be projected onto the road plane, such as... Figure 2 As shown. Thus, the intermediate coordinates of each scattered point on the lane line can be calculated using the following formula:

[0134]

[0135] in, Let t be the transpose of the intermediate rotation matrix. bc Let K represent the intermediate translation vector, K represent the camera intrinsic parameter matrix, [u, v] represent the pixel coordinates of a point on the lane line, and [x] represent the pixel coordinates of a point on the lane line. b y b [-h] represents the intermediate coordinates of a point on the lane line.

[0136] Then, the intermediate coordinates are transformed to the world coordinate system using the following formula to obtain the spatial coordinates corresponding to each scattered point of the lane line:

[0137] P w =R wb P b +t wb ;

[0138] Among them, P w twb represents the spatial coordinates of the scattered points on the lane line, Pb represents the intermediate coordinates of the scattered points on the lane line, Rwb represents the initial rotation matrix, and twb represents the initial translation vector.

[0139] In this way, by accumulating multiple frames, the coordinates of scattered points of lane lines within a certain range (such as 10 to 20 meters) of the vehicle can be obtained in the world coordinate system.

[0140] Furthermore, the reference point is the benchmark;

[0141] Step 102 above, based on the pixel coordinates of each scattered point of the reference object in each captured image, determines the spatial coordinates corresponding to each scattered point of the reference object, including:

[0142] Obtain the pixel coordinates of each scattered point of the benchmark in two target images, where two target images refer to any two images that contain the same benchmark.

[0143] By using the triangulation principle and the pixel coordinates of each point on the target in the two target images, the spatial coordinates of each point on the target are determined.

[0144] In one embodiment, when the reference object is a benchmark, two target images containing the same benchmark can be obtained from each captured image. Then, the pixel coordinates of each scattered point of the benchmark in the two target images are obtained. Then, the spatial coordinates corresponding to each scattered point of the benchmark are determined using the triangulation principle and the pixel coordinates of each scattered point of the benchmark in the two target images. As an optional implementation, in multi-frame captured images, the benchmark can be identified by perception and tracked through consecutive frames in a short period of time to find the position of the same benchmark in multiple frames. The position of the benchmark in the world coordinate system can be obtained by the triangulation principle, thereby generating sampling points on the benchmark, that is, the spatial coordinates corresponding to each scattered point of the benchmark.

[0145] See Figure 3 , Figure 3 This is a schematic diagram of a vehicle pose determination device provided in an embodiment of this application. Figure 3 As shown, the vehicle pose determination device 300 may include:

[0146] The first acquisition module 301 is used to acquire the initial pose of the vehicle at the current moment;

[0147] The second acquisition module 302 is used to acquire the captured images of reference objects within a preset range of the vehicle collected before the current moment, and determine the spatial coordinates corresponding to each scattered point of the reference object based on the pixel coordinates of each scattered point of the reference object in each captured image.

[0148] The projection module 303 is used to project the spatial coordinates of each scattered point of the reference object onto the target camera to obtain the projection point coordinates of each scattered point of the reference object.

[0149] The determination module 304 is used to compare the coordinates of the projection points corresponding to each scattered point of the reference object with the pixel coordinates of each scattered point of the reference object acquired by the target camera at the current time to determine the vehicle pose correction amount.

[0150] The correction module 305 is used to correct the initial pose according to the vehicle pose correction amount to obtain the target pose of the vehicle.

[0151] Furthermore, the initial pose includes an initial rotation matrix and an initial translation vector from the vehicle coordinate system to the world coordinate system; the projection module 303 includes:

[0152] The first determining submodule is used to determine the intermediate rotation matrix and intermediate translation vector from the camera coordinate system to the vehicle coordinate system based on the initial rotation matrix, the initial translation vector and the preset camera intrinsic parameter matrix. The camera intrinsic parameter matrix is ​​the intrinsic parameter matrix corresponding to the target camera, and the target camera is any camera on the vehicle.

[0153] The first calculation submodule is used to calculate the coordinates of the projection points corresponding to each scattered point of the reference object using the initial rotation matrix, the initial translation vector, the camera intrinsic parameter matrix, the intermediate rotation matrix, and the intermediate translation vector.

[0154] Furthermore, the formula for calculating the coordinates of the projected points corresponding to each scattered point of the reference object is as follows:

[0155]

[0156] Among them, u proj v represents the x-coordinate of the projected point corresponding to the target scatter point. proj This represents the ordinate of the projected point corresponding to the target scatter point. Let t be the transpose of the initial rotation matrix. wb Represents the initial translation vector. Let t be the transpose of the intermediate rotation matrix. bc Let P represent the intermediate translation vector, K represent the camera intrinsic parameter matrix, and P represent the intermediate translation vector. w This represents the spatial coordinates of the target point, where the target point is any point of the reference object in each captured image.

[0157] Furthermore, module 304 is determined to include:

[0158] The second determination submodule is used to compare the coordinates of the projection points corresponding to each scatter point of the reference object with the pixel coordinates of each scatter point of the reference object acquired by the target camera at the current time, and to determine the observation error of the coordinates of the projection points corresponding to each scatter point of the reference object.

[0159] The third determination submodule is used to determine the linearization matrix of the observation model based on the observation error;

[0160] The fourth determination submodule is used to determine the vehicle pose correction amount using a preset extended Kalman filter, observation error, and linearization matrix.

[0161] Furthermore, the second determining submodule includes:

[0162] The comparison unit is used to compare the coordinates of the projection points corresponding to the target scatter point with the pixel coordinates of each scatter point of the reference object acquired by the target camera at the current time, and to determine the pixel coordinates of the scatter point closest to the coordinates of the projection points corresponding to the target scatter point from the scatter points acquired by the target camera. The target scatter point is any scatter point of the reference object in each captured image.

[0163] The first determining unit is used to calculate the distance between the coordinates of the projected point corresponding to the target scatter point and the pixel coordinates of the nearest scatter point, and to determine the distance as the observation error of the coordinates of the projected point corresponding to the target scatter point.

[0164] Furthermore, the fourth determination submodule includes:

[0165] The second determining unit is used to determine the gain of the extended Kalman filter based on a preset extended Kalman filter and linearization matrix.

[0166] The third determining unit is used to determine the vehicle pose correction amount based on the gain and observation error.

[0167] Furthermore, the formula for calculating the vehicle pose correction is as follows:

[0168] δT wb =Nr;

[0169] Where N = PH T (HPH T +Q) -1 N represents the gain, P∈R 6x6 P represents the covariance of the initial pose, Q∈R 2x2 Q represents the zero-mean covariance of the observation error, H represents the linearization matrix, and δT wb This represents the vehicle pose correction amount; r = x proj -x, r represents the observation error, x proj denoted by x, which represents the coordinates of the projection point corresponding to the target scatter point. x represents the pixel coordinates of the scatter points acquired by the target camera that are closest to the coordinates of the projection point corresponding to the target scatter point. The target scatter point is any scatter point of the reference object in each captured image.

[0170] Furthermore, the reference points include lane lines and / or markers; the correction module 305 includes:

[0171] The first correction submodule is used to correct the heading angle and pitch angle in the initial pose according to the vehicle pose correction amount when the reference object is the lane line, so as to obtain the vehicle target pose.

[0172] The second correction submodule is used to correct the heading angle and roll angle in the initial pose based on the vehicle pose correction amount, with the reference object as the benchmark, to obtain the target vehicle pose.

[0173] Furthermore, the reference point is the lane line; the second acquisition module 302 includes:

[0174] The second calculation submodule is used to calculate the intermediate coordinates corresponding to each scattered point of the lane line based on the pixel coordinates of each scattered point of the lane line in each captured image. The intermediate coordinates are the coordinates of each scattered point of the lane line in the vehicle coordinate system.

[0175] The transformation submodule is used to transform intermediate coordinates to the world coordinate system, obtaining the spatial coordinates corresponding to each scattered point of the lane line.

[0176] Furthermore, the reference point is a benchmark; the second acquisition module 302 includes:

[0177] The acquisition submodule is used to acquire the pixel coordinates of each scattered point of the benchmark in two target images. The two target images refer to any two images that contain the same benchmark.

[0178] The fifth determination submodule is used to determine the spatial coordinates of each point of the benchmark by using the triangulation principle and the pixel coordinates of each point of the benchmark in the two target images.

[0179] It should be noted that the vehicle pose determination device 300 can implement the steps of the vehicle pose determination method provided in any of the aforementioned method embodiments and achieve the same technical effect, which will not be elaborated here.

[0180] like Figure 4 As shown in the illustration, this application also provides an electronic device, including a processor 411, a communication interface 412, a memory 413, and a communication bus 414, wherein the processor 411, the communication interface 412, and the memory 413 communicate with each other via the communication bus 414.

[0181] Memory 413 is used to store computer programs;

[0182] In one embodiment of this application, when the processor 411 executes the program stored in the memory 413, it implements the steps of the vehicle pose determination method provided in any of the foregoing method embodiments.

[0183] This application also provides a computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements the steps of the vehicle pose determination method provided in any of the foregoing method embodiments.

[0184] This application also provides a computer program product, which includes a computer program stored on a non-volatile computer-readable storage medium. The computer program includes program instructions, which, when executed by a computer, cause the computer to implement the steps of the vehicle pose determination method provided in any of the foregoing method embodiments.

[0185] This application embodiment also provides a mobile tool, which includes... Figure 4 The electronic device shown, this mobile tool can achieve the aforementioned Figure 4 The functions of the electronic devices shown are the same and can achieve the same technical effects, so they will not be described in detail here. This mobile tool can be any device with mobility capabilities, including vehicles with autonomous or intelligent driving functions (including passenger vehicles (e.g., cars, buses, coaches, minibuses, etc.), cargo vehicles (e.g., ordinary trucks, box trucks, trailer trucks, enclosed trucks, tank trucks, flatbed trucks, container trucks, dump trucks, special structure trucks), special vehicles (e.g., logistics delivery vehicles, automated guided vehicles (AGVs), patrol vehicles, cranes, excavators, bulldozers, loaders, road rollers, off-road engineering vehicles, armored engineering vehicles, sewage treatment vehicles, sanitation vehicles, vacuum trucks, floor scrubbers, water sprinkler trucks, sweeping robots, food delivery robots, shopping guide robots, lawnmowers, golf carts, etc.), vehicles with entertainment functions (e.g., amusement park vehicles, amusement park autonomous driving devices, balance bikes, etc.), rescue vehicles (e.g., fire trucks, ambulances, power repair vehicles, engineering emergency vehicles, etc.)), and robots (e.g., sweeping robots, food delivery robots, etc.).

[0186] It should be noted that, in this document, relational terms such as "first" and "second" are used merely to distinguish one entity or operation from another, without necessarily requiring or implying any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a series of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0187] The above description is merely a specific embodiment of the present invention, enabling those skilled in the art to understand or implement the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the present invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features claimed herein.

Claims

1. A method for determining the position and orientation of a vehicle, characterized in that, The method includes: Obtain the vehicle's initial pose at the current moment; Acquire images of reference objects within a preset range of the vehicle collected before the current time, and determine the spatial coordinates of each scatter point of the reference object based on the pixel coordinates of each scatter point of the reference object in each of the captured images; The spatial coordinates corresponding to each scattered point of the reference object are projected onto the target camera to obtain the coordinates of the projected points corresponding to each scattered point of the reference object. The coordinates of the projection points corresponding to each scatter point of the reference object are compared with the pixel coordinates of each scatter point of the reference object obtained by the target camera at the current time to determine the vehicle pose correction amount; The initial pose is corrected based on the vehicle pose correction amount to obtain the vehicle target pose; The step of comparing the projection point coordinates corresponding to each scattered point of the reference object with the pixel coordinates of each scattered point of the reference object acquired by the target camera at the current time to determine the vehicle pose correction includes: The coordinates of the projection points corresponding to each scattered point of the reference object are compared with the pixel coordinates of each scattered point of the reference object obtained by the target camera at the current time to determine the observation error of the coordinates of the projection points corresponding to each scattered point of the reference object. Based on the observation error, determine the linearization matrix of the observation model; The vehicle pose correction amount is determined using a preset extended Kalman filter, the observation error, and the linearization matrix; The step of comparing the projection point coordinates corresponding to each scattered point of the reference object with the pixel coordinates of each scattered point of the reference object acquired by the target camera at the current time to determine the observation error of the projection point coordinates corresponding to each scattered point of the reference object includes: The coordinates of the projection points corresponding to the target scattered points are compared with the pixel coordinates of each scattered point of the reference object acquired by the target camera at the current time. The pixel coordinates of the scattered points that are closest to the projection point coordinates corresponding to the target scattered points are determined from the scattered points acquired by the target camera. The target scattered point is any scattered point of the reference object in each captured image. Calculate the distance between the coordinates of the projected point corresponding to the target scatter point and the pixel coordinates of the nearest scatter point, and determine the distance as the observation error of the coordinates of the projected point corresponding to the target scatter point; The step of determining the vehicle pose correction amount using a preset extended Kalman filter, the observation error, and the linearization matrix includes: The gain of the extended Kalman filter is determined based on the preset extended Kalman filter and the linearization matrix. The vehicle pose correction amount is determined based on the gain and the observation error.

2. The method according to claim 1, characterized in that, The initial pose includes an initial rotation matrix and an initial translation vector from the vehicle coordinate system to the world coordinate system; The step of projecting the spatial coordinates corresponding to each scattered point of the reference object onto the target camera to obtain the projection point coordinates corresponding to each scattered point of the reference object includes: Based on the initial rotation matrix, the initial translation vector, and the preset camera intrinsic parameter matrix, the intermediate rotation matrix and intermediate translation vector from the camera coordinate system to the vehicle coordinate system are determined, wherein the camera intrinsic parameter matrix is ​​the intrinsic parameter matrix corresponding to the target camera, and the target camera is any camera on the vehicle; Using the initial rotation matrix, the initial translation vector, the camera intrinsic parameter matrix, the intermediate rotation matrix, and the intermediate translation vector, the coordinates of the projection points corresponding to each scattered point of the reference object are calculated.

3. The method according to claim 2, characterized in that, The formula for calculating the coordinates of the projection points corresponding to each scattered point of the reference object is as follows: ； in, This represents the x-coordinate of the projected point corresponding to the target scatter point. This represents the ordinate of the projected point corresponding to the target scatter point. Denotes the transpose of the initial rotation matrix. This represents the initial translation vector. Denotes the transpose of the intermediate rotation matrix. This represents the intermediate translation vector. This represents the camera intrinsic parameter matrix. The spatial coordinates corresponding to the target scatter point are indicated, where the target scatter point is any scatter point of the reference object in each of the captured images.

4. The method according to claim 1, characterized in that, The formula for calculating the vehicle pose correction amount is as follows: ； in, , Indicates the gain. This represents the covariance of the initial pose. This represents the zero-mean covariance of the observation error. Represents the linearized matrix, This represents the vehicle pose correction amount; r=x proj -x, The observation error, x, represents the error. proj The coordinates of the projection point corresponding to the target scatter point are represented by x, which represents the pixel coordinates of the scatter point acquired by the target camera that is closest to the projection point coordinates of the target scatter point. The target scatter point is any scatter point of the reference object in each of the captured images.

5. The method according to claim 1, characterized in that, The reference points include lane lines and / or markers; The step of correcting the initial pose based on the vehicle pose correction amount to obtain the vehicle target pose includes: When the reference point is the lane line, the heading angle and pitch angle in the initial pose are corrected according to the vehicle pose correction amount to obtain the vehicle target pose. When the reference point is the benchmark, the heading angle and roll angle in the initial pose are corrected according to the vehicle pose correction amount to obtain the vehicle target pose.

6. The method according to claim 1, characterized in that, The reference point is the lane line; The step of determining the spatial coordinates corresponding to each scattered point of the reference object based on the pixel coordinates of each scattered point of the reference object in each captured image includes: Based on the pixel coordinates of each scattered point of the lane line in each captured image, the intermediate coordinates corresponding to each scattered point of the lane line are calculated, wherein the intermediate coordinates are the coordinates of each scattered point of the lane line in the vehicle coordinate system. The intermediate coordinates are transformed to the world coordinate system to obtain the spatial coordinates of each scattered point of the lane line.

7. The method according to claim 1, characterized in that, The reference object is a benchmark; The step of determining the spatial coordinates corresponding to each scattered point of the reference object based on the pixel coordinates of each scattered point of the reference object in each captured image includes: Obtain the pixel coordinates of each scattered point of the benchmark in two target images, wherein the two target images refer to any two images containing the same benchmark. Using the triangulation principle and the pixel coordinates of each point of the benchmark in the two target images, the spatial coordinates corresponding to each point of the benchmark are determined.

8. A vehicle position determination device, characterized in that, The device includes: The first acquisition module is used to acquire the initial pose of the vehicle at the current moment; The second acquisition module is used to acquire images of reference objects within a preset range of the vehicle collected before the current time, and to determine the spatial coordinates of each scatter point of the reference object based on the pixel coordinates of each scatter point of the reference object in each captured image. The projection module is used to project the spatial coordinates corresponding to each scattered point of the reference object onto the target camera to obtain the projection point coordinates corresponding to each scattered point of the reference object. The determination module is used to compare the projection point coordinates corresponding to each scattered point of the reference object with the pixel coordinates of each scattered point of the reference object acquired by the target camera at the current time to determine the vehicle pose correction amount; The correction module is used to correct the initial pose according to the vehicle pose correction amount to obtain the vehicle target pose. The determining module includes: The second determining submodule is used to compare the projection point coordinates corresponding to each scattered point of the reference object with the pixel coordinates of each scattered point of the reference object obtained by the target camera at the current time, and determine the observation error of the projection point coordinates corresponding to each scattered point of the reference object. The third determining submodule is used to determine the linearization matrix of the observation model based on the observation error; The fourth determination submodule is used to determine the vehicle pose correction amount using a preset extended Kalman filter, the observation error, and the linearization matrix; The second determining submodule includes: The comparison unit is used to compare the projection point coordinates corresponding to the target scatter point with the pixel coordinates of each scatter point of the reference object acquired by the target camera at the current time, and to determine the pixel coordinates of the scatter point closest to the projection point coordinates corresponding to the target scatter point from the scatter points acquired by the target camera, wherein the target scatter point is any scatter point of the reference object in each captured image; The first determining unit is used to calculate the distance between the coordinates of the projection point corresponding to the target scatter point and the pixel coordinates of the nearest scatter point, and to determine the distance as the observation error of the coordinates of the projection point corresponding to the target scatter point; The fourth determining submodule includes: The second determining unit is used to determine the gain of the extended Kalman filter based on a preset extended Kalman filter and the linearization matrix. The third determining unit is used to determine the vehicle pose correction amount based on the gain and the observation error.

9. An electronic device, characterized in that, It includes a processor, a communication interface, a memory, and a communication bus, wherein the processor, the communication interface, and the memory communicate with each other through the communication bus; Memory, used to store computer programs; When a processor executes a program stored in a memory, it implements the steps of the vehicle pose determination method according to any one of claims 1-7.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of the vehicle pose determination method according to any one of claims 1-7.

11. A mobile tool, characterized in that, Includes the electronic device as described in claim 9.

Images