Vehicle-mounted detecting and guiding system during advancing

Abstract

The invention provides a vehicle-mounted detecting and guiding system during advancing, and belongs to the field of vehicle control. The problem that the vehicle auxiliary driving efficiency is low is solved. Specifically, the data acquisition module acquires driving data; the positioning correction module is used for carrying out primary analysis on the driving data and correcting the position of the vehicle; the driving assistance module carries out primary adjustment on the vehicle speed; analyzing the power oil consumption change of the vehicle according to the driving data, obtaining the remaining driving time of the vehicle to the destination, and carrying out secondary adjustment on the driving speed of the vehicle; the continuous adjustment module continuously obtains driving data and destination information of the vehicle and adjusts the positioning and driving speed of the vehicle; according to the invention, the positioning and speed of the vehicle are adjusted by acquiring, analyzing and processing related data of vehicle driving, so that the vehicle is stable in driving.

Description

Technical Field

[0001] The invention discloses a vehicle-mounted on-the-go detection and guidance system, which relates to the field of vehicle control. Background Art

[0002] The existing vehicle guidance system has the following shortcomings: Difficulties in data fusion: Existing systems often rely on multiple sensors for information fusion. Different types of sensors have different working principles, data output formats and accuracy. For example, lidar provides spatial coordinate data, while cameras provide two-dimensional image information. How to combine these data to generate a consistent and accurate environmental model is a complex technical issue.

[0003] Imperfect algorithms: Existing data fusion algorithms may not be able to effectively integrate data from different sensors in certain complex scenarios, resulting in errors or slow responses in the decision-making system. Especially in densely populated urban environments, conflicts and redundant information in sensor data can cause vehicles to make irrational decisions and even misunderstand the surrounding environment.

[0004] Update and maintenance issues: The vehicle guidance system requires regular software updates to fix vulnerabilities and optimize performance; however, too frequent or improper software updates may cause system incompatibility, errors or instability, thereby increasing vehicle safety risks. Summary of the invention

[0005] In view of the deficiencies in the prior art, an object of the present invention is to provide a vehicle-mounted on-the-go detection and guidance system, aiming to solve the problem of low efficiency of vehicle assisted driving.

[0006] In order to achieve the above object, the present invention is implemented through the following technical solutions: a vehicle-mounted on-the-go detection and guidance system comprises: Data acquisition module: used to obtain the driving time of the vehicle in the target area; during the driving time, the vehicle's speed per second, acceleration, heading angle, steering angle, laser radar positioning and fuel consumption per second are obtained to obtain driving data; Positioning correction module: used to construct the vehicle's motion state change equation; based on the motion state change equation and Kalman filter, the driving data is analyzed once, the position change and motion state change of the vehicle per second are analyzed, and the vehicle's positioning correction equation is constructed; the vehicle's position is corrected using the positioning correction equation; Driving assistance module: used to obtain safe braking distance, conduct secondary analysis of driving data, construct the safe driving speed equation of the vehicle, and adjust the driving speed of the vehicle once; analyze the power change and fuel consumption change of the vehicle during driving time, conduct tertiary analysis of driving data, and obtain the fuel consumption-power function of the vehicle; obtain the remaining driving time of the vehicle to the destination, and make secondary adjustments to the driving speed of the vehicle in combination with the fuel consumption-power function until the vehicle reaches the destination; Continuous adjustment module: used to continuously obtain the vehicle's driving data and destination, and adjust the vehicle's positioning and driving speed.

[0007] Furthermore, the workflow of the positioning correction module is as follows: Process A1: record the travel time as st; The speed of the vehicle at the 1st, 2nd, and stth seconds is recorded as: v （1） 、v （2） ~v （st） ; Acceleration is denoted by: a （1） 、a （2） ~a （st） ; The heading angle is denoted as: θ （1） ,θ （2） ~θ （st） ; The steering angle is denoted as: δ （1） , δ （2） ~δ （st） ; The position of the vehicle at the 1st, 2nd, and stth seconds is recorded as: (x （1） ,y （1） )、(x （2） ,y （2） )~(x （st） ,y （st） ); Process A2: Obtain the wheelbase of the vehicle, denoted as L; The position of the vehicle at the kth second is recorded as (x （k） ,y （k） ), speed is denoted as v （k） , the heading angle is denoted as θ （k） , acceleration is denoted as a （k） , the steering angle is denoted as δ （k） ; The position of the vehicle at (k+1) seconds is recorded as (x （k+1） ,y （k+1） ), speed is denoted as v （k+1） , the heading angle is denoted as θ （k+1） ; Construct the lateral displacement change equation of the vehicle as Formula A1-1: ; Construct the vertical displacement change equation of the vehicle as Formula A1-2: ; Construct the vehicle's speed change equation as Formula A1-3: ; Construct the vehicle's heading angle change equation as Formula A1-4: ; Process A3: Construct the vehicle's motion state matrix at the first second, denoted as matrix B （1） ; Matrix B （1） The mathematical expression is: ; With matrix B （1） is the initial state, and the Kalman gain of the vehicle motion state from the 1st to the 2nd second is calculated, which is recorded as; Kdk （1-2） ; Process A4: Get the speed v of the vehicle at the 3rd second （3） , acceleration a （3） , heading angle θ （3） , steering angle δ （3） , position (x （3） ,y （3） ); Construct the vehicle's motion state matrix at the 3rd second, denoted as matrix B （3） ; Matrix B （3） The mathematical expression is: ; Calculate the Kalman gain when the vehicle's motion state changes from the 2nd to the 3rd second, denoted as; Kdk （2-3） ; Process A5: Get the speed v of the vehicle at the (st-1)th second （st-1） , acceleration a （st-1） , heading angle θ （st-1） , steering angle δ （st-1） , position (x （st-1） ,y （st-1） ); Construct the vehicle's motion state matrix at the (st-1) second, denoted as matrix B （st-1） ; Matrix B （st-1） The mathematical expression is: ; Construct the vehicle's motion state matrix at the st second, denoted as matrix B （st） ; Matrix B （st） The mathematical expression is: ; Calculate the Kalman gain when the vehicle's motion state changes from the (st-1)th second to the stth second, denoted as; Kdk （（st-1）-st） ; Process A6: Summarize the Kalman gain of the vehicle motion state from the 1st second to the 2nd second, the 2nd second to the 3rd second, and finally the (st-1)th second to the stth second: Kdk （1-2） 、Kdk （2-3） ~Kdk （（st-1）-st） , construct the vehicle positioning correction equation; Use the positioning correction equation to correct the vehicle's position and enter the driving assistance module.

[0008] Furthermore, the specific process of process A3 is as follows: Process A31: Construct the vehicle's motion state matrix at the second second, denoted as matrix B （2） ; Matrix B （2） The mathematical expression is: ; Process A32: Calculate matrix B （1） Becomes matrix B （2） The state transfer matrix of （1-2） ; Matrix B （1-2） The calculation formula is: ; Process A33: v （1） 、a （1） ,θ （1） ,δ （1） and (x （1） ,y （1） ) into Formula A1-1 to Formula A1-4 to calculate the ideal lateral displacement of the vehicle at the second second, denoted as qx （2） , the ideal vertical displacement is denoted as qy （2） , the ideal speed is denoted as qv （2） , the ideal heading angle is denoted as qθ （2） ; Calculate the matrix B （2） The difference matrix of （2） ; Matrix RB （2） The calculation formula is: ; Calculate the matrix B （1） Becomes matrix B （2） The error transition matrix is ​​denoted as matrix QB （1-2） ; Matrix QB （1-2） The mathematical formula is: QB （1-2） =(RB （2） ×B （1-2） ); Process A34: Calculate matrix B （1） The decentralized matrix of （1） ; Bd （1） The calculation formula is: B （1） =B （1） -[(1 / 4)×(I （4） ) T ×B （1） ×I （4） ]; among them, I （4） represents a (4×1) matrix of all 1s; Calculate the matrix B （1） The error covariance matrix of （1） ; Matrix Pd （1） The calculation formula is: Pd （1） = (1 / 4) × (Bd （1） T ×B （1） ); Process A35: Calculate matrix B （2） The pre-error covariance matrix of （2） ;Pk （2） The calculation formula is: ; Calculate the Kalman gain Kdk when the vehicle motion state changes from the 1st to the 2nd second （1-2） ,Kdk （1-2） The calculation formula is: .

[0009] Furthermore, the specific process of process A4 is as follows: Process A41: Calculate matrix B （2） Becomes matrix B （3） The state transfer matrix of （2-3） ; Matrix B （2-3） The calculation formula is: ; Process A42: v （2） 、a （2） ,θ （2） ,δ （2） and (x （2） ,y （2） ) into Formula A1-1 to Formula A1-4 to calculate the ideal lateral displacement of the vehicle at the 3rd second, denoted as qx （3） , the ideal vertical displacement is denoted as qy （3） , the ideal speed is denoted as qv （3） , the ideal heading angle is denoted as qθ（3） ; Calculate the matrix B （3） The difference matrix of （3） ; Matrix RB （3） The calculation formula is: ; Where - represents matrix subtraction; Calculate the matrix B （2） Becomes matrix B （3） The error transition matrix is ​​denoted as matrix QB （2-3） ; Matrix QB （2-3） The mathematical formula is: QB （2-3） =RB （3） -RB （2） ; Process A43: Calculate matrix B （2） The decentralized matrix is ​​denoted as; Bd （2） The calculation formula is: B （2） =B （2） -[(1 / 4)×(I （4） ) T ×B （2） ×I （4） ]; among them, I （4） represents a (4×1) matrix of all 1s; Calculate the matrix B （2） The error covariance matrix of （2） ; Matrix Pd （2） The calculation formula is: Pd （2） = (1 / 4) × (Bd （2） T ×B （2） ); Process A44: Calculate matrix B （3） The pre-error covariance matrix of （3） ;Pk （3） The calculation formula is: ; Calculate the Kalman gain Kdk when the vehicle motion state changes from the 2nd to the 3rd second （2-3） ,Kdk （2-3） The calculation formula is: .

[0010] Furthermore, the specific process of process A5 is as follows: Process A51: Obtain the speed v of the vehicle at the (st-2)th second （st-2） , acceleration a （st-2）, heading angle θ （st-2） , steering angle δ （st-2） , position (x （st-2） ,y （st-2） ); v （st-2） 、a （st-2） ,θ （st-2） , δ （st-2） and (x （st-2） ,y （st-2） ) into Formula A1-1 to Formula A1-4, and calculate the ideal lateral displacement of the vehicle at (st-1) seconds, denoted as qx （st-1） , the ideal vertical displacement is denoted as qy （st-1） , the ideal speed is denoted as qv （st-1） , the ideal heading angle is denoted as qθ （st-1） ; Process A52: Calculate matrix B （st-1） Becomes matrix B （st） The state transfer matrix of （（st-1）-st） ; Matrix B （（st-1）-st） The calculation formula is: ; Where × represents matrix multiplication, T represents the transpose of the matrix, and -1 represents the inverse of the matrix; Process A53: v （st-1） 、a （st-1） ,θ （st-1） , δ （st-1） and (x （st-1） ,y （st-1） ) into Formula A1-1 to Formula A1-4 to calculate the ideal lateral displacement of the vehicle at the second second, denoted as qx （st） , the ideal vertical displacement is denoted as qy （st） , the ideal speed is denoted as qv （st） , the ideal heading angle is denoted as qθ （st） ; Calculate the matrix B （st） The difference matrix of （st） ; Matrix RB （st） The calculation formula is: ; Where - represents matrix subtraction; Calculate the matrix B （st） The difference matrix of （st） ; Matrix RB （st） The calculation formula is: ; Calculate the matrix B （st-1） Becomes matrix B （st） The error transition matrix is ​​denoted as matrix QB （（st-1）-st） ; Matrix QB（（st-1）-st） The mathematical formula is: QB （（st-1）-st） =(RB （st） ×B （（st-1）-st） ); where × represents matrix multiplication; Process A54: Calculate matrix B （st-1） The decentralized matrix is ​​denoted as; Bd （st-1） The calculation formula is: B （st-1） =B （st-1） -[(1 / 4)×(I （4） ) T ×B （st-1） ×I （4） ]; where - represents matrix subtraction, × represents matrix multiplication, T represents the transpose of the matrix, and I （4） represents a (4×1) matrix of all 1s; Calculate the matrix B （st-1） The error covariance matrix of （st-1） ; Matrix Pd （st-1） The calculation formula is: Pd （st-1） = (1 / 4) × (Bd （st-1） T ×B （st-1） ); Process A55: Calculate matrix B （st） The pre-error covariance matrix of （st） ;Pk （st） The calculation formula is: ; Calculate the Kalman gain Kdk when the vehicle motion state changes from (st-1) to st second （（st-1）-st） ,Kdk （（st-1）-st） The calculation formula is: .

[0011] Furthermore, the specific process of process A6 is as follows: Process A61: Extract Kdk （1-2） ~Kdk （（st-1）-st） The maximum value among them is denoted as Kk （（su-1）-su） ; Wherein, su represents the driving time of the vehicle in the target area, and the value range of su is: 2~st; Extract the vehicle's motion state matrix B at the (su-1) second （su-1） , matrix B （st-1） The mathematical expression is: ; Extract the vehicle's motion state matrix B at the suth second （su）, matrix B （st） The mathematical expression is: ; Extract matrix B （st-1） Becomes matrix B （st） The state transition matrix B （（st-1）-st） ; Process A62: Let the position of the vehicle at (i-1) seconds be (x （i-1） ,y （i-1） ), speed is denoted as v （i-1） , the heading angle is denoted as θ （i-1） , acceleration is denoted as a （i-1） , the steering angle is denoted as δ （i-1） ; The vehicle's motion state matrix at the (i-1) second is Bte （i-1） , matrix Bte （i-1） The mathematical expression is: ; Let the position of the vehicle at the i-th second be (x （i） ,y （i） ), speed is denoted as v （i） , the heading angle is denoted as θ （i） , acceleration is denoted as a （i） , the steering angle is denoted as δ （i） ; The vehicle's motion state matrix at the i-th second is Bte （i） , matrix Bte （i） The mathematical expression is: ; Process A63: Calculate the matrix Bte （i-1） Relative Matrix B （st-1） The offset coefficient is denoted as bl （1） ; The calculation formula is: bl （1） =(Bte （i-1） -B （st-1） ) T ×(Bte （i-1） -B （st-1） ); Calculate the matrix Bte （i） Relative Matrix B （st） The offset coefficient is denoted as bl （2） ; The calculation formula is: bl （2） =(Bte （i） -B （st） ) T ×(Bte （i） -B （st） ); Process A64: The corrected position of the vehicle at the i-th second is recorded as (qx （i） ,qy （i） ); by is the error range, and determines whether relation A2 is established; the mathematical expression of relation A2 is: (bl （1） ×Bte （i-1） ×B （（st-1）-st） )=Bte （i） ; If the relationship A2 holds, the vehicle positioning correction equation is: ; Among them, - represents matrix subtraction, and × represents matrix multiplication; If the relationship A2 does not hold, the vehicle positioning correction equation is: .

[0012] Furthermore, the working process of the driving assistance module is as follows: Process B1: Obtain the safe braking distance of the vehicle, record it as sad, obtain the real-time speed of the vehicle, record it as vt, and construct the safe driving speed equation of the vehicle; Process B2: Get the weight of the vehicle and record it as m （c） , the frontal area of ​​the vehicle is denoted as A （c） , the friction coefficient of the ground is denoted as μ, and the resistance coefficient of the air is denoted as Cd; Let the speed of the vehicle at the i-th second be v （i） , the total resistance of the vehicle at the i-th second is recorded as Ff （i） ;Define formula B2-1: ; Where g represents the acceleration due to gravity; v （1） ~v （st） Substituting into formula B2-1, we get the total resistance of the vehicle at the 1st, 2nd, and st seconds: Ff （1） , Ff （2） ~Ff （st） ; Process B3: Let the acceleration of the vehicle at the i-th second be a （i） , the vehicle power at the i-th second is recorded as P （i） , define formula B2-2: ; v （1） ~v （st） and a （1） ~a （st） Substituting into formula B2-2, we get the power of the vehicle at the 1st, 2nd, and stth seconds: P （1） , P （2） ~P （st） ; The fuel consumption of the vehicle at the 1st, 2nd, and stth seconds is recorded as: （1） 、ho （2） ~ho （st） ; Based on P （1） ~P （st） and ho （1） ~ho （st） Construct the vehicle's power-fuel consumption equation; Process B4: Let the vehicle power-fuel consumption equation be fh (P （i） ), that is, fh(P （i） ) indicates that the vehicle power is P （i） The fuel consumption of the vehicle at the i-th second, where i represents time; The current time is obtained and recorded as dt, and the remaining driving time of the vehicle to the destination is recorded as Tt; the minimum fuel consumption of the vehicle within the Tt time is calculated, and the speed of the vehicle is adjusted.

[0013] Furthermore, the specific process of process B1 is as follows: Process B11: Assume that the vehicle in front of the vehicle is vehicle A, and the distance between the vehicle and vehicle A is X （A） ; The car behind the vehicle is car B, and the distance between the vehicle and car B is X （B） ; Get the real-time acceleration of the vehicle and record it as at, calculate the acceleration of vehicle A and record it as vt （A） ; vt （A） The calculation formula is: ; Calculate car B and record it as vt （B） ; vt （B） The calculation formula is: ; The adjusted vehicle speed is denoted as v （car） ; Analysis X （A） and X （B） The change of v （car） Calculation formula of Process B12: If X （A） and X （B） If both remain unchanged, then increase the speed of the vehicle, v （car） The calculation formula is: ; Process B13: If only X （A） If the distance from the changed vehicle to vehicle A is changed, then the distance from the changed vehicle to vehicle A is recorded as gX （A） , v （car） The calculation formula is: ; If only X （B） If the distance from the changed vehicle to vehicle B is changed, then the distance from the changed vehicle to vehicle B is recorded as gX （B）, v （car） The calculation formula is: ; Process B14: If X （A） and X （B） If both vehicles are changed, the distance from the changed vehicle to vehicle A is recorded as tX （A） , the distance between the changed vehicle and vehicle B is recorded as tX （B） ; Define the relationship B1: tX （A） +tX （B） ≥ (2×sad); Determine whether the relationship B1 is true and determine v （car） Calculation formula of If the relation B1 holds, then v （car） The calculation formula is: ; If the relation B1 does not hold, then v （car） The calculation formula is: ; Among them, vtt （A） Indicates tX （A） The corresponding speed of car A, vtt （A） The calculation formula is: ; Process B15: Summarize the v in processes B11 to B14 （car） The calculation formula is used as the safe driving speed equation for the vehicle; Detect whether there is a vehicle in front of or behind the vehicle; if so, adjust the vehicle's speed according to the safe driving speed equation; if not, continue monitoring.

[0014] Furthermore, the specific process of process B3 is as follows: Process B31: Let P （1） ~P （st） with ho （1） ~ho （st） The corresponding transition parameters are: cd （1） ~cd （st） ;cd （1） ~cd （st） The initial value of is 1; Let P （1） ~P （st） About CD （1） ~cd （st） The equation is denoted as f(P （i） , cd （i） ); where cd （i） Indicates P （i） with ho （i）The corresponding transition parameter, ho （i） represents the fuel consumption of the vehicle at the i-th second; Define transition parameter set B （cd） , indicating cd （1） ~cd （st） ; Define P （1） ~P （st） with ho （1） ~ho （st） The residual sum is denoted as SSE (B （cd） ); SSE (B （cd） ) is calculated as: ; Process B32: Calculate SSE (B （cd） ) with respect to the partial derivative, we get formula B3-1; the mathematical expression of formula B3-1 is as follows: ;in, represents partial derivative; Based on the chain rule, formula B3-1 is simplified to obtain formula B3-2. The mathematical expression of formula B3-2 is: ; Performing a secondary transformation on formula B3-2, we obtain formula B3-3. The mathematical expression of formula B3-3 is: ; in, represents the gradient of the transition parameter; Process B33: The transition parameter set after the kth update is recorded as [(B （cd） ) （k） ], the transition parameter set after the (k+1)th update is recorded as [(B （cd） ) （k+1） ]; Define calculation formula B3-4: ; Among them, α represents the learning rate; Based on calculation formula B3-1 to calculation formula B3-4, the transition parameter set B is updated using the gradient descent algorithm. （cd） , so that SSE (B （cd） ) is the smallest, and the transition parameter is determined, which is recorded as: qd （1） ~qd （st） ; Process B34: Fitting equation f(P （i） , cd （i） ); P （1） ~P （st） About ho（1） ~ho （st） The highest order index is denoted as zn; the calculation formula of zn is: ; Wherein, zn is a positive integer, rounded up; Based on Taylor expansion, the equation f(P （i） , cd （i） ) to obtain formula B3-5, the mathematical expression of which is: ; in, Represents the equation f(P （i） , cd （i） ), j represents the calculation parameter, and the value range of j is: 1~zn; P （1） ~P （st） and ho （1） ~ho （st） Substitute it into calculation formula B3-5 and use Matlab software to fit it to obtain the vehicle's power and fuel consumption equation.

[0015] Furthermore, the specific process of process B4 is as follows: Process B41: Based on fh (P （i） ) The objective function of constructing the minimum fuel consumption is recorded as function J, and the mathematical expression of function J is: ; Calculate the function J with respect to P （i） The derivative function of , we get formula B4-1; the mathematical expression of formula B4-1 is: ;in, represents the derivative function; represents fh(P （i） )’s first-order derivative; Process B42: The power at the i-th second after the k-th update is recorded as [(P （i） ) （k） ], the power at the i-th second after the (k+1)th update is recorded as [(P （i） ) （k+1） ]; Define calculation formula B4-2 to represent the gradient update of function J; the mathematical expression of calculation formula B4-2 is: ; Where η represents the learning rate; Use Matlab software to fit the calculation formula B4-2 until the output of function J is minimum, and get the power of the vehicle from dt to (dt + Tt), recorded as Pt （dt） ~Pt（dt+Tt） ; Process B43: Define calculation formula B4-3: vv （q） =Pt （q） / m （c） ; Among them, vv （q） represents the speed of the vehicle at time Q, Pt （q） Indicates the power of the vehicle at the qth time; the value range of q is: dt~(dt+Tt); Pt （dt） ~Pt （dt+Tt） Substituting into equation B4-3, we get the speed of the vehicle from dt to (dt + Tt): vv （dt） ~vv （dt+Tt） ; Press vv （dt） ~vv （dt+Tt） Adjust the vehicle's speed until the vehicle reaches the destination.

[0016] Compared with the prior art, the present invention has the following beneficial effects: Improve traffic safety: The present invention can monitor the vehicle's surroundings and position, accurately identify potential dangers, and automatically adjust the steering wheel when the vehicle deviates from its route, helping the driver to keep the vehicle within the lane, thereby reducing lane departure accidents caused by driver inattention or fatigue driving, and avoiding collisions with other vehicles.

[0017] Automatic driving assistance: The positioning correction, adaptive cruise, automatic acceleration and deceleration and other functions provided by the present invention can reduce the driver's operating burden; when in traffic jams or driving at low speeds, the system can automatically control braking, acceleration and steering, allowing the driver to easily cope with busy traffic.

[0018] Improve traffic efficiency: The on-board detection and guidance system can not only improve the driving efficiency of a single vehicle, but also improve the traffic capacity of the overall traffic system and reduce traffic congestion. The present invention dynamically plans the optimal driving route based on information such as road conditions, traffic flow, and real-time conditions to avoid traffic congestion and reduce driving time. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Other features, objects and advantages of the present invention will become more apparent from the detailed description of non-limiting embodiments made with reference to the following drawings: Figure 1 It is a schematic diagram of the system of the present invention; Figure 2 This is a schematic diagram of data processing of the present invention; Figure 3 The figure is a schematic diagram of the vehicle driving of the present invention. DETAILED DESCRIPTION

[0020] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0021] See also Figure 1 and Figure 2 , a vehicle-mounted on-the-go detection and guidance system comprises: Data acquisition module: used to obtain the driving time of the vehicle in the target area; during the driving time, the vehicle's speed per second, acceleration, heading angle, steering angle, laser radar positioning and fuel consumption per second are obtained to obtain driving data; It should be noted that the "target area" in the present invention refers to the city-level area where the present invention (a vehicle-mounted on-the-go detection and guidance system) is used to perform vehicle on-the-go detection and guidance; Positioning correction module: used to construct the vehicle's motion state change equation; based on the motion state change equation and Kalman filter, the driving data is analyzed once, the position change and motion state change of the vehicle per second are analyzed, and the vehicle's positioning correction equation is constructed; the vehicle's position is corrected using the positioning correction equation; Process A: The workflow of the positioning correction module is as follows: Process A1: Record the driving time (of the vehicle in the target area) as st; The speed of the vehicle at the 1st, 2nd, and stth seconds (in the driving time) is recorded as: v （1） 、v （2） ~v （st） ; Acceleration is denoted by: a （1） 、a （2） ~a （st） ; The heading angle is denoted as: θ （1） ,θ （2） ~θ （st） ; The steering angle is denoted as: δ （1） , δ （2） ~δ （st） ; The laser radar position of the vehicle at the 1st, 2nd, and stth seconds during the driving time is recorded as: (x （1） ,y （1） )、(x （2） ,y （2） )~(x （st） ,y （st） ); Process A2: Obtain the wheelbase of the vehicle, denoted as L; The position of the vehicle at the kth second is recorded as (x （k） ,y （k） ), speed is denoted as v （k） , the heading angle is denoted as θ（k） , acceleration is denoted as a （k） , the steering angle is denoted as δ （k） ; The position of the vehicle at (k+1) seconds is recorded as (x （k+1） ,y （k+1） ), speed is denoted as v （k+1） , the heading angle is denoted as θ （k+1） ; Construct the lateral displacement change equation of the vehicle as Formula A1-1: ; Construct the vertical displacement change equation of the vehicle as Formula A1-2: ; Construct the vehicle's speed change equation as Formula A1-3: ; Construct the vehicle's heading angle change equation as Formula A1-4: ; Among them, formula A1-1 to formula A1-4 represent the equations for the change of the motion state of the vehicle; Process A3: Construct the vehicle's motion state matrix at the first second, denoted as matrix B （1） ; Matrix B （1） The mathematical expression is: ; With matrix B （1） is the initial state, and the Kalman gain of the vehicle motion state from the 1st to the 2nd second is calculated, which is recorded as; Kdk （1-2） ; Process A31: Construct the vehicle's motion state matrix at the second second, denoted as matrix B （2） ; Matrix B （2） The mathematical expression is: ; Process A32: Calculate matrix B （1） Becomes matrix B （2） The state transfer matrix of （1-2） ; Matrix B （1-2） The calculation formula is: ; Where × represents matrix multiplication, T represents the transpose of the matrix, and -1 represents the inverse of the matrix; Process A33: v （1） 、a （1） ,θ （1） , δ （1） and (x （1） ,y （1）) into Formula A1-1 to Formula A1-4 to calculate the ideal lateral displacement of the vehicle at the second second, denoted as qx （2） , the ideal vertical displacement is denoted as qy （2） , the ideal speed is denoted as qv （2） , the ideal heading angle is denoted as qθ （2） ; Calculate the matrix B （2） The difference matrix of （2） ; Matrix RB （2） The calculation formula is: ; Where - represents matrix subtraction; Calculate the matrix B （1） Becomes matrix B （2） The error transition matrix is ​​denoted as matrix QB （1-2） ; Matrix QB （1-2） The mathematical formula is: QB （1-2） =(RB （2） ×B （1-2） ); where × represents matrix multiplication; Process A34: Calculate matrix B （1） The decentralized matrix of （1） ; Bd （1） The calculation formula is: B （1） =B （1） -[(1 / 4)×(I （4） ) T ×B （1） ×I （4） ]; where - represents matrix subtraction, × represents matrix multiplication, T represents the transpose of the matrix, and I （4） represents a (4×1) matrix of all 1s; Calculate the matrix B （1） The error covariance matrix of （1） ; Matrix Pd （1） The calculation formula is: Pd （1） = (1 / 4) × (Bd （1） T ×B （1） ); Process A35: Calculate matrix B （2） The pre-error covariance matrix of （2） ;Pk （2） The calculation formula is: ; Among them, × represents matrix multiplication, T represents matrix transpose, and + represents matrix addition; Calculate the Kalman gain Kdk when the vehicle motion state changes from the 1st to the 2nd second （1-2） ,Kdk （1-2） The calculation formula is: ; Process A4: Get the speed v of the vehicle at the 3rd second (in driving time) （3） , acceleration a （3） , heading angle θ （3） , steering angle δ （3） , LiDAR position (x （3） ,y （3） ); Construct the vehicle's motion state matrix at the 3rd second, denoted as matrix B （3） ; Matrix B （3） The mathematical expression is: ; Calculate the Kalman gain when the vehicle's motion state changes from the 2nd to the 3rd second, denoted as; Kdk （2-3） ; Process A41: Calculate matrix B （2） Becomes matrix B （3） The state transfer matrix of （2-3） ; Matrix B （2-3） The calculation formula is: ; Where × represents matrix multiplication, T represents the transpose of the matrix, and -1 represents the inverse of the matrix; Process A42: v （2） 、a （2） ,θ （2） ,δ （2） and (x （2） ,y （2） ) into Formula A1-1 to Formula A1-4 to calculate the ideal lateral displacement of the vehicle at the 3rd second, denoted as qx （3） , the ideal vertical displacement is denoted as qy （3） , the ideal speed is denoted as qv （3） , the ideal heading angle is denoted as qθ （3） ; Calculate the matrix B （3） The difference matrix of （3） ; Matrix RB （3） The calculation formula is: ; Where - represents matrix subtraction; Calculate the matrix B （2） Becomes matrix B （3） The error transition matrix is ​​denoted as matrix QB （2-3） ; Matrix QB （2-3） The mathematical formula is: QB（2-3） =RB （3） -RB （2） ; Process A43: Calculate matrix B （2） The decentralized matrix is ​​denoted as; Bd （2） The calculation formula is: B （2） =B （2） -[(1 / 4)×(I （4） ) T ×B （2） ×I （4） ]; where - represents matrix subtraction, × represents matrix multiplication, T represents the transpose of the matrix, and I （4） represents a (4×1) matrix of all 1s; Calculate the matrix B （2） The error covariance matrix of （2） ; Matrix Pd （2） The calculation formula is: Pd （2） = (1 / 4) × (Bd （2） T ×B （2） ); Process A44: Calculate matrix B （3） The pre-error covariance matrix of （3） ;Pk （3） The calculation formula is: ; Among them, × represents matrix multiplication, T represents matrix transpose, and + represents matrix addition; Calculate the Kalman gain Kdk when the vehicle motion state changes from the 2nd to the 3rd second （2-3） ,Kdk （2-3） The calculation formula is: ; Process A5: Get the speed v of the vehicle at the (st-1)th second in the (travel time) （st-1） , acceleration a （st-1） , heading angle θ （st-1） , steering angle δ （st-1） , the laser radar position (x （st-1） ,y （st-1） ); Construct the vehicle's motion state matrix at the (st-1) second, denoted as matrix B （st-1） ; Matrix B （st-1） The mathematical expression is: ; Construct the vehicle's motion state matrix at the st second, denoted as matrix B （st） ; Matrix B（st） The mathematical expression is: ; Calculate the Kalman gain when the vehicle's motion state changes from the (st-1)th second to the stth second, denoted as; Kdk （（st-1）-st） ; Process A51: Obtain the speed v of the vehicle corresponding to the (st-2)th second in the (driving time) （st-2） , acceleration a （st-2） , heading angle θ （st-2） , steering angle δ （st-2） , the laser radar position (x （st-2） ,y （st-2） ); v （st-2） 、a （st-2） ,θ （st-2） , δ （st-2） and (x （st-2） ,y （st-2） ) into Formula A1-1 to Formula A1-4, and calculate the ideal lateral displacement of the vehicle at (st-1) seconds, denoted as qx （st-1） , the ideal vertical displacement is denoted as qy （st-1） , the ideal speed is denoted as qv （st-1） , the ideal heading angle is denoted as qθ （st-1） ; Process A52: Calculate matrix B （st-1） Becomes matrix B （st） The state transfer matrix of （（st-1）-st） ; Matrix B （（st-1）-st） The calculation formula is: ; Where × represents matrix multiplication, T represents the transpose of the matrix, and -1 represents the inverse of the matrix; Process A53: v （st-1） 、a （st-1） ,θ （st-1） , δ （st-1） and (x （st-1） ,y （st-1） ) into Formula A1-1 to Formula A1-4 to calculate the ideal lateral displacement of the vehicle at the second second, denoted as qx （st） , the ideal vertical displacement is denoted as qy （st） , the ideal speed is denoted as qv （st） , the ideal heading angle is denoted as qθ （st） ; Calculate the matrix B （st） The difference matrix of （st） ; Matrix RB （st） The calculation formula is: ; Where - represents matrix subtraction; Calculate the matrix B （st） The difference matrix of （st） ; Matrix RB （st） The calculation formula is: ; Calculate the matrix B （st-1） Becomes matrix B （st） The error transition matrix is ​​denoted as matrix QB （（st-1）-st） ; Matrix QB （（st-1）-st） The mathematical formula is: QB （（st-1）-st） =(RB （st） ×B （（st-1）-st） ); where × represents matrix multiplication; Process A54: Calculate matrix B （st-1） The decentralized matrix is ​​denoted as; Bd （st-1） The calculation formula is: B （st-1） =B （st-1） -[(1 / 4)×(I （4） ) T ×B （st-1） ×I （4） ]; where - represents matrix subtraction, × represents matrix multiplication, T represents the transpose of the matrix, and I （4） represents a (4×1) matrix of all 1s; Calculate the matrix B （st-1） The error covariance matrix of （st-1） ; Matrix Pd （st-1） The calculation formula is: Pd （st-1） = (1 / 4) × (Bd （st-1） T ×B （st-1） ); Process A55: Calculate matrix B （st） The pre-error covariance matrix of （st） ;Pk （st） The calculation formula is: ; Among them, × represents matrix multiplication, T represents matrix transpose, and + represents matrix addition; Calculate the Kalman gain Kdk when the vehicle motion state changes from (st-1) to st second （（st-1）-st） ,Kdk （（st-1）-st） The calculation formula is: ; Process A6: Summarize the Kalman gain of the vehicle motion state from the 1st second to the 2nd second, the 2nd second to the 3rd second, and finally the (st-1)th second to the stth second: Kdk （1-2） 、Kdk （2-3） ~Kdk （（st-1）-st） , construct vehicle positioning correction equation; Process A61: Extract Kdk （1-2） ~Kdk （（st-1）-st） The maximum value among them is denoted as Kk （（su-1）-su） ; Wherein, su represents the driving time of the vehicle in the target area, and the value range of su is: 2~st; Extract the vehicle's motion state matrix B at the (su-1) second （su-1） , matrix B （st-1） The mathematical expression is: ; Extract the vehicle's motion state matrix B at the suth second （su） , matrix B （st） The mathematical expression is: ; Extract matrix B （st-1） Becomes matrix B （st） The state transition matrix B （（st-1）-st） ; Process A62: Let the position of the vehicle at (i-1) seconds be (x （i-1） ,y （i-1） ), speed is denoted as v （i-1） , the heading angle is denoted as θ （i-1） , acceleration is denoted as a （i-1） , the steering angle is denoted as δ （i-1） ; The vehicle's motion state matrix at the (i-1) second is Bte （i-1） , matrix Bte （i-1） The mathematical expression is: ; Let the position of the vehicle at the i-th second be (x （i） ,y （i） ), speed is denoted as v （i） , the heading angle is denoted as θ （i） , acceleration is denoted as a （i） , the steering angle is denoted as δ （i） ; The motion state matrix of the vehicle at the i-th second is Bte （i） , matrix Bte （i） The mathematical expression is: ; Process A63: Calculate the matrix Bte （i-1） Relative Matrix B （st-1） The offset coefficient is denoted as bl （1）; The calculation formula is: bl （1） =(Bte （i-1） -B （st-1） ) T ×(Bte （i-1） -B （st-1） ); where - represents matrix subtraction, × represents matrix multiplication, and T represents matrix transpose; Calculate the matrix Bte （i） Relative Matrix B （st） The offset coefficient is denoted as bl （2） ; The calculation formula is: bl （2） =(Bte （i） -B （st） ) T ×(Bte （i） -B （st） ); Process A64: The corrected position of the vehicle at the i-th second is recorded as (qx （i） ,qy （i） ); by is the error range, and determines whether relation A2 is established; the mathematical expression of relation A2 is: (bl （1） ×Bte （i-1） ×B （（st-1）-st） )=Bte （i） ; If the relationship A2 holds, the vehicle positioning correction equation is: ; Among them, - represents matrix subtraction, and × represents matrix multiplication; If the relationship A2 does not hold, the vehicle positioning correction equation is: .

[0022] Driving assistance module: used to obtain safe braking distance, conduct secondary analysis of driving data, construct the safe driving speed equation of the vehicle, and make a primary adjustment to the driving speed of the vehicle; analyze the changes in (engine) power and fuel consumption of the vehicle during driving time, conduct a tertiary analysis of driving data, and obtain the fuel consumption-power function of the vehicle; obtain the remaining driving time of the vehicle to the destination, and make a secondary adjustment to the driving speed of the vehicle in combination with the fuel consumption-power function until the vehicle reaches the destination; Process B: The working process of the driving assistance module is as follows: Process B1: Please refer to Figure 3 , obtain the safe braking distance of the vehicle and record it as sad, obtain the real-time speed of the vehicle and record it as vt, and construct the safe driving speed equation of the vehicle; Process B11: Assume that the vehicle in front of the vehicle is vehicle A, and the distance between the vehicle and vehicle A is X（A） ; The car behind the vehicle is car B, and the distance between the vehicle and car B is X （B） ; (X （A） and X （B） The initial value is greater than sad) Get the real-time acceleration of the vehicle and record it as at, calculate the acceleration of vehicle A and record it as vt （A） ; vt （A） The calculation formula is: ; Calculate car B and record it as vt （B） ; vt （B） The calculation formula is: ; The adjusted vehicle speed is denoted as v （car） ; Analysis X （A） and X （B） The change of v （car） Calculation formula of Process B12: If X （A） and X （B） If both remain unchanged, then increase the speed of the vehicle, v （car） The calculation formula is: ; Process B13: If only X （A） If the distance from the changed vehicle to vehicle A is changed, then the distance from the changed vehicle to vehicle A is recorded as gX （A） , v （car） The calculation formula is: ; If only X （B） If the distance from the changed vehicle to vehicle B is changed, then the distance from the changed vehicle to vehicle B is recorded as gX （B） , v （car） The calculation formula is: ; Process B14: If X （A） and X （B） If both vehicles are changed, the distance from the changed vehicle to vehicle A is recorded as tX （A） , the distance between the changed vehicle and vehicle B is recorded as tX （B） ; Define the relationship B1: tX （A） +tX （B） ≥ (2×sad); Determine whether the relationship B1 is true and determine v （car） Calculation formula of If the relation B1 holds, then v （car） The calculation formula is: ; If the relation B1 does not hold, then v （car） The calculation formula is: ; Among them, vtt （A） Indicates tX （A） The corresponding speed of car A, vtt （A） The calculation formula is: ; (When the relationship B1 is not true, the vehicle moves to the right to make way for vehicle B to overtake) Process B15: Summarize the v in processes B11 to B14 （car） The calculation formula is used as the safe driving speed equation for the vehicle; Detect whether there is a vehicle in front of or behind the vehicle; if so, adjust the vehicle's speed according to the safe driving speed equation; if not, continue monitoring; Process B2: Get the weight of the vehicle and record it as m （c） , the frontal area of ​​the vehicle is denoted as A （c） , the friction coefficient of the ground is denoted as μ, and the resistance coefficient of the air is denoted as Cd; Let the speed of the vehicle at the i-th second be v （i） , the total resistance of the vehicle at the i-th second is recorded as Ff （i） ;Define formula B2-1: ; Where g represents the acceleration due to gravity; v （1） ~v （st） Substituting into formula B2-1, we get the total resistance of the vehicle at the 1st, 2nd, and st seconds: Ff （1） , Ff （2） ~Ff （st） ; Process B3: Let the acceleration of the vehicle at the i-th second be a （i） , the vehicle's (engine) power at the i-th second is denoted as P （i） , define formula B2-2: ; v （1） ~v （st） and a （1） ~a （st） Substituting into formula B2-2, we get the (engine) power of the vehicle at the 1st, 2nd, and stth seconds: P （1） , P （2） ~P （st） ; The fuel consumption of the vehicle at the 1st, 2nd, and stth seconds (in driving time) is recorded as: ho （1） 、ho （2） ~ho （st） ; Based on P （1） ~P （st） and ho （1） ~ho （st） Construct the vehicle's power-fuel consumption equation; Process B31: Let P （1） ~P （st） with ho （1） ~ho （st） The corresponding transition parameters are: cd （1） ~cd （st） ;cd （1） ~cd （st） The initial value of is 1; Let P （1） ~P （st） About CD （1） ~cd （st） The equation is denoted as f(P （i） , cd （i） ); where cd （i） Indicates P （i） with ho （i） The corresponding transition parameter, ho （i） represents the fuel consumption of the vehicle at the i-th second; (Equation f(P （i） , cd （i） ) is initially represented as: （i） =[a×(P （i） ) n +b]×[c×(cd （i） ) m +d]+e, where ho （i） , P （i） 、cd （i） , a, b, c, d, e, m, and n are all examples and have no special meanings; the initial values ​​of a, c, m, and n are 1, and the initial values ​​of b, d, and e are 0) Define transition parameter set B （cd） , indicating cd （1） ~cd （st） (i.e. set B （cd） ={cd （1） ~cd （st）}); Definition P （1） ~P （st） with ho （1） ~ho （st） The residual sum is denoted as SSE (B （cd） ); SSE (B （cd） ) is calculated as: ; Process B32: Calculate SSE (B （cd）) with respect to the partial derivative, we get formula B3-1; the mathematical expression of formula B3-1 is as follows: ;in, represents partial derivative; Based on the chain rule, formula B3-1 is simplified to obtain formula B3-2. The mathematical expression of formula B3-2 is: ; Performing a secondary transformation on formula B3-2, we obtain formula B3-3. The mathematical expression of formula B3-3 is: ; in, represents the gradient of the transition parameter; Process B33: The transition parameter set after the kth update is recorded as [(B （cd） ) （k） ], the transition parameter set after the (k+1)th update is recorded as [(B （cd） ) （k+1） ]; Define calculation formula B3-4: ; Where α represents the learning rate (the value of α is set to 0.001; users or relevant technical personnel can adjust the value of α according to actual needs); Based on calculation formula B3-1 to calculation formula B3-4, the transition parameter set B is updated using the gradient descent algorithm. （cd） , so that SSE (B （cd） ) is the smallest, and the transition parameter is determined, which is recorded as: qd （1） ~qd （st） ; Process B34: Fitting equation f(P （i） , cd （i） ); P （1） ~P （st） About ho （1） ~ho （st） The highest order index is denoted as zn; the calculation formula of zn is: ; Wherein, zn is a positive integer, rounded up; Based on Taylor expansion, the equation f(P （i） , cd （i） ) to obtain formula B3-5, the mathematical expression of which is: ; in, Represents the equation f(P （i） , cd（i） ), j represents the calculation parameter, and the value range of j is: 1~zn; P （1） ~P （st） and ho （1） ~ho （st） Substitute it into the calculation formula B3-5 and use Matlab software to fit it to get the power and fuel consumption equation of the vehicle; It should be noted that, because the engine power and engine fuel consumption of a vehicle are in a nonlinear relationship, the fitted power-fuel consumption function may be a completely different mathematical model such as exponential regression, power regression, logarithmic regression, etc. Therefore, the "process B3" in the present invention can only provide a general and simple mathematical model construction process, and cannot provide a specific mathematical model expression, which is hereby explained; Process B4: Let the vehicle power-fuel consumption equation be fh (P （i） ), that is, fh(P （i） ) indicates that the vehicle (engine) power is P （i） The fuel consumption of the vehicle at the i-th second, where i represents time; The current time is recorded as dt, and the remaining driving time of the vehicle to the destination is recorded as Tt; the minimum fuel consumption of the vehicle within the Tt time is calculated, and the speed of the vehicle is adjusted; Process B41: Based on fh (P （i） ) The objective function of constructing the minimum fuel consumption is recorded as function J, and the mathematical expression of function J is: ; Calculate the function J with respect to P （i） The derivative function of , we get formula B4-1; the mathematical expression of formula B4-1 is: ;in, represents the derivative function; represents fh(P （i） )’s first-order derivative; Process B42: The (engine) power at the i-th second after the k-th update is recorded as [(P （i） ) （k） ], the (engine) power at the i-th second after the (k+1)th update is recorded as [(P （i） ) （k+1） ]; Define calculation formula B4-2 to represent the gradient update of function J; the mathematical expression of calculation formula B4-2 is: ; Wherein, η represents the learning rate (the value of η is set to 0.001; users or relevant technical personnel can adjust the value of η according to actual needs); Use Matlab software to fit the calculation formula B4-2 until the output of function J is minimum, and obtain the vehicle's (engine) power from dt to (dt + Tt), recorded as Pt （dt） ~Pt （dt+Tt） ; Process B43: Define calculation formula B4-3: vv （q） =Pt （q） / m （c） ; Among them, vv （q） represents the speed of the vehicle at time Q, Pt （q） Indicates the (engine) power of the vehicle at the qth time; the value range of q is: dt~(dt+Tt); Pt （dt） ~Pt （dt+Tt） Substituting into equation B4-3, we get the speed of the vehicle from dt to (dt + Tt): vv （dt） ~vv （dt+Tt） ; Press vv （dt） ~vv （dt+Tt） Adjust the vehicle's speed until the vehicle reaches the destination.

[0023] Continuous adjustment module: used to continuously obtain the vehicle's driving data and destination, and adjust the vehicle's positioning and driving speed.

[0024] The above formulas are all dimensionless and numerical calculations. The formula is a formula for the most recent real situation obtained by collecting a large amount of data and performing software simulation. The preset parameters in the formula are set by technical personnel in this field according to actual conditions. For example, if there are weight coefficients and proportional coefficients, their set sizes are to quantify each parameter to obtain a specific value, which is convenient for subsequent comparison. Regarding the size of the weight coefficient and the proportional coefficient, it is sufficient as long as it does not affect the proportional relationship between the parameter and the quantized value.

[0025] Finally, it should be noted that the above-described embodiments are only specific implementations of the present invention, which are used to illustrate the technical solutions of the present invention, rather than to limit them. The protection scope of the present invention is not limited thereto. Although the present invention is described in detail with reference to the above-described embodiments, ordinary technicians in the field should understand that any technician familiar with the technical field can still modify the technical solutions recorded in the above-described embodiments within the technical scope disclosed by the present invention, or can easily think of changes, or make equivalent replacements for some of the technical features therein; and these modifications, changes or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention shall be based on the protection scope of the claims.

Claims

1. A vehicle-mounted on-the-go detection and guidance system, characterized in that: The system comprises: Data acquisition module: used to obtain the driving time of the vehicle in the target area; during the driving time, the vehicle's speed per second, acceleration, heading angle, steering angle, laser radar positioning and fuel consumption per second are obtained to obtain driving data; Positioning correction module: used to construct the vehicle's motion state change equation; based on the motion state change equation and Kalman filter, the driving data is analyzed once, the position change and motion state change of the vehicle per second are analyzed, and the vehicle's positioning correction equation is constructed; the vehicle's position is corrected using the positioning correction equation; Driving assistance module: used to obtain safe braking distance, conduct secondary analysis of driving data, construct the safe driving speed equation of the vehicle, and make a primary adjustment to the driving speed of the vehicle; analyze the power change and fuel consumption change of the vehicle during driving time, conduct a tertiary analysis of driving data, and obtain the fuel consumption-power function of the vehicle; obtain the remaining driving time of the vehicle to the destination, and make a secondary adjustment to the driving speed of the vehicle in combination with the fuel consumption-power function; Continuous adjustment module: used to continuously obtain the vehicle's driving data and destination, and adjust the vehicle's positioning and driving speed.

2. The vehicle-mounted on-the-go detection and guidance system according to claim 1, characterized in that: The workflow of the positioning correction module is as follows: Process A1: record the travel time as st; The speed of the vehicle at the 1st, 2nd, and stth seconds is recorded as: v （1） 、v （2） ~v （st） ; Acceleration is denoted by: a （1） 、a （2） ~a （st） ; The heading angle is denoted as: θ （1） ,θ （2） ~θ （st） ; The steering angle is recorded as: δ （1） , δ （2） ~δ （st） ; The position of the vehicle at the 1st, 2nd, and stth seconds is recorded as: (x （1） ,y （1） )、(x （2） ,y （2） )~(x （st） ,y （st） ); Process A2: Obtain the wheelbase of the vehicle, denoted as L; The position of the vehicle at the kth second is recorded as (x （k） ,y （k） ), speed is denoted as v （k） , the heading angle is denoted as θ （k） , acceleration is denoted as a （k） , the steering angle is denoted as δ （k） ; The position of the vehicle at (k+1) seconds is recorded as (x （k+1） ,y （k+1） ), speed is denoted as v （k+1） , the heading angle is denoted as θ （k+1） ; Construct the lateral displacement change equation of the vehicle as Formula A1-1: ； Construct the vertical displacement change equation of the vehicle as Formula A1-2: ； Construct the vehicle's speed change equation as Formula A1-3: ； Construct the vehicle's heading angle change equation as formula A1-4: ； Process A3: Construct the vehicle's motion state matrix at the first second, denoted as matrix B （1） ; Matrix B （1） The mathematical expression is: ； With matrix B （1） is the initial state, and the Kalman gain of the vehicle motion state from the 1st to the 2nd second is calculated, which is recorded as; Kdk （1-2） ; Process A4: Get the speed v of the vehicle at the 3rd second （3） , acceleration a （3） , heading angle θ （3） , steering angle δ （3） , position (x （3） ,y （3） ); Construct the vehicle's motion state matrix at the 3rd second, denoted as matrix B （3） ; Matrix B （3） The mathematical expression is: ； Calculate the Kalman gain when the vehicle's motion state changes from the 2nd to the 3rd second, denoted as; Kdk （2-3） ; Process A5: Get the speed v of the vehicle at the (st-1)th second （st-1） , acceleration a （st-1） , heading angle θ （st-1） , steering angle δ （st-1） , position (x （st-1） ,y （st-1） ); Construct the vehicle's motion state matrix at the (st-1) second, denoted as matrix B （st-1） ; Matrix B （st-1） The mathematical expression is: ; Construct the vehicle's motion state matrix at the st second, denoted as matrix B （st） ; Matrix B （st） The mathematical expression is: ； Calculate the Kalman gain when the vehicle's motion state changes from the (st-1)th second to the stth second, denoted as; Kdk （（st-1）-st） ; Process A6: Summarize the Kalman gain of the vehicle motion state from the 1st second to the 2nd second, the 2nd second to the 3rd second, and finally the (st-1)th second to the stth second: Kdk （1-2） 、Kdk （2-3） ~Kdk （（st-1）-st） , construct the vehicle positioning correction equation; Use the positioning correction equation to correct the vehicle's position and enter the driving assistance module.

3. The vehicle-mounted on-the-go detection and guidance system according to claim 2, characterized in that: The specific process of process A3 is as follows: Process A31: Construct the vehicle's motion state matrix at the second second, denoted as matrix B （2） ; Matrix B （2） The mathematical expression is: ； Process A32: Calculate matrix B （1） Becomes matrix B （2） The state transfer matrix of （1-2） ; Matrix B （1-2） The calculation formula is: ； Process A33: v （1） 、a （1） ,θ （1） , δ （1） and (x （1） ,y （1） ) into Formula A1-1 to Formula A1-4 to calculate the ideal lateral displacement of the vehicle at the second second, denoted as qx （2） , the ideal vertical displacement is denoted as qy （2） , the ideal speed is denoted as qv （2） , the ideal heading angle is denoted as qθ （2） ; Calculate the matrix B （2） The difference matrix of （2） ; Matrix RB （2） The calculation formula is: ； Calculate the matrix B （1） Becomes matrix B （2） The error transition matrix is ​​denoted as matrix QB （1-2） ; Matrix QB （1-2） The mathematical formula is: QB （1-2） =(RB （2） ×B （1-2） ); Process A34: Calculate matrix B （1） The decentralized matrix of （1） ; Bd （1） The calculation formula is: B （1） =B （1） -[(1 / 4)×(I （4） ) T ×B （1） ×I （4） ]; among them, I （4） represents a (4×1) matrix of all 1s; Calculate the matrix B （1） The error covariance matrix of （1） ; Matrix Pd （1） The calculation formula is: Pd （1） =(1 / 4)×(Bd （1） T ×Bd （1） ); Process A35: Calculate matrix B （2） The pre-error covariance matrix of （2） ;Pk （2） The calculation formula is: ； Calculate the Kalman gain Kdk when the vehicle motion state changes from the 1st to the 2nd second （1-2） ,Kdk （1-2） The calculation formula is: 。 4. The vehicle-mounted on-the-go detection and guidance system according to claim 2, characterized in that: The specific process of process A5 is as follows: Process A51: Obtain the speed v of the vehicle at the (st-2)th second （st-2） , acceleration a （st-2） , heading angle θ （st-2） , steering angle δ （st-2） , position (x （st-2） ,y （st-2） ); v （st-2） 、a （st-2） ,θ （st-2） , δ （st-2） and (x （st-2） ,y （st-2） ) into Formula A1-1 to Formula A1-4, and calculate the ideal lateral displacement of the vehicle at (st-1) seconds, denoted as qx （st-1） , the ideal vertical displacement is denoted as qy （st-1） , the ideal speed is denoted as qv （st-1） , the ideal heading angle is denoted as qθ （st-1） ; Process A52: Calculate matrix B （st-1） Becomes matrix B （st） The state transfer matrix of （（st-1）-st） ; Matrix B （（st-1）-st） The calculation formula is: ; Where × represents matrix multiplication, T represents the transpose of the matrix, and -1 represents the inverse of the matrix; Process A53: v （st-1） 、a （st-1） ,θ （st-1） , δ （st-1） and (x （st-1） ,y （st-1） ) into Formula A1-1 to Formula A1-4 to calculate the ideal lateral displacement of the vehicle at the second second, denoted as qx （st） , the ideal vertical displacement is denoted as qy （st） , the ideal speed is denoted as qv （st） , the ideal heading angle is denoted as qθ （st） ; Calculate the matrix B （st） The difference matrix of （st） ; Matrix RB （st） The calculation formula is: ; Where - represents matrix subtraction; Calculate the matrix B （st） The difference matrix of （st） ; Matrix RB （st） The calculation formula is: ； Calculate the matrix B （st-1） Becomes matrix B （st） The error transition matrix is ​​denoted as matrix QB （（st-1）-st） ; Matrix QB （（st-1）-st） The mathematical formula is: QB （（st-1）-st） =(RB （st） ×B （（st-1）-st） ); where × represents matrix multiplication; Process A54: Calculate matrix B （st-1） The decentralized matrix is ​​denoted as; Bd （st-1） The calculation formula is: B （st-1） =B （st-1） -[(1 / 4)×(I （4） ) T ×B （st-1） ×I （4） ]; where - represents matrix subtraction, × represents matrix multiplication, T represents the transpose of the matrix, and I （4） represents a (4×1) matrix of all 1s; Calculate the matrix B （st-1） The error covariance matrix of （st-1） ; Matrix Pd （st-1） The calculation formula is: Pd （st-1） =(1 / 4)×(Bd （st-1） T ×Bd （st-1） ); Process A55: Calculate matrix B （st） The pre-error covariance matrix of （st） ;Pk （st） The calculation formula is: ； Calculate the Kalman gain Kdk when the vehicle motion state changes from (st-1) to st second （（st-1）-st） ,Kdk （（st-1）-st） The calculation formula is: 。 5. The vehicle-mounted on-the-go detection and guidance system according to claim 2, characterized in that: The specific process of process A6 is as follows: Process A61: Extract Kdk （1-2） ~Kdk （（st-1）-st） The maximum value among them is denoted as Kk （（su-1）-su） ; Wherein, su represents the driving time of the vehicle in the target area, and the value range of su is: 2~st; Extract the vehicle's motion state matrix B at the (su-1) second （su-1） , matrix B （st-1） The mathematical expression is: ； Extract the vehicle's motion state matrix B at the suth second （su） , matrix B （st） The mathematical expression is: ； Extract matrix B （st-1） Becomes matrix B （st） The state transition matrix B （（st-1）-st） ; Process A62: Let the position of the vehicle at (i-1) seconds be (x （i-1） ,y （i-1） ), speed is denoted as v （i-1） , the heading angle is denoted as θ （i-1） , acceleration is denoted as a （i-1） , the steering angle is denoted as δ （i-1） ; The vehicle's motion state matrix at the (i-1) second is Bte （i-1） , matrix Bte （i-1） The mathematical expression is: ； Let the position of the vehicle at the i-th second be (x （i） ,y （i） ), speed is denoted as v （i） , the heading angle is denoted as θ （i） , acceleration is denoted as a （i） , the steering angle is denoted as δ （i） ; The vehicle's motion state matrix at the i-th second is Bte （i） , matrix Bte （i） The mathematical expression is: ； Process A63: Calculate the matrix Bte （i-1） Relative Matrix B （st-1） The offset coefficient is denoted as bl （1） ; The calculation formula is: bl （1） =(Bte （i-1） -B （st-1） ) T ×(Bte （i-1） -B （st-1） ); Calculate the matrix Bte （i） Relative Matrix B （st） The offset coefficient is denoted as bl （2） ; The calculation formula is: bl （2） =(Bte （i） -B （st） ) T ×(Bte （i） -B （st） ); Process A64: The corrected position of the vehicle at the i-th second is recorded as (qx （i） ,qy （i） ); by is the error range, and determines whether relation A2 is established; the mathematical expression of relation A2 is: (bl （1） ×Bte （i-1） ×B （（st-1）-st） )=Bte （i） ; If the relationship A2 holds, the vehicle positioning correction equation is: ； If the relationship A2 does not hold, the vehicle positioning correction equation is: 。 6. The vehicle-mounted on-the-go detection and guidance system according to claim 2, characterized in that: The working process of the driving assistance module is as follows: Process B1: Obtain the safe braking distance of the vehicle, record it as sad, obtain the real-time speed of the vehicle, record it as vt, and construct the safe driving speed equation of the vehicle; Process B2: Get the weight of the vehicle and record it as m （c） , the frontal area of ​​the vehicle is denoted as A （c） , the friction coefficient of the ground is denoted as μ, and the resistance coefficient of the air is denoted as Cd; Let the speed of the vehicle at the i-th second be v （i） , the total resistance of the vehicle at the i-th second is recorded as Ff （i） ;Define formula B2-1: ; Where g represents the acceleration due to gravity; v （1） ~v （st） Substituting into formula B2-1, we get the total resistance of the vehicle at the 1st, 2nd, and st seconds: Ff （1） , Ff （2） ~Ff （st） ; Process B3: Let the acceleration of the vehicle at the i-th second be a （i） , the vehicle power at the i-th second is recorded as P （i） , define formula B2-2: ； v （1） ~v （st） and a （1） ~a （st） Substituting into formula B2-2, we get the power of the vehicle at the 1st, 2nd, and stth seconds: P （1） , P （2） ~P （st） ; The fuel consumption of the vehicle at the 1st, 2nd, and stth seconds is recorded as: ho （1） 、ho （2） ~ho （st） ; Based on P （1） ~P （st） and ho （1） ~ho （st） Construct the vehicle's power-fuel consumption equation; Process B4: Let the vehicle power-fuel consumption equation be fh (P （i） ), that is, fh(P （i） ) indicates that the vehicle power is P （i） The fuel consumption of the vehicle at the i-th second, where i represents time; The current time is obtained and recorded as dt, and the remaining driving time of the vehicle to the destination is recorded as Tt; the minimum fuel consumption of the vehicle within the Tt time is calculated, and the speed of the vehicle is adjusted.

7. The vehicle-mounted on-the-go detection and guidance system according to claim 6, characterized in that: The specific process of process B1 is as follows: Process B11: Assume that the vehicle in front of the vehicle is vehicle A, and the distance between the vehicle and vehicle A is X （A） ; The car behind the vehicle is car B, and the distance between the vehicle and car B is X （B） ; Get the real-time acceleration of the vehicle and record it as at, calculate the acceleration of vehicle A and record it as vt （A） ; vt （A） The calculation formula is: ; Calculate car B and record it as vt （B） ; vt （B） The calculation formula is: ; The adjusted vehicle speed is denoted as v （car） ; Analysis X （A） and X （B） The change of v （car） Calculation formula of Process B12: If X （A） and X （B） If both remain unchanged, then increase the speed of the vehicle, v （car） The calculation formula is: ; Process B13: If only X （A） If the distance from the changed vehicle to vehicle A is changed, then the distance from the changed vehicle to vehicle A is recorded as gX （A） , v （car） The calculation formula is: ； If only X （B） If the distance from the changed vehicle to vehicle B is changed, then the distance from the changed vehicle to vehicle B is recorded as gX （B） , v （car） The calculation formula is: ； Process B14: If X （A） and X （B） If both vehicles are changed, the distance from the changed vehicle to vehicle A is recorded as tX （A） , the distance between the changed vehicle and vehicle B is recorded as tX （B） ; Define the relationship B1: tX （A） +tX （B） ≥ (2×sad); Determine whether the relationship B1 is true and determine v （car） Calculation formula of If the relation B1 holds, then v （car） The calculation formula is: ； If the relation B1 does not hold, then v （car） The calculation formula is: ; Among them, vtt （A） Indicates tX （A） The corresponding speed of car A, vtt （A） The calculation formula is: ； Process B15: Summarize the v in processes B11 to B14 （car） The calculation formula is used as the safe driving speed equation for the vehicle; Detect whether there is a vehicle in front of or behind the vehicle; if so, adjust the vehicle's speed according to the safe driving speed equation; if not, continue monitoring.

8. The vehicle-mounted on-the-go detection and guidance system according to claim 6, characterized in that: The specific process of process B3 is as follows: Process B31: Let P （1） ~P （st） with ho （1） ~ho （st） The corresponding transition parameters are: cd （1） ~cd （st） ; Let P （1） ~P （st） About CD （1） ~cd （st） The equation is denoted as f(P （i） , cd （i） ); where cd （i） Indicates P （i） with ho （i） The corresponding transition parameter, ho （i） represents the fuel consumption of the vehicle at the i-th second; Define transition parameter set B （cd） , indicating cd （1） ~cd （st） ; Define P （1） ~P （st） with ho （1） ~ho （st） The residual sum is denoted as SSE (B （cd） ); SSE (B （cd） ) is calculated as: ； Process B32: Calculate SSE (B （cd） ) with respect to the partial derivative, we get formula B3-1; the mathematical expression of formula B3-1 is as follows: ;in, represents partial derivative; Based on the chain rule, formula B3-1 is simplified to obtain formula B3-2. The mathematical expression of formula B3-2 is: ； Performing a secondary transformation on formula B3-2, we obtain formula B3-3. The mathematical expression of formula B3-3 is: ； in, represents the gradient of the transition parameter; Process B33: The transition parameter set after the kth update is recorded as [(B （cd） ) （k） ], the transition parameter set after the (k+1)th update is recorded as [(B （cd） ) （k+1） ]; Define calculation formula B3-4: ; Among them, α represents the learning rate; Based on calculation formula B3-1 to calculation formula B3-4, the transition parameter set B is updated using the gradient descent algorithm. （cd） , so that SSE (B （cd） ) is the smallest, and the transition parameter is determined, which is recorded as: qd （1） ~qd （st） ; Process B34: Fitting equation f(P （i） , cd （i） ); P （1） ~P （st） About ho （1） ~ho （st） The highest order index is denoted as zn; the calculation formula of zn is: ; Wherein, zn is a positive integer, rounded up; Based on Taylor expansion, the equation f(P （i） , cd （i） ) to obtain formula B3-5, the mathematical expression of which is: ； in, Represents the equation f(P （i） , cd （i） ), j represents the calculation parameter, and the value range of j is: 1~zn; P （1） ~P （st） and ho （1） ~ho （st） Substitute it into calculation formula B3-5 and use Matlab software to fit it to obtain the vehicle's power and fuel consumption equation.

9. The vehicle-mounted on-the-go detection and guidance system according to claim 6, characterized in that: The specific process of process B4 is as follows: Process B41: Based on fh (P （i） ) The objective function of constructing the minimum fuel consumption is recorded as function J, and the mathematical expression of function J is: ； Calculate the function J with respect to P （i） The derivative function of , we get formula B4-1; the mathematical expression of formula B4-1 is: ;in, represents the derivative function; represents fh(P （i） )’s first-order derivative; Process B42: The power at the i-th second after the k-th update is recorded as [(P （i） ) （k） ], the power at the i-th second after the (k+1)th update is recorded as [(P （i） ) （k+1） ]; Define calculation formula B4-2 to represent the gradient update of function J; the mathematical expression of calculation formula B4-2 is: ; Where η represents the learning rate; Use Matlab software to fit the calculation formula B4-2 until the output of function J is minimum, and get the power of the vehicle from dt to (dt + Tt), recorded as Pt （dt） ~Pt （dt+Tt） ; Process B43: Define calculation formula B4-3: vv （q） =Pt （q） / m （c） ; Among them, vv （q） represents the speed of the vehicle at time Q, Pt （q） Indicates the power of the vehicle at the qth time; the value range of q is: dt~(dt+Tt); Pt （dt） ~Pt （dt+Tt） Substituting into equation B4-3, we get the speed of the vehicle from dt to (dt + Tt): vv （dt） ~vv （dt+Tt） ; Press vv （dt） ~vv （dt+Tt） Adjust the vehicle's speed until the vehicle reaches the destination.

Images