A method and device for accurately measuring moving trajectory based on acceleration induction and magnetic induction

Abstract

The present invention uses high-precision acceleration sensing, magnetic induction technology and accurate mathematical models to provide a method and device for accurately measuring a moving trajectory based on acceleration sensing and magnetic induction. Acceleration sensing and magnetic induction data are collected by an acceleration module and a magnetic induction module installed on two rear wheels of a vehicle, and the wheel rotation angle is calculated by using a mathematical model. The travel distance of the wheel module is calculated by the wheel rotation angle. According to the Ackerman steering geometry, the process of turning of the two rear wheels of the vehicle is modeled, and the coordinate offset and the deviation angle change of the axle center point are substituted into trigonometric functions to calculate the displacement (Δx, Δy) of the vehicle ground coordinate system. Compared with the traditional trajectory measurement method, the present measurement system calculates the position change and angle change of the rear wheel axle of the vehicle in each time interval, which represents the movement of the entire vehicle. The mathematical model requires fewer parameters, the measurement or acquisition method is simpler, the anti-interference ability is strong, and the measurement accuracy is higher.

Description

Technical Field

[0001] The present invention relates to the technical field of determining vehicle displacement and track measurement by using sensors, and in particular to a rotation angle accurate measurement system based on an acceleration induction sensor and a magnetic induction sensor. Background Art

[0002] Precision positioning technology has made great progress in the past few decades and can be basically divided into two categories. One is relative positioning, which starts from a certain reference point and records the direction and distance for positioning, such as inertial navigation, direction and ranging, etc. The other is absolute position (coordinate) positioning, such as GPS, visual positioning, terrain matching, and star positioning. Absolute positioning has now achieved a relatively high level of accuracy, especially the differential positioning technology based on GPS, which can now reach the centimeter level or even the millimeter level, but relative positioning in the civilian field is still at a relatively low level.

[0003] In the prior art, the measurement of rotation angle is usually carried out by a pulse counting device based on the Hall effect, or by a dividing plate and a rotary encoder / decoder linked to the rotating shaft, or by an AC-powered synchro:

[0004] Pulse counting devices are widely used in vehicle travel data collection. Hall elements are fixed on the wheels, and the vehicle mileage is obtained by collecting the number of wheel rotations. The minimum measurement unit of this method is the circumference of one wheel rotation, and the accuracy is not high.

[0005] The indexing disk and rotary encoder / decoder are mostly used for precision servo motor rotation angle control. The motor rotation angle is controlled by a given pulse, and the current angle of the motor is known through the indexing disk and the decoder. This method is costly and has strict restrictions on the use environment.

[0006] Synchro motors powered by AC have been widely used in control systems. This method is costly and bulky, and generally requires external AC power, which places strict restrictions on the use environment. Summary of the invention

[0007] The purpose of the present invention is to overcome the deficiencies of the prior art and to provide a high-precision, low-error method for accurately measuring a moving trajectory by using an acceleration module, a magnetic sensor module and a mathematical model established thereby.

[0008] In order to achieve the above object, the present invention provides a method for accurately measuring a moving trajectory based on acceleration induction and magnetic induction, comprising the following steps:

[0009] s1. An acceleration module for collecting acceleration data of the left-hand coordinate system is set at the center of the wheel. The acceleration data (v x1 , v y1),(v x2 , v y2 ) calculate the wheel rotation angles α1, α2 and the wheel rotation angle changes Δα1, Δα2 of the two rear wheels of the vehicle;

[0010] s2, substitute the wheel rotation angle changes Δα1 and Δα2 and the radius D of the wheel module into the formula to calculate the wheel module travel distances Δs1 and Δs2, and calculate the vehicle movement trajectory through the wheel movement distances Δs1 and Δs2;

[0011] s3. Calculate the inner circle radius r according to the outer curve L of the wheel moving arc, the inner curve R of the wheel moving arc, and the axle length W;

[0012] s4. Substitute the inner circle radius r into the formula to calculate the radian angle from t to t+1

[0013] s5, radian angle The angle conversion is performed to obtain the vehicle's steering angle change Δθ;

[0014] s6. Take the center point of the axle at time t as the origin, establish a coordinate system with the positive direction of the y-axis as the forward direction and the direction from the wheel module outside the moving arc to the wheel module inside the moving arc as the positive direction of the x-axis, and name it the vehicle coordinate system;

[0015] s7. Substitute the deviation angle change Δθ, the axle length W, and the inner circle radius r into the formula to calculate the coordinate offset d of the axle center point;

[0016] s8. Substitute the deflection angle θ(t) at time t, the coordinate offset d of the center point of the axle from time t to time t+1, and the deflection angle change Δθ into the trigonometric function to solve the displacement Δx and Δy of the vehicle in the ground coordinate system from time t to time t+1;

[0017] s9. Use the vehicle's steering angle change Δθ, displacement Δx and displacement Δy to calculate the vehicle's position x(t+1), y(t+1) and steering angle θ(t+1) at time t+1 in the ground coordinate system.

[0018] s10. Use the vehicle's position x(t), y(t), θ(t) and x(t+1), y(t+1), θ(t+1) in the ground coordinate system to draw the vehicle's moving trajectory.

[0019] The calculation method of the wheel rotation angle changes Δα1 and Δα2 is:

[0020] According to the force balance equation, the wheel rotation angles α1(t), α2(t) at time t, the wheel rotation angles α1(t+1), α2(t+1) at time t+1 are calculated, and then the wheel rotation angle changes Δα1, Δα2 are calculated:

[0021]

[0022] Δα1=α1(t+1)-α1(t)

[0023] Δα2=α2(t+1)-α2(t)

[0024] The calculation method of the wheel module travel distance Δs1, Δs2 is:

[0025]

[0026] The calculation method of the inner radius r of the wheel movement is:

[0027]

[0028] The wheel moves at time t+1 to form an arc angle The calculation method is

[0029]

[0030] The radian angle The calculation method for converting the vehicle's steering angle change Δθ is:

[0031]

[0032] The calculation method of the coordinate offset d of the axle center point at time t+1 is:

[0033]

[0034] The calculation method of the ground coordinate system displacement Δx and Δy of the vehicle at time t+1 is:

[0035]

[0036] The new vehicle deviation angle θ(t+1)=θ(t)+Δθ.

[0037] The wheel rotation angle α1, the wheel module travel distance Δs1 and the vehicle steering angle change Δθ in steps s1 to s5 may be used to measure the movement trajectory of a single wheel.

[0038] A device for accurately measuring a moving track based on acceleration induction and magnetic induction, comprising: a sensor arranged at the center of a wheel, a memory storing data executable to implement the method; a data transmission module for transmitting data for implementing the method; and a data processor for executing the method; and a data fusion module for fusing data of the method;

[0039] The sensor comprises:

[0040] The acceleration module is used to detect the acceleration signals of the object on three axes in the carrier coordinate system and output them;

[0041] The magnetic induction module is used to detect the strength and direction of the magnetic field and output it.

[0042] The beneficial effects of the present invention are as follows: a method and a device for accurately measuring a moving trajectory based on acceleration induction and magnetic induction are provided by using high-precision acceleration induction, magnetic induction technology and an accurate mathematical model; acceleration induction and magnetic induction data are collected by an acceleration module and a magnetic induction module installed on two rear wheels of a vehicle; a wheel rotation angle is calculated by using a mathematical model; and a wheel module travel distance is calculated by using the wheel rotation angle; a process of turning the two rear wheels of the vehicle is modeled according to Ackerman steering geometry; the coordinate offset of the center point of the axle and the change in the deflection angle are substituted into trigonometric functions to calculate the displacement (Δx, Δy) of the ground coordinate system of the vehicle; compared with the traditional trajectory measurement method, the present measurement system calculates the position change and angle change of the rear wheel axle of the vehicle in each time interval, representing the movement of the entire vehicle; the mathematical model requires fewer parameters, the measurement or acquisition method is simpler, the anti-interference ability is strong and the measurement accuracy is higher. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] For ease of explanation, the present invention is described in detail with reference to the following preferred embodiments and the accompanying drawings.

[0044] Figure 1 It is a flow chart of vehicle moving distance calculation of a moving track accurate measurement method based on acceleration induction and magnetic induction and a device thereof according to the present invention;

[0045] Figure 2 It is a flow chart of movement trajectory calculation of a movement trajectory accurate measurement method based on acceleration induction and magnetic induction and a device thereof according to the present invention;

[0046] Figure 3 It is a schematic diagram of wheel movement of a method and device for accurately measuring a moving track based on acceleration induction and magnetic induction according to the present invention;

[0047] Figure 4 It is a principle diagram of measuring the planar moving trajectory of two rear wheels of a vehicle according to a method and device for accurately measuring moving trajectory based on acceleration induction and magnetic induction of the present invention;

[0048] Figure 5 It is a schematic diagram of the movement trajectory of two rear wheels of a vehicle in a ground coordinate system of a method and device for accurately measuring movement trajectory based on acceleration induction and magnetic induction according to the present invention;

[0049] Figure 6This is a schematic diagram of the moving trajectory measurement of four wheels of a vehicle according to a method and device for accurately measuring moving trajectory based on acceleration induction and magnetic induction described in the present invention;

[0050] Figure 7 It is a schematic side view of the gears under the influence of the earth's magnetic force in a method and device for accurately measuring the moving trajectory based on acceleration induction and magnetic induction according to the present invention;

[0051] Figure 8 It is a schematic diagram of a gear under the influence of the earth's magnetic force in a method and device for accurately measuring a moving track based on acceleration induction and magnetic induction according to the present invention;

[0052] Fig. 9 It is an experimental vehicle trajectory diagram of a method and device for accurately measuring a moving trajectory based on acceleration induction and magnetic induction according to the present invention;

[0053] Fig.10 It is a schematic diagram of the installation of a magnetic induction module of a method for accurately measuring a moving trajectory based on acceleration induction and magnetic induction and a device thereof according to the present invention;

[0054] Fig.11 It is a module schematic diagram of a method and device for accurately measuring a moving trajectory based on acceleration induction and magnetic induction as described in the present invention. DETAILED DESCRIPTION

[0055] The following is a further description of a method and device for accurately measuring a moving track based on acceleration induction and magnetic induction according to the present invention with reference to the accompanying drawings:

[0056] Embodiment 1:

[0057] A method for accurately measuring a moving trajectory based on acceleration induction and magnetic induction comprises the following steps:

[0058] s1. An acceleration module for collecting acceleration data of the left-hand coordinate system is set at the center of the wheel. The acceleration data (v x1 ,v y1 ),(v x2 ,v y2 ), calculate the wheel rotation angles α1, α2 and wheel rotation angle changes Δα1, Δα2 of the two rear wheels of the vehicle;

[0059] s2, substitute the wheel rotation angle changes Δα1 and Δα2 and the radius D of the wheel module into the formula to calculate the wheel module travel distances Δs1 and Δs2, and calculate the vehicle movement trajectory through the wheel movement distances Δs1 and Δs2;

[0060] s3. Calculate the inner circle radius r according to the outer curve L of the wheel moving arc, the inner curve R of the wheel moving arc, and the axle length W;

[0061] s4. Substitute the inner circle radius r into the formula to calculate the radian angle from time t to time t+1

[0062] s5, radian angle The angle conversion is performed to obtain the vehicle's steering angle change Δθ;

[0063] s6. Take the center point of the axle at time t as the origin, establish a coordinate system with the positive direction of the y-axis as the forward direction and the direction from the wheel module outside the moving arc to the wheel module inside the moving arc as the positive direction of the x-axis, and name it the vehicle coordinate system;

[0064] s7. Substitute the deviation angle change Δθ, the axle length W, and the inner circle radius r into the formula to calculate the coordinate offset d of the axle center point;

[0065] s8. Substitute the deflection angle θ(t) at time t, the coordinate offset d of the center point of the axle from time t to time t+1, and the deflection angle change Δθ into the trigonometric function to solve the displacement Δx and Δy of the vehicle in the ground coordinate system from time t to time t+1;

[0066] s9. Use the vehicle's steering angle change Δθ, displacement Δx and displacement Δy to calculate the vehicle's position x(t+1), y(t+1) and steering angle θ(t+1) at time t+1 in the ground coordinate system.

[0067] s10. Use the vehicle's position x(t), y(t), θ(t) and x(t+1), y(t+1), θ(t+1) in the ground coordinate system to draw the vehicle's moving trajectory.

[0068] like Figure 1 , 3 As shown, in an ideal situation, when the wheel rotates a certain angle, the wheel rotation angle can be deduced from the force change, and then the wheel travel distance can be calculated; in practical applications, it is impossible to perfectly place the sensor on the geometric center of the wheel, so the sensor will be subjected to radial centrifugal force to generate acceleration. Because the value of the centripetal acceleration is smaller than the acceleration, and the direction changes periodically during driving, the impact in a specific form environment (slow and uniform speed) can be offset by the same factor within a cycle, and the centripetal acceleration can be ignored. Only the constant acceleration and the possible lateral acceleration of the vehicle speed change are considered; according to the force balance, the equation is established to calculate the wheel rotation angle α1(t), α2(t) at time t, and the wheel rotation angle α1(t+1), α2(t+1) at time t+1, and then calculate the wheel rotation angle change Δα1, Δα2:

[0069]

[0070] Δα1=α1(t+1)-α1(t)

[0071] Δα2=α2(t+1)-α2(t)

[0072] The calculation method of the wheel module travel distance Δs1, Δs2 is:

[0073]

[0074] like Figure 2 , 4 As shown in the figure, when the vehicle is driving, if the distance traveled by the two wheels is the same within a short time interval, the vehicle is driving in a straight line; if the distance traveled by one wheel is greater than that of the other wheel, it means that the vehicle is turning. According to Ackerman steering geometry, the process of the two rear wheels of the vehicle turning is modeled. The travel distances Δs1 and Δs2 of the two wheels are obtained by the change in the rotation angle of the two wheels of the vehicle. According to the fact that the trajectory of the two rear wheels of the vehicle when turning is the arc corresponding to the same angle on two concentric circles, the equation can be established by the known distance between the two wheels of the vehicle, that is, the length of the axle:

[0075] The calculation method of the inner radius r of the wheel movement is:

[0076]

[0077] The wheel moves at time t+1 to form an arc angle The calculation method is:

[0078]

[0079] The radian angle The calculation method for converting to the vehicle deviation angle Δθ is:

[0080]

[0081] like Figure 5 As shown, v 1 represents the velocity vector at time t, v 2 represents the velocity vector at time t+1, and the coordinate offset d and the deflection angle Δθ of the axle center point at time t+1 are substituted into the trigonometric function to solve the displacement Δx, Δy of the vehicle ground coordinate system at time t+1 and the new vehicle deflection angle. The calculation method of the coordinate offset d of the axle center point at time t+1 is:

[0082]

[0083] The calculation method of the ground coordinate system displacement Δx and Δy of the vehicle at time t+1 is:

[0084]

[0085] The new vehicle deviation angle θ(t+1)=θ(t)+Δθ.

[0086] The wheel rotation angle α1, the wheel module travel distance Δs1 and the vehicle steering angle change Δθ in steps s1 to s5 may be used to measure the movement trajectory of a single wheel.

[0087] A device for accurately measuring a moving track based on acceleration induction and magnetic induction, comprising: a sensor arranged at the center of a wheel, a memory storing data executable to implement the method; a data transmission module for transmitting data for implementing the method; and a data processor for executing the method; and a data fusion module for fusing data of the method;

[0088] The sensor comprises:

[0089] The acceleration module is used to detect the acceleration signals of the object on three axes in the carrier coordinate system and output them;

[0090] The magnetic induction module is used to detect the strength and direction of the magnetic field and output it.

[0091] Example 2 Figure 6 As shown, the principle of modeling four wheels of a vehicle is the same as that of two wheels. According to Ackerman steering geometry, the center of the arc of each wheel trajectory falls on the same point when turning. The subscripts are 1 for the front wheel, 2 for the rear wheel, L for the left wheel, R for the right wheel, H for the wheelbase between the front and rear wheels on the same side, and W for the wheelbase between the rear wheels. Each pair of wheels can actually establish a relationship about r 1 The equations are as follows: Take the two front wheels as one group and the two rear wheels as one group, and establish the equation system in two groups, as follows:

[0092]

[0093] After solving the above two equations, we can get r 1 , we can take the average to reduce the impact of the potential error of one of the wheels, and calculate Afterwards, the algorithm for measuring the trajectory is consistent with the steps of measuring the moving trajectory of the two wheels; the advantage of the four-wheel model is that compared with the two-wheel model, it can judge higher-dimensional motion changes. For example, when the vehicle encounters an uphill slope during driving, the rotation of the wheels can be found that the front wheels slow down consistently, and the front and rear wheels have the same speed when moving in a straight line, or when turning, the radius of the front wheels is larger than the rear wheels, so the speed of the front wheels is generally greater than that of the rear wheels.

[0094] Example 3

[0095] like Figure 7 As shown, the present invention can also be used to measure the speed of horizontally rotating gears or wheels. In some scenarios where acceleration cannot be used, such as horizontally rotating motors or gears, the measurement method described in the present invention can be referred to, and a magnetic induction module can be used to capture the change in the magnetic field component during the horizontal rotation process, and the rotation angle can be calculated to obtain the speed. Figure 7 Medium B || Indicates the horizontal geomagnetic direction, B ⊥ Indicates the vertical geomagnetic direction; the geomagnetic field and gravitational potential field exist in every corner of the earth. The direction of gravity is vertically downward to the center of the earth, while the direction of the geomagnetic field is rather special. It is neither completely parallel to the ground nor pointing from the geographic South Pole to the geographic North Pole. Instead, there is a magnetic declination. The steps for measuring the horizontal rotation of a motor rotor or gear are as follows: first, calibrate the geomagnetic direction on the measuring device. After calibration, install the sensor device that can capture the geomagnetic signal at the center of the motor rotor or gear. Calculate the real-time rotation angle of the motor rotor or gear based on the device's geomagnetic three-axis components. Compare it with the rotation angle at the previous moment to get the instantaneous speed. Calculate the rotation angle θ of the positive x-axis and the geomagnetic horizontal component. B The formula is:

[0096]

[0097] Embodiment 4:

[0098] Take two wheels as an example, there are two wheel modules 1 and 2, wherein wheel module 1 includes acceleration module 1, data transmission module 1, data processor 1, and wheel module 2 includes acceleration module 2, data transmission module 2, and data processor 2. They are installed at the center of the wheel respectively, the acceleration module collects the wheel rotation angle data, and transmits it to the data processing module through the data transmission module, and the angle to distance calculation is performed in the data processing module to obtain the distance that a single wheel moves in a small time such as 50ms.

[0099] Data processing module 1 collects data to data fusion module 1, in which the forward distances of the two wheels are fused to obtain a trajectory in a two-dimensional plane. The trajectory graph after fusion is as follows: Fig. 9 As shown, the wheel radius of the experimental vehicle is 35 cm, the wheelbase is 180-corrected value cm (because the tire has a certain contact area with the ground, the corrected value here is 13 after experiments), and the sensor records data at a period of 50 ms, that is, 20 sets of data per second.

[0100] The driving path simulates driving forward from the parking space, turning left to about 90 degrees, and then reversing back to the original position. After receiving the data with time tags, the interpolation method is used to supplement the missing data and correct the time difference between the receiving devices, align the data of the two wheels, and then calculate and draw according to the above two-wheel trajectory modeling method to obtain the following Fig. 9 As shown in the trajectory diagram, it can be seen from the trajectory that the calculation results and the driving trajectory are basically consistent.

[0101] Embodiment 5:

[0102] like Fig.11 As shown in FIG. 1 , when conditions permit, an artificial magnetic field can be arranged outside the wheel equipped with the magnetic induction module, for example, two permanent magnets can be arranged on the outer frame of the wheel to establish an artificial fixed magnetic field. The magnetic field strength of the permanent magnet should be much greater than the earth's magnetic field strength, such as reaching 10 -3 The artificial magnetic field can also be installed at any position of the wheel, or the magnetic induction module can be installed at any position of the wheel. By establishing an artificial fixed magnetic field, accurate positioning can be achieved when the geomagnetic field is shielded or there is electromagnetic interference. When this method is used, the artificial magnetic field can be fixed on the wheel, and the sensor is fixed on the edge of the board. The artificial magnetic field can also be fixed on the edge of the board, and the sensor is fixed on the wheel. The advantage of using an artificial magnetic field is that the influence of geomagnetic disturbance can be avoided. The separate design of the magnet and the sensor also facilitates installation and implementation.

[0103] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A method for accurately measuring a moving trajectory based on acceleration induction and magnetic induction, characterized in that: The following steps are involved: s1. An acceleration module for collecting acceleration data of the left-hand coordinate system is set at the center of the wheel, or a magnetic induction module is set at any position of the wheel, and the acceleration data (v x1 ,v y1 ),(v x2 ,v y2 ), calculate the wheel rotation angles α1, α2 and wheel rotation angle changes Δα1, Δα2 of the two rear wheels of the vehicle; the calculation method of the wheel rotation angle changes Δα1, Δα2 is: According to the force balance equation, the wheel rotation angles α1(t), α2(t) at time t, the wheel rotation angles α1(t+1), α2(t+1) at time t+1 are calculated, and then the wheel rotation angle changes Δα1, Δα2 are calculated: Δα1=α1(t+1)-α1(t) Δα2=α2(t+1)-α2(t) s2, substitute the wheel rotation angle changes Δα1 and Δα2 and the radius D of the wheel module into the formula to calculate the wheel module travel distances Δs1 and Δs2, and calculate the vehicle movement trajectory through the wheel module travel distances Δs1 and Δs2; The calculation method of the wheel module travel distance Δs1, Δs2 is: s3. Calculate the inner radius r according to the outer curve L of the wheel moving arc, the inner curve R of the wheel moving arc, and the axle length W; the calculation method of the inner radius r of the wheel moving arc is: s4. Substitute the inner circle radius r into the formula to calculate the radian angle from t to t+1 The wheel moves at time t+1 to form an arc angle The calculation method is s5, radian angle The angle conversion is performed to obtain the vehicle's steering angle change Δθ; The radian angle The calculation method for converting the vehicle's steering angle change Δθ is: s6. Take the center point of the axle at time t as the origin, establish a coordinate system with the positive direction of the y-axis as the forward direction and the direction from the wheel module outside the moving arc to the wheel module inside the moving arc as the positive direction of the x-axis, and name it the vehicle coordinate system; s7. Substitute the deviation angle change Δθ, the axle length W, and the inner circle radius r into the formula to calculate the coordinate offset d of the axle center point; The calculation method of the coordinate offset d of the axle center point at time t+1 is: s8. Substitute the deflection angle θ(t) at time t, the coordinate offset d of the center point of the axle from time t to time t+1, and the deflection angle change Δθ into the trigonometric function to solve the displacement Δx and Δy of the vehicle in the ground coordinate system from time t to time t+1; The calculation method of the ground coordinate system displacement Δx and Δy of the vehicle at time t+1 is: The new vehicle deviation angle is θ(t+1)=θ(t)+Δθ; s9. Calculate the position x(t+1), y(t+1) and the deflection angle θ(t+1) of the vehicle on the ground coordinate system at time t+1 using the vehicle deflection angle change Δθ, displacement Δx and displacement Δy; s10. Use the vehicle's position x(t), y(t), θ(t) and x(t+1), y(t+1), θ(t+1) in the ground coordinate system to draw the vehicle's moving trajectory.

2. The method for accurately measuring a moving trajectory based on acceleration induction and magnetic induction according to claim 1 is characterized in that: The wheel rotation angle α1, the wheel module travel distance Δs1 and the vehicle steering angle change Δθ in steps s1 to s5 may be used to measure the movement trajectory of a single wheel.

3. A device for accurately measuring moving trajectories based on acceleration induction and magnetic induction, characterized in that: include: A sensor disposed at the center of the wheel, and a memory storing data executable to implement the method as claimed in claim 1; A data transmission module for transmitting a data transmission module for implementing the method as claimed in claim 1; and a data processor for executing the method as claimed in claim 1; A data fusion module for fusing data according to the method of claim 1; The sensor comprises: The acceleration module is used to detect the acceleration signals of the object on three axes in the carrier coordinate system and output them; The magnetic induction module is used to detect the strength and direction of the magnetic field and output it.

Images