A vehicle pose determination method and apparatus, vehicle, and storage medium

Abstract

The embodiment of the application provides a vehicle pose determination method and device, a vehicle and a storage medium, the method comprises the following steps: determining an estimated scale according to adjacent two frames of images collected by a camera and an estimated speed detected by a wheel speed meter; acquiring a first residual term determined based on the wheel speed meter, acquiring a second residual term determined based on the IMU, acquiring a third residual term determined based on a space point of a plurality of frames of images collected by the camera and the estimated scale, and performing nonlinear optimization processing on the first residual term, the second residual term and the third residual term to obtain a vehicle position and a vehicle attitude. Therefore, the calculation of the vehicle pose is more accurate, the calculation amount is relatively small, and the demand for a hardware platform is reduced.

Description

Technical Field

[0001] This invention relates to the field of vehicle technology, and in particular to a method for determining the pose of a vehicle, a device for determining the pose of a vehicle, an electronic device, and a computer-readable storage medium. Background Technology

[0002] With the development of vehicle technology, determining vehicle pose has become an essential technology. Current techniques typically involve collecting point cloud data from the vehicle's surrounding environment, matching and associating point clouds across multiple frames, calculating the vehicle's relative displacement over time, and then matching this data with a constructed environmental point cloud map to eliminate accumulated errors and thus determine the vehicle's pose. However, this method utilizes an IMU (Inertial Measurement Unit) and IMU pre-integration to eliminate scale errors. IMU pre-integration presents challenges in scale estimation, especially under constant speed conditions, making it difficult to accurately determine the vehicle's pose. Summary of the Invention

[0003] In view of the above problems, embodiments of the present invention are proposed to provide a vehicle pose determination method, a vehicle pose determination device, a vehicle, and a computer-readable storage medium that overcome or at least partially solve the above problems.

[0004] To address the aforementioned problems, in a first aspect, embodiments of the present invention disclose a method for determining the pose of a vehicle, wherein the vehicle includes a wheel speedometer, a camera, and an IMU, and the method includes:

[0005] The prediction scale is determined based on two adjacent frames of images captured by the camera and the estimated speed detected by the wheel speed meter.

[0006] Obtain a first residual term based on the wheel speed gauge, the first residual term including: the difference between the estimated speed of the wheel speed gauge and the actual speed of the vehicle, and the difference between the estimated angular velocity of the wheel speed gauge and the actual angular velocity of the vehicle;

[0007] Obtain a second residual term based on the IMU, the second residual term including the difference between the pre-integrated rotational attitude and the actual rotational attitude of the IMU, the difference between the pre-integrated velocity and the actual velocity of the IMU, and the difference between the pre-integrated position and the actual position of the IMU;

[0008] Obtain the spatial points based on the multi-frame images acquired by the camera and the third residual term determined by the estimated scale. The third residual term includes: the difference between the estimated position of the camera and the actual position of the camera, and the difference between the estimated pose of the camera and the actual pose of the camera.

[0009] The first residual term, the second residual term, and the third residual term are subjected to nonlinear optimization processing to obtain the vehicle position and vehicle attitude.

[0010] Optionally, determining the prediction scale based on two adjacent frames of images captured by the camera and the predicted speed detected by the wheel speedometer includes:

[0011] Based on the estimated velocity v detected by the wheel speed gauge, the time difference Δt between two adjacent frames, and the formula Δx = vΔt, the estimated displacement Δx is determined.

[0012] For two adjacent frames, the relative translation t under the relative scale is determined based on the epipolar constraint;

[0013] Based on the estimated displacement Δx, the relative translation t, and the formula Δx≈st, the estimated scale s is determined.

[0014] Optionally, the nonlinear optimization processing of the first residual term, the second residual term, and the third residual term to obtain the vehicle position and vehicle attitude includes:

[0015] Nonlinear least squares is used to solve the first residual term, the second residual term, and the third residual term to obtain the vehicle position and vehicle attitude.

[0016] Optionally, the first residual term is:

[0017] R oi =ω ov (|v i -v oi |)+ω ow (|w i -w oi |); where ω ov and ω ow For the weighting of the velocity and angular velocity measured by the wheel speedometer, v oi v is the estimated speed of the wheel speed gauge. i w represents the vehicle's actual speed. oi For the estimated angular velocity of the wheel speed gauge, w i This represents the vehicle's actual angular velocity.

[0018] Optionally, the second residual term is:

[0019] R ik = [ΔX, ΔV, ΔR, ΔB] a ΔB w ] T ,

[0020] Where ΔX is the position difference, ΔV is the velocity difference, ΔR is the rotational attitude difference, and ΔB is the position difference. a For acceleration bias difference, ΔB w This is the angular velocity offset difference. This is the vehicle's actual location. The vehicle's true pose is represented by Δt, where Δt is the time interval between two frames. k ,t k+1 ], For the pre-integration position of the IMU, For the pre-integration speed of the IMU; b is the pre-integral rotational attitude of the IMU; a For image acceleration bias noise, b w This is the angular velocity bias noise of the image.

[0021] Optionally, the third residual term is:

[0022] in For the rotation of the camera, For the camera's translation, s i This represents the estimated scale corresponding to a spatial point. This is the vehicle's actual location. This represents the vehicle's actual posture. The mounting orientation of the camera relative to the IMU. K represents the camera's mounting position relative to the IMU, and K is the camera's intrinsic parameter matrix.

[0023] Secondly, embodiments of the present invention disclose a vehicle pose determination device, wherein the vehicle includes a wheel speedometer, a camera, and an IMU, and the device includes:

[0024] The determination module is used to determine the prediction scale based on two adjacent frames of images captured by the camera and the predicted speed detected by the wheel speed meter;

[0025] The first acquisition module is used to acquire a first residual term determined based on the wheel speed meter. The first residual term includes: the difference between the estimated speed of the wheel speed meter and the actual speed of the vehicle, and the difference between the estimated angular velocity of the wheel speed meter and the actual angular velocity of the vehicle.

[0026] The second acquisition module is used to acquire a second residual term determined based on the IMU. The second residual term includes the difference between the pre-integrated rotational attitude and the actual rotational attitude of the IMU, the difference between the pre-integrated velocity and the actual velocity of the IMU, and the difference between the pre-integrated position and the actual position of the IMU.

[0027] The third acquisition module is used to acquire spatial points based on the multi-frame images acquired by the camera and the third residual term determined by the estimated scale. The third residual term includes: the difference between the estimated position of the camera and the actual position of the camera, and the difference between the estimated pose of the camera and the actual pose of the camera.

[0028] The processing module is used to perform nonlinear optimization processing using the first residual term, the second residual term, and the third residual term to obtain the vehicle position and vehicle attitude.

[0029] Optionally, the determining module includes:

[0030] The first determining submodule is used to determine the estimated displacement Δx based on the estimated speed v detected by the wheel speed gauge, the time difference Δt between two adjacent frames, and the formula Δx = vΔt.

[0031] The second determination submodule is used to determine the relative translation t under the relative scale based on the epipolar constraint for two adjacent frames of images.

[0032] The third determining submodule is used to determine the estimated scale s based on the estimated displacement Δx, the relative translation t, and the formula Δx≈st.

[0033] Optionally, the processing module includes:

[0034] The solution module is used to solve the first residual term, the second residual term, and the third residual term using nonlinear least squares to obtain the vehicle position and vehicle attitude.

[0035] Optionally, the first residual term is:

[0036] R oi =ω ov (|v i -v oi |)+ω ow (|w i -w oi |); where ω ov and ω ow For the weighting of the velocity and angular velocity measured by the wheel speedometer, v oi v is the estimated speed of the wheel speed gauge. i w represents the vehicle's actual speed. oi For the estimated angular velocity of the wheel speed gauge, w i This represents the vehicle's actual angular velocity.

[0037] Optionally, the second residual term is:

[0038] R ik = [ΔX, ΔV, ΔR, ΔB] a ΔB w ] T ,

[0039] Where ΔX is the position difference, ΔV is the velocity difference, ΔR is the rotational attitude difference, and ΔB is the position difference. a For acceleration bias difference, ΔB w This is the angular velocity offset difference. This is the vehicle's actual location. The vehicle's true pose is represented by Δt, where Δt is the time interval between two frames. k ,t k+1 ], For the pre-integration position of the IMU, For the pre-integration speed of the IMU; b is the pre-integral rotational attitude of the IMU; a For image acceleration bias noise, b w This is the angular velocity bias noise of the image.

[0040] Optionally, the third residual term is:

[0041] in For the rotation of the camera, For the camera's translation, s i This represents the estimated scale corresponding to a spatial point. This is the vehicle's actual location. This represents the vehicle's actual posture. The mounting orientation of the camera relative to the IMU. K represents the camera's mounting position relative to the IMU, and K is the camera's intrinsic parameter matrix.

[0042] Thirdly, the present invention discloses an electronic device including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the above-described vehicle pose determination method.

[0043] Fourthly, the present invention discloses a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the above-described vehicle pose determination method.

[0044] The embodiments of the present invention have the following advantages:

[0045] In this embodiment, a prediction scale is determined based on two adjacent frames of images captured by the camera and the predicted speed detected by the wheel speed meter. A first residual term based on the wheel speed meter is obtained, comprising the difference between the predicted speed and the actual vehicle speed, and the difference between the estimated angular velocity and the actual vehicle angular velocity. A second residual term based on the IMU is obtained, comprising the difference between the IMU's pre-integrated rotational attitude and the actual rotational attitude, the difference between the IMU's pre-integrated velocity and the actual velocity, and the difference between the IMU's pre-integrated position and the actual position. A third residual term is obtained based on the spatial points of multiple frames of images captured by the camera and the prediction scale, comprising the difference between the camera's estimated position and the actual camera position, and the difference between the camera's estimated attitude and the actual camera attitude. Nonlinear optimization processing is performed on the first, second, and third residual terms to obtain the vehicle position and vehicle attitude. Therefore, by restoring the scale with the assistance of cameras and wheel speed gauges, the vehicle's pose can be determined more accurately than existing technologies that rely on calculations between sensors such as IMUs. Moreover, the computational load is relatively small, reducing the demand on the hardware platform. Attached Figure Description

[0046] Figure 1 This is a flowchart of the steps of a vehicle pose determination method provided in an embodiment of the present invention;

[0047] Figure 2 This is a structural block diagram of a vehicle pose determination device provided in an embodiment of the present invention. Detailed Implementation

[0048] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0049] Existing technologies use IMU pre-integration to eliminate scale errors and thus determine the vehicle's pose. However, IMU pre-integration can cause some difficulties in scale estimation, especially under constant speed conditions, making it difficult to accurately determine the vehicle's pose.

[0050] Reference Figure 1 The diagram illustrates a flowchart of a vehicle pose determination method according to an embodiment of the present invention. The vehicle includes a wheel speedometer, a camera, and an IMU. The method may specifically include the following steps:

[0051] Step 101: Determine the estimated scale based on two adjacent frames of images captured by the camera and the estimated speed detected by the wheel speed meter.

[0052] In this embodiment of the invention, the vehicle may be equipped with a wheel speed sensor, a camera, and an IMU, wherein the camera may be a monocular camera. During vehicle use, the camera can acquire images in real time, and the wheel speed sensor can detect the vehicle in real time to obtain its estimated speed. The estimated speed of the vehicle is determined based on two adjacent frames acquired by the camera and the estimated speed detected by the wheel speed sensor. In this invention, two adjacent frames can be the image acquired at the current moment and the image acquired at the previous moment. The previous moment is a moment that is 0.5 seconds before the current moment, defined as a preset time interval. For example, if the preset time is 0.5 seconds, then the previous moment is 0.5 seconds before the current moment. Using a monocular camera for image acquisition in this invention can save on hardware costs.

[0053] In one example, the estimated scale can be determined as follows: based on the estimated velocity v detected by the wheel speed gauge, the time difference Δt between two adjacent frames, and the formula Δx = vΔt, the estimated displacement Δx is determined; for two adjacent frames, the relative translation t under the relative scale is determined based on the epipolar constraint.

[0054] Based on the estimated displacement Δx, the relative translation t, and the formula Δx≈st, the estimated scale s is determined.

[0055] Specifically: Obtain the vehicle's velocity V0 (v) in the xoy plane coordinate system from the wheel speed gauge. x ,v y ,0) and angular velocity W0(0,0,w z The coordinate system is: x-axis pointing forward of the vehicle, y-axis pointing to the right side of the vehicle, and z-axis pointing to the ground; acquire two adjacent frames captured by the camera. k and b k+1 The time difference dt; the relative displacement Δx(v) is obtained from the time difference. x *d t ,v y *d t The time difference is 0), and the rotation angle around the z-axis is 0. The ground is a plane, the roll angle around the x-axis is 0, the pitch angle around the y-axis is 0, and the translation along the z-axis is 0. The translation t and rotation R at the relative scale are obtained by using the epipolar constraint of vision. The coordinate system under the first frame image is set as the global coordinate system, and the spatial position of the spatial point P is P(x,y,z). The pixel positions of the spatial point on the two frames are p1 and p2, respectively. The pixel positions satisfy s1p1=KP, s2p2=K(RP+t). In the relationship satisfied by the pixel positions, K is the camera intrinsic parameter matrix, R and t are the relative motion corresponding to the two frames acquired by the camera, R is the rotation, t is the translation, and x1 and x2 are the coordinates of the two pixels in the normalized plane. R and t are obtained by using the essential matrix or fundamental matrix of the epipolar constraint. The essential matrix is ​​E=t∧R, and the fundamental matrix is ​​F=K -T EK-1 x2 T Ex1 = p2 T Fp1 = 0; the symbol ^ in the essential matrix represents the antisymmetric matrix operator, such as... The estimated scale is obtained according to the formula Δx≈st; in the formula, t is the translation under the relative scale, and Δx is the relative displacement estimated by the wheel speed gauge.

[0056] Step 102: Obtain the first residual term determined based on the wheel speed gauge. The first residual term includes: the difference between the estimated speed of the wheel speed gauge and the actual speed of the vehicle, and the difference between the estimated angular velocity of the wheel speed gauge and the actual angular velocity of the vehicle.

[0057] In this embodiment of the invention, a first residual term can be determined based on the wheel speed meter of the vehicle. The first residual term may include: the difference between the wheel speed meter's estimated speed and the vehicle's actual speed, and the difference between the wheel speed meter's estimated angular velocity and the vehicle's actual angle.

[0058] In one example, the first residual term can be determined as follows: R oi =ω ov (|v i -v oi |)+ω ow (|w i -w oi |); where ω ov and ω ow For the weighting of the velocity and angular velocity measured by the wheel speedometer, v oi v is the estimated speed of the wheel speed gauge. i w represents the vehicle's actual speed. oi For the estimated angular velocity of the wheel speed gauge, w i R represents the vehicle's true angular velocity. oi This is the first residual term.

[0059] Step 103: Obtain the second residual term determined based on the IMU. The second residual term includes the difference between the pre-integrated rotational attitude and the actual rotational attitude of the IMU, the difference between the pre-integrated velocity and the actual velocity of the IMU, and the difference between the pre-integrated position and the actual position of the IMU.

[0060] In this embodiment of the invention, after determining the first residual term, a second residual term can be determined based on the IMU, thereby obtaining the second residual term. The second residual term includes the difference between the IMU's pre-integrated rotational attitude and the actual rotational attitude, the difference between the IMU's pre-integrated velocity and the actual velocity, and the difference between the IMU's pre-integrated position and the actual position.

[0061] In one example, the second residual term can be determined as follows: R ik =[ΔX,ΔV,ΔR,ΔB]a ,ΔB w ] T , Where ΔX is the position difference, ΔV is the velocity difference, ΔR is the rotational attitude difference, and ΔB is the position difference. a For acceleration bias difference, ΔB w This is the angular velocity offset difference. This is the vehicle's actual location. The vehicle's true pose is represented by Δt, where Δt is the time interval between two frames. k ,t k+1 ], For the pre-integration position of the IMU, For the pre-integration speed of the IMU; b is the pre-integral rotational attitude of the IMU; a For image acceleration bias noise, b w For the image's angular velocity bias noise, R ik This is the second residual term.

[0062] Step 104: Obtain the spatial points based on the multi-frame images acquired by the camera and the third residual term determined by the estimated scale. The third residual term includes: the difference between the estimated position of the camera and the actual position of the camera, and the difference between the estimated pose of the camera and the actual pose of the camera.

[0063] In this embodiment of the invention, after determining the second residual term, a third residual term can be determined based on the spatial points and estimated scale of multiple frames of images acquired by the camera, thereby obtaining the third residual term. The third residual term may include: the difference between the estimated camera position and the actual camera position, and the difference between the estimated camera pose and the actual camera pose.

[0064] In one example, the third residual term can be determined as follows: in For the rotation of the camera, For the camera's translation, s i This represents the estimated scale corresponding to a spatial point. This is the vehicle's actual location. This represents the vehicle's actual posture. The mounting orientation of the camera relative to the IMU. Let K be the camera's mounting position relative to the IMU, K be the camera intrinsic parameter matrix, and R be the IMU's mounting position. cj This is the third residual term.

[0065] Step 105: Perform nonlinear optimization processing on the first residual term, the second residual term, and the third residual term to obtain the vehicle position and vehicle attitude.

[0066] In this embodiment of the invention, after determining the estimated scale, the first residual term, the second residual term, and the third residual term, nonlinear optimization processing can be performed on the first residual term, the second residual term, and the third residual term to obtain the vehicle position and the vehicle attitude.

[0067] In one example, nonlinear least squares can be used to solve for the first, second, and third residual terms to obtain the vehicle position and attitude. The specific formula is as follows:

[0068]

[0069] In the formula, R oi For the first residual term, R ik For the second residual term, R cj This is the third residual term. For the location of the vehicle, The vehicle's posture.

[0070] In this embodiment, the prediction scale is determined based on two adjacent frames of images captured by the camera and the predicted speed detected by the wheel speed meter; a first residual term determined based on the wheel speed meter is obtained, which includes the difference between the predicted speed of the wheel speed meter and the actual speed of the vehicle, and the difference between the estimated angular velocity of the wheel speed meter and the actual angular velocity of the vehicle; a second residual term determined based on the IMU is obtained, which includes the difference between the pre-integrated rotational attitude of the IMU and the actual rotational attitude, the difference between the pre-integrated speed of the IMU and the actual speed, and the difference between the pre-integrated position of the IMU and the actual position; a third residual term determined based on the spatial points of multiple frames of images captured by the camera and the prediction scale is obtained, which includes the difference between the estimated position of the camera and the actual position of the camera, and the difference between the estimated attitude of the camera and the actual attitude of the camera; nonlinear optimization processing is performed on the first, second, and third residual terms to obtain the vehicle position and vehicle attitude. Therefore, by restoring the scale with the assistance of cameras and wheel speed gauges, the vehicle's pose can be determined more accurately than existing technologies that rely on calculations between sensors such as IMUs. Moreover, the computational load is relatively small, reducing the demand on the hardware platform.

[0071] It should be noted that, for the sake of simplicity, the method embodiments are all described as a series of actions. However, those skilled in the art should understand that the embodiments of the present invention are not limited to the described order of actions, because according to the embodiments of the present invention, some steps can be performed in other orders or simultaneously. Furthermore, those skilled in the art should also understand that the embodiments described in the specification are preferred embodiments, and the actions involved are not necessarily essential to the embodiments of the present invention.

[0072] Reference Figure 2The diagram illustrates a structural block diagram of a vehicle position determination device according to an embodiment of the present invention. The vehicle includes a wheel speed sensor, a camera, and an IMU, and may specifically include the following modules:

[0073] The determination module 201 is used to determine the prediction scale based on two adjacent frames of images captured by the camera and the predicted speed detected by the wheel speed meter;

[0074] The first acquisition module 202 is used to acquire a first residual term determined based on the wheel speed meter. The first residual term includes: the difference between the estimated speed of the wheel speed meter and the actual speed of the vehicle, and the difference between the estimated angular velocity of the wheel speed meter and the actual angular velocity of the vehicle.

[0075] The second acquisition module 203 is used to acquire a second residual term determined based on the IMU. The second residual term includes the difference between the pre-integrated rotational attitude and the actual rotational attitude of the IMU, the difference between the pre-integrated velocity and the actual velocity of the IMU, and the difference between the pre-integrated position and the actual position of the IMU.

[0076] The third acquisition module 204 is used to acquire spatial points based on the multi-frame images acquired by the camera and a third residual term determined by the estimated scale. The third residual term includes: the difference between the estimated position of the camera and the actual position of the camera, and the difference between the estimated pose of the camera and the actual pose of the camera.

[0077] The processing module 205 is used to perform nonlinear optimization processing using the first residual term, the second residual term, and the third residual term to obtain the vehicle position and vehicle attitude.

[0078] Optionally, the determining module 201 includes:

[0079] The first determining submodule is used to determine the estimated displacement Δx based on the estimated speed v detected by the wheel speed gauge, the time difference Δt between two adjacent frames, and the formula Δx = vΔt.

[0080] The second determination submodule is used to determine the relative translation t under the relative scale based on the epipolar constraint for two adjacent frames of images.

[0081] The third determining submodule is used to determine the estimated scale s based on the estimated displacement Δx, the relative translation t, and the formula Δx≈st.

[0082] Optionally, the processing module 205 includes:

[0083] The solution module is used to solve the first residual term, the second residual term, and the third residual term using nonlinear least squares to obtain the vehicle position and vehicle attitude.

[0084] Optionally, the first residual term is:

[0085] R oi =ω ov (|v i -v oi |)+ω ow (|w i -w oi |); where ω ov and ω ow For the weighting of the velocity and angular velocity measured by the wheel speedometer, v oi v is the estimated speed of the wheel speed gauge. i w represents the vehicle's actual speed. oi For the estimated angular velocity of the wheel speed gauge, w i This represents the vehicle's actual angular velocity.

[0086] Optionally, the second residual term is:

[0087] R ik = [ΔX, ΔV, ΔR, ΔB] a ΔB w ] T ,

[0088] Where ΔX is the position difference, ΔV is the velocity difference, ΔR is the rotational attitude difference, and ΔB is the position difference. a For acceleration bias difference, ΔB w This is the angular velocity offset difference. This is the vehicle's actual location. The vehicle's true pose is represented by Δt, where Δt is the time interval between two frames. k ,t k+1 ], For the pre-integration position of the IMU, For the pre-integration speed of the IMU; b is the pre-integral rotational attitude of the IMU; a For image acceleration bias noise, b w This is the angular velocity bias noise of the image.

[0089] Optionally, the third residual term is:

[0090] in For the rotation of the camera, For the camera's translation, s i This represents the estimated scale corresponding to a spatial point. This is the vehicle's actual location. This represents the vehicle's actual posture. The mounting orientation of the camera relative to the IMU. K represents the camera's mounting position relative to the IMU, and K is the camera's intrinsic parameter matrix.

[0091] In this embodiment of the invention, a determining module is used to determine the estimated scale based on two adjacent frames of images acquired by the camera and the estimated speed detected by the wheel speed meter; a first acquisition module is used to acquire a first residual term determined based on the wheel speed meter, the first residual term including: the difference between the estimated speed of the wheel speed meter and the actual speed of the vehicle, and the difference between the estimated angular velocity of the wheel speed meter and the actual angular velocity of the vehicle; a second acquisition module is used to acquire a second residual term determined based on the IMU, the second residual term including: the difference between the pre-integrated rotational attitude of the IMU and the actual rotational attitude, the difference between the pre-integrated speed of the IMU and the actual speed, and the difference between the pre-integrated position of the IMU and the actual position; a third acquisition module is used to acquire a third residual term determined based on the spatial points of multiple frames of images acquired by the camera and the estimated scale, the third residual term including: the difference between the estimated position of the camera and the actual position of the camera, and the difference between the estimated attitude of the camera and the actual attitude of the camera; a processing module is used to perform nonlinear optimization processing on the first residual term, the second residual term and the third residual term to obtain the vehicle position and the vehicle attitude. Therefore, by restoring the scale with the assistance of cameras and wheel speed gauges, the vehicle's pose can be determined more accurately than existing technologies that rely on calculations between sensors such as IMUs. Moreover, the computational load is relatively small, reducing the demand on the hardware platform.

[0092] As the device embodiment is basically similar to the method embodiment, the description is relatively simple, and relevant parts can be found in the description of the method embodiment.

[0093] This invention also provides an electronic device, comprising:

[0094] It includes a processor, a memory, and a computer program stored in the memory and capable of running on the processor. When the computer program is executed by the processor, it implements the various processes of the above-described vehicle pose determination method embodiment and achieves the same technical effect. To avoid repetition, it will not be described again here.

[0095] This invention also provides a computer-readable storage medium storing a computer program. When the computer program is executed by a processor, it implements the various processes of the above-described vehicle pose determination method embodiment and achieves the same technical effect. To avoid repetition, it will not be described again here.

[0096] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. The same or similar parts between the various embodiments can be referred to each other.

[0097] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, apparatus, or computer program products. Therefore, embodiments of the present invention can take the form of entirely hardware embodiments, entirely software embodiments, or embodiments combining software and hardware aspects. Furthermore, embodiments of the present invention can take the form of computer program products implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0098] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, terminal devices (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing terminal device to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing terminal device, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0099] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing terminal device to operate in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0100] These computer program instructions can also be loaded onto a computer or other programmable data processing terminal equipment, causing a series of operational steps to be performed on the computer or other programmable terminal equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable terminal equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0101] Although preferred embodiments of the present invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments as well as all changes and modifications falling within the scope of the embodiments of the present invention.

[0102] Finally, it should be noted that in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply 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 terminal device that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or terminal device. 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 terminal device that includes said element.

[0103] The present invention has provided a detailed description of a method for determining vehicle pose, a device for determining vehicle pose, a vehicle, and a computer-readable storage medium. Specific examples have been used to illustrate the principles and implementation methods of the present invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of the present invention. At the same time, those skilled in the art will recognize that, based on the ideas of the present invention, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of the present invention.

Claims

1. A method for determining the pose of a vehicle, characterized in that, The vehicle includes a wheel speedometer, a camera, and an inertial measurement unit (IMU). The method includes: The prediction scale is determined based on two adjacent frames of images captured by the camera and the estimated speed detected by the wheel speed meter. Obtain a first residual term based on the wheel speed gauge, the first residual term including: the difference between the estimated speed of the wheel speed gauge and the actual speed of the vehicle, and the difference between the estimated angular velocity of the wheel speed gauge and the actual angular velocity of the vehicle; Obtain a second residual term based on the IMU, the second residual term including the difference between the pre-integrated rotational attitude and the actual rotational attitude of the IMU, the difference between the pre-integrated velocity and the actual velocity of the IMU, and the difference between the pre-integrated position and the actual position of the IMU; Obtain the spatial points based on the multi-frame images acquired by the camera and the third residual term determined by the estimated scale. The third residual term includes: the difference between the estimated position of the camera and the actual position of the camera, and the difference between the estimated pose of the camera and the actual pose of the camera. The first residual term, the second residual term, and the third residual term are subjected to nonlinear optimization processing to obtain the vehicle position and vehicle attitude; The second residual term is: 、 、 、 、 、 ； in, For positional difference, Due to speed difference, For rotational attitude difference, For acceleration bias difference, This is the angular velocity offset difference. This is the vehicle's actual location. This represents the vehicle's actual posture. The time interval between two frames , For the pre-integration position of the IMU, For the pre-integration speed of the IMU; The pre-integrated rotational attitude of the IMU; Acceleration bias noise in the image, Angular velocity bias noise for the image; The step of determining the prediction scale based on two adjacent frames of images captured by the camera and the predicted speed detected by the wheel speedometer includes: The estimated speed obtained from the wheel speed gauge Time difference between two adjacent frames and formula Determine the estimated displacement ; For two adjacent image frames, the relative translation at the relative scale is determined based on the epipolar constraint. ; According to the estimated displacement and the relative translation and formula Determine the prediction scale s.

2. The vehicle pose determination method according to claim 1, characterized in that, The nonlinear optimization process performed on the first residual term, the second residual term, and the third residual term to obtain the vehicle position and vehicle attitude includes: Nonlinear least squares is used to solve the first residual term, the second residual term, and the third residual term to obtain the vehicle position and vehicle attitude.

3. The vehicle pose determination method according to claim 1, characterized in that, The first residual term is: ;in, and Weights for the velocity and angular velocity measured by the wheel speed gauge. The estimated speed from the wheel speed gauge. This represents the vehicle's actual speed. For the estimated angular velocity of the wheel speed gauge, This represents the vehicle's actual angular velocity.

4. The vehicle pose determination method according to claim 1, characterized in that, The third residual term is: , , ;in For the rotation of the camera, For camera translation, This represents the estimated scale corresponding to a spatial point. This is the vehicle's actual location. This represents the vehicle's actual posture. The mounting orientation of the camera relative to the IMU. K represents the camera's mounting position relative to the IMU, and K is the camera's intrinsic parameter matrix.

5. A vehicle pose determination device, characterized in that, The vehicle includes a wheel speedometer, a camera, and an IMU; the device includes: The determination module is used to determine the prediction scale based on two adjacent frames of images captured by the camera and the predicted speed detected by the wheel speed meter; The first acquisition module is used to acquire a first residual term determined based on the wheel speed meter. The first residual term includes: the difference between the estimated speed of the wheel speed meter and the actual speed of the vehicle, and the difference between the estimated angular velocity of the wheel speed meter and the actual angular velocity of the vehicle. The second acquisition module is used to acquire a second residual term determined based on the IMU. The second residual term includes the difference between the pre-integrated rotational attitude and the actual rotational attitude of the IMU, the difference between the pre-integrated velocity and the actual velocity of the IMU, and the difference between the pre-integrated position and the actual position of the IMU. The third acquisition module is used to acquire spatial points based on the multi-frame images acquired by the camera and the third residual term determined by the estimated scale. The third residual term includes: the difference between the estimated position of the camera and the actual position of the camera, and the difference between the estimated pose of the camera and the actual pose of the camera. The processing module is used to perform nonlinear optimization processing on the first residual term, the second residual term and the third residual term to obtain the vehicle position and vehicle attitude; The second residual term is: 、 、 、 、 、 ； in, For positional difference, Due to speed difference, For rotational attitude difference, For acceleration bias difference, This is the angular velocity offset difference. This is the vehicle's actual location. This represents the vehicle's actual posture. The time interval between two frames , For the pre-integration position of the IMU, For the pre-integration speed of the IMU; The pre-integrated rotational attitude of the IMU; Acceleration bias noise in the image, Angular velocity bias noise for the image; The determining module includes: The first determining submodule is used to determine the estimated speed based on the wheel speed sensor. Time difference between two adjacent frames and formula Determine the estimated displacement ; The second determination submodule is used to determine the relative translation at a relative scale for two adjacent image frames based on epipolar constraints. ; The third determining submodule is used to determine the estimated displacement. and the relative translation and formula Determine the prediction scale s.

6. A vehicle, characterized in that, include: A processor, a memory, and a computer program stored in the memory and capable of running on the processor, wherein the computer program, when executed by the processor, implements the steps of the vehicle pose determination method as described in any one of claims 1-4.

7. A computer-readable storage medium, characterized in that, A computer program is stored on the computer-readable storage medium, which, when executed by a processor, implements the steps of the vehicle pose determination method as described in any one of claims 1-4.

Images