CN112665593B - Vehicle positioning method and device - Google Patents
Vehicle positioning method and device Download PDFInfo
- Publication number
- CN112665593B CN112665593B CN202011494137.9A CN202011494137A CN112665593B CN 112665593 B CN112665593 B CN 112665593B CN 202011494137 A CN202011494137 A CN 202011494137A CN 112665593 B CN112665593 B CN 112665593B
- Authority
- CN
- China
- Prior art keywords
- position coordinates
- quay
- target
- preset
- vehicle
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 45
- 239000011159 matrix material Substances 0.000 claims description 43
- 238000012216 screening Methods 0.000 claims description 18
- 238000013507 mapping Methods 0.000 claims description 15
- 230000004927 fusion Effects 0.000 abstract 1
- 238000005259 measurement Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 239000003973 paint Substances 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 238000002310 reflectometry Methods 0.000 description 3
- 239000003550 marker Substances 0.000 description 2
- 238000012544 monitoring process Methods 0.000 description 2
- 238000007476 Maximum Likelihood Methods 0.000 description 1
- 239000011248 coating agent Substances 0.000 description 1
- 238000000576 coating method Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000002708 enhancing effect Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Navigation (AREA)
Abstract
The invention provides a vehicle positioning method and a device, which are applied to the technical field of automobiles, and the method comprises the steps of firstly identifying a target quay crane and relative position coordinates of preset feature points in the target quay crane according to first positioning information after acquiring preset parameter values and first positioning information fed back by vehicle-mounted positioning equipment, and further acquiring actual position coordinates of the preset feature points on the target quay crane based on a quay crane high-precision map; and finally, inputting the preset parameter value, the relative position coordinates of the preset characteristic points and the actual position coordinates of the preset characteristic points on the target quay bridge into a positioning model to obtain positioning information of the vehicle to be positioned. The positioning model is obtained based on a Kalman filter, positioning information of the vehicle to be positioned meeting the use requirement is finally obtained through fusion of the positioning model, and the problem of lower accuracy caused by simply positioning the vehicle by means of vehicle-mounted positioning equipment in the prior art is solved.
Description
Technical Field
The invention belongs to the technical field of automobiles, and particularly relates to a vehicle positioning method and device.
Background
In order to improve the accuracy of vehicle positioning, vehicles are mostly provided with various vehicle-mounted positioning devices, such as differential GPS (Global Positioning System ), IMU (Inertial Measurement Unit, inertial navigation measurement unit), camera, laser radar, and the like, and position coordinates of the vehicles are finally determined by fusing positioning information fed back by the vehicle-mounted positioning devices.
However, in practical use, various vehicle-mounted positioning devices have certain disadvantages, such as that the measurement accuracy of the IMU cannot be maintained for a long time, and the camera is extremely obviously affected by light and obstacles. Particularly in the application scene of a port and a dock, a large number of stacked containers in the port can not capture enough positioning information by a traditional vision and laser positioning system, and even a differential GPS can often fail in the port, so that vehicles in the port and the dock can not be effectively positioned.
Therefore, how to realize accurate positioning of the running vehicle in the port and dock is one of the technical problems to be solved urgently by those skilled in the art.
Disclosure of Invention
In view of the above, the present invention aims to provide a vehicle positioning method and apparatus, which implement vehicle positioning based on a preset parameter value representing a current running state of a vehicle to be positioned, an actual position coordinate of a preset feature point of a shore bridge in a port, and a relative position coordinate, so as to solve the problem of low positioning precision caused by simply relying on vehicle-mounted positioning equipment in the prior art, and specifically adopts the following scheme:
in a first aspect, the present invention provides a vehicle positioning method, including:
acquiring a preset parameter value representing the current running state of a vehicle to be positioned and first positioning information fed back by vehicle-mounted positioning equipment in the vehicle to be positioned;
identifying a target quay bridge and relative position coordinates of preset feature points in the target quay bridge according to the first positioning information;
acquiring actual position coordinates of preset feature points on the target quay based on a quay high-precision map; the shore bridge high-precision map comprises actual position coordinates of each shore bridge;
inputting the preset parameter value, the relative position coordinates of the preset feature points and the actual position coordinates of the preset feature points on the target quay bridge into a positioning model to obtain positioning information of the vehicle to be positioned;
wherein the positioning model is obtained based on a Kalman filter.
Optionally, the obtaining, based on the shore bridge high-precision map, the actual position coordinates of the preset feature points on the target shore bridge includes:
acquiring actual position coordinates and a quay type of the target quay in the quay high-precision map;
and determining the actual position coordinates of preset feature points on the target quay according to the actual position coordinates of the target quay and the quay type of the target quay.
Optionally, the actual position coordinates of the quay are represented by actual position coordinates of a designated position on the quay, and determining, according to the actual position coordinates of the target quay and the quay type of the target quay, the actual position coordinates of the preset feature point on the target quay includes:
inquiring a first preset mapping relation, and determining a target relative position data set corresponding to the target shore bridge according to the type of the target shore bridge;
the corresponding relation between the type of the shore bridge and the relative position data set is recorded in the first preset mapping relation, and any one of the relative position data sets comprises the relative position relation between the appointed position on the shore bridge of the same type of the shore bridge and preset characteristic points;
and determining the actual position coordinates of the preset feature points on the target quay according to the target relative position data set and the actual position coordinates of the target quay.
Optionally, before the inputting the preset parameter value, the relative position coordinates of the preset feature point, and the actual position coordinates of the preset feature point on the target quay bridge into the positioning model, the method further includes:
screening the relative position coordinates meeting the preset screening rule from the relative position coordinates of the preset feature points according to the preset screening rule to obtain the screened relative position coordinates of the preset feature points; the preset screening rule is determined according to geometric features of the shore bridge;
carrying out data association on the relative position coordinates of the screened preset feature points and the actual position coordinates of the preset feature points on the target quay bridge to obtain the relative position coordinates of the associated preset feature points and the actual position coordinates of the associated preset feature points on the target quay bridge;
correspondingly, the inputting the preset parameter value, the relative position coordinates of the preset feature point and the actual position coordinates of the preset feature point on the target quay bridge into a positioning model includes:
and inputting the preset parameter value, the relative position coordinates of the associated preset feature points and the actual position coordinates of the associated preset feature points on the target quay bridge into a positioning model.
Optionally, the obtaining the actual position coordinates and the type of the target bridge in the bridge crane high-precision map includes:
determining a target quay number of the target quay according to the first positioning information;
and acquiring the actual position coordinates and the type of the quay, corresponding to the target quay number, recorded in the quay high-precision map, and obtaining the actual position coordinates and the type of the target quay.
Optionally, the first positioning information includes a picture including a preset identification image, and the determining, according to the first positioning information, the target bridge number of the target bridge includes:
identifying a target preset identifier in the picture;
inquiring a second preset mapping relation, and determining a target quay bridge number of the target quay bridge corresponding to the target preset identifier, wherein the second preset mapping relation records a corresponding relation between the preset identifier and the quay bridge number.
Optionally, the preset parameter values include wheel speed and IMU attitude information.
Optionally, the process of outputting the positioning information of the vehicle to be positioned by the positioning model includes:
acquiring positioning information of the vehicle to be positioned in the previous period, wherein the positioning information comprises a vehicle pose and a first covariance matrix representing positioning accuracy;
calculating the predicted pose of the vehicle to be positioned and a second covariance matrix representing the precision of the predicted pose according to the preset parameter value and the positioning information of the vehicle to be positioned in the previous period;
calculating a Kalman gain matrix based on the predicted pose, the second covariance matrix, the relative position coordinates of the preset feature points, the actual position coordinates of the preset feature points and a preset third covariance matrix representing the relative position coordinates and the actual position coordinate precision;
and calculating the positioning information of the vehicle to be positioned based on the predicted pose, the second covariance matrix, the relative position coordinates of the preset feature points, the actual position coordinates of the preset feature points, the third covariance matrix and the Kalman gain matrix.
In a second aspect, the present invention provides a vehicle positioning device comprising:
the first acquisition unit is used for acquiring preset parameter values representing the current running state of the vehicle to be positioned and first positioning information fed back by vehicle-mounted positioning equipment in the vehicle to be positioned;
the identification unit is used for identifying the target quay crane and the relative position coordinates of preset feature points in the target quay crane according to the first positioning information;
the second acquisition unit is used for acquiring actual position coordinates of preset feature points on the target shore bridge based on the shore bridge high-precision map; the shore bridge high-precision map comprises actual position coordinates of each shore bridge;
the positioning unit is used for inputting the preset parameter value, the relative position coordinates of the preset feature points and the actual position coordinates of the preset feature points on the target quay bridge into a positioning model to obtain positioning information of the vehicle to be positioned;
wherein the positioning model is obtained based on a Kalman filter.
Optionally, the second obtaining unit is configured to, when obtaining the actual position coordinates of the preset feature point on the target quay based on the quay high-precision map, specifically include:
acquiring actual position coordinates and a quay type of the target quay in the quay high-precision map;
and determining the actual position coordinates of preset feature points on the target quay according to the actual position coordinates of the target quay and the quay type of the target quay.
According to the vehicle positioning method provided by the invention, after the preset parameter value and the first positioning information fed back by the vehicle-mounted positioning equipment are obtained, the relative position coordinates of the target quay bridge and the preset characteristic point in the target quay bridge are firstly identified according to the first positioning information, and then the actual position coordinates of the preset characteristic point on the target quay bridge are obtained based on the quay bridge high-precision map; and finally, inputting the preset parameter value, the relative position coordinates of the preset characteristic points and the actual position coordinates of the preset characteristic points on the target quay bridge into a positioning model to obtain positioning information of the vehicle to be positioned.
In the vehicle positioning method provided by the invention, the positioning model is obtained based on the Kalman filter, and because the Kalman filter can realize the optimal weighted estimation of different precision data under the meaning of minimum variance, the influence of different precision data on a final output result is balanced.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings that are required in the embodiments or the description of the prior art will be briefly described, and it is obvious that the drawings in the following description are some embodiments of the present invention, and other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a flow chart of a vehicle positioning method provided by an embodiment of the invention;
FIG. 2 is a block diagram of a vehicle positioning device according to an embodiment of the present invention;
fig. 3 is a block diagram of another vehicle positioning device according to an embodiment of the present invention.
Detailed Description
For the purpose of making the objects, technical solutions and advantages of the embodiments of the present invention more apparent, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention, and it is apparent that the described embodiments are some embodiments of the present invention, but not all embodiments of the present invention. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
Optionally, referring to fig. 1, fig. 1 is a flowchart of a vehicle positioning method provided by an embodiment of the present invention, where the method may be applied to a vehicle body controller of a vehicle, or may be applied to other controllers capable of acquiring corresponding parameters and having a data processing capability on a vehicle, and obviously, in some cases, a server on a network side may also be selected to implement the method. Referring to fig. 1, a flow of a vehicle positioning method provided by an embodiment of the present invention may include:
s100, obtaining a preset parameter value representing the current running state of the vehicle to be positioned and first positioning information fed back by vehicle-mounted positioning equipment in the vehicle to be positioned.
In the embodiment of the invention, a plurality of preset parameters representing the current running state of the vehicle can be provided, such as the wheel rotation speed, the gesture information fed back by the IMU and the like, and when the vehicle to be positioned is positioned, the preset parameter values corresponding to the preset parameters are required to be obtained. In the positioning method provided by the embodiment of the invention, after the preset parameter value is mainly used for calling the positioning model, the predicted pose of the vehicle to be positioned in the current period and the second covariance matrix representing the precision of the predicted pose are predicted, and corresponding contents are developed in the subsequent part and are not described in detail herein.
As described above, the preset parameter value is used as the input information of the positioning model, so in practical application, the selection of the preset parameter and the number of the preset parameters need to be specifically selected according to the setting of the positioning model and the installation condition of the vehicle-mounted positioning device on the vehicle to be positioned, and the invention does not specifically limit the selection and the number of the preset parameters.
The first positioning information fed back by the vehicle-mounted positioning device, namely the positioning information of the vehicle-mounted positioning device, is distinguished from the finally obtained positioning information of the vehicle to be positioned, and is called as first positioning information. The vehicle-mounted positioning device can be any electronic device capable of collecting relevant positioning information in the prior art, such as a laser radar, a camera, an IMU (inertial measurement unit), a differential GPS (global positioning system) and the like, and accordingly, the positioning information fed back by the vehicle-mounted positioning device is the positioning information which can be provided by each existing positioning device, such as a picture fed back by the camera, a point cloud fed back by the laser radar, position coordinates fed back by the differential GPS, vehicle gestures and the like. In practical applications, the vehicle to be positioned is often configured with various vehicle-mounted positioning devices, and correspondingly, the positioning information obtained in the step may also be various.
S110, identifying the target quay bridge and the relative position coordinates of preset feature points in the target quay bridge according to the first positioning information.
In the step, the identification of the target quay bridge is realized based on the positioning information fed back by the vehicle-mounted positioning equipment. Different vehicle-mounted positioning devices correspondingly have differences in the method for identifying the target quay bridge according to the positioning information fed back by the different vehicle-mounted positioning devices.
In order to facilitate identification of the target quay, a plurality of markers which are helpful for identification can be preset on the quay in advance, for example, a specified position of the quay is manually painted with a high-reflectivity paint, or a special image is pasted, and the like. Optionally, if the vehicle positioning device is a camera, the first positioning information in this embodiment refers to a picture including the image of the target quay bridge, and by identifying the content of the picture, it can be determined whether the obtained positioning information includes the target quay bridge. Furthermore, if the marker is preset on the surface of the quay, the identification process of the target quay can be more accurate and quicker.
Optionally, if the vehicle-mounted positioning device includes a laser radar, a differential GPS, and other devices, the positioning information fed back by the vehicle-mounted positioning device should include point cloud information, position information, and the like, and for the point cloud information, whether the obtained positioning information includes the target quay can be determined according to the point cloud characteristics of the quay, that is, whether the surrounding of the vehicle to be positioned has the target quay is determined.
It may be conceivable that in practical applications, a plurality of shore bridges are often provided in a port, and the positioning information fed back by the vehicle-mounted positioning device of the vehicle to be positioned may also include a plurality of shore bridges, and accordingly, the plurality of shore bridges may be identified simultaneously based on the above identification method.
Further, the preset feature points of the target shore bridge in the embodiment of the present invention may be geometric features of the shore bridge, such as end points of a bridge arm of the shore bridge, or information such as manually installed high-reflectivity paint, pictures, etc. The selection of the preset feature points can be determined according to the type of the shore bridge and the specific structural characteristics of the shore bridge, and the specific selection of the preset feature points is not limited.
The identification of the relative position coordinates of the preset feature points on the target quay bridge is similar to the identification process of the target quay bridge, and the identification process can be realized based on laser radar, differential GPS and other equipment, however, in order to facilitate the identification of the preset feature points, the processing which facilitates the identification, such as the preset markers, the coating of the high-reflectivity paint and the like, can be arranged at the corresponding positions, and the details are not repeated here.
S120, acquiring actual position coordinates of preset feature points on the target quay based on the quay high-precision map.
In actual management of a port, each quay is provided with an encoder, RCMS (Remote Crane Monitoring System, remote monitoring system) of the port receives quay position information and corresponding quay numbers of the quays fed back from the encoder on the port quay, and at the same time, the RCMS also records the quay type corresponding to each quay, and the quay position information can be represented by actual position coordinates of designated positions on the quays. It is conceivable that the quads belonging to the same quay type have identical external dimensions and structural characteristics. The communication between the encoder and the RCMS is stable and reliable, and the accuracy of information such as the actual position coordinates of each quay in the port, the corresponding relation between the actual position coordinates of the quay and the quay number, the quay type and the like obtained by the RCMS is very high.
In this embodiment, the RCMS records the actual position coordinates of each bridge in the port, the type of the bridge and the corresponding bridge number of each bridge in the port in the high-precision map, and sends updated high-precision map data to the T-BOX of the vehicle to be positioned according to a preset update period through a system background program, so that the T-BOX of the vehicle can obtain the actual position coordinates of each bridge in the port in time.
Based on the above, the method for obtaining the actual position coordinates of the identified quay and the quay type according to the positioning method provided by the embodiment of the present invention is implemented by accessing the T-BOX of the vehicle to be positioned. Meanwhile, as the actual position coordinates of a plurality of quads and corresponding quads are stored in the high-precision map in the T-BOX, how to obtain the actual position coordinates of the target quads and the corresponding quads in the high-precision map becomes the first problem to be solved.
Optionally, in order to solve the above-mentioned problem, the present invention provides a method for obtaining the actual position coordinates of the target quay and the quay type in the high-precision map. As described above, each bridge in the high-precision map corresponds to a unique bridge number, and therefore, it is first required to determine the target bridge number of the target bridge according to the first positioning information.
Specifically, in order to further determine the number of the quay after the vehicle to be positioned can identify the target quay through the vehicle-mounted positioning device, a preset identifier, such as a designated graphic or the like, may be set on the quay. It should be noted that, the marker for enhancing the identification of the quay bridge described in the foregoing description and the preset identifier described in the step may be implemented in the same manner, for example, the preset identifier made of a high-reflection material may enhance the identification of the quay bridge, and also include information such as the identification of the quay bridge. Of course, it can also be implemented in different ways at different locations of the quay bridge. The embodiment is not limited to the specific implementation manner of the two information.
Based on the above, if the positioning information fed back by the vehicle-mounted positioning device is a picture containing a preset identification image, after identifying the quay, the target preset identification in the picture can be further identified, and after obtaining the target preset identification, the target quay number of the target quay corresponding to the target preset identification is further queried and determined according to a second preset mapping relation recorded with the corresponding relation between the preset identification and the quay number.
Of course, if the content of the preset mark is the number of the quay, after the picture containing the preset mark image is obtained, the image of the preset mark in the picture can be directly identified, and the corresponding target quay number can be directly obtained.
Optionally, if the positioning information is the point cloud information fed back by the laser radar and the position coordinate of the vehicle to be positioned fed back by the differential GPS under the global coordinate, the position coordinate of the target quay can be obtained by reversely solving the position coordinate of the target quay according to the relative position relation between the vehicle to be positioned and the target quay and the position coordinate of the vehicle to be positioned under the global coordinate system (specifically, the method can be realized by referring to the prior art) because the relative position relation between the vehicle to be positioned and the target quay can be obtained according to the point cloud information. Meanwhile, the actual position coordinates and the quay numbers of the quays stored in the T-BOX are obtained, and in each quay, the quay closest to the obtained position coordinates of the target quay is determined, so that the corresponding target quay number is obtained.
After the target quay serial number corresponding to the target quay is determined, the actual position coordinate and the quay type corresponding to the target quay serial number recorded in the high-precision map are inquired, and the actual position coordinate and the quay type of the target quay can be obtained.
After the actual position coordinates of the target quay and the quay type of the target quay are obtained, the actual position coordinates of the preset feature points on the target quay can be further determined.
Optionally, in practical application, the actual position coordinates of the quay bridge may be represented by actual position coordinates of a designated position on the quay bridge, for example, the actual position coordinates of the quay bridge base center are used to represent the actual position coordinates of the quay bridge, and of course, the actual position coordinates of other designated positions on the quay bridge may also be selected to represent the actual position coordinates of the quay bridge.
Based on the above, the method provides a first preset mapping relationship, in which a corresponding relationship between a type of a quay and a relative position data set is recorded, and any one of the relative position data sets includes a relative position relationship between a specified position on a quay of the same type and a preset feature point. After the type of the shore bridge of the target shore bridge is determined, the first preset mapping relation is queried, and then a target relative position data set corresponding to the target shore bridge can be determined according to the type of the shore bridge of the target shore bridge.
Since the actual position coordinates of the target quay, i.e. the actual position coordinates of the designated position on the target quay, are known, the actual position coordinates of each preset feature point on the target quay can be further determined based on the target relative position dataset.
S130, inputting the preset parameter value, the relative position coordinates of the preset feature points and the actual position coordinates of the preset feature points on the target quay bridge into a positioning model to obtain positioning information of the vehicle to be positioned.
Optionally, based on the basic application principle of the kalman filter, the kalman filter can realize optimal weighted estimation of different precision data in the sense of minimum variance, balance the influence of different precision data on a final output result, and effectively combine predicted data and actual measurement data.
After the input information required by the positioning model is obtained through the steps, the preset parameter value, the relative position coordinates of the preset characteristic points and the actual position coordinates of the preset characteristic points are input into the positioning model, and the positioning information of the vehicle to be positioned can be obtained.
Specifically, after the data is input into the positioning model, the positioning model can acquire positioning information of the previous period, wherein the positioning information comprises a vehicle pose and a first covariance matrix representing positioning accuracy; then calculating the predicted pose of the vehicle to be positioned and a second covariance matrix representing the precision of the predicted pose according to the preset parameter value and the positioning information of the previous period; and further calculating a Kalman gain matrix based on the predicted pose, the second covariance matrix, the relative position coordinates, the actual position coordinates of the preset feature points and a preset third covariance matrix representing the accuracy of the relative position coordinates and the actual position coordinates.
Finally, the positioning model calculates positioning information of the vehicle to be positioned based on the predicted pose, the second covariance matrix, the relative position coordinates, the actual position coordinates of the preset feature points, the third covariance matrix and the Kalman gain matrix.
It is conceivable that the positioning information of the vehicle to be positioned calculated in the present cycle will be used as the basic data of the predicted pose in the next cycle.
In summary, in the vehicle positioning method provided by the invention, the positioning model is obtained based on the kalman filter, and because the kalman filter can realize the optimal weighted estimation of different precision data in the minimum variance sense, the influence of different precision data on the final output result is balanced.
As described above, the first positioning information described in the above embodiment is fed back by the vehicle-mounted positioning device, and the first positioning information may become inaccurate due to various reasons, so that the relative position coordinates between the vehicle to be positioned and each preset feature point are determined by analyzing the obtained first positioning information, which is also inaccurate, and even if there is a possibility that error data exists, in order to improve the accuracy of the positioning result, the obtained relative position coordinates need to be screened, so as to obtain the relative position coordinates between the vehicle to be positioned and each preset feature point, which meet the use requirement.
Optionally, before S130 is performed, the following operations may be performed to improve accuracy of vehicle positioning.
Firstly, according to a preset screening rule, screening the relative position coordinates meeting the preset screening rule from the relative position coordinates of all preset feature points to obtain the screened relative position coordinates of the preset feature points.
Specifically, each preset feature point includes, but is not limited to, corner information, image information, and the like. For convenience of description, z i ＝[r i ,θ i ]I= 1:m denotes relative position coordinates (hereinafter referred to as candidate relative position coordinates for convenience of description) which are preliminarily determined according to the first positioning information, where m is the number of candidate relative position coordinates, r is the distance between a preset feature point in any one of the subsequent relative position coordinates and the vehicle to be positioned, and θ is the angle between the preset feature point and the vehicle to be positioned. In m i ＝[x i ,y i ]I=1:n represents the actual position coordinates of each preset feature point, where n represents the number of preset feature points, and x i 、y i Respectively representing the abscissa and the ordinate of any preset feature point in the global coordinate system. The preset screening rule can be set based on geometric constraint conditions, and error data in candidate relative position coordinates can be removed through the preset screening conditions. For example, if the cross section at 30 meters to the left is detected, but the width of the geometric constraint condition of the actual quay bridge is not reached, redundant information related to the cross section at 30 meters to the left can be removed through screening.
Then, carrying out data association on the relative position coordinates of the screened preset feature points and the actual position coordinates of the preset feature points on the target quay bridge to obtain associated dataThe relative position coordinates of the preset feature points and the actual position coordinates of the preset feature points on the associated target quay bridge are preset. For example, in one embodiment, the geometric constraint pair z is used i ＝[r i ,θ i ]Error data in i= 1:m are removed, and then the relative position coordinates and m of the screened preset feature points are estimated by using a maximum likelihood estimation method i ＝[x i ,y i ]I=1:n for data association. The relative position coordinates of the associated preset feature points and the actual position coordinates of the associated preset feature points on the target quay bridge are z respectively i ＝[r i ,θ i ],i＝1:k，m i ＝[x i ,y i ]I= 1:k. For example, using the 4 corners of the target bridge for positioning, the actual position coordinates of the 4 corners of the target bridge are determined, and we recognize that the relative position coordinates of the 4 corners are the unreasonable relative position coordinates in the obtained relative position coordinates are truncated by geometric constraint. And matching the rest relative position coordinates with the actual position coordinates to complete data association, thereby obtaining the relative position coordinates between the vehicle to be positioned and the preset feature points.
Correspondingly, after the processing is executed, the preset parameter value, the relative position coordinates of the associated preset feature points and the actual position coordinates of the associated preset feature points on the target quay bridge can be input into a positioning model, so that more accurate positioning information of the vehicle to be positioned can be obtained.
It will be appreciated that: other vehicle-mounted positioning methods exist in the embodiment of the invention, for example, the actual position coordinates of the preset feature points on each shore bridge can be added into the shore bridge high-precision map, so that the actual position coordinates of the preset feature points on the shore bridge can be obtained directly according to the actual position coordinates of each shore bridge. For another example, other methods, such as bayesian estimation, may be used in the data correlation.
The following describes a vehicle positioning device provided by an embodiment of the present invention, where the vehicle positioning device described below may be regarded as a functional module architecture to be set in a central device for implementing the vehicle positioning method provided by the embodiment of the present invention; the following description may be referred to with respect to the above.
Optionally, referring to fig. 2, fig. 2 is a block diagram of a vehicle positioning device according to an embodiment of the present invention, where the vehicle positioning device provided in the embodiment includes:
a first obtaining unit 10, configured to obtain a preset parameter value representing a current running state of a vehicle to be positioned, and first positioning information fed back by a vehicle-mounted positioning device in the vehicle to be positioned;
an identifying unit 20, configured to identify, according to the first positioning information, a target quay and a relative position coordinate of a preset feature point in the target quay;
a second obtaining unit 30, configured to obtain actual position coordinates of a preset feature point on a target shore bridge based on a shore bridge high-precision map; the shore bridge high-precision map comprises actual position coordinates of each shore bridge;
the positioning unit 40 is configured to input a preset parameter value, a relative position coordinate of a preset feature point, and an actual position coordinate of the preset feature point on the target quay bridge into the positioning model, so as to obtain positioning information of the vehicle to be positioned;
wherein the positioning model is obtained based on a Kalman filter.
Optionally, the second obtaining unit 30 is configured to, when obtaining the actual position coordinates of the preset feature point on the target quay based on the quay high-precision map, specifically include:
acquiring actual position coordinates and a quay type of a target quay in a quay high-precision map;
and determining the actual position coordinates of preset feature points on the target quay according to the actual position coordinates of the target quay and the quay type of the target quay.
Optionally, the actual position coordinates of the quay are expressed by actual position coordinates of a designated position on the quay, and the second obtaining unit 30 is configured to determine, according to the actual position coordinates of the target quay and the quay type of the target quay, the actual position coordinates of the preset feature point on the target quay, where the second obtaining unit specifically includes:
inquiring a first preset mapping relation, and determining a target relative position data set corresponding to a target shore bridge according to the type of the shore bridge of the target shore bridge;
the corresponding relation between the type of the shore bridge and the relative position data set is recorded in the first preset mapping relation, and any relative position data set comprises the relative position relation between the appointed position on the shore bridge with the same type of the shore bridge and preset characteristic points;
and determining the actual position coordinates of preset feature points on the target quay according to the target relative position data set and the actual position coordinates of the target quay.
Optionally, the second obtaining unit 30 is configured to obtain the actual position coordinates of the target quay and the type of the quay in the quay high-precision map, and specifically includes:
determining a target quay number of the target quay according to the first positioning information;
and acquiring the actual position coordinates and the type of the quay, which correspond to the target quay number recorded in the quay high-precision map, and obtaining the actual position coordinates and the type of the target quay.
Optionally, the first positioning information includes a picture including a preset identification image, and the second obtaining unit 30 is configured to, when determining the number of the target bank according to the first positioning information, specifically include:
identifying a target preset mark in the picture;
inquiring a second preset mapping relation, and determining a target quay bridge number of a target quay bridge corresponding to the target preset identifier, wherein the second preset mapping relation records a corresponding relation between the preset identifier and the quay bridge number.
Optionally, the process of outputting the positioning information of the vehicle to be positioned by the positioning model in the positioning unit 40 specifically includes:
acquiring positioning information of a vehicle expected to be positioned in the previous week, wherein the positioning information comprises a vehicle pose and a first covariance matrix representing positioning accuracy;
calculating the predicted pose of the vehicle to be positioned and a second covariance matrix representing the precision of the predicted pose according to the preset parameter value and the positioning information of the vehicle expected to be positioned in the previous week;
calculating a Kalman gain matrix based on the predicted pose, the second covariance matrix, the relative position coordinates of the preset feature points, the actual position coordinates of the preset feature points and a third covariance matrix which is preset and used for representing the relative position coordinates and the actual position coordinate precision;
and calculating positioning information of the vehicle to be positioned based on the predicted pose, the second covariance matrix, the relative position coordinates of the preset feature points, the actual position coordinates of the preset feature points, the third covariance matrix and the Kalman gain matrix.
Optionally, referring to fig. 3, fig. 3 is a block diagram of another vehicle positioning device according to an embodiment of the present invention, where, on the basis of the embodiment shown in fig. 2, the device further includes:
the screening unit 50 is configured to screen, according to a preset screening rule, relative position coordinates that satisfy the preset screening rule from the relative position coordinates of each preset feature point, so as to obtain screened relative position coordinates of the preset feature point; the preset screening rule is determined according to geometric features of the shore bridge;
and the associating unit 60 is configured to perform data association on the screened relative position coordinates of the preset feature points and actual position coordinates of the preset feature points on the target quay bridge, so as to obtain the associated relative position coordinates of the preset feature points and the associated actual position coordinates of the preset feature points on the target quay bridge.
In the present specification, each embodiment is described in a progressive manner, and each embodiment is mainly described in a different point from other embodiments, and identical and similar parts between the embodiments are all enough to refer to each other. For the device disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant points refer to the description of the method section.
Those of skill would further appreciate that the various illustrative elements and algorithm steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware, computer software, or combinations of both, and that the various illustrative elements and steps are described above generally in terms of functionality in order to clearly illustrate the interchangeability of hardware and software. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the solution. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present invention.
The steps of a method or algorithm described in connection with the embodiments disclosed herein may be embodied directly in hardware, in a software module executed by a processor, or in a combination of the two. The software modules may be disposed in Random Access Memory (RAM), memory, read Only Memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disk, a removable disk, a CD-ROM, or any other form of storage medium known in the art.
The previous description of the disclosed embodiments is provided to enable any person skilled in the art to make or use the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other embodiments without departing from the spirit or scope of the invention. Thus, the present invention is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.
Claims (9)
1. A vehicle positioning method, characterized by comprising:
acquiring a preset parameter value representing the current running state of a vehicle to be positioned and first positioning information fed back by vehicle-mounted positioning equipment in the vehicle to be positioned;
identifying a target quay bridge and relative position coordinates of preset feature points in the target quay bridge according to the first positioning information;
acquiring actual position coordinates of preset feature points on the target quay based on a quay high-precision map; the shore bridge high-precision map comprises actual position coordinates of each shore bridge;
inputting the preset parameter value, the relative position coordinates of the preset feature points and the actual position coordinates of the preset feature points on the target quay bridge into a positioning model to obtain positioning information of the vehicle to be positioned;
the positioning model is obtained based on a Kalman filter, and the preset parameter value is used for predicting the predicted pose of the vehicle to be positioned in the current period and representing the covariance matrix of the predicted pose precision after the preset parameter value is input into the positioning model;
the process of outputting the positioning information of the vehicle to be positioned by the positioning model comprises the following steps:
acquiring positioning information of the vehicle to be positioned in the previous period, wherein the positioning information comprises a vehicle pose and a first covariance matrix representing positioning accuracy;
calculating the predicted pose of the vehicle to be positioned and a second covariance matrix representing the precision of the predicted pose according to the preset parameter value and the positioning information of the vehicle to be positioned in the previous period;
calculating a Kalman gain matrix based on the predicted pose, the second covariance matrix, the relative position coordinates of the preset feature points, the actual position coordinates of the preset feature points and a preset third covariance matrix representing the relative position coordinates and the actual position coordinate precision;
and calculating the positioning information of the vehicle to be positioned based on the predicted pose, the second covariance matrix, the relative position coordinates of the preset feature points, the actual position coordinates of the preset feature points, the third covariance matrix and the Kalman gain matrix.
2. The vehicle positioning method according to claim 1, wherein the obtaining actual position coordinates of the preset feature point on the target quay based on the quay high-precision map includes:
acquiring actual position coordinates and a quay type of the target quay in the quay high-precision map;
and determining the actual position coordinates of preset feature points on the target quay according to the actual position coordinates of the target quay and the quay type of the target quay.
3. The vehicle positioning method according to claim 2, wherein the actual position coordinates of the quay are represented by actual position coordinates of a specified position on the quay, and the determining the actual position coordinates of the preset feature point on the target quay according to the actual position coordinates of the target quay and the quay type of the target quay includes:
inquiring a first preset mapping relation, and determining a target relative position data set corresponding to the target shore bridge according to the type of the target shore bridge;
the corresponding relation between the type of the shore bridge and the relative position data set is recorded in the first preset mapping relation, and any one of the relative position data sets comprises the relative position relation between the appointed position on the shore bridge of the same type of the shore bridge and preset characteristic points;
and determining the actual position coordinates of the preset feature points on the target quay according to the target relative position data set and the actual position coordinates of the target quay.
4. The vehicle positioning method according to claim 1, characterized by further comprising, before said inputting the preset parameter value, the relative position coordinates of the preset feature point, and the actual position coordinates of the preset feature point on the target quay into the positioning model:
screening the relative position coordinates meeting the preset screening rule from the relative position coordinates of the preset feature points according to the preset screening rule to obtain the screened relative position coordinates of the preset feature points; the preset screening rule is determined according to geometric features of the shore bridge;
carrying out data association on the relative position coordinates of the screened preset feature points and the actual position coordinates of the preset feature points on the target quay bridge to obtain the relative position coordinates of the associated preset feature points and the actual position coordinates of the associated preset feature points on the target quay bridge;
correspondingly, the inputting the preset parameter value, the relative position coordinates of the preset feature point and the actual position coordinates of the preset feature point on the target quay bridge into a positioning model includes:
and inputting the preset parameter value, the relative position coordinates of the associated preset feature points and the actual position coordinates of the associated preset feature points on the target quay bridge into a positioning model.
5. The vehicle positioning method according to claim 2, wherein the obtaining the actual position coordinates and the type of the target bridge in the bridge crane high-precision map includes:
determining a target quay number of the target quay according to the first positioning information;
and acquiring the actual position coordinates and the type of the quay, corresponding to the target quay number, recorded in the quay high-precision map, and obtaining the actual position coordinates and the type of the target quay.
6. The vehicle positioning method according to claim 5, wherein the first positioning information includes a picture including a preset identification image, and the determining the target bridge number of the target bridge according to the first positioning information includes:
identifying a target preset identifier in the picture;
inquiring a second preset mapping relation, and determining a target quay bridge number of the target quay bridge corresponding to the target preset identifier, wherein the second preset mapping relation records a corresponding relation between the preset identifier and the quay bridge number.
7. The vehicle positioning method according to claim 1, characterized in that the preset parameter values include wheel speed and IMU attitude information.
8. A vehicle positioning device, characterized by comprising:
the first acquisition unit is used for acquiring preset parameter values representing the current running state of the vehicle to be positioned and first positioning information fed back by vehicle-mounted positioning equipment in the vehicle to be positioned;
the identification unit is used for identifying the target quay crane and the relative position coordinates of preset feature points in the target quay crane according to the first positioning information;
the second acquisition unit is used for acquiring actual position coordinates of preset feature points on the target shore bridge based on the shore bridge high-precision map; the shore bridge high-precision map comprises actual position coordinates of each shore bridge;
the positioning unit is used for inputting the preset parameter value, the relative position coordinates of the preset feature points and the actual position coordinates of the preset feature points on the target quay bridge into a positioning model to obtain positioning information of the vehicle to be positioned;
the positioning model is obtained based on a Kalman filter, and the preset parameter value is used for predicting the predicted pose of the vehicle to be positioned in the current period and representing the covariance matrix of the predicted pose precision after the preset parameter value is input into the positioning model;
the process of outputting the positioning information of the vehicle to be positioned by the positioning model comprises the following steps:
acquiring positioning information of the vehicle to be positioned in the previous period, wherein the positioning information comprises a vehicle pose and a first covariance matrix representing positioning accuracy;
calculating the predicted pose of the vehicle to be positioned and a second covariance matrix representing the precision of the predicted pose according to the preset parameter value and the positioning information of the vehicle to be positioned in the previous period;
calculating a Kalman gain matrix based on the predicted pose, the second covariance matrix, the relative position coordinates of the preset feature points, the actual position coordinates of the preset feature points and a preset third covariance matrix representing the relative position coordinates and the actual position coordinate precision;
and calculating the positioning information of the vehicle to be positioned based on the predicted pose, the second covariance matrix, the relative position coordinates of the preset feature points, the actual position coordinates of the preset feature points, the third covariance matrix and the Kalman gain matrix.
9. The vehicle positioning device according to claim 8, wherein the second obtaining unit is configured to, when obtaining the actual position coordinates of the preset feature point on the target quay based on the quay high-precision map, specifically include:
acquiring actual position coordinates and a quay type of the target quay in the quay high-precision map;
and determining the actual position coordinates of preset feature points on the target quay according to the actual position coordinates of the target quay and the quay type of the target quay.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011494137.9A CN112665593B (en) | 2020-12-17 | 2020-12-17 | Vehicle positioning method and device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011494137.9A CN112665593B (en) | 2020-12-17 | 2020-12-17 | Vehicle positioning method and device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112665593A CN112665593A (en) | 2021-04-16 |
| CN112665593B true CN112665593B (en) | 2024-01-26 |
Family
ID=75404543
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011494137.9A Active CN112665593B (en) | 2020-12-17 | 2020-12-17 | Vehicle positioning method and device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112665593B (en) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107422730A (en) * | 2017-06-09 | 2017-12-01 | 武汉市众向科技有限公司 | The AGV transportation systems of view-based access control model guiding and its driving control method |
| CN110887494A (en) * | 2019-11-29 | 2020-03-17 | 腾讯科技（深圳）有限公司 | Vehicle positioning method and device |
| CN111175788A (en) * | 2020-01-20 | 2020-05-19 | 北京主线科技有限公司 | Transverse positioning method and positioning system for automatic driving vehicle |
| CN111220154A (en) * | 2020-01-22 | 2020-06-02 | 北京百度网讯科技有限公司 | Vehicle positioning method, device, equipment and medium |
| CN211149277U (en) * | 2019-09-29 | 2020-07-31 | 中电骏途新能源科技有限公司 | Intelligent navigation system for operation of container truck in port |
| CN111578928A (en) * | 2020-05-07 | 2020-08-25 | 北京邮电大学 | Positioning method and device based on multi-source fusion positioning system |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP6689659B2 (en) * | 2016-04-19 | 2020-04-28 | クラリオン株式会社 | Position estimation device and estimation method |
| KR20200044420A (en) * | 2018-10-19 | 2020-04-29 | 삼성전자주식회사 | Method and device to estimate position |
-
2020
- 2020-12-17 CN CN202011494137.9A patent/CN112665593B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107422730A (en) * | 2017-06-09 | 2017-12-01 | 武汉市众向科技有限公司 | The AGV transportation systems of view-based access control model guiding and its driving control method |
| CN211149277U (en) * | 2019-09-29 | 2020-07-31 | 中电骏途新能源科技有限公司 | Intelligent navigation system for operation of container truck in port |
| CN110887494A (en) * | 2019-11-29 | 2020-03-17 | 腾讯科技（深圳）有限公司 | Vehicle positioning method and device |
| CN111175788A (en) * | 2020-01-20 | 2020-05-19 | 北京主线科技有限公司 | Transverse positioning method and positioning system for automatic driving vehicle |
| CN111220154A (en) * | 2020-01-22 | 2020-06-02 | 北京百度网讯科技有限公司 | Vehicle positioning method, device, equipment and medium |
| CN111578928A (en) * | 2020-05-07 | 2020-08-25 | 北京邮电大学 | Positioning method and device based on multi-source fusion positioning system |
Non-Patent Citations (3)
| Title |
|---|
| SC-KF mobile robot localization: A stochastic cloning Kalman filter for processing relative-state measurements;Mourikis, AI等;《IEEE TRANSACTIONS ON ROBOTICS》;全文 * |
| 基于PMD相机的特征跟踪位姿测量方法;张旭东;李文龙;胡良梅;叶子瑞;;电子测量与仪器学报(第07期);全文 * |
| 基于地面特征点匹配的无人驾驶车全局定位;方辉;杨明;杨汝清;;机器人(第01期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112665593A (en) | 2021-04-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109143207B (en) | Laser radar internal reference precision verification method, device, equipment and medium | |
| US10552982B2 (en) | Method for automatically establishing extrinsic parameters of a camera of a vehicle | |
| WO2018142900A1 (en) | Information processing device, data management device, data management system, method, and program | |
| CN109435955B (en) | Performance evaluation method, device and equipment for automatic driving system and storage medium | |
| DE102017101466A1 (en) | TRACKING OBJECTS IN A DYNAMIC ENVIRONMENT FOR IMPROVED LOCALIZATION | |
| CN111127584A (en) | Method and device for establishing visual map, electronic equipment and storage medium | |
| CN104167109A (en) | Detection method and detection apparatus for vehicle position | |
| CN110906953A (en) | Relative position precision evaluation method and device for automatic driving positioning | |
| CN110271553A (en) | Method and apparatus for steadily positioning vehicle | |
| CN112964291A (en) | Sensor calibration method and device, computer storage medium and terminal | |
| EP3994043A1 (en) | Sourced lateral offset for adas or ad features | |
| CN113740871A (en) | Laser SLAM method, system equipment and storage medium in high dynamic environment | |
| CN114943952A (en) | Method, system, device and medium for obstacle fusion under multi-camera overlapped view field | |
| CN111241224B (en) | Method, system, computer device and storage medium for target distance estimation | |
| CN114063622B (en) | Unmanned ship autonomous berthing positioning method and device and related components | |
| CN113566817B (en) | Vehicle positioning method and device | |
| CN111103584A (en) | Device and method for determining height information of objects in the surroundings of a vehicle | |
| US11908206B2 (en) | Compensation for vertical road curvature in road geometry estimation | |
| CN113671454A (en) | Position parameter calibration method and device of vehicle-mounted radar and storage medium | |
| CN110633800B (en) | Lane position determination method, apparatus, and storage medium based on autonomous vehicle | |
| CN112665593B (en) | Vehicle positioning method and device | |
| CN114419573A (en) | Dynamic occupancy grid estimation method and device | |
| CN116691659A (en) | Method and device for evaluating vehicle running track without GPS signal | |
| CN116543271A (en) | Method, device, electronic equipment and medium for determining target detection evaluation index | |
| CN114325662A (en) | External parameter calibration method, device, equipment and storage medium for vehicle-mounted radar |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |