Vehicle positioning method, device, computer equipment, storage medium and vehicle

Abstract

The present invention relates to the field of positioning technology, and specifically provides a vehicle positioning method, device, computer equipment, storage medium and vehicle, aiming to solve the problem of accurate positioning of vehicles. The method of the present invention includes positioning the current driving road of the vehicle through a two-dimensional map, determining the two-dimensional coordinates of the road point in the two-dimensional rectangular coordinate system; obtaining the relative height of the road point relative to the vehicle, and back-projecting the road point from the world coordinate system to the image coordinate system according to the two-dimensional coordinates and the relative height; back-projecting the road point at the location of the road element around the vehicle to the back-projection position of the image coordinate system and determining the acquisition position of the road point at the location of the road element in the image coordinate system through the image acquisition device of the vehicle, matching the above-mentioned back-projection position with the acquisition position, and then determining the position of the vehicle in the world coordinate system. In the above manner, the position of the vehicle in the world coordinate system can be accurately obtained through the two-dimensional map.

Description

Technical Field

[0001] The present invention relates to the field of positioning technology, and specifically provides a vehicle positioning method, device, computer equipment, storage medium and vehicle. Background Art

[0002] The real-time kinematic technology based on the Global Navigation Satellite System can achieve centimeter-level positioning of the vehicle, but when the communication network between the vehicle and the Global Navigation Satellite System is abnormal, the positioning accuracy will drop from centimeter level to mileage level. After long-term positioning at mileage-level positioning accuracy, positioning drift will occur, making it impossible to obtain the exact position of the vehicle. In this case, the map position of the vehicle in the three-dimensional rectangular coordinate system can be obtained through a three-dimensional map, and the acquisition position of the vehicle in the three-dimensional rectangular coordinate system can be obtained through a three-dimensional image acquisition device. The map position and the acquisition position can be matched to obtain the real position of the vehicle in the world coordinate system. However, if the on-board map set on the vehicle is a two-dimensional map, then only the map position of the vehicle in the two-dimensional rectangular coordinate system can be obtained through the on-board map, and the position of the vehicle in the world coordinate system cannot be obtained by the above-mentioned position matching method.

[0003] Accordingly, the art needs a technical solution to solve the above problems. Summary of the invention

[0004] In order to overcome the above-mentioned defects, the present invention is proposed to provide a vehicle positioning method, device, computer equipment, storage medium and vehicle that solve or at least partially solve the technical problem of how to accurately locate a vehicle and determine the position of the vehicle in a world coordinate system when a global navigation satellite system cannot be used for positioning and the on-board map is a two-dimensional map.

[0005] In a first aspect, the present invention provides a vehicle positioning method, the method comprising:

[0006] The vehicle is positioned on the road it is currently traveling on through a two-dimensional map, and the two-dimensional coordinates of road points on the road it is currently traveling on in a two-dimensional rectangular coordinate system are determined;

[0007] Acquire a relative height of the road point relative to the vehicle, determine the origin, x-axis and y-axis of the world coordinate system according to the origin, x-axis and y-axis of the two-dimensional rectangular coordinate system, use the relative height as the coordinate of the z-axis of the road point in the world coordinate system, and back-project the road point from the world coordinate system to the image coordinate system according to the two-dimensional coordinate of the road point and the relative height;

[0008] Determine the road elements located around the vehicle, obtain the back-projected position of the road point at the location of the road element to the image coordinate system, and obtain the acquisition position of the road point at the location of the road element determined by the image acquisition device of the vehicle in the image coordinate system;

[0009] The back-projected position of the road point at the position of the road element is matched with the acquisition position, and the position of the vehicle in the world coordinate system is determined according to the result of the position matching.

[0010] In a technical solution of the above vehicle positioning method, the step of "obtaining the relative height of the road point relative to the vehicle" specifically includes:

[0011] Obtaining a plane vector of a plane where the vehicle is located in a vehicle body coordinate system and obtaining a unit normal vector of the plane according to the plane vector;

[0012] According to the two-dimensional coordinates of the road point, the coordinate zh of the z axis of the road point in the world coordinate system with the origin, x axis and y axis of the two-dimensional rectangular coordinate system as the origin, x axis and y axis respectively is obtained by solving the following equation:

[0013] (xh-x1)×nx+(yh-y1)×ny+(zh-z1)×nz=0

[0014] Wherein, x1, y1 and z1 represent the coordinates of the x-axis, y-axis and z-axis of the vehicle in the vehicle body coordinate system, nx, ny and nz represent the coordinates of the x-axis, y-axis and z-axis of the unit normal vector in the vehicle body coordinate system, and xh and yh represent the coordinates of the x-axis and y-axis in the two-dimensional coordinates of the road point;

[0015] The relative height of the road point with respect to the vehicle is determined according to the coordinate zh.

[0016] In a technical solution of the above-mentioned vehicle positioning method, the method also includes displaying the road connectivity of the vehicle's current road and the road the vehicle has traveled previously on on a two-dimensional map.

[0017] In a technical solution of the above vehicle positioning method, the step of "matching the back-projected position of the road point at the location of the road element with the acquisition position, and determining the position of the vehicle in the world coordinate system according to the result of the position matching" specifically includes:

[0018] Matching the back-projected position of the road point at the location of the road element with the acquisition position to obtain the posture parameters for converting the device coordinate system of the image acquisition device with the world coordinate system;

[0019] Obtain the back-projected position of the road point at the vehicle's location back-projected to the image coordinate system;

[0020] The position of the vehicle in the world coordinate system is determined according to the back-projected position of the road point at the position of the vehicle and the posture parameters.

[0021] In a technical solution of the above-mentioned vehicle positioning method, the step of "matching the back-projected position of the road point at the location of the road element with the acquisition position to obtain the posture parameters for converting the device coordinate system of the image acquisition device with the world coordinate system" specifically includes:

[0022] The distance error equation shown in the following formula is established based on the back-projected position and the acquisition position of the road point at the location of the road element:

[0023] loss = d(f(A,O),cd)

[0024] Wherein, f(A,O) represents the back-projected position of the current road point, A represents the position of the current road point in the world coordinate system, O represents the pose parameter for converting the device coordinate system and the world coordinate system, cd represents the line segment formed by the acquisition positions of the two road points closest to the back-projected position of the current road point as endpoints, d represents the distance calculation function from the back-projected position f(A,O) to the line segment cd, and loss represents the distance calculated by the distance calculation function d;

[0025] With the goal of making the distance loss less than a preset distance threshold, the Levenberg-Marquardt algorithm is used to iteratively optimize the posture parameter O in the distance error equation, and the posture parameter O that makes the distance loss less than the preset distance threshold is obtained, and the posture parameter O is used as the final posture parameter for converting the device coordinate system of the image acquisition device and the world coordinate system.

[0026] In a technical solution of the above vehicle positioning method, before the step of "determining road elements located around the vehicle, back-projecting the road points at the positions of the road elements to the back-projected positions of the image coordinate system, and determining the acquisition positions of the road points at the positions of the road elements in the image coordinate system by the image acquisition device of the vehicle", the method further includes:

[0027] Capturing images of road points at locations where the road elements are located by at least two image acquisition devices to obtain road point images of the road points at locations where the road elements are located;

[0028] According to the road point image, the acquisition position of the road point at the location of the road element in the image coordinate system is determined.

[0029] In a second aspect, a position determination device is provided, the device comprising:

[0030] A road positioning module is configured to locate the current driving road of the vehicle through a two-dimensional map, and determine the two-dimensional coordinates of the road points on the current driving road in a two-dimensional rectangular coordinate system;

[0031] a road back-projection module, configured to obtain a relative height of the road point relative to the vehicle, determine an origin, an x-axis and a y-axis of a world coordinate system according to the origin, an x-axis and a y-axis of the two-dimensional rectangular coordinate system, use the relative height as a coordinate of the z-axis of the road point in the world coordinate system, and back-project the road point from the world coordinate system to an image coordinate system according to the two-dimensional coordinate of the road point and the relative height;

[0032] A road point position acquisition module, which is configured to determine road elements located around the vehicle, obtain a back-projected position of a road point at the location of the road element to an image coordinate system, and obtain a captured position of the road point at the location of the road element determined by an image acquisition device of the vehicle in the image coordinate system;

[0033] The vehicle position determination module is configured to match the back-projected position of the road point at the location of the road element with the acquisition position, and determine the position of the vehicle in the world coordinate system according to the result of the position matching.

[0034] In a third aspect, a computer device is provided, which includes a processor and a storage device, wherein the storage device is suitable for storing multiple program codes, and the program codes are suitable for being loaded and run by the processor to execute the vehicle positioning method described in any one of the technical solutions of the above-mentioned vehicle positioning method.

[0035] In a fourth aspect, a computer-readable storage medium is provided, wherein a plurality of program codes are stored therein, wherein the program codes are suitable for being loaded and run by a processor to execute the vehicle positioning method described in any one of the technical solutions of the above-mentioned vehicle positioning method.

[0036] In a fifth aspect, a vehicle is provided, which includes the position determination device described in the technical solution of the above-mentioned position determination device or the computer device described in the above-mentioned computer device.

[0037] The above one or more technical solutions of the present invention have at least one or more of the following beneficial effects:

[0038] In the technical solution of the present invention, the current road of the vehicle can be located by a two-dimensional map, and the two-dimensional coordinates of the road point in the current road in the two-dimensional rectangular coordinate system (including the coordinates of the x-axis and y-axis of the vehicle in the two-dimensional rectangular coordinate system) can be determined. At the same time, the relative height of the road point relative to the vehicle is obtained, and the origin, x-axis and y-axis of the world coordinate system are determined according to the origin, x-axis and y-axis of the two-dimensional rectangular coordinate system, that is, the coordinates of the x-axis and y-axis in the above two-dimensional coordinates are used as the coordinates of the x-axis and y-axis of the road point in the world coordinate system (three-dimensional rectangular coordinate system), and then the above relative height is used as the coordinate of the z-axis of the road point in the world coordinate system, and then the road point is back-projected from the world coordinate system to the image coordinate system according to the above two-dimensional coordinates of the road point and the above relative height. After the road point is back-projected to the image coordinate system, the back-projected position of the road point at the position of the road element around the vehicle and the acquisition position of the road point at the position of the road element determined by the image acquisition device of the vehicle can be matched, and then the position of the vehicle in the world coordinate system is determined according to the result of the position matching.

[0039] Through the above method, even if the vehicle's on-board map is a two-dimensional map, when the global navigation satellite system cannot be used for positioning, the position of the vehicle in the world coordinate system can be obtained through the two-dimensional map. At the same time, the positioning accuracy of the vehicle can be adjusted according to the density of road points (the higher the density, the higher the positioning accuracy, and the lower the density, the lower the positioning accuracy), thereby achieving centimeter-level positioning of the vehicle, and no positioning drift will occur after long-term positioning.

[0040] Furthermore, in the technical solution for implementing the present invention, when a vehicle is traveling on a road network comprising a plurality of different road layers, the road on which the vehicle is currently traveling and the roads on which the vehicle has previously traveled can be displayed as connected on a two-dimensional map. After the roads are displayed as connected, the influence of roads in other road layers on the vehicle positioning display using the two-dimensional map can be eliminated, and it can be more clearly displayed on which road and on which position the vehicle is located. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] The disclosure of the present invention will become more easily understood with reference to the accompanying drawings. It is easy for those skilled in the art to understand that these drawings are only for illustrative purposes and are not intended to limit the scope of protection of the present invention. In addition, similar numbers in the figures are used to represent similar components, among which:

[0042] Figure 1 is a schematic flow chart of main steps of a vehicle positioning method according to an embodiment of the present invention;

[0043] Figure 2is a schematic diagram of a plane vector a and a unit normal vector n of a plane where a vehicle is located in a vehicle body coordinate system according to an embodiment of the present invention;

[0044] Figure 3 is a schematic diagram of a line segment formed by a back-projected position a of a road point and collection positions c and d of two road points closest to the back-projected position a as endpoints according to an embodiment of the present invention;

[0045] Figure 4 is a schematic diagram of a road network including a plurality of different road layers according to an embodiment of the present invention;

[0046] Figure 5 It is a schematic diagram of the main structure block diagram of a position determination device according to an embodiment of the present invention.

[0047] Reference numerals list :

[0048] 11: Road positioning module; 12: Road back projection module; 13: Road point position acquisition module; 14: Vehicle position determination module. DETAILED DESCRIPTION

[0049] Some embodiments of the present invention are described below with reference to the accompanying drawings. It should be understood by those skilled in the art that these embodiments are only used to explain the technical principles of the present invention and are not intended to limit the protection scope of the present invention.

[0050] In the description of the present invention, "module" and "processor" may include hardware, software or a combination of the two. A module may include hardware circuits, various suitable sensors, communication ports, and memories, and may also include software parts, such as program codes, or a combination of software and hardware. The processor may be a central processing unit, a microprocessor, an image processor, a digital signal processor, or any other suitable processor. The processor has data and / or signal processing functions. The processor may be implemented in software, hardware, or a combination of the two. Computer-readable storage media include any suitable media that can store program codes, such as magnetic disks, hard disks, optical disks, flash memory, read-only memory, random access memory, and the like.

[0051] Here we first explain some terms involved in the present invention.

[0052] A two-dimensional map is a map that is represented in a two-dimensional form on a plane. In an embodiment of the present invention, by locating a target object through a two-dimensional map, the two-dimensional coordinates of the target object in a two-dimensional rectangular coordinate system can be obtained, that is, the coordinates of the target object on the x-axis and the y-axis in the two-dimensional rectangular coordinate system.

[0053] The world coordinate system refers to a three-dimensional rectangular coordinate system with a point in three-dimensional space as its origin.

[0054] The image coordinate system is a two-dimensional rectangular coordinate system with the center of the image plane formed when the image acquisition device performs image acquisition as the origin.

[0055] The device coordinate system of the image acquisition device is a three-dimensional rectangular coordinate system with the focus center of the image acquisition device as the origin and the optical axis as the z-axis. It should be noted that the device coordinate system of the image acquisition device has the same meaning as the camera coordinate system in the field of computer vision technology, and the device coordinate system will not be described in detail here.

[0056] The pose parameters (internal parameters) converted by the image coordinate system and the device coordinate system can project the points in the image coordinate system to the device coordinate system, and can also back-project the points in the device coordinate system to the image coordinate system. The pose parameters (external parameters) converted by the device coordinate system and the world coordinate system can project the points in the device coordinate system to the world coordinate system, and can also back-project the points in the world coordinate system to the device coordinate system. Further, if the above-mentioned internal and external parameters are used at the same time, the points in the world coordinate system can be back-projected to the image coordinate system. It should be noted that the above-mentioned internal and external parameters have the same meanings as the camera internal parameters and camera external parameters in the field of computer vision technology, respectively. Both include rotation matrices and translation vectors, which will not be repeated here.

[0057] The vehicle body coordinate system refers to a three-dimensional coordinate system, whose origin is at the center of mass of the carrier and the point where the carrier is fixed. The x-axis points to the right along the axis of the carrier, the y-axis points forward, and the z-axis, x-axis and y-axis point to the sky in accordance with the right-hand rule. The vehicle body coordinate system can also be called the "right-front-up (RFU)" coordinate system.

[0058] The following is a description of an embodiment of the vehicle positioning method of the present invention.

[0059] See attached Figure 1 , Figure 1 FIG. 1 is a flow chart of the main steps of a vehicle positioning method according to an embodiment of the present invention. Figure 1 As shown, the vehicle positioning method in the embodiment of the present invention mainly includes the following steps S101 to S105.

[0060] Step S101: positioning the current driving road of the vehicle through a two-dimensional map, and determining the two-dimensional coordinates of the road points on the current driving road in a two-dimensional rectangular coordinate system.

[0061] A two-dimensional map is a map that is represented in two dimensions on a plane. By locating the vehicle's current road through the two-dimensional map, the two-dimensional coordinates of each road point on the current road in the two-dimensional rectangular coordinate system corresponding to the two-dimensional map can be obtained. The two-dimensional coordinates include the coordinates of the road point on the x-axis and the y-axis in the two-dimensional rectangular coordinate system.

[0062] Step S102: Obtain the relative height of the road point with respect to the vehicle.

[0063] The relative height refers to the relative height of the road point relative to the vehicle along the z-axis direction of the world coordinate system in the world coordinate system determined by the origin, x-axis and y-axis of the two-dimensional rectangular coordinate system in step S101.

[0064] Step S103: Determine the origin, x-axis and y-axis of the world coordinate system according to the origin, x-axis and y-axis of the two-dimensional rectangular coordinate system, use the relative height as the coordinate of the z-axis of the road point in the world coordinate system, and back-project the road point from the world coordinate system to the image coordinate system according to the two-dimensional coordinates and the relative height of the road point.

[0065] In this embodiment, an image acquisition device is provided on the vehicle, and the device parameters of the image acquisition device include internal parameters and external parameters. The internal parameters refer to the posture parameters for converting the image coordinate system and the device coordinate system, and the external parameters refer to the posture parameters for converting the device coordinate system and the world coordinate system. After obtaining the two-dimensional coordinates and relative height of the road point, the road point can be back-projected from the world coordinate system to the image coordinate system using the internal parameters and external parameters of the image acquisition device.

[0066] In addition, in some embodiments, an image acquisition device whose image coordinate system is not a two-dimensional rectangular coordinate system may also be used. After the corresponding image is acquired by the image acquisition device through image acquisition, the above image may be converted to a two-dimensional rectangular coordinate system according to the coordinate system conversion relationship between the image coordinate system used by the image acquisition device itself and the two-dimensional rectangular coordinate system, and then the method described in step S103 is performed. For example, the image coordinate system of the image acquisition device may be a three-dimensional rectangular coordinate system, and the image acquisition device may be a three-dimensional camera and radar. In the embodiment of the present invention, the radar includes but is not limited to a millimeter-wave radar and a laser radar. In a preferred embodiment, the radar may be a laser radar.

[0067] Step S104: determine the road elements located around the vehicle, obtain the back-projected positions of the road points at the locations of the road elements to the image coordinate system, and obtain the acquisition positions of the road points at the locations of the road elements determined by the image acquisition device of the vehicle in the image coordinate system.

[0068] Road elements include at least traffic signs on the road and / or other objects that can serve as markings, wherein traffic signs include at least lane lines, stop lines, road signs (such as left turn arrows), traffic lights and traffic signs, and other objects that can serve as markings include at least rod-shaped objects.

[0069] It should be noted that in the embodiment of the present invention, an image recognition method can be used to identify road elements and determine the location of road elements. The embodiment of the present invention does not specifically limit the method for identifying road elements, as long as the road elements can be identified and the location of road elements can be obtained by identifying images (two-dimensional maps and road images collected by the image acquisition device of the vehicle). For example, a road element recognition model based on a neural network can be used to recognize road elements in images.

[0070] After the road point is back-projected from the world coordinate system to the image coordinate system, a back-projection point corresponding to the road point is formed in the image coordinate system, and the position of the back-projection point in the image coordinate system is the back-projection position of the road point.

[0071] The image acquisition device of the vehicle is used to acquire images of road points at the locations of road elements around the vehicle to obtain road point images of the road points at the locations of road elements around the vehicle. Based on these road point images, the acquisition positions of the road points at the locations of the road elements in the image coordinate system can be determined.

[0072] Furthermore, in some embodiments, in order to prevent the image acquisition device from malfunctioning or being blocked, resulting in the inability to acquire images of road points at the locations of road elements around the vehicle, and thus the inability to determine the vehicle's position, at least two image acquisition devices may be used to acquire images of road points at the locations of road elements around the vehicle, thereby improving the reliability of image acquisition and vehicle positioning. Specifically, in these embodiments, at least two image acquisition devices may be used to acquire images of road points at the locations of road elements around the vehicle, and road point images of road points at the locations of road elements around the vehicle may be obtained, and then, based on the road point images, the acquisition positions of road points at the locations of road elements around the vehicle in the image coordinate system may be determined. Before acquiring images of road points by at least two image acquisition devices, these image acquisition devices must be calibrated so as to unify the device coordinate systems of these image acquisition devices. In the embodiments of the present invention, a conventional calibration method for multiple image acquisition devices may be used for calibration, and the embodiments of the present invention do not specifically limit this.

[0073] Step S105: performing position matching on the back-projected position of the road point at the location of the road element and the acquisition position, and determining the position of the vehicle in the world coordinate system according to the result of the position matching.

[0074] The internal parameters of the image acquisition device are inherent parameters of the image acquisition device, which are determined after the production and deployment of the image acquisition device are completed, while the external parameters of the image acquisition device can be adjusted. Therefore, whether the external parameters of the image acquisition device are accurate will greatly affect the accuracy of the conversion between the device coordinate system of the image acquisition device and the world coordinate system. The purpose of the embodiment of the present invention to match the back-projected position of the road point located at the position of the road element around the vehicle with the acquisition position is to obtain accurate external parameters. After obtaining accurate external parameters, the internal parameters and external parameters of the image acquisition device can be used simultaneously to project the back-projected position of the road point in the image coordinate system from the image coordinate system to the world coordinate system (project the road point from the image coordinate system to the device coordinate system according to the internal parameters, and project the road point from the device coordinate system to the world coordinate system according to the external parameters), and further select the position of the road point at the position of the vehicle in the world coordinate system as the position of the vehicle in the world coordinate system. Specifically, in some embodiments, the back-projection position of the road point located at the road elements around the vehicle can be matched with the acquisition position to obtain the posture parameters for converting the device coordinate system of the image acquisition device and the world coordinate system; then, the back-projection position of the road point at the vehicle's location to the image coordinate system is obtained, and the position of the vehicle in the world coordinate system is determined based on the back-projection position and posture parameters of the road point at the vehicle's location.

[0075] Through the method described in the above steps S101 to S105, the position of the vehicle in the world coordinate system can be obtained through the two-dimensional map, so that even if the vehicle's onboard map is a two-dimensional map, the position of the vehicle in the world coordinate system can be obtained when the global navigation satellite system cannot be used for positioning. In addition, the density of road points is different, the spacing between adjacent road points in the world coordinate system is also different, and the number of road points at the location of the vehicle is also different. The spacing and number of road points will affect the accuracy of the position of the vehicle in the world coordinate system (positioning accuracy), and the higher the density, the higher the positioning accuracy, and the lower the density, the lower the positioning accuracy. Therefore, in this embodiment, the positioning accuracy of the vehicle in the world coordinate system can also be adjusted by adjusting the density of road points, so as to achieve centimeter-level positioning of the vehicle when the global navigation satellite system cannot be used for positioning, and no positioning drift will occur after long-term positioning.

[0076] The above steps S102 and S105 are further explained below.

[0077] First, the above step S102 is further explained.

[0078] In some implementations according to the above step S102, since the posture of the vehicle changes with the change of the driving road (for example, if the plane where the driving road is located has a certain angle with the preset horizontal position, then the vehicle has the same angle with the preset horizontal position), therefore, a body coordinate system can be first established according to the vehicle (relative to the world coordinate system, the body coordinate system is a local coordinate system), and the relative height of the road point relative to the vehicle is calculated by using the principle that the vector formed by the road point and the origin of the body coordinate system is perpendicular to the normal vector of the plane where the vehicle is located in the body coordinate system, and the vector product is zero. Specifically, in this implementation, the relative height of the road point relative to the vehicle can be obtained through the following steps S1021 to S1023.

[0079] Step S1021: Obtain the plane vector of the plane where the vehicle is located in the vehicle body coordinate system and obtain the unit normal vector of the plane based on the plane vector.

[0080] The plane where the vehicle is located in the vehicle body coordinate system refers to the plane formed by the x-axis and y-axis in the vehicle body coordinate system. Figure 2 As shown in the figure, x1 and z1 represent the x-axis and z-axis in the vehicle coordinate system, a represents the plane vector of the plane where the vehicle is located, and n represents the unit normal vector of the plane where the vehicle is located. x2 and z2 represent the x-axis and z-axis in the "world coordinate system with the origin of the two-dimensional rectangular coordinate system, the x-axis and the y-axis as the origin, respectively".

[0081] If the vehicle is provided with devices that operate based on data from the vehicle body coordinate system, the plane vector of the plane where the vehicle is located can be directly obtained using these devices. For example, the plane vector of the plane where the vehicle is located in the vehicle body coordinate system can be directly obtained using an accelerometer.

[0082] After obtaining the plane vector of the plane where the vehicle is located, the conventional calculation method of the normal vector in the field of mathematical technology can be used to calculate the unit normal vector of the plane where the vehicle is located according to the plane vector of the plane where the vehicle is located, which will not be elaborated here.

[0083] Step S1022: According to the two-dimensional coordinates of the road point and by solving the following equation, the coordinate zh of the z axis of the road point in the world coordinate system with the origin, x axis and y axis of the two-dimensional rectangular coordinate system as the origin, x axis and y axis respectively is obtained.

[0084] (xh-x1)×nx+(yh-y1)×ny+(zh-z1)×nz=0

[0085] In the above equation, x1, y1 and z1 represent the x-axis, y-axis and z-axis coordinates of the vehicle in the body coordinate system, nx, ny and nz represent the x-axis, y-axis and z-axis coordinates of the unit normal vector in the body coordinate system, and xh and yh represent the x-axis and y-axis coordinates of the road point in the two-dimensional coordinates.

[0086] (xh-x1, yh-y1, zh-z1) actually represents the vector formed by the vehicle and the road point in the vehicle coordinate system, (nx, ny, nz) represents the unit normal vector of the plane where the vehicle is located in the vehicle coordinate system, and the vector (xh-x1, yh-y1, zh-z1) is perpendicular to the vector (nx, ny, nz), so the vector product of these two vectors is equal to zero. Among them, xh and yh can be directly obtained from the two-dimensional coordinates of the road point, and the value of the coordinate zh can be solved by solving the above equation.

[0087] Step S1023: Determine the relative height of the road point with respect to the vehicle according to the coordinate zh.

[0088] Through the method described in steps S1021 to S1023 above, the relative height of the road point with respect to the vehicle can be accurately obtained, which is conducive to accurately converting the road point from the world coordinate system to the image coordinate system.

[0089] The following further describes step S105.

[0090] In some implementations according to the above step S105, when the back-projected position of the road point located at the position of the road element around the vehicle is matched with the acquisition position to obtain the pose parameter for converting the device coordinate system of the image acquisition device with the world coordinate system, this pose parameter can be used as an equation parameter, and a distance error equation is established according to the back-projected position of the road point located at the position of the road element around the vehicle and the acquisition position (the equation parameters of the distance error equation include the above pose parameter), and then a nonlinear iterative optimization algorithm is used, with the distance error equation satisfying the preset convergence condition as the goal, to iteratively optimize the pose parameters in the distance error equation, and obtain the optimized pose parameters as the pose parameters for converting the device coordinate system of the final image acquisition device with the world coordinate system. Specifically, in this implementation, the back-projected position of the road point located at the position of the road element around the vehicle and the acquisition position can be matched with the following steps S1051 to S1052:

[0091] Step S1051: Establish the distance error equation shown in the following formula according to the back-projected position and the acquisition position of the road point located at the position of the road element around the vehicle:

[0092] loss = d(f(A,O),cd)

[0093] In the above distance error equation, f(A,O) represents the back-projected position of the current road point, A represents the position of the current road point in the world coordinate system, O represents the pose parameter for converting the device coordinate system to the world coordinate system, cd represents the line segment formed by the acquisition positions of the two road points closest to the back-projected position of the current road point as endpoints, d represents the distance calculation function from the back-projected position f(A,O) to the line segment cd, and loss represents the distance calculated by the distance calculation function d. Figure 3 As shown, Figure 3 Point a in the figure represents the back-projection point when the road point at the position of the road element is back-projected to the image coordinate system, and points c and d respectively represent the two points of the road point at the position of the road element determined by the image acquisition device that are closest to the back-projection point a in the image coordinate system.

[0094] Step S1052: With the goal of making the distance loss less than a preset distance threshold, the Levenberg-Marquardt algorithm is used to iteratively optimize the posture parameter O in the distance error equation, and the posture parameter O is obtained when the distance loss is less than the preset distance threshold, and the posture parameter O is used as the posture parameter for converting the device coordinate system and the world coordinate system of the final image acquisition device.

[0095] It should be noted that the Levenberg-Marquardt algorithm is a conventional nonlinear least squares algorithm in the field of mathematical technology, and its algorithm principle and calculation process are not described in detail here.

[0096] The above is a further explanation of step S105.

[0097] When a vehicle is traveling on a road network containing multiple different road layers, the heights of different roads cannot be displayed because the two-dimensional map lacks the coordinates of the z-axis in the world coordinate system. Figure 4As shown, when there are multiple roads belonging to different road layers at the same location, these roads will be displayed crosswise on the two-dimensional map, and it is impossible to clearly show which road the vehicle is located on. In a vehicle positioning method according to another embodiment of the present invention, the vehicle positioning method may include steps S101 to S105 in the aforementioned method embodiment, and may also display the road currently traveled by the vehicle and the road previously traveled on the two-dimensional map for road connectivity before or after each step, so as to eliminate the influence of roads in other road layers on the use of the two-dimensional map for vehicle positioning display, and more clearly show which position of which road the vehicle is located on. In a preferred embodiment, before the road point is back-projected from the world coordinate system to the image coordinate system according to the two-dimensional coordinates and relative heights of the road point, the road currently traveled by the vehicle and the road previously traveled on the two-dimensional map may be displayed for road connectivity.

[0098] It should be pointed out that although the various steps in the above embodiments are described in a specific order, those skilled in the art can understand that in order to achieve the effects of the present invention, different steps do not have to be performed in such an order. They can be performed simultaneously (in parallel) or in other orders. These changes are within the scope of protection of the present invention.

[0099] Furthermore, the present invention also provides a position determination device.

[0100] See attached Figure 5 , Figure 5 FIG. 1 is a main structural block diagram of a position determination device according to an embodiment of the present invention. Figure 5As shown, the position determination device in the embodiment of the present invention mainly includes a road positioning module 11, a road back-projection module 12, a road point position acquisition module 13 and a vehicle position determination module 14. In some embodiments, the road positioning module 11 can be configured to locate the current road of the vehicle through a two-dimensional map, and determine the two-dimensional coordinates of the road points in the current road in the two-dimensional rectangular coordinate system; the road back-projection module 12 can be configured to obtain the relative height of the road point relative to the vehicle, determine the origin, x-axis and y-axis of the world coordinate system according to the origin, x-axis and y-axis of the two-dimensional rectangular coordinate system, use the relative height as the coordinate of the z-axis of the road point in the world coordinate system, and back-project the road point from the world coordinate system to the image coordinate system according to the two-dimensional coordinates and relative height of the road point; the road point position acquisition module 13 can be configured to determine the road elements located around the vehicle, obtain the back-projection position of the road point at the location of the road element to the image coordinate system, and obtain the acquisition position of the road point at the location of the road element determined by the image acquisition device of the vehicle in the image coordinate system; the vehicle position determination module 14 can be configured to match the back-projection position of the road point at the location of the road element with the acquisition position, and determine the position of the vehicle in the world coordinate system according to the result of the position matching.

[0101] The above-mentioned position determination device is used to perform Figure 1 The vehicle positioning method embodiment shown in the figure has similar technical principles, technical problems solved and technical effects produced. Technicians in this technical field can clearly understand that for the convenience and conciseness of description, the specific working process and related instructions of the position determination device can refer to the contents described in the embodiment of the vehicle positioning method, which will not be repeated here.

[0102] It is understood by those skilled in the art that the present invention implements all or part of the processes in the method of the above embodiment, and can also be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a computer-readable storage medium, and the computer program can implement the steps of the above-mentioned various method embodiments when executed by the processor. Among them, the computer program includes computer program code, and the computer program code can be in source code form, object code form, executable file or some intermediate form. The computer-readable storage medium may include: any entity or device, medium, U disk, mobile hard disk, disk, optical disk, computer memory, read-only memory, random access memory, electric carrier signal, telecommunication signal and software distribution medium that can carry the computer program code. It should be noted that the content contained in the computer-readable storage medium can be appropriately increased or decreased according to the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, computer-readable storage media do not include electric carrier signals and telecommunication signals.

[0103] Furthermore, the present invention also provides a computer device. In one embodiment of a computer device according to the present invention, the computer device includes a processor and a storage device, the storage device can be configured to store a program for executing the vehicle positioning method of the above method embodiment, and the processor can be configured to execute the program in the storage device, which includes but is not limited to the program for executing the vehicle positioning method of the above method embodiment. For ease of explanation, only the parts related to the embodiment of the present invention are shown. For specific technical details not disclosed, please refer to the method part of the embodiment of the present invention. The computer device can be a device formed by various electronic devices.

[0104] Furthermore, the present invention also provides a computer-readable storage medium. In a computer-readable storage medium embodiment according to the present invention, the computer-readable storage medium can be configured to store a program for executing the vehicle positioning method of the above-mentioned method embodiment, and the program can be loaded and run by the processor to implement the above-mentioned vehicle positioning method. For ease of explanation, only the parts related to the embodiment of the present invention are shown. For specific technical details not disclosed, please refer to the method part of the embodiment of the present invention. The computer-readable storage medium can be a storage device formed by various electronic devices. Optionally, the computer-readable storage medium in the embodiment of the present invention is a non-temporary computer-readable storage medium.

[0105] Furthermore, the present invention also provides a vehicle. In an embodiment of a vehicle according to the present invention, the vehicle may include the position determination device described in the above position determination device embodiment, and may also include the computer device described in the above computer device embodiment. In this embodiment, the vehicle may be an autonomous driving vehicle, an unmanned vehicle, or other vehicles. In addition, according to the type of power source, the vehicle in this embodiment may be a fuel vehicle, an electric vehicle, a hybrid vehicle that mixes electric energy and fuel, or a vehicle that uses other new energy sources.

[0106] Further, it should be understood that since the setting of each module is only for illustrating the functional units of the device of the present invention, the physical devices corresponding to these modules may be the processor itself, or a part of the software in the processor, a part of the hardware, or a part of the combination of software and hardware. Therefore, the number of each module in the figure is only schematic.

[0107] Those skilled in the art will appreciate that the modules in the device can be adaptively split or merged. Such splitting or merging of specific modules will not cause the technical solution to deviate from the principle of the present invention, and therefore, the technical solutions after splitting or merging will fall within the protection scope of the present invention.

[0108] So far, the technical solutions of the present invention have been described in conjunction with the preferred embodiments shown in the accompanying drawings. However, it is easy for those skilled in the art to understand that the protection scope of the present invention is obviously not limited to these specific embodiments. Without departing from the principle of the present invention, those skilled in the art can make equivalent changes or substitutions to the relevant technical features, and the technical solutions after these changes or substitutions will fall within the protection scope of the present invention.

Claims

1. A vehicle positioning method, characterized in that: The method comprises: The vehicle is positioned on the road it is currently traveling on through a two-dimensional map, and the two-dimensional coordinates of road points on the road it is currently traveling on in a two-dimensional rectangular coordinate system are determined; Acquire a relative height of the road point relative to the vehicle, determine the origin, x-axis and y-axis of the world coordinate system according to the origin, x-axis and y-axis of the two-dimensional rectangular coordinate system, use the relative height as the coordinate of the z-axis of the road point in the world coordinate system, and back-project the road point from the world coordinate system to the image coordinate system according to the two-dimensional coordinate of the road point and the relative height; Determine the road elements located around the vehicle, obtain the back-projected position of the road point at the location of the road element to the image coordinate system, and obtain the acquisition position of the road point at the location of the road element in the image coordinate system determined by the image acquisition device of the vehicle; The back-projected position of the road point at the position of the road element is matched with the acquisition position, and the position of the vehicle in the world coordinate system is determined according to the result of the position matching.

2. The vehicle positioning method according to claim 1, characterized in that: The step of "obtaining the relative height of the road point relative to the vehicle" specifically includes: Obtaining a plane vector of a plane where the vehicle is located in a vehicle body coordinate system and obtaining a unit normal vector of the plane according to the plane vector; According to the two-dimensional coordinates of the road point, the coordinate zh of the z axis of the road point in the world coordinate system with the origin, x axis and y axis of the two-dimensional rectangular coordinate system as the origin, x axis and y axis respectively is obtained by solving the following equation: (xh-x1)×nx+(yh-y1)×ny+(zh-z1)×nz=0 Wherein, x1, y1 and z1 represent the coordinates of the x-axis, y-axis and z-axis of the vehicle in the vehicle body coordinate system, nx, ny and nz represent the coordinates of the x-axis, y-axis and z-axis of the unit normal vector in the vehicle body coordinate system, and xh and yh represent the coordinates of the x-axis and y-axis in the two-dimensional coordinates of the road point; The relative height of the road point with respect to the vehicle is determined according to the coordinate zh.

3. The vehicle positioning method according to claim 1, characterized in that: The method also includes displaying, on the two-dimensional map, road connectivity between the road the vehicle is currently traveling on and the road the vehicle has previously traveled on.

4. The vehicle positioning method according to claim 1, characterized in that: The step of "matching the back-projected position of the road point at the location of the road element with the acquisition position, and determining the position of the vehicle in the world coordinate system according to the result of the position matching" specifically includes: Matching the back-projected position of the road point at the location of the road element with the acquisition position to obtain the posture parameters for converting the device coordinate system of the image acquisition device with the world coordinate system; Obtain the back-projected position of the road point at the vehicle's location back-projected to the image coordinate system; The position of the vehicle in the world coordinate system is determined according to the back-projected position of the road point at the position of the vehicle and the posture parameters.

5. The vehicle positioning method according to claim 4, characterized in that: The step of "matching the back-projected position of the road point at the location of the road element with the acquisition position to obtain the posture parameters for converting the device coordinate system of the image acquisition device with the world coordinate system" specifically includes: The distance error equation shown in the following formula is established based on the back-projected position and the acquisition position of the road point at the location of the road element: loss = d(f(A,O),cd) Wherein, f(A,O) represents the back-projected position of the current road point, A represents the position of the current road point in the world coordinate system, O represents the pose parameter for converting the device coordinate system and the world coordinate system, cd represents the line segment formed by the acquisition positions of the two road points closest to the back-projected position of the current road point as endpoints, d represents the distance calculation function from the back-projected position f(A,O) to the line segment cd, and loss represents the distance calculated by the distance calculation function d; With the goal of making the distance loss less than a preset distance threshold, the Levenberg-Marquardt algorithm is used to iteratively optimize the posture parameter O in the distance error equation, and the posture parameter O that makes the distance loss less than the preset distance threshold is obtained, and the posture parameter O is used as the final posture parameter for converting the device coordinate system of the image acquisition device and the world coordinate system.

6. The vehicle positioning method according to claim 1, characterized in that: Before the step of "determining the road elements located around the vehicle, obtaining the back-projected positions of the road points at the locations of the road elements to the image coordinate system, and obtaining the acquisition positions of the road points at the locations of the road elements in the image coordinate system determined by the image acquisition device of the vehicle", the method further includes: Capturing images of road points at locations where the road elements are located by at least two image acquisition devices to obtain road point images of the road points at locations where the road elements are located; According to the road point image, the acquisition position of the road point at the location of the road element in the image coordinate system is determined.

7. A position determination device, characterized in that: The device comprises: A road positioning module is configured to locate the current driving road of the vehicle through a two-dimensional map, and determine the two-dimensional coordinates of the road points on the current driving road in a two-dimensional rectangular coordinate system; a road back-projection module, configured to obtain a relative height of the road point relative to the vehicle, determine an origin, an x-axis and a y-axis of a world coordinate system according to the origin, an x-axis and a y-axis of the two-dimensional rectangular coordinate system, use the relative height as a coordinate of the z-axis of the road point in the world coordinate system, and back-project the road point from the world coordinate system to an image coordinate system according to the two-dimensional coordinate of the road point and the relative height; A road point position acquisition module, which is configured to determine road elements located around the vehicle, obtain a back-projected position of a road point at the location of the road element to an image coordinate system, and obtain a captured position of the road point at the location of the road element in the image coordinate system determined by an image acquisition device of the vehicle; The vehicle position determination module is configured to match the back-projected position of the road point at the location of the road element with the acquisition position, and determine the position of the vehicle in the world coordinate system according to the result of the position matching.

8. A computer device comprising a processor and a storage device, wherein the storage device is suitable for storing a plurality of program codes, characterized in that: The program code is suitable for being loaded and run by the processor to execute the vehicle positioning method according to any one of claims 1 to 6.

9. A computer-readable storage medium storing a plurality of program codes, characterized in that: The program code is suitable for being loaded and run by a processor to execute the vehicle positioning method according to any one of claims 1 to 6.

10. A vehicle, characterized in that: The vehicle comprises the position determination device of claim 7 or the computer device of claim 8.

Images