CN112985426A - Positioning method for two-wheel vehicle - Google Patents
Positioning method for two-wheel vehicle Download PDFInfo
- Publication number
- CN112985426A CN112985426A CN202110348925.5A CN202110348925A CN112985426A CN 112985426 A CN112985426 A CN 112985426A CN 202110348925 A CN202110348925 A CN 202110348925A CN 112985426 A CN112985426 A CN 112985426A
- Authority
- CN
- China
- Prior art keywords
- angle
- moment
- measurement
- wheeled vehicle
- gyroscope
- 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.)
- Granted
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/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Length Measuring Devices With Unspecified Measuring Means (AREA)
- Gyroscopes (AREA)
Abstract
The invention provides a positioning method for a two-wheel vehicle, which comprises the following steps: s1: mounting a speedometer, a gyroscope and an angle sensor, and calibrating the speedometer and the angle sensor; s2: obtaining a standard deviation parameter of a measurement error; s3: establishing a state equation and an observation equation; s4: measuring the motion parameters of the two-wheel vehicle in the motion process; s5: the measurement results are processed. The invention can overcome the angle error caused by only using the gyroscope to a certain extent, in particular the influence of the angle accumulated error measured for a long time on the positioning precision and the stability.
Description
Technical Field
The invention relates to the technical field of outdoor positioning and navigation, in particular to a positioning method for a two-wheel vehicle.
Background
At present, a satellite positioning navigation technology is mainly adopted for outdoor positioning, but the technology is limited by a plurality of external conditions, particularly in urban complex environments, such as dense-space buildings, long tunnels or scenes similar to tunnels, and the like, satellite signals are weak and even can not receive signals completely, so that the positioning accuracy is damaged and even no positioning data exists. The solution is to install auxiliary positioning sensors on the moving object (usually a moving vehicle), wherein an odometer and a gyroscope are used as main positioning sensors, wherein the odometer provides displacement and speed information of the moving object, and the gyroscope provides moving direction (angle) information of the moving object. In some moving objects, since the driving wheels are controlled separately, moving direction (angle) information of the moving object can be estimated through measurement results of odometers installed on different driving wheels, respectively.
A two-wheel vehicle is composed of bicycle, motorcycle, etc. and features that its front wheel is used for guiding and its rear wheel is used for driving. Taking a motorcycle as an example, positioning sensors commonly installed are odometers and gyroscopes, in addition to satellite positioning equipment, for cost reasons. The gyroscope is used for measuring the moving direction of the two-wheeled vehicle, and the two-wheeled vehicle does not have the characteristics of a plurality of driving wheels, so that the odometer can only measure displacement and speed information. Typical odometer error sources include rear wheel diameter measurement errors and errors due to road surface smoothness, and gyroscopic error sources are zero offset instability and measurement angle drift, and the effect increases with time. Angular errors in the odometer + gyroscope scheme are the main sources of error affecting the final positioning accuracy and stability.
Therefore, a technical solution capable of overcoming the influence of the accumulated angle error of the long-time measurement on the positioning accuracy and stability is needed in the prior art.
Disclosure of Invention
In view of the above-mentioned deficiencies of the prior art, the present invention provides a positioning method for a two-wheeled vehicle.
In order to achieve the purpose, the invention is realized by the following technical scheme:
a positioning method for a two-wheeled vehicle, comprising the steps of:
s1: mounting a speedometer, a gyroscope and an angle sensor, and calibrating the speedometer and the angle sensor;
s2: obtaining a standard deviation parameter of a measurement error;
s3: establishing a state equation and an observation equation;
s4: measuring the motion parameters of the two-wheel vehicle in the motion process;
s5: the measurement results are processed.
The odometer comprises a Hall sensor and a magnet piece;
the installation odometer specifically comprises the following steps: mounting a magnet piece on a hub of a rear wheel of the two-wheel vehicle, and mounting a Hall sensor on a bracket of the rear wheel of the two-wheel vehicle;
the specific calibration of the odometer is as follows: the diameter of the rear wheel of the two-wheel vehicle is measured.
The installation of the gyroscope specifically comprises the following steps: the gyroscope is arranged on the position of the two-wheel vehicle body except the front handle and the front wheel, and the gyroscope is kept horizontal to the ground, so that the rotation of the gyroscope is consistent with the rotation of the two-wheel vehicle body.
The installation angle sensor is specifically as follows: the method comprises the following steps of mounting an angle sensor on a front handle of the two-wheeled vehicle, wherein a central shaft of the angle sensor is mounted on a shaft of the front handle of the two-wheeled vehicle; the shell of the angle sensor is arranged on the connecting body of the front handle of the bicycle;
the calibration of the angle sensor specifically comprises the following substeps:
s101: the two-wheel vehicle is straightened, the landing point of the front wheel is positioned at the original point of the angle ruler, and the measured value of the angle sensor is output as a reference;
s102: the front wheel is deflected by a certain angle, the measurement value of the angle ruler is a real angle, and the measurement value of the angle sensor is a measurement voltage;
s103: and repeating the step S102 for multiple times, continuously measuring the measurement voltage under different real angles for multiple times, and fitting according to the measurement voltage and the data of the real angles to obtain an angle correction curve, wherein the value of 0 and the slope are final correction parameters.
Step S2 specifically includes: testing the two-wheeled vehicle to make the two-wheeled vehicle make linear motion for a certain time, and make the two-wheeled vehicle keep constant speed as far as possible, recording the real-time measured values of the odometer, the gyroscope and the angle sensor, after making multiple tests, making statistical calculation to obtain the standard deviation of the measured values, namely the standard deviation parameter of the measurement error,
step S3 includes the following substeps:
s301: constructing a state equation:
the state variable is [ Delta x ]k Δyk ΔSk θk Δθk]TWherein, in the step (A),
Δxk Δyk: the position offset of the two-wheeled vehicle at the X, Y shaft at the moment k;
ΔSk: the integral position offset of the two-wheel vehicle at the moment k;
θk: the angle corresponding to the moving direction of the body of the two-wheeled vehicle at the moment k;
Δθk: the angle offset corresponding to the movement direction of the two-wheel vehicle at the moment k;
the equation of state is as follows:
[Δxk+1 Δyk+1 ΔSk+1 θk+1 Δθk+1]T＝f([Δxk Δyk ΔSk θk Δθk]T) (1)
wherein the content of the first and second substances,
wherein the content of the first and second substances,
lfand lrDistance of front and rear wheels to the center of gravity of the vehicle, deltafkIs the angle of deflection, omega, of the front wheel of a two-wheeled vehiclesAnd ωθThe variance of the displacement measurement error and the variance of the angle measurement error are shown, and the state transition matrix is as follows:
the error variance of the equation of state is determined as Q1 × 10-2；
S302: and (3) constructing an observation equation:
the observed variables were: the measurement result of the odometer at the moment k +1, the measurement result of the angle sensor at the moment k and the measurement result of the gyroscope at the moment k + 1;
wherein the relationship with the state variables is as follows:
wherein, Delta Sk+1The measurement from the moment of the odometer k +1,βkfrom the measurement at time k of the angle sensor, thetagyro_k+1And Δ θgyro_k+1A measurement from the moment of gyroscope k + 1; the observation matrix is:
the standard deviation of the observation error employs the observation error parameters for different sensors obtained in step S302, namely: and R is [ observation error variance of the odometer observation error variance angle sensor observation error variance gyroscope observation error variance ].
The motion parameters comprise displacement, course angle, pitch angle and roll angle of the two-wheeled vehicle, and deflection angle of a front wheel of the two-wheeled vehicle relative to a vehicle body;
step S4 specifically includes: setting timing parameters, and periodically reading the measurement values of a speedometer, a gyroscope and an angle sensor according to the timing parameters, wherein the measurement value of the speedometer is the displacement of the two-wheeled vehicle, the measurement value of the gyroscope comprises a course angle, a pitch angle and a roll angle, and the measurement value of the angle sensor is the deflection angle of a front wheel of the two-wheeled vehicle relative to a vehicle body.
The displacement measuring method of the two-wheel vehicle is as follows: the magnet piece number of the design mileage meter installation is M, the number of times that hall sensor is aroused by the magnet piece in the timing cycle is N, and the diameter of the rear wheel of the two-wheel vehicle is D, and then the displacement in the timing cycle is:
the method for measuring the deflection angle of the front wheels of the two-wheeled vehicle relative to the vehicle body comprises the following steps: and taking the heading angle as the vehicle body motion angle, and correcting the measurement result of the angle sensor according to the correction parameters obtained in the step S1, wherein the corrected data is the deflection angle of the front wheel of the two-wheeled vehicle relative to the vehicle body.
Step S5 specifically includes the following substeps:
s501: and (3) state prediction:
predicting the state variable at the k moment according to the state variable X (k-1) at the k-1 moment and the state transition matrix phi (k, k-1) at the k-1 moment;
s502: and (3) variance prediction:
predicting the variance at the k moment according to the variance P (k-1) at the k-1 moment, the state transition matrix phi (k, k-1) at the k-1 moment and the error variance Q of the state equation;
s503: and (3) gain calculation:
K(k)＝P(k,k-1)HT(k)[H(k)P(k,k-1)HT(k)+R]-1 (9)
predicting the gain at the k moment according to the variance P (k-1) at the k-1 moment, an observation matrix H (k) at the k-1 moment and an error variance R of an observation equation;
s504: calculating the difference value between the observed value and the predicted value:
obtaining a difference value between the observation value and the predicted value according to the difference between the state predicted value at the moment k and the observation value at the moment k, wherein the observation value of the angle sensor is the observation value at the moment k-1;
s505: final filter result and final filter variance calculation:
the final filtering result is calculated on the basis of the state prediction values as follows:
according to the predicted value
And a gain K (k) ofAnd the difference D (k) obtained in the fourth step calculates the final filtering result;
the final filtering result is calculated on the basis of the variance prediction as follows:
and calculating a final variance result according to the variance prediction value P (k, k-1), the gain K (k) and the state transition matrix H (k).
Compared with the prior art, the invention has the beneficial effects that:
1. the angle sensor and the gyroscope complete angle measurement together, and can overcome angle errors caused by only using the gyroscope to a certain extent, particularly the influence of angle accumulated errors during long-time measurement on positioning accuracy and stability.
2. When Kalman filtering processing is adopted as an algorithm for fusing measurement results of multiple sensors in vehicle positioning, the function of an offset value of an estimated angle as a state variable is highlighted, the precision and the stability of angle estimation are improved, and the precision and the stability of position estimation are finally improved.
Drawings
Fig. 1 is a schematic view showing the mounting positions of the odometer, the gyroscope, and the angle sensor on the two-wheeled vehicle.
Fig. 2 is a schematic view of the angle sensor structure.
Fig. 3 is a schematic view of the mounting of the angle sensor on the front handlebar of the two-wheeled vehicle.
FIG. 4 is a schematic diagram of a process for angle zero correction of an angle sensor.
Fig. 5 is a schematic diagram of a process of angle positive angle correction of the angle sensor.
Fig. 6 is a schematic diagram of a process of making an angular negative angle correction for an angle sensor.
Fig. 7 is a schematic view of a model of the movement of the two-wheeled vehicle.
FIG. 8 is a schematic view of an angle sensor angle correction curve.
In the figure: 1-Hall sensor, 2-magnet piece, 3-angle sensor, 4-gyroscope, 5-central shaft and 6-shell.
Detailed Description
The invention is further illustrated by the following specific embodiments.
The positioning method for a two-wheeled vehicle as shown in fig. 1 to 8 comprises the steps of:
s1: installing the odometer, the gyroscope 4 and the angle sensor 3, and calibrating the odometer and the angle sensor 3:
in this embodiment, a six-degree-of-freedom gyroscope, i.e., a three-axis accelerator + a three-axis gyroscope, is used.
As shown in fig. 2, the angle sensor 3 has a central shaft 5 capable of rotating within a certain range, the rotation angle of the central shaft 5 relative to the outer housing 6 corresponds to the output voltage value of the angle sensor 3, the angle sensor 3 and the gyroscope 4 complete angle measurement together, and the influence of angle errors caused by using only the gyroscope 4, especially the influence of angle accumulated errors during long-time measurement on positioning accuracy and stability, can be overcome to a certain extent.
The odometer comprises a Hall sensor 1 and a magnet piece 2;
the installation odometer specifically comprises the following steps: the magnet pieces 2 are arranged on a hub of a rear wheel of the two-wheel vehicle, the number of the magnet pieces 2 is determined according to the requirement of displacement precision, if the displacement precision is high, the number of the magnet pieces 2 is large, otherwise, if the displacement precision is low, the number of the magnet pieces 2 is small; install hall sensor 1 on the rear wheel support of bicycle, hall sensor 1 active surface keeps effectual short distance with magnet piece 2 to magnet piece 2 and hall sensor 1 can effectively arouse when guaranteeing the rotation of bicycle rear wheel.
The specific calibration of the odometer is as follows: the diameter of the rear wheel of the two-wheel vehicle is measured.
The installation of the gyroscope 4 is specifically as follows: the gyroscope 4 is mounted on the body of the two-wheeled vehicle at a position other than the front grip and the front wheel, and the gyroscope 4 is kept horizontal with the ground, so that the rotation of the gyroscope 4 is kept the same as the rotation of the body of the two-wheeled vehicle.
The installation angle sensor 3 is specifically: the angle sensor 3 is arranged on the front handle of the two-wheeled vehicle and keeps consistent with the rotation of the front wheel of the two-wheeled vehicle, wherein a central shaft 5 of the angle sensor 3 is arranged on a shaft of the front handle of the two-wheeled vehicle; the housing 6 of the angle sensor 3 is mounted on the connecting body of the front handle of the bicycle;
the installation direction of the angle sensor 3 does not coincide with the vertical direction of the front wheel of the bicycle, and a certain included angle exists, so the result measured by the angle sensor 3 is not the real deflection angle of the front wheel, and therefore correction is needed.
The calibration of the angle sensor 3 specifically comprises the following substeps:
s101: the two-wheeled vehicle is set up, the landing point of the front wheel is positioned at the origin of the angle ruler, the measured value of the angle sensor 3 is calibrated to be 0 degrees at the moment, and the landing point of the front wheel is positioned at the origin of the angle ruler.
S102: the front wheel is deflected by a certain angle, the measurement value of the angle ruler is a real angle, and the measurement value of the angle sensor 3 is a measurement voltage; in step S102, when the front wheel is deflected to the positive direction side of the two-wheel vehicle, the real angle value is positive; when the front wheel is deflected to the negative direction side of the two-wheel vehicle, the true angle value is negative.
S103: and repeating the step S102 for multiple times, continuously measuring the measurement voltage under different real angles for multiple times, and fitting according to the measurement voltage and the data of the real angles to obtain an angle correction curve, wherein the value of 0 and the slope are final correction parameters.
S2: obtaining a standard deviation parameter of a measurement error;
step S2 specifically includes: the method comprises the following steps of testing a two-wheeled vehicle, wherein in order to ensure the consistency of a testing process, a testing path is a straight line, namely the two-wheeled vehicle moves on the straight line, the length of the straight line is kept to be more than 20 meters, the two-wheeled vehicle moves linearly for a certain time, the two-wheeled vehicle keeps constant speed as much as possible, the speed is about 1m/s, enough testing samples are obtained to carry out statistical analysis, the real-time measurement values of a speedometer, a gyroscope 4 and an angle sensor 3 are recorded, and in the linear motion, the front wheel of the two-wheeled vehicle does not deflect theoretically, so that the measurement result is 0 degree; since the 20m movement time is not long, the angle error drift phenomenon of the gyroscope 4 is not serious, and the measurement result should be kept unchanged theoretically; under the condition of keeping the constant speed as much as possible, the measurement result of the odometer at each time should be maintained at a stable value, and after multiple tests, the standard deviation of the measurement value is obtained through statistical calculation, namely the standard deviation parameter of the measurement error.
The measured value of the odometer is the displacement of the vehicle body in a timing period (such as 0.1s), the measured value of the gyroscope 4 is a course angle, a pitch angle and a roll angle, and the measured value of the angle sensor 3 is the deflection angle of the front wheel of the two-wheeled vehicle relative to the vehicle body.
The included angle between the transverse axis of the carrier and the horizontal line is called a roll angle, and the horizontal dip angle used for marking a target in the navigation system is guided, and the value of the horizontal dip angle is equal to the included angle between the line which is perpendicular to the fore-aft line on the plane where the target object is located and the projection of the line on the horizontal plane. Roll represents rotation of the vehicle about the longitudinal axis, with positive clockwise rotation about the longitudinal axis.
Generally, the right, front and upper directions of the carrier form a right-hand system, the rotation around a forward axis is a roll angle, the rotation around a right axis is a pitch angle, and the rotation around an upward axis is a heading angle.
Pitch angle: and the included angle between the X axis of the machine body coordinate system and the horizontal plane. When the positive semi-axis of the X-axis is located above the horizontal plane passing through the origin of coordinates, the pitch angle is positive, and according to the convention, the range of the pitch angle theta is as follows: phi/2 is more than or equal to theta and less than or equal to phi/2.
The displacement measuring method of the two-wheel vehicle is as follows: 2 numbers of magnet piece of the installation of design mileage meter are M, and the number of times that hall sensor 1 is aroused by magnet piece 2 in the timing cycle is N, and the diameter of the rear wheel of the cart is D, and then the displacement is in the timing cycle:
the method for measuring the deflection angle of the front wheels of the two-wheeled vehicle relative to the vehicle body comprises the following steps: the gyroscope 4 is horizontally installed, so that the heading angle is taken as the vehicle body motion angle, the measurement result of the angle sensor 3 is corrected according to the correction parameters obtained in the step S1, and the corrected data is the deflection angle of the front wheel of the two-wheeled vehicle relative to the vehicle body.
S3: establishing a state equation and an observation equation:
step S3 includes the following substeps:
s301: constructing a state equation:
kalman filtering is a commonly used algorithm for solving the fusion of measurement results of multiple sensors in vehicle positioning design, and related state variables comprise position coordinates of a moving vehicle and a moving direction of the vehicle as follows:
[Δxk Δyk θk]T
wherein, Δ xk Δyk: the position offset of the X, Y axis at time k;
θk: the angle corresponding to the moving direction of the body of the two-wheeled vehicle at the moment k;
to correspond to the odometer result in the measurement equation, a displacement increment Δ S is added to the state variablek. Further, since the angle measurement result of the gyroscope 4 changes with time due to the cumulative effect and reflects the instability of the measurement result, but the angle measurement result is relatively stable in a short time, the present invention describes the differential operation of the angle corresponding to the moving direction of the vehicle body of the two-wheeled vehicle, that is, the difference between the front and rear angle values (angle offset value) in a short time as a state variable, and the final state variable is: [ Delta xk Δyk ΔSk θk Δθk]TWherein, in the step (A),
Δxk Δyk: the position offset of the two-wheeled vehicle at the X, Y shaft at the moment k;
ΔSk: the integral position offset of the two-wheel vehicle at the moment k;
θk: the angle corresponding to the moving direction of the body of the two-wheeled vehicle at the moment k;
Δθk: the angle offset corresponding to the movement direction of the two-wheel vehicle at the moment k;
the equation of state is as follows:
[Δxk+1 Δyk+1 ΔSk+1 θk+1 Δθk+1]T＝f([Δxk Δyk ΔSk θk Δθk]T) (1)
wherein the content of the first and second substances,
wherein the content of the first and second substances,
lfand lrDistance of front and rear wheels to the center of gravity of the vehicle, deltafkIs the angle of deflection, omega, of the front wheel of a two-wheeled vehiclesAnd ωθThe variance of the displacement measurement error and the variance of the angle measurement error are shown, and the state transition matrix is as follows:
the error variance of the equation of state is determined as Q1 × 10-2；
S302: and (3) constructing an observation equation:
the observed variables were: the measurement result at the moment of the odometer k +1, the measurement result at the moment of the angle sensor 3k and the measurement result at the moment of the gyroscope 4k + 1;
wherein the relationship with the state variables is as follows:
wherein, Delta Sk+1Measurement from the moment k +1 of the odometer, betakFrom the measurement at the 3k instant of the angle sensor, thetagyro_k+1And Δ θgyro_k+1Measurement results from moment 4k +1 of the gyroscope; the observation matrix is:
the standard deviation of the observation error employs the observation error parameters for different sensors obtained in step S302, namely: r ═ the odometer observation error variance angle sensor 3 observation error variance gyroscope 4 observation error variance.
S4: measuring the motion parameters of the two-wheel vehicle in the motion process:
the motion parameters comprise displacement, course angle, pitch angle and roll angle of the two-wheeled vehicle and deflection angle of a front wheel of the two-wheeled vehicle relative to a vehicle body; step S4 specifically includes: timing parameters are set, and measurement values of the odometer, the gyroscope 4, and the angle sensor 3 are periodically read according to the timing parameters.
S5: processing the measurement result:
step S5 specifically includes the following substeps:
s501: and (3) state prediction:
predicting the state variable at the k moment according to the state variable X (k-1) at the k-1 moment and the state transition matrix phi (k, k-1) at the k-1 moment;
s502: and (3) variance prediction:
predicting the variance at the k moment according to the variance P (k-1) at the k-1 moment, the state transition matrix phi (k, k-1) at the k-1 moment and the error variance Q of the state equation;
s503: and (3) gain calculation:
K(k)＝P(k,k-1)HT(k)[H(k)P(k,k-1)HT(k)+R]-1 (9)
predicting the gain at the k moment according to the variance P (k-1) at the k-1 moment, an observation matrix H (k) at the k-1 moment and an error variance R of an observation equation;
s504: calculating the difference value between the observed value and the predicted value:
obtaining a difference value between the observation value and the predicted value according to the difference between the state predicted value at the time k and the observation value at the time k, wherein the observation value of the angle sensor 3 is the observation value at the time k-1;
s505: final filter result and final filter variance calculation:
the final filtering result is calculated on the basis of the state prediction values as follows:
according to the predicted value
Calculating a final filtering result by using the gain K (k) and the difference D (k) obtained in the fourth step;
the final filtering result is calculated on the basis of the variance prediction as follows:
and calculating a final variance result according to the variance prediction value P (k, k-1), the gain K (k) and the state transition matrix H (k).
The above description is only for the preferred embodiment of the present invention, but the present invention is not limited to the above specific embodiments, and those skilled in the art can make various changes and modifications without departing from the inventive concept, which falls into the protection scope of the present invention.
Claims (10)
1. A positioning method for a two-wheeled vehicle is characterized by comprising the following steps:
s1: mounting a speedometer, a gyroscope and an angle sensor, and calibrating the speedometer and the angle sensor;
s2: acquiring standard deviation parameters of measurement errors of the odometer, the gyroscope and the angle sensor;
s3: establishing a state equation and an observation equation;
s4: measuring the motion parameters of the two-wheel vehicle in the motion process;
s5: the measurement results are processed.
2. The positioning method for a two-wheeled vehicle according to claim 1, wherein the odometer includes a hall sensor and a magnet piece;
the installation odometer specifically comprises the following steps: mounting a magnet piece on a hub of a rear wheel of the two-wheel vehicle, and mounting a Hall sensor on a bracket of the rear wheel of the two-wheel vehicle;
the specific calibration of the odometer is as follows: the diameter of the rear wheel of the two-wheel vehicle is measured.
3. The positioning method for a two-wheeled vehicle according to claim 1, wherein the step of installing the gyroscope is specifically: the gyroscope is arranged on the position of the two-wheel vehicle body except the front handle and the front wheel, and the gyroscope is kept horizontal to the ground, so that the rotation of the gyroscope is consistent with the rotation of the two-wheel vehicle body.
4. The positioning method for a two-wheeled vehicle according to claim 1, wherein the mounting angle sensor is specifically: the method comprises the following steps of mounting an angle sensor on a front handle of the two-wheeled vehicle, wherein a central shaft of the angle sensor is mounted on a shaft of the front handle of the two-wheeled vehicle; the shell of the angle sensor is arranged on the connecting body of the front handle of the bicycle;
the calibration of the angle sensor specifically comprises the following substeps:
s101: the two-wheel vehicle is righted, the landing point of the front wheel is positioned at the original point of the angle ruler, and the measured value of the angle sensor is calibrated to be 0 degree at the moment;
s102: the front wheel is deflected by a certain angle, the measured value of the angle ruler is a real angle at the moment, and the measured voltage of the angle sensor at the moment is recorded;
s103: and repeating the step S102 for multiple times, continuously recording the measured voltages under different real angles, and fitting according to the measured voltages and the data of the real angles to obtain an angle correction curve, wherein the value of 0 and the slope are final correction parameters.
5. The positioning method for the motorcycle according to claim 4, wherein in step S102, when the front wheel is deflected to the positive direction side of the motorcycle, the true angle value is positive; when the front wheel is deflected to the negative direction side of the two-wheel vehicle, the true angle value is negative.
6. The positioning method for a two-wheeled vehicle according to claim 1, wherein step S2 is specifically: the method comprises the steps of testing the two-wheeled vehicle, enabling the two-wheeled vehicle to conduct linear motion for a certain time, enabling the two-wheeled vehicle to keep constant speed as far as possible, recording real-time measurement values of a speedometer, a gyroscope and an angle sensor, conducting multiple tests, and then conducting statistical calculation to obtain a standard deviation of the measurement values, namely a standard deviation parameter of a measurement error, wherein the measurement values of the speedometer are displacement of a two-wheeled vehicle body, the measurement values of the gyroscope are a course angle, a pitch angle and a roll angle, and the measurement values of the angle sensor are a deflection angle of a front wheel of the two-wheeled vehicle relative to the.
7. The positioning method for a two-wheeled vehicle as set forth in claim 1, wherein the step S3 includes the substeps of:
s301: constructing a state equation:
the state variable is [ Delta x ]k Δyk ΔSk θk Δθk]TWherein, in the step (A),
Δxk Δyk: the position offset of the two-wheeled vehicle at the X, Y shaft at the moment k;
ΔSk: the integral position offset of the two-wheel vehicle at the moment k;
θk: the angle corresponding to the moving direction of the body of the two-wheeled vehicle at the moment k;
Δθk: the angle offset corresponding to the movement direction of the two-wheel vehicle at the moment k;
the equation of state is as follows:
[Δxk+1 Δyk+1 ΔSk+1 θk+1 Δθk+1]T＝f([Δxk Δyk ΔSk θk Δθk]T) (1)
wherein the content of the first and second substances,
wherein the content of the first and second substances,
lfand lrDistance of front and rear wheels to the center of gravity of the vehicle, deltafkIs the angle of deflection, omega, of the front wheel of a two-wheeled vehiclesAnd ωθThe variance of the displacement measurement error and the variance of the angle measurement error are shown, and the state transition matrix is as follows:
the error variance of the equation of state is determined as Q1 × 10-2；
S302: and (3) constructing an observation equation:
the observed variables were: the measurement result of the odometer at the moment k +1, the measurement result of the angle sensor at the moment k and the measurement result of the gyroscope at the moment k + 1;
wherein the relationship with the state variables is as follows:
wherein, Delta Sk+1Measurement from the moment k +1 of the odometer, betakFrom the measurement at time k of the angle sensor, thetagyro_k+1And Δ θgyro_k+1A measurement from the moment of gyroscope k + 1; the observation matrix is:
the standard deviation of the observation error employs the observation error parameters for different sensors obtained in step S302, namely: and R is [ observation error variance of the odometer observation error variance angle sensor observation error variance gyroscope observation error variance ].
8. The positioning method for the motorcycle as claimed in claim 1, wherein the motion parameters include displacement, heading angle, pitch angle and roll angle of the motorcycle, and a yaw angle of a front wheel of the motorcycle with respect to a vehicle body;
step S4 specifically includes: setting timing parameters, and periodically reading the measurement values of a speedometer, a gyroscope and an angle sensor according to the timing parameters, wherein the measurement value of the speedometer is the displacement of the body of the bicycle, the measurement value of the gyroscope comprises a course angle, a pitch angle and a roll angle, and the measurement value of the angle sensor is the deflection angle of the front wheel of the bicycle relative to the body.
9. The positioning method for a two-wheeled vehicle according to claim 8, wherein the displacement of the two-wheeled vehicle is measured by the following method: the magnet piece number of the design mileage meter installation is M, the number of times that hall sensor is aroused by the magnet piece in the timing cycle is N, and the diameter of the rear wheel of the two-wheel vehicle is D, and then the displacement in the timing cycle is:
the method for measuring the deflection angle of the front wheels of the two-wheeled vehicle relative to the vehicle body comprises the following steps: and taking the heading angle as the vehicle body motion angle, and correcting the measurement result of the angle sensor according to the correction parameters obtained in the step S1, wherein the corrected data is the deflection angle of the front wheel of the two-wheeled vehicle relative to the vehicle body.
10. The positioning method for a two-wheeled vehicle as set forth in claim 1, wherein the step S5 includes the following steps:
s501: and (3) state prediction:
predicting the state variable at the k moment according to the state variable X (k-1) at the k-1 moment and the state transition matrix phi (k, k-1) at the k-1 moment;
s502: and (3) variance prediction:
predicting the variance at the k moment according to the variance P (k-1) at the k-1 moment, the state transition matrix phi (k, k-1) at the k-1 moment and the error variance Q of the state equation;
s503: and (3) gain calculation:
K(k)＝P(k,k-1)HT(k)[H(k)P(k,k-1)HT(k)+R]-1 (9)
predicting the gain at the k moment according to the variance P (k-1) at the k-1 moment, an observation matrix H (k) at the k-1 moment and an error variance R of an observation equation;
s504: calculating the difference value between the observed value and the predicted value:
obtaining a difference value between the observation value and the predicted value according to the difference between the state predicted value at the moment k and the observation value at the moment k, wherein the observation value of the angle sensor is the observation value at the moment k-1;
s505: final filter result and final filter variance calculation:
the final filtering result is calculated on the basis of the state prediction values as follows:
according to the predicted value
Calculating a final filtering result by using the gain K (k) and the difference D (k) obtained in the fourth step;
the final filtering result is calculated on the basis of the variance prediction as follows:
and calculating a final variance result according to the variance prediction value P (k, k-1), the gain K (k) and the state transition matrix H (k).
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110348925.5A CN112985426B (en) | 2021-03-31 | 2021-03-31 | Positioning method for two-wheel vehicle |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110348925.5A CN112985426B (en) | 2021-03-31 | 2021-03-31 | Positioning method for two-wheel vehicle |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112985426A true CN112985426A (en) | 2021-06-18 |
| CN112985426B CN112985426B (en) | 2022-09-13 |
Family
ID=76338662
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110348925.5A Active CN112985426B (en) | 2021-03-31 | 2021-03-31 | Positioning method for two-wheel vehicle |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112985426B (en) |
Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104460665A (en) * | 2014-10-13 | 2015-03-25 | 上海交通大学 | Magnetic navigation unmanned vehicle based on road curvature map and road curvature map establishment method |
| CN104537829A (en) * | 2014-12-09 | 2015-04-22 | 北京工业大学 | Intelligent car and positioning method used for intelligent transportation physical simulation platform |
| CN104977004A (en) * | 2015-07-13 | 2015-10-14 | 湖北航天技术研究院总体设计所 | Method and system for integrated navigation of laser inertial measuring unit and odometer |
| CN105172793A (en) * | 2015-09-25 | 2015-12-23 | 广州橙行智动汽车科技有限公司 | Position and orientation estimation method for self-driving car |
| CN106393104A (en) * | 2016-08-25 | 2017-02-15 | 北京创想智控科技有限公司 | Stroke calibration method of mobile robot |
| CN106772517A (en) * | 2016-12-29 | 2017-05-31 | 华南农业大学 | Agricultural machinery roll angle method of testing based on double antenna GNSS receiver/gyroscope information fusion |
| CN110515381A (en) * | 2019-08-22 | 2019-11-29 | 浙江迈睿机器人有限公司 | Multi-sensor Fusion algorithm for positioning robot |
| CN110567481A (en) * | 2019-09-05 | 2019-12-13 | 上海凌泽信息科技有限公司 | object displacement monitoring method |
| CN110849349A (en) * | 2019-10-18 | 2020-02-28 | 浙江天尚元科技有限公司 | Fusion positioning method based on magnetic sensor and wheel type odometer |
| CN110865403A (en) * | 2019-10-18 | 2020-03-06 | 浙江天尚元科技有限公司 | Positioning method based on neural network pre-learning and wheel-type odometer fusion |
| CN110864685A (en) * | 2019-10-18 | 2020-03-06 | 浙江天尚元科技有限公司 | Vehicle monocular vision wheeled type odometer positioning method based on loose coupling |
| CN112050809A (en) * | 2020-10-08 | 2020-12-08 | 吉林大学 | Wheel type odometer and gyroscope information fusion unmanned vehicle directional positioning method |
-
2021
- 2021-03-31 CN CN202110348925.5A patent/CN112985426B/en active Active
Patent Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104460665A (en) * | 2014-10-13 | 2015-03-25 | 上海交通大学 | Magnetic navigation unmanned vehicle based on road curvature map and road curvature map establishment method |
| CN104537829A (en) * | 2014-12-09 | 2015-04-22 | 北京工业大学 | Intelligent car and positioning method used for intelligent transportation physical simulation platform |
| CN104977004A (en) * | 2015-07-13 | 2015-10-14 | 湖北航天技术研究院总体设计所 | Method and system for integrated navigation of laser inertial measuring unit and odometer |
| CN105172793A (en) * | 2015-09-25 | 2015-12-23 | 广州橙行智动汽车科技有限公司 | Position and orientation estimation method for self-driving car |
| CN106393104A (en) * | 2016-08-25 | 2017-02-15 | 北京创想智控科技有限公司 | Stroke calibration method of mobile robot |
| CN106772517A (en) * | 2016-12-29 | 2017-05-31 | 华南农业大学 | Agricultural machinery roll angle method of testing based on double antenna GNSS receiver/gyroscope information fusion |
| CN110515381A (en) * | 2019-08-22 | 2019-11-29 | 浙江迈睿机器人有限公司 | Multi-sensor Fusion algorithm for positioning robot |
| CN110567481A (en) * | 2019-09-05 | 2019-12-13 | 上海凌泽信息科技有限公司 | object displacement monitoring method |
| CN110849349A (en) * | 2019-10-18 | 2020-02-28 | 浙江天尚元科技有限公司 | Fusion positioning method based on magnetic sensor and wheel type odometer |
| CN110865403A (en) * | 2019-10-18 | 2020-03-06 | 浙江天尚元科技有限公司 | Positioning method based on neural network pre-learning and wheel-type odometer fusion |
| CN110864685A (en) * | 2019-10-18 | 2020-03-06 | 浙江天尚元科技有限公司 | Vehicle monocular vision wheeled type odometer positioning method based on loose coupling |
| CN112050809A (en) * | 2020-10-08 | 2020-12-08 | 吉林大学 | Wheel type odometer and gyroscope information fusion unmanned vehicle directional positioning method |
Non-Patent Citations (1)
| Title |
|---|
| SOFIA YOUSUF, MUHAMMAD BILAL KADRI: "Sensor Fusion of INS, OdomeLocalizationter and GPS for Robot", 《2016 IEEE CONFERENCE ON SYSTEMS, PROCESS AND CONTROL (ICSPC 2016)》 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112985426B (en) | 2022-09-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| KR102018450B1 (en) | A method for determining a vehicle attitude of UNS-IENS based on a single antenna | |
| CN103206965B (en) | The angular velocity error correction device of vehicle gyroscope, modification method | |
| CN105865461B (en) | A kind of car position system and method based on Multi-sensor Fusion algorithm | |
| CN113311411B (en) | Laser radar point cloud motion distortion correction method for mobile robot | |
| CN111551175B (en) | Complementary filtering attitude resolving method of navigation attitude reference system | |
| CN109443349A (en) | A kind of posture Course Measure System and its fusion method, storage medium | |
| CN110631574B (en) | inertia/odometer/RTK multi-information fusion method | |
| US20200064136A1 (en) | Inertial navigation system | |
| CN110530361B (en) | Steering angle estimator based on agricultural machinery double-antenna GNSS automatic navigation system | |
| CN110873575B (en) | Mileage measurement method based on inertial sensor | |
| CN111272158B (en) | Dynamic azimuth angle resolving method of MEMS electronic compass in complex magnetic disturbance scene | |
| CN110631579A (en) | Combined positioning method for agricultural machine navigation | |
| CN109470276B (en) | Odometer calibration method and device based on zero-speed correction | |
| CN115615430B (en) | Positioning data correction method and system based on strapdown inertial navigation | |
| KR20200130423A (en) | How to calibrate the gyrometer installed in the vehicle | |
| CN110823224A (en) | Vehicle positioning method and vehicle | |
| CN111307114B (en) | Water surface ship horizontal attitude measurement method based on motion reference unit | |
| CN112429006A (en) | Vehicle-mounted road surface gradient measuring system and measuring method | |
| CN114526731A (en) | Inertia combination navigation direction positioning method based on moped | |
| CN112985426B (en) | Positioning method for two-wheel vehicle | |
| CN111141285B (en) | Aviation gravity measuring device | |
| CN108663036A (en) | A kind of vehicle front-drive tracking system and algorithm | |
| CN112577513A (en) | State quantity error determination method and vehicle-mounted terminal | |
| CN113484542B (en) | Single-point rapid calibration method for three-dimensional velocimeter | |
| CN109596139B (en) | Vehicle-mounted navigation method based on MEMS |
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 |