CN114820769A - Vehicle positioning method and device, computer equipment, storage medium and vehicle - Google Patents

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

Info

Publication number
CN114820769A
CN114820769A CN202210501755.4A CN202210501755A CN114820769A CN 114820769 A CN114820769 A CN 114820769A CN 202210501755 A CN202210501755 A CN 202210501755A CN 114820769 A CN114820769 A CN 114820769A
Authority
CN
China
Prior art keywords
road
coordinate system
vehicle
axis
point
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202210501755.4A
Other languages
Chinese (zh)
Inventor
郝立良
余红松
杨东升
易世春
袁弘渊
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Anhui Weilai Zhijia Technology Co Ltd
Original Assignee
Anhui Weilai Zhijia Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Anhui Weilai Zhijia Technology Co Ltd filed Critical Anhui Weilai Zhijia Technology Co Ltd
Priority to CN202210501755.4A priority Critical patent/CN114820769A/en
Publication of CN114820769A publication Critical patent/CN114820769A/en
Priority to PCT/CN2023/073864 priority patent/WO2023216651A1/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T3/00—Geometric image transformations in the plane of the image
    • G06T3/08—Projecting images onto non-planar surfaces, e.g. geodetic screens
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle
    • G06T2207/30256—Lane; Road marking

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Navigation (AREA)

Abstract

The invention relates to the technical field of positioning, in particular to a vehicle positioning method, a vehicle positioning device, computer equipment, a storage medium and a vehicle, and aims to solve the problem of accurately positioning the vehicle. The method comprises the steps of positioning a current running road of a vehicle through a two-dimensional map, and determining two-dimensional coordinates of a road point in a two-dimensional rectangular coordinate system; acquiring 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 coordinate and the relative height; and carrying out position matching on the back projection position and the acquisition position so as to determine the position of the vehicle in the world coordinate system. By the method, the position of the vehicle in the world coordinate system can be accurately acquired through the two-dimensional map.

Description

Vehicle positioning method and device, computer equipment, storage medium and vehicle
Technical Field
The invention relates to the technical field of positioning, and particularly provides a vehicle positioning method and device, computer equipment, a storage medium and a vehicle.
Background
The centimeter-level positioning of the vehicle can be realized by a Real-time differential positioning (Real-time kinematic) technology based on a Global Navigation Satellite System (Global Navigation Satellite System), but when the communication network of the vehicle and the Global Navigation Satellite System is abnormal, the positioning precision can be reduced to a mileage level from the centimeter level, and the positioning drift can occur after long-time positioning is carried out under the mileage-level positioning precision, so that the accurate position of the vehicle cannot be obtained. In this case, the map position of the vehicle in the three-dimensional rectangular coordinate system can be acquired through the three-dimensional map, the acquisition position of the vehicle in the three-dimensional rectangular coordinate system can be acquired through the three-dimensional image acquisition device, and the real position of the vehicle in the world coordinate system can be obtained by matching the map position with the acquisition position. However, if the on-vehicle map installed on the vehicle is a two-dimensional map, only the map position of the vehicle in the two-dimensional rectangular coordinate system can be obtained from the on-vehicle map, and the position of the vehicle in the world coordinate system cannot be obtained by the above-described position matching method.
Accordingly, there is a need in the art for a solution to the above-mentioned problems.
Disclosure of Invention
In order to overcome the above-mentioned drawbacks, the present invention is proposed to provide a vehicle positioning method, a device, a computer device, a storage medium, and a vehicle, which solve or at least partially solve the technical problem of determining the position of the vehicle in the world coordinate system by accurately positioning the vehicle in the case where the vehicle-mounted map is a two-dimensional map and the positioning cannot be performed by using the global navigation satellite system.
In a first aspect, the present invention provides a vehicle localization method, the method comprising:
positioning a current running road of a vehicle through a two-dimensional map, and determining two-dimensional coordinates of road points in the current running road in a two-dimensional rectangular coordinate system;
acquiring the relative height of the road point relative to a vehicle, determining the origin, the x axis and the y axis of a world coordinate system according to the origin, the x axis and the y axis of the two-dimensional rectangular coordinate system, taking the relative height as the coordinate of the road point on the z axis in the world coordinate system, and back-projecting 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;
determining road elements positioned around a vehicle, acquiring a back projection position of a road point at the position of the road element in an image coordinate system by back projection, and acquiring an acquisition position of the road point at the position of the road element in the image coordinate system, which is determined by an image acquisition device of the vehicle;
and carrying out position matching on the back projection position of the road point at the position of the road element and the acquisition position, and determining the position of the vehicle in a world coordinate system according to the position matching result.
In one embodiment of the above vehicle positioning method, the step of "obtaining the relative height of the road point with respect to the vehicle" specifically includes:
acquiring a plane vector of a plane where a vehicle is located in a vehicle body coordinate system and acquiring a unit normal vector of the plane according to the plane vector;
obtaining the coordinate zh of the road point on the z axis in a world coordinate system with the origin, the x axis and the y axis of the two-dimensional rectangular coordinate system as the origin, the x axis and the y axis respectively according to the two-dimensional coordinates of the road point and by solving the following equation:
(xh-x1)×nx+(yh-y1)×ny+(zh-z1)×nz＝0
wherein x1, y1 and z1 represent coordinates of the vehicle in an x-axis, a y-axis and a z-axis in a body coordinate system, nx, ny and nz represent coordinates of the unit normal vector in the x-axis, the y-axis and the z-axis in the body coordinate system, and xh and yh represent coordinates of the x-axis and the y-axis in two-dimensional coordinates of the road point;
the relative height of the road point with respect to the vehicle is determined from the coordinates zh.
In one technical solution of the above vehicle positioning method, the method further includes performing road communication display on a two-dimensional map on a current driving road and a road that is driven before the current driving road of the vehicle.
In one technical solution of the vehicle positioning method, the step of performing position matching on the back projection position of the road point at the position 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 specifically includes:
carrying out position matching on the back projection position of the road point at the position of the road element and the acquisition position to obtain a pose parameter for converting a device coordinate system and a world coordinate system of the image acquisition device;
acquiring a back projection position of a road point at the position of a vehicle to an image coordinate system in a back projection manner;
and determining the position of the vehicle in a world coordinate system according to the back projection position of the road point at the position of the vehicle and the pose parameter.
In one technical solution of the vehicle positioning method, the step of performing position matching on the back projection position and the collection position of the road point at the position of the road element to obtain the pose parameter for converting the device coordinate system of the image collection device and the world coordinate system specifically includes:
establishing a distance error equation shown in the following formula according to the back projection position and the acquisition position of the road point at the position of the road element:
loss＝d(f(A,O),cd)
wherein f (A, O) represents the back projection position of the current road point, A represents the position of the current road point in a world coordinate system, O represents pose parameters for converting a device coordinate system and the world coordinate system, cd represents a line segment formed by taking the acquisition positions of two road points closest to the back projection position of the current road point as end points, d represents a distance calculation function from the back projection position f (A, O) to the line segment cd, and loss represents the distance calculated by the distance calculation function d;
and performing iterative optimization on the pose parameter O in the distance error equation by adopting a Levenberg-Marquardt algorithm with the distance less than a preset distance threshold as a target, acquiring the pose parameter O when the distance less than the preset distance threshold, and taking the pose parameter O as the final pose parameter for converting the device coordinate system and the world coordinate system of the image acquisition device.
In an aspect of the above vehicle positioning method, before the step of "determining road elements located around the vehicle, back-projecting road points at positions of the road elements to a back-projection position of an image coordinate system, and determining, by an image capturing device of the vehicle, capturing positions of the road points at the positions of the road elements in the image coordinate system", the method further includes:
acquiring images of road points at the positions of the road elements by at least two image acquisition devices to obtain road point images of the road points at the positions of the road elements;
and according to the road point image, determining the acquisition position of the road point at the position of the road element in an image coordinate system.
In a second aspect, there is provided a position determination apparatus, the apparatus comprising:
the road positioning module is configured to position a current driving road of the vehicle through a two-dimensional map and determine two-dimensional coordinates of a road point in the current driving road in a two-dimensional rectangular coordinate system;
a road back projection module configured to acquire a relative height of the road point with respect to a vehicle, determine an origin, an x-axis, and a y-axis of a world coordinate system from the origin, the x-axis, and the y-axis of the two-dimensional rectangular coordinate system, use the relative height as a coordinate of the road point on a z-axis of the world coordinate system, and back-project the road point from the world coordinate system to an image coordinate system from the two-dimensional coordinate of the road point and the relative height;
a road point position acquisition module configured to determine road elements located around a vehicle, acquire a back-projection position of a road point at a position of the road element in an image coordinate system by back-projecting the road point to the image coordinate system, and acquire a collection position of the road point at the position of the road element in the image coordinate system determined by an image collection device of the vehicle;
and the vehicle position determination module is configured to perform position matching on the back projection position of the road point at the position of the road element and the acquisition position, and determine the position of the vehicle in the world coordinate system according to the position matching result.
In a third aspect, a computer apparatus is provided, comprising a processor and a storage device adapted to store a plurality of program codes adapted to be loaded and run by the processor to perform the vehicle localization method of any of the above-mentioned vehicle localization methods.
In a fourth aspect, a computer readable storage medium is provided, having stored therein a plurality of program codes adapted to be loaded and run by a processor to perform the vehicle localization method of any one of the above-mentioned aspects of the vehicle localization method.
In a fifth aspect, a vehicle is provided, where the vehicle includes the position determining apparatus according to the above technical solution or the computer device according to the above computer device.
One or more technical schemes of the invention at least have one or more of the following beneficial effects:
in the technical scheme of the implementation of the invention, a current running road of a vehicle can be positioned through a two-dimensional map, two-dimensional coordinates (including coordinates of an x axis and a y axis of the vehicle in the two-dimensional rectangular coordinate system) of a road point in the current running road in the two-dimensional rectangular coordinate system are determined, meanwhile, the relative height of the road point relative to the vehicle is obtained, an origin, an x axis and a y axis of a world coordinate system are determined according to the origin, the x axis and the y axis of the two-dimensional rectangular coordinate system, namely, the coordinates of the x axis and the y axis in the two-dimensional coordinates are used as coordinates of the x axis and the y axis of the road point in the world coordinate system (three-dimensional rectangular coordinate system), further, the relative height is used as a coordinate of a z axis of the road point in the world coordinate system, and then the road point is back-projected to an image coordinate system from the world coordinate system according to the two-dimensional coordinates and the relative height of the road point. After the road points are back-projected to the image coordinate system, the position matching can be performed according to the back-projection position of the road points at the positions of the road elements around the vehicle to the image coordinate system and the collection position of the road points at the positions of the road elements determined by the image collection device of the vehicle in the image coordinate system, and then the position of the vehicle in the world coordinate system can be determined according to the result of the position matching.
Through the mode, even if the vehicle-mounted map of the vehicle is a two-dimensional map, the position of the vehicle in a world coordinate system can be obtained through the two-dimensional map when the vehicle-mounted map cannot be positioned by using a global navigation satellite system, and meanwhile, the positioning accuracy of the vehicle can be adjusted according to the density degree of road points (the higher the density degree is, the higher the positioning accuracy is, the lower the density degree is), the centimeter-level positioning of the vehicle is realized, and the positioning drift can not occur after the long-time positioning.
Further, in the technical solution of the present invention, when the vehicle travels through a road network including a plurality of different road layers, the road connectivity display may be performed on the two-dimensional map on the road on which the vehicle is currently traveling and the road on which the vehicle has traveled before, and after the road connectivity display is performed on the road, so that the influence of the road on the two-dimensional map on the vehicle positioning display using the two-dimensional map may be eliminated, and which position of the vehicle on which road can be more clearly displayed.
Drawings
The disclosure of the present invention will become more readily understood with reference to the accompanying drawings. As is readily understood by those skilled in the art: these drawings are for illustrative purposes only and are not intended to constitute a limitation on the scope of the present invention. Moreover, in the drawings, like numerals are used to indicate like parts, and in which:
FIG. 1 is a flow chart illustrating the main steps of a vehicle localization method according to an embodiment of the present invention;
FIG. 2 is 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 invention;
fig. 3 is a schematic diagram of a line segment formed by end points of a back projection position a of a road point and collection positions c and d of two road points nearest to the back projection position a according to an embodiment of the present invention;
FIG. 4 is a schematic illustration of a road network comprising a plurality of different road layers according to one embodiment of the present invention;
fig. 5 is a block diagram illustrating a main structure of a position determination apparatus according to an embodiment of the present invention.
List of reference numerals：
11: a road positioning module; 12: a road back projection module; 13: a road point position acquisition module; 14: a vehicle position determination module.
Detailed Description
Some embodiments of the 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 for explaining the technical principle of the present invention, and are not intended to limit the scope of the present invention.
In the description of the present invention, a "module" or "processor" may include hardware, software, or a combination of both. A module may comprise hardware circuitry, various suitable sensors, communication ports, memory, may comprise software components such as program code, or may be a combination of software and hardware. The processor may be a central processing unit, microprocessor, image processor, digital signal processor, or any other suitable processor. The processor has data and/or signal processing functionality. The processor may be implemented in software, hardware, or a combination thereof. The computer readable storage medium includes any suitable medium that can store program code, such as magnetic disks, hard disks, optical disks, flash memory, read-only memory, random-access memory, and the like.
Some terms to which the present invention relates are explained first.
The two-dimensional map is a map expressed on a plane in a two-dimensional form, and in the embodiment of the invention, the target object is positioned through the two-dimensional map, so that the two-dimensional coordinates of the target object in a two-dimensional rectangular coordinate system, namely the coordinates of the target object in the x axis and the y axis in the two-dimensional rectangular coordinate system, can be obtained.
The world coordinate system is a three-dimensional rectangular coordinate system with one point in a three-dimensional space as an origin.
The image coordinate system is a two-dimensional rectangular coordinate system in which the center of an imaging plane imaged by the image acquisition device is an origin.
The device coordinate system of the image acquisition device is a three-dimensional rectangular coordinate system established by taking the focusing center of the image acquisition device as an origin and taking the optical axis as a z axis. It should be noted that the device coordinate system of the image capturing device has the same meaning as the camera coordinate system in the computer vision technical field, and the device coordinate system is not described herein again.
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, and further can back project the points in the world coordinate system to the image coordinate system if the internal parameters and the external parameters are simultaneously utilized. It should be noted that the above internal reference and external reference have the same meanings as the camera internal reference and the camera external reference in the technical field of computer vision, and both include a rotation matrix and a translation vector, which are not described herein again.
The vehicle body coordinate system is a three-dimensional coordinate system, the origin of the three-dimensional coordinate system is located at the position where the carrier mass center is fixedly connected with the carrier, the x axis points to the right along the axial direction of the carrier, the y axis points to the front, the z axis, the x axis and the y axis meet the requirement that the right-hand rule points to the sky direction, and the vehicle body coordinate system can also be called as a 'right-front-upper (R-F-U)' coordinate system.
The following describes an embodiment of a vehicle positioning method according to the present invention.
Referring to FIG. 1, FIG. 1 is a flow chart illustrating the main steps of a vehicle locating method according to one embodiment of the present invention. As shown in fig. 1, the vehicle positioning method in the embodiment of the present invention mainly includes the following steps S101 to S105.
Step S101: and positioning the current running road of the vehicle through a two-dimensional map, and determining the two-dimensional coordinates of the road points in the current running road in a two-dimensional rectangular coordinate system.
The two-dimensional map is a map expressed on a plane in a two-dimensional form, the current running road of the vehicle is positioned through the two-dimensional map, and two-dimensional coordinates of each road point in the current running road in a two-dimensional rectangular coordinate system corresponding to the two-dimensional map can be obtained, wherein the two-dimensional coordinates comprise coordinates of an x axis and a y axis of the road point in the two-dimensional rectangular coordinate system.
Step S102: the relative height of the road point with respect to the vehicle is obtained.
The relative height refers to the relative height of the road point with respect to the vehicle in the z-axis direction of the world coordinate system in the world coordinate system determined with the origin of the two-dimensional rectangular coordinate system, the x-axis, and the y-axis in step S101.
Step S103: and determining the origin, the x axis and the y axis of a world coordinate system according to the origin, the x axis and the y axis of the two-dimensional rectangular coordinate system, taking the relative height as the coordinate of the road point on the z axis in the world coordinate system, and back-projecting the road point from the world coordinate system to an image coordinate system according to the two-dimensional coordinate and the relative height of the road point.
In this embodiment, the vehicle is provided with an image capturing device, and device parameters of the image capturing device include an internal reference and an external reference, the internal reference is a pose parameter obtained by converting an image coordinate system and a device coordinate system, and the external reference is a pose parameter obtained by converting the device coordinate system and a world coordinate system. After obtaining the two-dimensional coordinates and relative heights of the road points, the road points may be back-projected from the world coordinate system to the image coordinate system using the internal and external parameters of the image capture device.
In addition, in some embodiments, an image capturing device whose image coordinate system is not a two-dimensional rectangular coordinate system may also be used, and after the image capturing device captures an image to obtain a corresponding image, the image may be converted into the two-dimensional rectangular coordinate system according to a coordinate system conversion relationship between the image coordinate system used by the image capturing device itself and the two-dimensional rectangular coordinate system, and then the method described in step S103 may be performed. For example, the image coordinate system of the image capturing device may be a three-dimensional rectangular coordinate system or the like, and the image capturing device may be a three-dimensional camera, a radar or the like. In embodiments of the present invention, the Radar includes, but is not limited to, Millimeter-wave Radar (Millimeter-wave Radar) and Laser Radar (Laser Radar), and in a preferred embodiment the Radar may be a Laser Radar.
Step S104: the method comprises the steps of determining road elements located around a vehicle, obtaining a back projection position of a road point at the position of the road element in the image coordinate system through back projection, and obtaining a collection position of the road point at the position of the road element in the image coordinate system, wherein the collection position is determined by an image collection device of the vehicle.
The road elements at least comprise traffic signs and/or other objects capable of playing a role of identification on the road, wherein the traffic signs at least comprise lane lines, stop lines, road sign signs (such as left-turning arrows), traffic display lamps, traffic signboards and the like, and the other objects capable of playing a role of identification at least comprise rod-shaped objects.
It should be noted that, in the embodiment of the present invention, an image recognition method may be adopted to perform road element recognition, and determine the position of the road element. The embodiment of the invention does not specifically limit the method for identifying the road elements, as long as the method can identify the images (the two-dimensional map and the road images acquired by the image acquisition device of the vehicle), determine the road elements and obtain the positions of the road elements. For example, road element recognition can be performed on the image by adopting a road element recognition model established based on a neural network.
After the road point is back projected to the image coordinate system from the world 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.
The image acquisition device of the vehicle is used for carrying out image acquisition on road points at the positions of the road elements around the vehicle to obtain road point images of the road points at the positions of the road elements around the vehicle, and the acquisition positions of the road points at the positions of the road elements in the image coordinate system can be determined according to the road point images.
Further, in some embodiments, in order to prevent the image acquisition devices from being faulty or being blocked, which may result in that image acquisition cannot be performed on the road points at the positions where the road elements around the vehicle are located, and further, determination of the vehicle position cannot be achieved, image acquisition may be performed on the road points at the positions where the road elements around the vehicle are located by using at least two image acquisition devices, so that reliability of image acquisition and vehicle positioning is improved. Specifically, in these embodiments, at least two image capturing devices may capture images of road points located at positions of road elements around the vehicle, so as to obtain road point images of road points located at positions of road elements around the vehicle, and then determine, according to the road point images, capturing positions of road points located at positions of road elements around the vehicle in the image coordinate system. Before the image acquisition of the road point by at least two image acquisition devices, the image acquisition devices are calibrated so as to unify the device coordinate systems of the image acquisition devices. In the embodiment of the present invention, a conventional calibration method for multiple image capturing devices may be adopted for calibration, which is not specifically limited in the embodiment of the present invention.
Step S105: and carrying out position matching on the back projection position of the road point at the position of the road element and the acquisition position, and determining the position of the vehicle in the world coordinate system according to the position matching result.
The internal parameters of the image acquisition device are the intrinsic parameters of the image acquisition device, and are determined after the image acquisition device is produced and allocated, and the external parameters of the image acquisition device can be adjusted. Therefore, whether the external parameters of the image acquisition device are accurate or not will greatly affect the accuracy of the image acquisition device in converting the device coordinate system and the world coordinate system. The purpose of carrying out position matching on the back projection position and the acquisition position of the road point at the position of the road element around the vehicle is to acquire accurate external parameters, after the accurate external parameters are acquired, the back projection position of the road point in the image coordinate system can be projected to a world coordinate system from the image coordinate system by using the internal parameters and the external parameters of the image acquisition device at the same time (the road point is projected to the device coordinate system from the image coordinate system according to the internal parameters and then projected to the world coordinate system from the device coordinate system according to the external parameters), and the position of the road point at the position of the vehicle in the world coordinate system is further selected as the position of the vehicle in the world coordinate system. Specifically, in some embodiments, the position of the back projection position of the road point at the position of the road element around the vehicle may be matched with the acquisition position to obtain the pose parameter of the image acquisition device for converting the device coordinate system and the world coordinate system; and then obtaining the back projection position of the road point at the position of the vehicle to the image coordinate system, and determining the position of the vehicle in the world coordinate system according to the back projection position and the pose parameter of the road point at the position of the vehicle.
By the method described in the above steps S101 to S105, the position of the vehicle in the world coordinate system can be acquired by the two-dimensional map, and thus even if the vehicle-mounted map of the vehicle is a two-dimensional map, the position of the vehicle in the world coordinate system can be acquired in a case where positioning by the global navigation satellite system is not possible. In addition, the density degree of the road points is different, the distance between adjacent road points in a world coordinate system is also different, and the number of the road points at the position of the vehicle is also different. The distance and the number of the road points affect the accuracy (positioning accuracy) of the position of the vehicle in the world coordinate system, and the positioning accuracy is higher when the density degree is higher, and the positioning accuracy is lower when the density degree is lower. Therefore, in the embodiment, the positioning accuracy of the vehicle in the world coordinate system can be adjusted by adjusting the density degree of the road points, so that centimeter-level positioning of the vehicle can be realized under the condition that the positioning cannot be performed by using a global navigation satellite system, and the positioning drift cannot occur after long-time positioning.
The following further describes step S102 and step S105.
First, the above step S102 will be further explained.
In some embodiments according to step S102, since the attitude of the vehicle changes with the change of the driving road (if the plane of the driving road has a certain angle with the preset horizontal position, the vehicle has the same angle with the preset horizontal position), a vehicle coordinate system (the vehicle coordinate system is a local coordinate system relative to the world coordinate system) may be first established according to the vehicle, and the relative height of the road point with respect to the vehicle may be calculated based on the principle that a vector formed by the origin of the road point and the vehicle coordinate system is perpendicular to a normal vector of the plane of the vehicle in the vehicle coordinate system, and the vector product is zero. Specifically, the relative height of the road point with respect to the vehicle may be acquired in the present embodiment through the following steps S1021 to S1023.
Step S1021: a plane vector of a plane where the vehicle is located in the vehicle body coordinate system is acquired and a unit normal vector of the plane is acquired from the plane vector.
The plane of the vehicle in the body coordinate system refers to a plane formed by an x axis and a y axis in the body coordinate system. As shown in FIG. 2, x 1 And z 1 Respectively represent an x axis and a z axis in a vehicle body coordinate system, a represents a plane vector of a plane where the vehicle is located, and n represents a unit normal vector of the plane where the vehicle is located. x is the number of 2 And z 2 Respectively, an x-axis and a z-axis in "a world coordinate system having an origin, an x-axis, and a y-axis of a two-dimensional rectangular coordinate system as the origin, the x-axis, and the y-axis, respectively".
If the vehicle is provided with devices which operate depending on the data of the body coordinate system, the devices can be directly used for acquiring the plane vector of the plane where the vehicle is located. For example, a plane vector of a plane in which the vehicle is located in the body coordinate system may be directly acquired by the accelerometer.
After the plane vector of the plane where the vehicle is located is obtained, a normal calculation method of the normal vector in the field of mathematical technology may be adopted, and the unit normal vector of the plane where the vehicle is located is calculated according to the plane vector of the plane where the vehicle is located, which is not described herein again.
Step S1022: and obtaining the z-axis coordinate zh of the road point in a world coordinate system taking the origin, the x-axis and the y-axis of the two-dimensional rectangular coordinate system as the origin, the x-axis and the y-axis respectively according to the two-dimensional coordinates of the road point and by solving the following equation.
(xh-x1)×nx+(yh-y1)×ny+(zh-z1)×nz＝0
In the above equation, x1, y1, and z1 represent coordinates of the vehicle in the body coordinate system of the vehicle on the x axis, the y axis, and the z axis, nx, ny, and nz represent coordinates of the unit normal vector in the body coordinate system of the vehicle on the x axis, the y axis, and the z axis, and xh and yh represent coordinates of the x axis and the y axis in two-dimensional coordinates of the road point.
(xh-x1, yh-y1, zh-z1) actually represents a vector formed by two points, namely the vehicle and the road point, in the vehicle body coordinate system, (nx, ny, nz) represents a unit normal vector of a plane in which the vehicle is located in the vehicle body coordinate system, and the vector (xh-x1, yh-y1, zh-z1) is perpendicular to the vector (nx, ny, nz), so that the vector product of the two vectors is equal to zero. Wherein, 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 obtained by solving the equation.
Step S1023: the relative height of the road point with respect to the vehicle is determined from the coordinates zh.
By the method described in the above steps S1021 to S1023, the relative height of the road point with respect to the vehicle can be accurately obtained, which is beneficial to accurately converting the road point from the world coordinate system to the image coordinate system.
The following continues with the description of step S105.
In some embodiments according to step S105 described above, after the back-projection position of the road points located at the position of the road elements around the vehicle is position-matched with the acquisition position, when the pose parameter for converting the device coordinate system of the image acquisition device and the world coordinate system is obtained, the pose parameter can be used as an equation parameter, establishing a distance error equation according to the back projection position and the acquisition position of the road points at the positions of the road elements around the vehicle (the equation parameters of the distance error equation comprise the pose parameters), then adopting a nonlinear iterative optimization algorithm, taking the distance error equation meeting the preset convergence condition as a target, and performing iterative optimization on the pose parameters in the distance error equation, and acquiring the optimized pose parameters as final pose parameters for converting the device coordinate system and the world coordinate system of the image acquisition device. Specifically, in the present embodiment, the back projection position of the road point at the position where the road element around the vehicle is located and the collection position may be position-matched by the following steps S1051 to S1052:
step S1051: establishing a distance error equation shown in the following formula according to the back projection position and the collection position of the road point at the position of the road element around the vehicle:
loss＝d(f(A,O),cd)
in the above distance error equation, f (a, O) represents the back projection position of the current road point, a represents the position of the current road point in the world coordinate system, O represents a pose parameter for converting the device coordinate system and the world coordinate system, cd represents a line segment formed by using the acquisition positions of two road points closest to the back projection position of the current road point as end points, d represents a distance calculation function from the back projection position f (a, O) to the line segment cd, and loss represents a distance calculated by the distance calculation function d. As shown in fig. 3, a point a in fig. 3 represents a 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 represent two points, which are closest to the back projection point a in the image coordinate system, of the road point at the position of the road element determined by the image capturing device, respectively.
Step S1052: and taking the distance loss smaller than the preset distance threshold as a target, adopting Levenberg-Marquardt algorithm to carry out iterative optimization on the pose parameter O in the distance error equation, acquiring the pose parameter O when the distance loss is smaller than the preset distance threshold, and taking the pose parameter O as the final pose parameter for converting the device coordinate system and the world coordinate system of the image acquisition device.
It should be noted that the Levenberg-Marquardt (Levenberg-Marquardt) algorithm is a conventional non-linear least square algorithm in the field of mathematical technology, and the algorithm principle and the calculation process thereof are not described herein in detail.
The above is a further description of step S105.
When a vehicle travels in a road network including a plurality of different road layers, the height of different roads cannot be displayed because the two-dimensional map lacks the z-axis coordinate in the world coordinate system. As shown in fig. 4, when a plurality of roads belonging to different road layers exist at the same position, the roads are displayed in a cross manner on the two-dimensional map, and it is not clearly shown which road the vehicle is located on. In the vehicle positioning method according to another embodiment of the present invention, the vehicle positioning method may include, in addition to the steps S101 to S105 in the foregoing method embodiments, road connectivity display may be performed on the two-dimensional map before or after each step on the road on which the vehicle is currently traveling and the road on which the vehicle has traveled before, so that the influence of the roads in other road layers on the vehicle positioning display using the two-dimensional map may be eliminated, and which position of which road the vehicle is located can be displayed more clearly. In a preferred embodiment, the road connection display may be performed on the two-dimensional map for the road on which the vehicle is currently traveling and the road on which the vehicle has traveled before, before the road point is back-projected from the world coordinate system to the image coordinate system according to the two-dimensional coordinates and the relative height of the road point.
It should be noted that, although the foregoing embodiments describe each step in a specific sequence, those skilled in the art will understand that, in order to achieve the effect of the present invention, different steps do not necessarily need to be executed in such a sequence, and they may be executed simultaneously (in parallel) or in other sequences, and these changes are all within the protection scope of the present invention.
Furthermore, the invention also provides a position determining device.
Referring to fig. 5, fig. 5 is a main structural block diagram of a position determination apparatus according to an embodiment of the present invention. As shown in fig. 5, the position determining apparatus in the embodiment of the present invention mainly includes a road positioning module 11, a road back projection module 12, a road point position obtaining module 13, and a vehicle position determining module 14. In some embodiments, the road positioning module 11 may be configured to position the current driving road of the vehicle through a two-dimensional map, and determine two-dimensional coordinates of a road point in the current driving road in a two-dimensional rectangular coordinate system; the road back projection module 12 may be configured to obtain a relative height of the road point with respect to the vehicle, determine an origin, an x-axis, and a y-axis of a world coordinate system according to the origin, the x-axis, and the y-axis of the two-dimensional rectangular coordinate system, use the relative height as a coordinate of the road point on a z-axis of 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; the road point position obtaining module 13 may be configured to determine road elements located around the vehicle, obtain a back-projection position of a road point at the position of the road element to the image coordinate system, and obtain a collection position of the road point at the position of the road element determined by the image collection device of the vehicle in the image coordinate system; the vehicle position determination module 14 may be configured to perform position matching on the back-projected position of the road point where the road element is located and the acquisition position, and determine the position of the vehicle in the world coordinate system according to the result of the position matching.
For the above-mentioned position determining apparatus to be used for executing the embodiment of the vehicle positioning method shown in fig. 1, the technical principles, the solved technical problems and the generated technical effects of the two are similar, and it can be clearly understood by those skilled in the art that for convenience and brevity of description, the specific working process and related descriptions of the position determining apparatus may refer to the content described in the embodiment of the vehicle positioning method, and are not repeated herein.
It will be understood by those skilled in the art that all or part of the flow of the method according to the above-described embodiment may be implemented by a computer program, which may be stored in a computer-readable storage medium and used to implement the steps of the above-described embodiments of the method when the computer program is executed by a processor. Wherein the computer program comprises computer program code, which may be in the form of source code, object code, an executable file or some intermediate form, etc. The computer-readable storage medium may include: any entity or device capable of carrying said computer program code, media, usb disk, removable hard disk, magnetic diskette, optical disk, computer memory, read-only memory, random access memory, electrical carrier wave signals, telecommunication signals, software distribution media, etc. It should be noted that the computer readable storage medium may contain content that is subject to appropriate increase or decrease as required by legislation and patent practice in jurisdictions, for example, in some jurisdictions, computer readable storage media that does not include electrical carrier signals and telecommunications signals in accordance with legislation and patent practice.
Furthermore, the invention also provides computer equipment. In one computer apparatus embodiment according to the present invention, the computer apparatus comprises a processor and a storage device, the storage device may be configured to store a program for executing the vehicle localization method of the above-described method embodiment, and the processor may be configured to execute the program in the storage device, the program including but not limited to the program for executing the vehicle localization method of the above-described method embodiment. For convenience of explanation, only the parts related to the embodiments of the present invention are shown, and specific technical details are not disclosed. The computer device may be a device formed by including various electronic devices.
Further, the invention also provides a computer readable storage medium. In one computer-readable storage medium embodiment according to the present invention, a computer-readable storage medium may be configured to store a program that executes the vehicle localization method of the above-described method embodiment, which may be loaded and executed by a processor to implement the above-described vehicle localization method. For convenience of explanation, only the parts related to the embodiments of the present invention are shown, and details of the specific techniques are not disclosed. The computer readable storage medium may be a storage device formed by including various electronic devices, and optionally, the computer readable storage medium is a non-transitory computer readable storage medium in the embodiment of the present invention.
Further, the invention also provides a vehicle. In an embodiment of a vehicle according to the present invention, the vehicle may include the position determining apparatus described in the embodiment of the position determining apparatus, and may also include the computer device described in the embodiment of the computer device. The vehicle may be an autonomous vehicle, an unmanned vehicle, or the like in the present embodiment. In addition, the vehicle in the embodiment may be a fuel vehicle, an electric vehicle, a hybrid vehicle in which electric energy and fuel are mixed, or a vehicle using other new energy, and the like, classified according to the power source type.
Further, it should be understood that, since the configuration of each module is only for explaining the functional units of the apparatus of the present invention, the corresponding physical devices of the modules may be the processor itself, or a part of software, a part of hardware, or a part of a combination of software and hardware in the processor. Thus, the number of individual modules in the figures is merely illustrative.
Those skilled in the art will appreciate that the various modules in the apparatus may be adaptively split or combined. Such splitting or combining of specific modules does not cause the technical solutions to deviate from the principle of the present invention, and therefore, the technical solutions after splitting or combining will fall within the protection scope of the present invention.
So far, the technical solutions of the present invention have been described in connection with the preferred embodiments shown in the drawings, but it is apparent to those skilled in the art that the scope of the present invention is not limited to these specific embodiments. Equivalent changes or substitutions of related technical features can be made by those skilled in the art without departing from the principle of the invention, and the technical scheme after the changes or substitutions can fall into the protection scope of the invention.

Claims (10)

1. A vehicle positioning method, characterized in that the method comprises:
positioning a current running road of a vehicle through a two-dimensional map, and determining two-dimensional coordinates of road points in the current running road in a two-dimensional rectangular coordinate system;
acquiring the relative height of the road point relative to a vehicle, determining the origin, the x axis and the y axis of a world coordinate system according to the origin, the x axis and the y axis of the two-dimensional rectangular coordinate system, taking the relative height as the coordinate of the road point on the z axis in the world coordinate system, and back-projecting 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;
determining road elements positioned around a vehicle, acquiring a back projection position of a road point at the position of the road element in an image coordinate system by back projection, and acquiring an acquisition position of the road point at the position of the road element in the image coordinate system, which is determined by an image acquisition device of the vehicle;
and carrying out position matching on the back projection position of the road point at the position of the road element and the acquisition position, and determining the position of the vehicle in a world coordinate system according to the position matching result.
2. The vehicle positioning method according to claim 1, wherein the step of "obtaining the relative height of the road point with respect to the vehicle" specifically comprises:
acquiring a plane vector of a plane where a vehicle is located in a vehicle body coordinate system and acquiring a unit normal vector of the plane according to the plane vector;
obtaining a z-axis coordinate zh of the road point in a world coordinate system taking an origin, an x-axis and a y-axis of the two-dimensional rectangular coordinate system as the origin, the x-axis and the y-axis respectively according to the two-dimensional coordinates of the road point and by solving the following equation:
(xh-x1)×nx+(yh-y1)×ny+(zh-z1)×nz＝0
wherein x1, y1 and z1 represent coordinates of the vehicle in an x-axis, a y-axis and a z-axis in a body coordinate system, nx, ny and nz represent coordinates of the unit normal vector in the x-axis, the y-axis and the z-axis in the body coordinate system, and xh and yh represent coordinates of the x-axis and the y-axis in two-dimensional coordinates of the road point;
the relative height of the road point with respect to the vehicle is determined from the coordinates zh.
3. The vehicle positioning method according to claim 1, further comprising displaying a road connection between a current road on which the vehicle is traveling and a road on which the vehicle has traveled before on a two-dimensional map.
4. The vehicle positioning method according to claim 1, wherein the step of performing position matching between the back-projection position of the road point at the position of the road element and the collection position, and determining the position of the vehicle in the world coordinate system according to the result of the position matching specifically comprises:
carrying out position matching on the back projection position of the road point at the position of the road element and the acquisition position to obtain a pose parameter for converting a device coordinate system and a world coordinate system of the image acquisition device;
acquiring a back projection position of a road point at the position of a vehicle to an image coordinate system in a back projection manner;
and determining the position of the vehicle in a world coordinate system according to the back projection position of the road point at the position of the vehicle and the pose parameter.
5. The vehicle positioning method according to claim 4, wherein the step of performing position matching on the back projection position and the collection position of the road point at the position of the road element to obtain the pose parameter converted by the device coordinate system and the world coordinate system of the image collection device specifically comprises:
establishing a distance error equation shown in the following formula according to the back projection position and the acquisition position of the road point at the position of the road element:
loss＝d(f(A,O),cd)
wherein f (A, O) represents the back projection position of the current road point, A represents the position of the current road point in a world coordinate system, O represents pose parameters for converting a device coordinate system and the world coordinate system, cd represents a line segment formed by taking the acquisition positions of two road points closest to the back projection position of the current road point as end points, d represents a distance calculation function from the back projection position f (A, O) to the line segment cd, and loss represents the distance calculated by the distance calculation function d;
and performing iterative optimization on the pose parameter O in the distance error equation by adopting a Levenberg-Marquardt algorithm with the distance less than a preset distance threshold as a target, acquiring the pose parameter O when the distance less than the preset distance threshold, and taking the pose parameter O as the final pose parameter for converting the device coordinate system and the world coordinate system of the image acquisition device.
6. The vehicle positioning method according to claim 1, wherein before the steps of determining road elements located around the vehicle, acquiring the back-projection positions of the road points at the positions of the road elements to the image coordinate system, and acquiring the acquisition positions of the road points at the positions of the road elements determined by the image acquisition device of the vehicle in the image coordinate system, the method further comprises:
acquiring images of road points at the positions of the road elements by at least two image acquisition devices to obtain road point images of the road points at the positions of the road elements;
and according to the road point image, determining the acquisition position of the road point at the position of the road element in an image coordinate system.
7. A position determining apparatus, characterized in that the apparatus comprises:
the road positioning module is configured to position a current driving road of the vehicle through a two-dimensional map and determine two-dimensional coordinates of a road point in the current driving road in a two-dimensional rectangular coordinate system;
a road back projection module configured to acquire a relative height of the road point with respect to a vehicle, determine an origin, an x-axis, and a y-axis of a world coordinate system from the origin, the x-axis, and the y-axis of the two-dimensional rectangular coordinate system, use the relative height as a coordinate of the road point on a z-axis of the world coordinate system, and back-project the road point from the world coordinate system to an image coordinate system from the two-dimensional coordinate of the road point and the relative height;
a road point position acquisition module configured to determine road elements located around a vehicle, acquire a back-projection position of a road point at a position of the road element in an image coordinate system by back-projecting the road point to the image coordinate system, and acquire a collection position of the road point at the position of the road element in the image coordinate system determined by an image collection device of the vehicle;
and the vehicle position determination module is configured to perform position matching on the back projection position of the road point at the position of the road element and the acquisition position, and determine the position of the vehicle in the world coordinate system according to the position matching result.
8. A computer arrangement comprising a processor and a storage device adapted to store a plurality of program codes, characterized in that said program codes are adapted to be loaded and run by said processor to perform the vehicle localization method according to any of claims 1 to 6.
9. A computer readable storage medium having a plurality of program codes stored therein, wherein the program codes are adapted to be loaded and run by a processor to perform the vehicle localization method according to any one of claims 1 to 6.
10. A vehicle, characterized in that the vehicle comprises a position determination apparatus as claimed in claim 7 or a computer device as claimed in claim 8.
CN202210501755.4A 2022-05-09 2022-05-09 Vehicle positioning method and device, computer equipment, storage medium and vehicle Pending CN114820769A (en)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202210501755.4A CN114820769A (en) 2022-05-09 2022-05-09 Vehicle positioning method and device, computer equipment, storage medium and vehicle
PCT/CN2023/073864 WO2023216651A1 (en) 2022-05-09 2023-01-30 Lane positioning method, computer device, computer-readable storage medium and vehicle

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210501755.4A CN114820769A (en) 2022-05-09 2022-05-09 Vehicle positioning method and device, computer equipment, storage medium and vehicle

Publications (1)

Publication Number Publication Date
CN114820769A true CN114820769A (en) 2022-07-29

Family

ID=82513209

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210501755.4A Pending CN114820769A (en) 2022-05-09 2022-05-09 Vehicle positioning method and device, computer equipment, storage medium and vehicle

Country Status (1)

Country Link
CN (1) CN114820769A (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116503482A (en) * 2023-06-26 2023-07-28 小米汽车科技有限公司 Vehicle position acquisition method and device and electronic equipment
WO2023216651A1 (en) * 2022-05-09 2023-11-16 安徽蔚来智驾科技有限公司 Lane positioning method, computer device, computer-readable storage medium and vehicle

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2023216651A1 (en) * 2022-05-09 2023-11-16 安徽蔚来智驾科技有限公司 Lane positioning method, computer device, computer-readable storage medium and vehicle
CN116503482A (en) * 2023-06-26 2023-07-28 小米汽车科技有限公司 Vehicle position acquisition method and device and electronic equipment
CN116503482B (en) * 2023-06-26 2023-10-20 小米汽车科技有限公司 Vehicle position acquisition method and device and electronic equipment

Similar Documents

Publication Publication Date Title
CN114820769A (en) Vehicle positioning method and device, computer equipment, storage medium and vehicle
CN111261016B (en) Road map construction method and device and electronic equipment
CN111830953A (en) Vehicle self-positioning method, device and system
CN111932627B (en) Marker drawing method and system
KR20210049581A (en) Apparatus for acquisition distance for all directions of vehicle
CN112700486B (en) Method and device for estimating depth of road surface lane line in image
CN105551020A (en) Method and device for detecting dimensions of target object
CN111699410A (en) Point cloud processing method, device and computer readable storage medium
CN113804100B (en) Method, device, equipment and storage medium for determining space coordinates of target object
CN114255252B (en) Obstacle contour acquisition method, device, equipment and computer-readable storage medium
CN116402978A (en) Dense three-dimensional reconstruction method based on binocular vision structural characteristics
CN113847927A (en) Path generation method, device, equipment, storage medium and program product
CN110515110B (en) Method, device, equipment and computer readable storage medium for data evaluation
WO2022199195A1 (en) Map updating method and system, vehicle-mounted terminal, server, and storage medium
CN112527009A (en) Radar data processing method and device and operation equipment
CN116433865B (en) Space-ground collaborative acquisition path planning method based on scene reconstructability analysis
CN109598199B (en) Lane line generation method and device
CN111243021A (en) Vehicle-mounted visual positioning method and system based on multiple combined cameras and storage medium
CN116205986A (en) Calibration method and device for external parameters of vehicle-mounted looking-around camera, computer equipment and medium
CN111784798B (en) Map generation method and device, electronic equipment and storage medium
CN115077563A (en) Vehicle positioning accuracy evaluation method and device and electronic equipment
CN117011481A (en) Method and device for constructing three-dimensional map, electronic equipment and storage medium
CN114910085A (en) Vehicle fusion positioning method and device based on road administration facility identification
CN113469045A (en) Unmanned card-collecting visual positioning method and system, electronic equipment and storage medium
WO2023216651A1 (en) Lane positioning method, computer device, computer-readable storage medium and vehicle

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination