CN111060126B - Positioning method and device and vehicle - Google Patents
Positioning method and device and vehicle Download PDFInfo
- Publication number
- CN111060126B CN111060126B CN201911410360.8A CN201911410360A CN111060126B CN 111060126 B CN111060126 B CN 111060126B CN 201911410360 A CN201911410360 A CN 201911410360A CN 111060126 B CN111060126 B CN 111060126B
- Authority
- CN
- China
- Prior art keywords
- deviation
- transformation matrix
- coordinate transformation
- vehicle
- travel
- 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
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C1/00—Measuring angles
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
Abstract
The embodiment of the application provides a positioning method, a positioning device and a vehicle. The positioning method is applied to a vehicle, and comprises the following steps: obtaining an initial coordinate transformation matrix for transforming coordinates obtained by the positioning device into coordinates in a set coordinate system established at a preset part of the vehicle, obtaining a deviation between an actual travel track and an expected travel track of the vehicle, adjusting a rotational coordinate transformation matrix in the initial coordinate transformation matrix using the deviation, and determining the corresponding rotation coordinate transformation matrix when the deviation is smaller than the set value as a target rotation coordinate transformation matrix to obtain a target coordinate transformation matrix including the target rotation coordinate transformation matrix, and converting the coordinates obtained by the positioning device into coordinates in a set coordinate system through the target coordinate transformation matrix to obtain an accurate coordinate conversion result, and performing coordinate conversion based on the adjusted coordinate transformation matrix to ensure the accuracy of the positioning coordinates obtained by the functional module in the vehicle.
Description
Technical Field
The application relates to the field of vehicles, in particular to a positioning method, a positioning device and a vehicle.
Background
The automatic driving automobile is also called as an unmanned automobile, and a computer can automatically and safely operate the motor automobile without any active operation of human by means of the cooperative cooperation of artificial intelligence, visual calculation, radar and a positioning system.
The autonomous vehicle is equipped with a positioning device, such as a laser radar, and the autonomous vehicle converts positioning coordinates obtained by the positioning device into positioning coordinates in a vehicle coordinate system using a preset coordinate conversion relationship, and performs vehicle driving control on the vehicle based on the positioning coordinates in the vehicle coordinate system. However, the currently used coordinate transformation relationship has a setting deviation, so that the coordinate transformation result obtained based on the coordinate transformation relationship is inaccurate, the positioning is inaccurate, and the vehicle operation cannot be accurately controlled.
Disclosure of Invention
In order to overcome the problems in the related art, the application provides a positioning method, a positioning device and a vehicle.
In a first aspect, a positioning method is provided, which is applied to a vehicle equipped with a positioning device, and the method includes:
obtaining an initial coordinate transformation matrix, wherein the initial coordinate transformation matrix is used for converting the coordinates obtained by the positioning device into coordinates in a set coordinate system established at a preset part of the vehicle, and the initial coordinate transformation matrix comprises an initial rotation coordinate transformation matrix;
obtaining a deviation between an actual travel track and an expected travel track of the vehicle;
adjusting the initial rotating coordinate transformation matrix by using the deviation, determining a corresponding rotating coordinate transformation matrix when the deviation is smaller than a set value as a target rotating coordinate transformation matrix, and obtaining a target coordinate transformation matrix comprising the target rotating coordinate transformation matrix;
and converting the coordinates obtained by the positioning device into coordinates in the set coordinate system through the target coordinate transformation matrix.
Optionally, the initial rotational coordinate transformation matrix comprises a parameter related to a heading angle; the adjusting the initial rotation coordinate transformation matrix by using the deviation, and determining the corresponding rotation coordinate transformation matrix as a target rotation coordinate transformation matrix when the deviation is smaller than a set value, including:
and adjusting the course angle in the initial rotating coordinate transformation matrix by using the deviation, and determining the corresponding course angle as a target course angle when the deviation is smaller than the set value to obtain a target rotating coordinate transformation matrix comprising the target course angle.
Optionally, the adjusting the heading angle in the initial rotational coordinate transformation matrix by using the deviation, determining a heading angle corresponding to zero of the deviation as a target heading angle, and obtaining a target rotational coordinate transformation matrix including the target heading angle includes:
determining a deviation angle of the course angle based on the deviation;
and determining the target course angle based on the course angle and the deviation angle in the initial rotation coordinate transformation matrix.
Optionally, the deviation is a function of travel time; determining a deviation angle of the heading angle based on the deviation, including:
multiplying the deviation by the preset proportion to obtain first data;
taking the running time as an integral variable, and carrying out integral calculation on the deviation to obtain second data;
taking the running time as a differential variable, and carrying out differential calculation on the deviation to obtain third data;
obtaining the deviation angle based on the first data, the second data, and the third data.
Optionally, the obtaining a deviation between an actual running track and a desired running track of the vehicle includes:
determining an actual driving position of the vehicle by the positioning device;
determining a distance from the actual travel position to the desired travel trajectory as a deviation.
Optionally, the determining the actual driving position of the vehicle by the positioning device includes:
acquiring actual running position coordinates of the vehicle obtained by the positioning device;
the determining the distance from the actual driving position to the expected driving track as a deviation comprises:
determining a distance from the actual travel position to the desired travel trajectory based on the actual travel position coordinates;
determining the distance as the deviation.
Optionally, the determining the distance from the actual driving position to the desired driving track based on the actual driving position coordinates comprises:
inputting the actual driving position coordinates into a distance determination model obtained in advance, and obtaining the distance output by the distance determination model; alternatively, the first and second electrodes may be,
and directly calculating the distance from the actual driving position to the expected driving track based on the actual driving position coordinates.
Optionally, the method further comprises:
acquiring running position coordinates of a plurality of sample running positions in the set coordinate system and an expected running track expression of the expected running track in the set coordinate system;
calculating, for each sample travel position, a distance from the sample travel position to the desired travel trajectory based on the travel position coordinates of the sample travel position and the desired travel trajectory expression;
constructing the distance determination model based on the travel position coordinates and the corresponding distances of the plurality of sample travel positions.
In a second aspect, there is provided a positioning device for a vehicle, the vehicle having the positioning device mounted thereon, the device comprising:
a first obtaining module configured to obtain an initial coordinate transformation matrix for converting coordinates obtained by the positioning apparatus into coordinates in a set coordinate system established at a preset part of the vehicle, the initial coordinate transformation matrix including an initial rotational coordinate transformation matrix;
a second obtaining module configured to obtain a deviation between an actual travel track and an expected travel track of the vehicle;
the adjusting module is configured to adjust the initial rotating coordinate transformation matrix by using the deviation, determine a corresponding rotating coordinate transformation matrix when the deviation is smaller than a set value as a target rotating coordinate transformation matrix, and obtain a target coordinate transformation matrix comprising the target rotating coordinate transformation matrix;
a conversion module configured to convert the coordinates obtained by the positioning device into coordinates in the set coordinate system through the target coordinate transformation matrix.
Optionally, the initial rotational coordinate transformation matrix comprises a parameter related to a heading angle; the adjusting module is configured to adjust the course angle in the initial rotating coordinate transformation matrix by using the deviation, and determine the corresponding course angle when the deviation is smaller than the set value as a target course angle to obtain a target rotating coordinate transformation matrix comprising the target course angle.
Optionally, the adjusting module includes:
a first determination submodule configured to determine a deviation angle of the heading angle based on the deviation;
a second determination submodule configured to determine the target heading angle based on the heading angle and the deviation angle in the initial rotational coordinate transformation matrix.
Optionally, the deviation is a function of travel time; the first determination submodule includes:
a first obtaining unit configured to multiply the deviation by the preset ratio to obtain first data;
a second obtaining unit, configured to take the running time as an integral variable, and perform integral calculation on the deviation to obtain second data;
a third obtaining unit configured to perform differential calculation on the deviation with the travel time as a differential variable to obtain third data;
an obtaining unit configured to obtain the deviation angle based on the first data, the second data, and the third data.
Optionally, the second obtaining module includes:
a third determination submodule configured to determine an actual travel position of the vehicle by the positioning device;
a fourth determination submodule configured to determine a distance from the actual travel position to the desired travel trajectory as a deviation.
Optionally, the third determining submodule is configured to acquire the actual driving position coordinates of the vehicle obtained by the positioning device;
the fourth determination submodule is configured to determine a distance from the actual travel position to the desired travel trajectory based on the actual travel position coordinates, and determine the distance as the deviation.
Optionally, the fourth determining submodule is configured to input the actual driving position coordinates into a distance determining model obtained in advance, and obtain the distance output by the distance determining model; alternatively, the first and second electrodes may be,
the fourth determination submodule is configured to directly calculate a distance from the actual travel position to the desired travel track based on the actual travel position coordinates.
Optionally, the apparatus further comprises:
an acquisition module configured to acquire travel position coordinates of a plurality of sample travel positions in the set coordinate system and a desired travel locus expression of the desired travel locus in the set coordinate system;
a calculation module configured to calculate, for each sample travel position, a distance from the sample travel position to the desired travel locus based on the travel position coordinates of the sample travel position and the desired travel locus expression;
a construction module configured to construct the distance determination model based on the travel position coordinates and the corresponding distances of the plurality of sample travel positions.
In a third aspect, a vehicle is provided, comprising: the system comprises an internal bus, a memory, a processor and an external interface which are connected through the internal bus; wherein, the first and the second end of the pipe are connected with each other,
the external interface is used for acquiring data;
the memory to store machine readable instructions to locate a correspondence;
the processor is configured to read the machine-readable instructions on the memory and execute the instructions to implement the following operations:
obtaining an initial coordinate transformation matrix, wherein the initial coordinate transformation matrix is used for converting the coordinates obtained by the positioning device into coordinates in a set coordinate system established at a preset part of the vehicle, and the initial coordinate transformation matrix comprises an initial rotation coordinate transformation matrix;
obtaining a deviation between an actual travel track and an expected travel track of the vehicle;
adjusting the initial rotating coordinate transformation matrix by using the deviation, determining a corresponding rotating coordinate transformation matrix when the deviation is smaller than a set value as a target rotating coordinate transformation matrix, and obtaining a target coordinate transformation matrix comprising the target rotating coordinate transformation matrix;
and converting the coordinates obtained by the positioning device into coordinates in the set coordinate system through the target coordinate transformation matrix.
It is to be understood that both the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the application.
Drawings
The accompanying drawings, which are incorporated in and constitute a part of this application, illustrate embodiments consistent with the present application and together with the description, serve to explain the principles of the application.
FIG. 1 is a flow chart illustrating a method of positioning according to an exemplary embodiment of the present application;
FIG. 2 is a schematic view of a positioning device shown in an exemplary embodiment of the present application;
FIG. 3 is a schematic illustration of a vehicle shown in an exemplary embodiment of the present application.
Detailed Description
Reference will now be made in detail to the exemplary embodiments, examples of which are illustrated in the accompanying drawings. When the following description refers to the accompanying drawings, like numbers in different drawings represent the same or similar elements unless otherwise indicated. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the present application. Rather, they are merely examples of apparatus and methods consistent with certain aspects of the present application, as detailed in the appended claims.
The embodiments of the present application will be described in detail below with reference to the accompanying drawings.
Fig. 1 is a flowchart illustrating a positioning method according to an exemplary embodiment of the present application, applied to a vehicle equipped with a positioning device, the method including:
in step 101, an initial coordinate transformation matrix for converting coordinates obtained by the positioning device into coordinates in a set coordinate system established at a preset part of the vehicle is obtained, the initial coordinate transformation matrix including an initial rotational coordinate transformation matrix.
The positioning device uses a positioning device coordinate system for obtaining coordinates of the positioning object in the positioning device coordinate system. For example, the positioning device is a laser radar, the positioning object is a vehicle, and the laser radar is used for acquiring coordinates of the vehicle in a laser radar coordinate system to realize vehicle positioning.
The preset component may be a rear axle center of the vehicle, and a coordinate system established at the rear axle center of the vehicle may be referred to as a vehicle coordinate system. The embodiments of the present disclosure do not specifically limit the preset components.
The initial coordinate transformation matrix is determined based on the installation positions of the positioning device and the preset component, and comprises an initial translation coordinate transformation matrix and an initial rotation coordinate transformation matrix, wherein the initial translation coordinate transformation matrix is used for performing translation processing, and the initial rotation coordinate transformation matrix is used for performing rotation processing.
The vehicle may convert the coordinates obtained by the positioning device into coordinates in a set coordinate system established at the preset portion using the initial coordinate transformation matrix, and control the vehicle to travel based on the coordinates in the set coordinate system. For example, the vehicle may convert coordinates obtained by the laser radar into coordinates in a vehicle coordinate system using the initial coordinate transformation matrix, and control the vehicle to travel based on the coordinates in the vehicle coordinate system.
In step 102, a deviation between an actual travel trajectory and a desired travel trajectory of the vehicle is obtained.
The actual travel track is a track on which the vehicle actually travels in the past period of time.
The desired travel trajectory is a trajectory in which the desired vehicle travels from a start position to a destination. The vehicle may be equipped with a navigator, and after a destination is input to the navigator, an expected travel track output by the navigator is acquired.
Under the condition that the initial coordinate transformation matrix has setting deviation, so that the coordinate transformation result is inaccurate, the actual running track of the vehicle is not coincident with the expected running track, and the actual running track is deviated from the expected running track.
In an alternative embodiment, the vehicle may determine the actual driving position of the vehicle by the positioning device, and determine the distance from the actual driving position of the vehicle to the desired driving trajectory as the deviation.
Alternatively, the vehicle may acquire the actual driving position coordinates of the vehicle obtained by the positioning device, and determine the distance from the actual driving position of the vehicle to the expected driving track based on the actual driving position coordinates of the vehicle.
In one implementation, the vehicle acquires an expected travel track expression of the expected travel track in a coordinate system of the positioning device, and directly calculates the distance from the actual travel position coordinate of the vehicle to the expected travel track based on the actual travel position coordinate of the vehicle acquired by the positioning device and the expected travel track expression.
In another implementation, the vehicle may acquire in advance the travel position coordinates of a plurality of sample travel positions in the set coordinate system and the expected travel locus expression of the expected travel locus in the set coordinate system, calculate, for each sample travel position, the distance from the sample travel position to the expected travel locus based on the travel position coordinates of the sample travel position and the expected travel locus expression, and construct the distance determination model based on the travel position coordinates of the plurality of sample travel positions and the corresponding distances. The input of the distance determination model is the actual driving position coordinates of the vehicle in the set coordinate system, and the output is the distance from the actual driving position of the vehicle to the expected driving track.
In the case where the distance determination model is constructed in advance, the vehicle may acquire the actual travel position coordinates of the vehicle acquired by the positioning device, input the actual travel position coordinates into the distance determination model acquired in advance, and acquire the distance output by the distance determination model.
The deviation between the actual driving track and the expected driving track can be parameters such as an included angle between the actual driving track and the expected driving track, besides the distance between the current actual driving position of the vehicle and the expected driving track.
In step 103, the initial rotational coordinate transformation matrix is adjusted by using the deviation, and the corresponding rotational coordinate transformation matrix when the deviation is smaller than the set value is determined as the target rotational coordinate transformation matrix, so as to obtain the target coordinate transformation matrix including the target rotational coordinate transformation matrix.
The set value may be zero or a small value.
In an alternative embodiment, the initial rotational coordinate transformation matrix includes parameters related to the heading angle, such as the sine of the heading angle, the cosine of the course angle, etc., where the heading angle may be understood as the angle of rotation of the vehicle about the Z axis perpendicular to the ground.
The vehicle can use the deviation between the actual running track and the expected running track to adjust the course angle in the initial rotation transformation matrix, and determines the corresponding course angle when the deviation is smaller than the set value as the target course angle to obtain the target rotation coordinate transformation matrix comprising the target course angle.
In an alternative embodiment, the vehicle may determine a deviation angle of the heading angle in the initial rotational coordinate transformation matrix based on a deviation between the actual travel track and the desired travel track, and determine the target heading angle based on the heading angle in the initial rotational coordinate transformation matrix and the deviation angle.
Optionally, the deviation between the actual driving track and the expected driving track is a function of the driving time, and the vehicle may determine the deviation angle of the heading angle in the initial coordinate transformation matrix by: firstly, multiplying the deviation between the actual running track and the expected running track by a preset proportion to obtain first data; secondly, taking the running time as an integral variable, and carrying out integral calculation on the deviation to obtain second data; thirdly, taking the running time as a differential variable, and carrying out differential calculation on the deviation to obtain third data; and finally, obtaining the deviation angle of the course angle in the initial coordinate transformation matrix based on the first data, the second data and the third data.
In one embodiment, the vehicle may calculate the deviation angle of the heading angle in the initial coordinate transformation matrix using the following formula:
wherein, theta (t) is the deviation angle of the course angle in the initial coordinate transformation matrix; e (t) is the deviation between the actual and expected travel trajectories; t is the travel time; kp、Tt、TDAre all constants.
And after the deviation angle of the course angle in the initial rotation coordinate transformation matrix is determined, the course angle in the initial rotation coordinate transformation matrix is compensated by using the deviation angle, and the target course angle is obtained. For example, the angle of the heading angle in the initial rotation coordinate transformation matrix is θ1Deviation angle of theta2The obtained target course angle is theta1+θ2。θ2And may be positive or negative or zero.
In step 104, the coordinates obtained by the positioning device are converted into coordinates in the set coordinate system by the target coordinate transformation matrix.
In the case where the vehicle is equipped with the laser radar and is located using the laser radar, the vehicle may convert coordinates obtained by the laser radar into coordinates in a vehicle coordinate system using the target coordinate transformation matrix after obtaining the target coordinate transformation matrix.
During the running process of the vehicle, after the positioning method provided by the embodiment of the present application is executed once, a small deviation may still exist between the actual running track and the expected running track of the vehicle at the next running time based on some factors, such as a calculation error, and the like, at this time, the vehicle may execute the positioning method provided by the embodiment of the present application again, and the target coordinate transformation matrix obtained after the execution of the last positioning method is finished may be used as the initial coordinate transformation matrix when the positioning method is executed this time, and the accurate adjustment of the heading angle is realized by executing the positioning method for multiple times until the deviation between the actual running track and the expected running position of the vehicle is zero.
In an optional embodiment, the vehicle is provided with a laser radar, an initial coordinate conversion matrix is obtained based on the position relation between the laser radar and the center of the rear axle of the vehicle, and the vehicle uses the initial coordinate conversion matrix to convert the coordinates obtained by the laser radar into coordinates in a vehicle coordinate system.
And performing performance test on the vehicle before the vehicle leaves a factory, wherein the performance test comprises the test of the heading angle in the initial coordinate transformation matrix, and the expected driving track used in the test process can be a straight-line track.
By using the positioning method provided by the embodiment of the application, the course angle in the initial transformation matrix used by the vehicle is adjusted before the vehicle leaves the factory, so that the performance of the vehicle when leaving the factory is ensured.
In an optional embodiment, the vehicle is provided with a laser radar, an initial coordinate conversion matrix is obtained based on the position relation between the laser radar and the center of the rear axle of the vehicle, and the vehicle uses the initial coordinate conversion matrix to convert the coordinates obtained by the laser radar into coordinates in a vehicle coordinate system.
And in the driving process of the vehicle after leaving the factory, the course angle in the initial coordinate conversion matrix is adjusted in real time based on the deviation between the actual driving track and the expected driving track, so that the driving direction of the vehicle is finally adjusted in real time, and the vehicle is ensured to drive according to the expected driving track.
The embodiment of the application provides a novel positioning method, a course angle in an initial coordinate transformation matrix is adjusted by using deviation between an actual running track and an expected running track of a vehicle, coordinate transformation is carried out by using an adjusted target coordinate transformation matrix, an accurate coordinate transformation result is obtained, and the accuracy of positioning data obtained by a functional module in the vehicle is ensured. There are various functional modules, such as a perception module, a prediction module, a planning module, etc.
Corresponding to the positioning method, the application also provides embodiments of the positioning device and the vehicle.
Referring to fig. 2, a schematic diagram of a positioning device according to an exemplary embodiment of the present application is shown, applied to a vehicle, on which the positioning device is mounted, the device including: a first obtaining module 21, a second obtaining module 22, an adjusting module 23 and a converting module 24; wherein the content of the first and second substances,
the first obtaining module 21 is configured to obtain an initial coordinate transformation matrix, where the initial coordinate transformation matrix is used to transform coordinates obtained by the positioning device into coordinates in a set coordinate system established at a preset component of the vehicle, and the initial coordinate transformation matrix includes an initial rotation coordinate transformation matrix;
the second obtaining module 22 is configured to obtain a deviation between an actual running track and a desired running track of the vehicle;
the adjusting module 23 is configured to adjust the initial rotational coordinate transformation matrix by using the deviation, and determine a corresponding rotational coordinate transformation matrix when the deviation is smaller than a set value as a target rotational coordinate transformation matrix, so as to obtain a target coordinate transformation matrix including the target rotational coordinate transformation matrix;
the converting module 24 is configured to convert the coordinates obtained by the positioning device into the coordinates in the set coordinate system through the target coordinate transformation matrix.
In an alternative embodiment, on the basis of the positioning device shown in fig. 2, the initial rotation coordinate transformation matrix includes a parameter related to a heading angle; the adjusting module 23 may be configured to adjust a heading angle in the initial rotational coordinate transformation matrix by using the deviation, and determine a corresponding heading angle when the deviation is smaller than the set value as a target heading angle, so as to obtain a target rotational coordinate transformation matrix including the target heading angle.
In an optional embodiment, the adjusting module 23 may include: a first determination submodule and a second determination submodule; wherein the content of the first and second substances,
the first determination submodule is configured to determine a deviation angle of the heading angle based on the deviation;
the second determination submodule is configured to determine the target heading angle based on the heading angle and the deviation angle in the initial rotational coordinate transformation matrix.
In an alternative embodiment, the deviation is a function of travel time; the first determining sub-module may include: a first obtaining unit, a second obtaining unit, a third obtaining unit and an obtaining unit; wherein, the first and the second end of the pipe are connected with each other,
the first obtaining unit is configured to multiply the deviation by the preset proportion to obtain first data;
the second obtaining unit is configured to perform integral calculation on the deviation by taking the running time as an integral variable to obtain second data;
the third obtaining unit is configured to perform differential calculation on the deviation by using the travel time as a differential variable to obtain third data;
the obtaining unit is configured to obtain the deviation angle based on the first data, the second data, and the third data.
In an alternative embodiment, on the basis of the positioning apparatus shown in fig. 2, the second obtaining module 22 may include: a third determination submodule and a fourth determination submodule; wherein the content of the first and second substances,
the third determination submodule is configured to determine an actual traveling position of the vehicle by the positioning device;
the fourth determination submodule is configured to determine a distance from the actual travel position to the desired travel trajectory as a deviation.
In an alternative embodiment, the third determination submodule may be configured to acquire the actual travel position coordinates of the vehicle obtained by the positioning apparatus;
the fourth determination submodule may be configured to determine a distance from the actual travel position to the desired travel track based on the actual travel position coordinates, the distance being determined as the deviation.
In an alternative embodiment, the fourth determination submodule may be configured to input the actual travel position coordinates into a distance determination model obtained in advance, and obtain the distance output by the distance determination model; alternatively, the first and second electrodes may be,
the fourth determination submodule may be configured to directly calculate a distance from the actual travel position to the desired travel track based on the actual travel position coordinates.
In an optional embodiment, the apparatus may further include: the system comprises an acquisition module, a calculation module and a construction module; wherein the content of the first and second substances,
the acquisition module is configured to acquire the running position coordinates of a plurality of sample running positions in the set coordinate system and the expected running track expression of the expected running track in the set coordinate system;
the calculation module is configured to calculate, for each sample travel position, a distance from the sample travel position to the desired travel locus based on the travel position coordinates of the sample travel position and the desired travel locus expression;
the construction module configured to construct the distance determination model based on the travel position coordinates and the corresponding distances of the plurality of sample travel positions.
Referring to fig. 3, which is a schematic view of a vehicle shown in an exemplary embodiment of the present application, the apparatus may include: a memory 320, a processor 330, and an external interface 340 connected by an internal bus 310.
The external interface 340 is used for acquiring data;
a memory 320 for storing machine readable instructions for locating a correspondence;
a processor 330 configured to read the machine-readable instructions on the memory 320 and execute the instructions to:
obtaining an initial coordinate transformation matrix, wherein the initial coordinate transformation matrix is used for converting the coordinates obtained by the positioning device into coordinates in a set coordinate system established at a preset part of the vehicle, and the initial coordinate transformation matrix comprises an initial rotation coordinate transformation matrix;
obtaining a deviation between an actual travel track and an expected travel track of the vehicle;
adjusting the initial rotating coordinate transformation matrix by using the deviation, determining a corresponding rotating coordinate transformation matrix when the deviation is smaller than a set value as a target rotating coordinate transformation matrix, and obtaining a target coordinate transformation matrix comprising the target rotating coordinate transformation matrix;
and converting the coordinates obtained by the positioning device into coordinates in the set coordinate system through the target coordinate transformation matrix.
In the disclosed embodiments, the computer readable storage medium may take many forms, such as, in various examples: a RAM (random Access Memory), a volatile Memory, a non-volatile Memory, a flash Memory, a storage drive (e.g., a hard drive), a solid state drive, any type of storage disk (e.g., an optical disk, a dvd, etc.), or similar storage medium, or a combination thereof. In particular, the computer readable medium may be paper or another suitable medium upon which the program is printed. Using these media, the programs can be electronically captured (e.g., optically scanned), compiled, interpreted, and processed in a suitable manner, and then stored in a computer medium.
The above description is only exemplary of the present application and should not be taken as limiting the present application, as any modification, equivalent replacement, or improvement made within the spirit and principle of the present application should be included in the scope of protection of the present application.
Claims (11)
1. A positioning method applied to a vehicle equipped with a positioning device, comprising:
obtaining an initial coordinate transformation matrix, wherein the initial coordinate transformation matrix is used for converting the coordinates obtained by the positioning device into coordinates in a set coordinate system established at a preset part of the vehicle, the initial coordinate transformation matrix comprises an initial rotation coordinate transformation matrix, and the initial rotation coordinate transformation matrix comprises parameters related to a course angle;
obtaining a deviation between an actual travel track and an expected travel track of the vehicle;
adjusting the initial rotating coordinate transformation matrix by using the deviation, determining a corresponding rotating coordinate transformation matrix when the deviation is smaller than a set value as a target rotating coordinate transformation matrix, and obtaining a target coordinate transformation matrix comprising the target rotating coordinate transformation matrix, wherein the method comprises the following steps: adjusting the course angle in the initial rotating coordinate transformation matrix by using the deviation, and determining the corresponding course angle as a target course angle when the deviation is smaller than the set value to obtain a target rotating coordinate transformation matrix comprising the target course angle;
converting the coordinates obtained by the positioning device into coordinates in the set coordinate system through the target coordinate transformation matrix;
the step of adjusting the course angle in the initial rotating coordinate transformation matrix by using the deviation, determining the corresponding course angle when the deviation is smaller than the set value as a target course angle, and obtaining a target rotating coordinate transformation matrix comprising the target course angle comprises the following steps:
determining a deviation angle of the course angle based on the deviation;
determining the target course angle based on the course angle and the deviation angle in the initial rotation coordinate transformation matrix;
said deviation being a function of travel time; determining a deviation angle of the heading angle based on the deviation, including:
multiplying the deviation by a preset proportion to obtain first data;
taking the running time as an integral variable, and carrying out integral calculation on the deviation to obtain second data;
taking the running time as a differential variable, and carrying out differential calculation on the deviation to obtain third data;
obtaining the deviation angle based on the first data, the second data, and the third data.
2. The method of claim 1, wherein the obtaining a deviation between an actual travel trajectory and a desired travel trajectory of the vehicle comprises:
determining an actual driving position of the vehicle by the positioning device;
determining a distance from the actual travel position to the desired travel trajectory as a deviation.
3. The method of claim 2, wherein said determining an actual travel position of said vehicle by a locating device comprises:
acquiring actual running position coordinates of the vehicle obtained by the positioning device;
the determining the distance from the actual driving position to the expected driving track as a deviation comprises:
determining a distance from the actual travel position to the desired travel trajectory based on the actual travel position coordinates;
determining the distance as the deviation.
4. The method of claim 3, wherein said determining a distance from said actual travel location to said desired travel track based on said actual travel location coordinates comprises:
inputting the actual driving position coordinates into a distance determination model obtained in advance, and obtaining the distance output by the distance determination model; alternatively, the first and second electrodes may be,
and directly calculating the distance from the actual driving position to the expected driving track based on the actual driving position coordinates.
5. The method of claim 4, further comprising:
acquiring running position coordinates of a plurality of sample running positions in the set coordinate system and an expected running track expression of the expected running track in the set coordinate system;
calculating, for each sample travel position, a distance from the sample travel position to the desired travel trajectory based on the travel position coordinates of the sample travel position and the desired travel trajectory expression;
constructing the distance determination model based on the travel position coordinates and the corresponding distances of the plurality of sample travel positions.
6. A locating device for use with a vehicle having a locating device mounted thereon, the device comprising:
a first obtaining module configured to obtain an initial coordinate transformation matrix for converting coordinates obtained by the positioning device into coordinates in a set coordinate system established at a preset component of the vehicle, the initial coordinate transformation matrix including an initial rotational coordinate transformation matrix including a parameter related to a heading angle;
a second obtaining module configured to obtain a deviation between an actual travel track and an expected travel track of the vehicle;
the adjusting module is configured to adjust the initial rotating coordinate transformation matrix by using the deviation, determine the corresponding rotating coordinate transformation matrix as a target rotating coordinate transformation matrix when the deviation is smaller than a set value, and obtain a target coordinate transformation matrix comprising the target rotating coordinate transformation matrix, and adjust a course angle in the initial rotating coordinate transformation matrix by using the deviation, and determine the corresponding course angle when the deviation is smaller than the set value as a target course angle, so as to obtain the target rotating coordinate transformation matrix comprising the target course angle;
a conversion module configured to convert the coordinates obtained by the positioning device into coordinates in the set coordinate system through the target coordinate transformation matrix;
the adjustment module includes:
a first determination submodule configured to determine a deviation angle of the heading angle based on the deviation;
a second determination submodule configured to determine the target heading angle based on the heading angle and the deviation angle in the initial rotational coordinate transformation matrix;
said deviation being a function of travel time; the first determination submodule includes:
a first obtaining unit configured to multiply the deviation by a preset ratio to obtain first data;
a second obtaining unit, configured to take the running time as an integral variable, and perform integral calculation on the deviation to obtain second data;
a third obtaining unit configured to perform differential calculation on the deviation with the travel time as a differential variable to obtain third data;
an obtaining unit configured to obtain the deviation angle based on the first data, the second data, and the third data.
7. The apparatus of claim 6, wherein the second obtaining module comprises:
a third determination submodule configured to determine an actual travel position of the vehicle by the positioning device;
a fourth determination submodule configured to determine a distance from the actual travel position to the desired travel trajectory as a deviation.
8. The apparatus of claim 7, wherein:
the third determination submodule is configured to acquire the actual travel position coordinates of the vehicle obtained by the positioning device;
the fourth determination submodule is configured to determine a distance from the actual travel position to the desired travel trajectory based on the actual travel position coordinates, and determine the distance as the deviation.
9. The apparatus of claim 8, wherein:
the fourth determination submodule is configured to input the actual travel position coordinates into a distance determination model obtained in advance, and obtain the distance output by the distance determination model; alternatively, the first and second electrodes may be,
the fourth determination submodule is configured to directly calculate a distance from the actual travel position to the desired travel track based on the actual travel position coordinates.
10. The apparatus of claim 9, further comprising:
an acquisition module configured to acquire travel position coordinates of a plurality of sample travel positions in the set coordinate system and a desired travel locus expression of the desired travel locus in the set coordinate system;
a calculation module configured to calculate, for each sample travel position, a distance from the sample travel position to the desired travel locus based on the travel position coordinates of the sample travel position and the desired travel locus expression;
a build module configured to build the distance determination model based on the travel position coordinates and corresponding distances of the plurality of sample travel positions.
11. A vehicle, characterized by comprising: the vehicle comprises an internal bus, a memory, a processor and an external interface which are connected through the internal bus, wherein the vehicle is provided with a positioning device; wherein the content of the first and second substances,
the external interface is used for acquiring data;
the memory to store machine readable instructions to locate a correspondence;
the processor is configured to read the machine-readable instructions on the memory and execute the instructions to implement the following operations:
obtaining an initial coordinate transformation matrix, wherein the initial coordinate transformation matrix is used for converting the coordinates obtained by the positioning device into coordinates in a set coordinate system established at a preset part of the vehicle, the initial coordinate transformation matrix comprises an initial rotation coordinate transformation matrix, and the initial rotation coordinate transformation matrix comprises parameters related to a course angle;
obtaining a deviation between an actual travel track and an expected travel track of the vehicle;
adjusting the initial rotating coordinate transformation matrix by using the deviation, determining a corresponding rotating coordinate transformation matrix when the deviation is smaller than a set value as a target rotating coordinate transformation matrix, and obtaining a target coordinate transformation matrix comprising the target rotating coordinate transformation matrix, wherein the method comprises the following steps: adjusting the course angle in the initial rotating coordinate transformation matrix by using the deviation, and determining the corresponding course angle as a target course angle when the deviation is smaller than the set value to obtain a target rotating coordinate transformation matrix comprising the target course angle;
converting the coordinates obtained by the positioning device into coordinates in the set coordinate system through the target coordinate transformation matrix;
the step of adjusting the course angle in the initial rotating coordinate transformation matrix by using the deviation, determining the corresponding course angle when the deviation is smaller than the set value as a target course angle, and obtaining a target rotating coordinate transformation matrix comprising the target course angle comprises the following steps:
determining a deviation angle of the course angle based on the deviation;
determining the target course angle based on the course angle and the deviation angle in the initial rotation coordinate transformation matrix;
said deviation being a function of travel time; determining a deviation angle of the heading angle based on the deviation, including:
multiplying the deviation by a preset proportion to obtain first data;
taking the running time as an integral variable, and carrying out integral calculation on the deviation to obtain second data;
taking the running time as a differential variable, and carrying out differential calculation on the deviation to obtain third data;
obtaining the deviation angle based on the first data, the second data, and the third data.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911410360.8A CN111060126B (en) | 2019-12-31 | 2019-12-31 | Positioning method and device and vehicle |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911410360.8A CN111060126B (en) | 2019-12-31 | 2019-12-31 | Positioning method and device and vehicle |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111060126A CN111060126A (en) | 2020-04-24 |
| CN111060126B true CN111060126B (en) | 2022-06-07 |
Family
ID=70305480
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911410360.8A Active CN111060126B (en) | 2019-12-31 | 2019-12-31 | Positioning method and device and vehicle |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111060126B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113706370B (en) * | 2020-05-22 | 2023-08-25 | 杭州海康威视数字技术股份有限公司 | Image generation method, device and system |
| CN115343730B (en) * | 2022-10-18 | 2023-02-17 | 苏州挚途科技有限公司 | GNSS antenna external parameter determination method, device and computer readable storage medium |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108139212A (en) * | 2015-10-15 | 2018-06-08 | 三菱电机株式会社 | Positioning device and localization method |
| CN109141410A (en) * | 2018-07-25 | 2019-01-04 | 深圳市集大自动化有限公司 | The Multi-sensor Fusion localization method of AGV integrated navigation |
| CN109767475A (en) * | 2018-12-28 | 2019-05-17 | 广州小鹏汽车科技有限公司 | A kind of method for calibrating external parameters and system of sensor |
| CN109987097A (en) * | 2019-03-13 | 2019-07-09 | 深兰科技（上海）有限公司 | A kind of method and apparatus adjusting target vehicle motion state |
| TW201932843A (en) * | 2017-12-18 | 2019-08-16 | 德商羅伯特博斯奇股份有限公司 | Method and apparatus for operating an inertial sensor unit for a vehicle |
| CN110211176A (en) * | 2019-05-31 | 2019-09-06 | 驭势科技（北京）有限公司 | A kind of Camera extrinsic number correction System and method for |
| CN110398969A (en) * | 2019-08-01 | 2019-11-01 | 北京主线科技有限公司 | Automatic driving vehicle adaptive prediction time domain rotating direction control method and device |
-
2019
- 2019-12-31 CN CN201911410360.8A patent/CN111060126B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108139212A (en) * | 2015-10-15 | 2018-06-08 | 三菱电机株式会社 | Positioning device and localization method |
| TW201932843A (en) * | 2017-12-18 | 2019-08-16 | 德商羅伯特博斯奇股份有限公司 | Method and apparatus for operating an inertial sensor unit for a vehicle |
| CN109141410A (en) * | 2018-07-25 | 2019-01-04 | 深圳市集大自动化有限公司 | The Multi-sensor Fusion localization method of AGV integrated navigation |
| CN109767475A (en) * | 2018-12-28 | 2019-05-17 | 广州小鹏汽车科技有限公司 | A kind of method for calibrating external parameters and system of sensor |
| CN109987097A (en) * | 2019-03-13 | 2019-07-09 | 深兰科技（上海）有限公司 | A kind of method and apparatus adjusting target vehicle motion state |
| CN110211176A (en) * | 2019-05-31 | 2019-09-06 | 驭势科技（北京）有限公司 | A kind of Camera extrinsic number correction System and method for |
| CN110398969A (en) * | 2019-08-01 | 2019-11-01 | 北京主线科技有限公司 | Automatic driving vehicle adaptive prediction time domain rotating direction control method and device |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111060126A (en) | 2020-04-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108609014B (en) | Transverse control method and device of intelligent vehicle | |
| CN110609290B (en) | Laser radar matching positioning method and device | |
| CN109084782B (en) | Lane line map construction method and construction system based on camera sensor | |
| CN111123950B (en) | Driving control method and device and vehicle | |
| WO2019047641A1 (en) | Method and device for estimating orientation error of onboard camera | |
| RU2753004C2 (en) | System and method for positioning and controlling the aircraft as it moves along the taxiway | |
| CN111060126B (en) | Positioning method and device and vehicle | |
| JP7391701B2 (en) | Method, device, storage medium, and program for removing steady lateral deviation | |
| CN1521056A (en) | Vehicle behavior detector, in-vehicle processing system, detection information calibrator, and in-vehicle processor | |
| CN103914068A (en) | Service robot autonomous navigation method based on raster maps | |
| CN111650598A (en) | External parameter calibration method and device for vehicle-mounted laser scanning system | |
| CN112731354B (en) | Self-calibration method for laser radar pose on AGV | |
| CN112046504B (en) | Unmanned vehicle, transverse control method thereof and electronic equipment | |
| CN105752154A (en) | Vehicle steering control system and method | |
| CN111158379B (en) | Steering wheel zero-bias self-learning unmanned vehicle track tracking method | |
| CN110956214B (en) | Training method and device for automatic driving vision positioning model | |
| CN109916417B (en) | Map establishing method, map establishing device, computer equipment and storage medium thereof | |
| CN112078570A (en) | Automobile positioning method based on Ackerman steering model | |
| CN112722076B (en) | Steering calibration method and device and unmanned automobile | |
| CN112665506A (en) | Method, device, equipment and storage medium for detecting installation deviation of positioning device | |
| CN110155080A (en) | Sensor stabilization control method, device, stabilizer and medium | |
| CN115655311A (en) | Ackerman robot odometer calibration method based on scanning matching | |
| CN112158196B (en) | Automatic parking method and device | |
| CN112747752A (en) | Vehicle positioning method, device, equipment and storage medium based on laser odometer | |
| US20230221125A1 (en) | Method for ascertaining an initial pose of a 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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |