Vehicle frequency shift positioning method based on high and low orbit satellite network fusion, automatic driving control method and equipment

Abstract

The invention discloses a vehicle frequency shift positioning method based on high and low orbit satellite network fusion, an automatic driving control method and equipment, and aims at transforming a traditional satellite pseudo-range equation to better reflect the dynamic change of a vehicle, especially the accurate positioning requirement in a high-speed or complex environment. A wide-area reference signal is provided by utilizing a GEO satellite signal to serve as a positioning reference layer, more accurate space vector information is provided, and particularly, high-precision real-time positioning can still be ensured under the condition of complex terrains or relatively large satellite signal interference. On the basis, a vehicle transverse dynamic model is established, the model not only considers vehicle transverse motion characteristics, but also fuses satellite positioning information and actual motion vector data of the vehicle, and more accurate input is provided for a vehicle two-degree-of-freedom model. And speed feed-forward control and LQR control are combined to realize accurate path tracking and control. The vehicle can keep high-precision positioning under variable road conditions, and high efficiency, safety and reliability in the driving process are ensured.

Description

Technical Field

[0001] The present invention belongs to the field of intelligent vehicle autonomous driving, and relates to a vehicle frequency shift positioning method, an autonomous driving control method and device for the integration of high and low orbit satellite networks. Background Art

[0002] With the continuous development of satellite communication, navigation and remote sensing technologies, the space-earth integrated information network has gradually become the key infrastructure for future communication and positioning. In the field of vehicle positioning, traditional single satellite systems (such as GPS or Beidou) are vulnerable to multipath effects, signal blockage and interference in complex environments, especially in urban canyons or areas with dense high-rise buildings, resulting in a decline in positioning accuracy. For this reason, the low-orbit satellite joint frequency shift positioning technology has emerged. It combines ground sensor networks and significantly improves the accuracy and stability of vehicle real-time positioning. This technology can provide more reliable positioning support for autonomous driving and intelligent transportation systems and is the basis for achieving precise navigation and intelligent decision-making. In recent years, low-orbit satellites (LEO satellites), as a new positioning technology, have gradually been applied to the field of high-precision positioning. Low-orbit satellites have a lower orbital altitude and a shorter period, and they can provide more accurate positioning information, especially in high-dynamic environments. However, in the prior art, the application of low-orbit satellite signals still faces certain challenges. Especially in terms of how to efficiently integrate low-orbit satellite frequency shift information to improve positioning accuracy, there are still many technical problems. Therefore, in order to give full play to the advantages of low-orbit satellites, it is urgent to solve these challenges and combine them with high-orbit satellite systems (GEO satellites) and ground sensors to form a multi-dimensional and multi-source information fusion positioning and speed determination architecture.

[0003] The application of the Doppler frequency shift technology in satellite positioning is the key to achieving this goal. The Doppler frequency shift refers to the change phenomenon of the wave frequency or wavelength caused by the relative motion between the wave source and the observer. Specifically, when there is relative motion between the wave source and the observer, the wave frequency received by the observer will change. If the wave source approaches the observer, the received wave frequency will increase and the wavelength will shorten; conversely, when the wave source moves away from the observer, the received wave frequency will decrease and the wavelength will become longer. In a satellite positioning system, the value of the Doppler frequency shift can be calculated by the difference between the frequency received by the receiver and the satellite transmission frequency. This information can help determine the relative speed between the vehicle and the satellite and indirectly deduce the actual speed and other motion states of the vehicle. This is crucial for high-precision positioning and control, especially in situations where the vehicle dynamics are complex and the road environment is changeable.

[0004] In an autonomous driving system, the control input of a vehicle usually depends on various complex factors, especially the real-time feedback from sensing devices and algorithms. Traditional manual driving relies on the direct operations of the driver, such as the control of the steering wheel, accelerator, and brake pedal, while autonomous driving hands over the decision-making power to a computer system. An autonomous vehicle obtains information about the surrounding environment through sensors (such as radar, lidar, cameras, etc.) and uses algorithms to calculate appropriate control inputs. However, due to limitations such as sensor configuration, on-vehicle computing power, and algorithm performance, the autonomous driving system still faces certain difficulties in dealing with complex environments and cannot meet the safety design requirements of autonomous vehicles at L3 level and above, resulting in problems such as dangerous driving behaviors and high takeover rates. Therefore, more accurate and comprehensive sensing input means are needed, especially in high-dynamic environments. The fusion of low-earth orbit satellite Doppler shift information and ground sensor data can provide higher-precision and more stable positioning support for autonomous driving. How to efficiently fuse this information and provide real-time feedback to the control system is the key to further promoting the development of autonomous driving technology.

[0005] In view of the above situation, the combination of satellite Doppler shift technology and vehicle autonomous driving system has great potential. By accurately analyzing the Doppler shift effect of low-earth orbit (LEO) satellite signals, it is possible to achieve high-precision estimation of the vehicle's position and attitude, thereby providing more reliable navigation support for autonomous driving. Specifically, the relative speed between the vehicle and the low-earth orbit satellite is calculated through the pseudorange equation, and based on the change of the Doppler shift, the actual speed and other motion state parameters of the vehicle are indirectly obtained. This information can not only provide the real-time position of the vehicle but also deduce the dynamic changes of the vehicle, and then obtain more accurate attitude and motion information. Summary of the Invention

[0006] Based on the above background, the present invention proposes a vehicle frequency shift positioning and autonomous driving control method that fuses high and low earth orbit satellite networks, as well as vehicle-mounted equipment.

[0007] First, the present invention designs a frequency shift positioning method for high and low orbit satellite vehicles, aiming to improve the accuracy and real-time performance of vehicle positioning by precisely utilizing the frequency shift signals of low orbit satellites. In view of the dimensional characteristics of vehicle spatial position information, the present invention modifies the traditional satellite pseudorange equation to enable it to better reflect the dynamic changes of the vehicle, especially the precise positioning requirements in high-speed or complex environments. In addition, a multi-satellite joint observation scheme is adopted. By receiving the frequency shift information from multiple low orbit satellites, the tracking ability of the vehicle's moving speed is enhanced. Finally, the GEO satellite signal provides a wide-area reference signal as the reference layer for positioning. Utilizing the characteristics of strong stability and wide coverage of high orbit satellites, basic positioning and navigation services are provided. This scheme effectively improves the positioning accuracy and provides more accurate spatial vector information. Especially in complex terrains or situations with strong satellite signal interference, high-precision real-time positioning can still be guaranteed.

[0008] On this basis, the present invention further establishes a lateral dynamics model of the vehicle. This model not only considers the lateral motion characteristics of the vehicle but also fuses satellite positioning information with the actual motion vector data of the vehicle, providing more accurate inputs for the two-degree-of-freedom motion model of the vehicle. These input data can reflect the complex motion patterns of the vehicle on actual roads and provide important support for subsequent control decisions. Combining speed feedforward control with the LQR (Linear Quadratic Regulator) module, the present invention achieves precise path tracking and control. Through these advanced control strategies, the vehicle can maintain high-precision positioning under changing road conditions and achieve efficient autonomous driving, ensuring the safety and reliability of the driving process.

[0009] The present invention proposes a vehicle frequency shift positioning and autonomous driving control method for the integration of high and low orbit satellite networks under the integration of space and ground, and its main components include the following parts: 1. A multi-low orbit satellite Doppler frequency shift speed determination module. 2. A vehicle lateral dynamics model based on the vehicle two-degree-of-freedom equation. 3. A feedforward control module. 4. An LQR controller module. 5. A preview control module.

[0010] The specific steps of the method are as follows:

[0011] Step 1, in the environment where the vehicle is traveling, establish a low orbit satellite joint frequency shift speed determination module. Specifically as follows:

[0012] First, establish a single low orbit satellite vehicle positioning system in the absolute coordinate system. Install a satellite navigation receiver (GPS) on the vehicle. Through triangulation or quadrilateral positioning algorithms, the relative position coordinates (x, y, z) of the vehicle can be accurately obtained, and the relative position coordinates of the satellite are (x s , y s , z s ). Define the Doppler frequency shift value f dIt is the difference between the receiving frequency of the receiver and the transmitting frequency of the satellite signal. Let v = [v x , v y , v z T be the relative velocity vector between the receiver and the satellite, e s = [e s,x , e s,y , e s,z T be the unit observation vector from the receiver to the satellite, where the subscripts x, y, and z represent the components on the x, y, and z axes, λ is the carrier wavelength, and α is the angle between the velocity vector and the unit observation vector. Then there is:

[0013]

[0014] Step 2: Transform the satellite positioning equation to establish a high and low satellite fusion positioning mechanism. Specifically as follows:

[0015] In the vehicle driving environment, the pseudorange is the approximate distance from the vehicle receiver to the satellite. According to the transmission time t s of the satellite signal and the time t u when the receiver receives the signal, the propagation time of the signal can be obtained, and then the relative distance from the receiver to the satellite can be obtained. Considering the clock drift between the satellite and the receiver, and the influence of other factors such as the atmosphere, it is not the real distance, which is called the pseudorange ρ = c(t u - t s ), where c represents the speed of light.

[0016] The real distance is r s , the clock difference between the receiver and the GPS time is δ t , the clock difference between the satellite and the GPS time is δ t,s , and the signal will also be delayed when passing through the atmosphere during propagation. Assuming the delay caused by the ionosphere is I, the delay caused by the troposphere is T, and the delay caused by other various factors not considered and noise is ε, then:

[0017] r s = c[(t u - δ t ) - (t s - δ t,s ) - T - I - ε] (2)

[0018] The GPS positioning equation contains 3 vehicle parameters and 1 delay parameter. Let Then the basic GPS positioning equation is:

[0019]

[0020] Take the derivative of equation (3):​​

[0021]

[0022] Among them, obtained by the Doppler equation, the delay I caused by the atmospheric ionosphere and the delay T caused by the atmospheric troposphere do not change significantly in a short time. and is zero, and the delay parameter is an unknown coefficient, which can be obtained from the satellite navigation message. Equation (4) can be rewritten as:

[0023]

[0024] Project the relative velocity (v s - v) between the satellite and the vehicle onto the unit observation vector e s . v s is the satellite velocity, and the velocity of the vehicle receiver can be solved inversely through the GPS positioning equation, that is, the velocity of the vehicle during driving. Expand Equation (5) to multi-satellite observations and organize it into a matrix form to obtain the satellite frequency shift speed determination equation set:

[0025] G·V = b + n d (6)

[0026] Among them:

[0027]

[0028] Since the vehicle is in the space coordinate system, V = [v x , v y , v z , δ t T is the unknown in the speed determination equation. At least 4 low-earth orbit satellites are required to solve the speed determination equation set. A positioning system combining 4 satellites is designed. The relative velocity of the satellites is v i (i = 1, 2, 3, 4), and their respective unit observation vectors are e i = [e i,x , e i,y , e i,z , i = 1, 2, 3, 4. The clock drift delays of the 4 low-earth orbit satellites are δ t,i (i = 1, 2, 3, 4). n d = [n d,1 , n d,2 , n d,3 , n d,4 T ​​is an unconsidered error noise term. This constant-speed equation can be solved by Newton iteration or the least squares method, and the obtained results are the speed and time delay of the receiver, that is, the speed and time delay of the vehicle are obtained.

[0029] Step 3: Establish a high-low orbit satellite cooperative transmission model. Specifically as follows:

[0030] Through the network of ground stations and sky base stations, fuse the GEO and LEO signal data, correct the positioning error and signal delay. Through the observations of multiple LEO satellites, transmit the high-precision speed of the target obtained by calculating the Doppler frequency shift to the upper high-orbit satellite, and the GEO satellite fuses and distributes this information.

[0031] The distance between the high-low orbit satellites can be calculated by the Euclidean distance d formula of their orbital coordinates:

[0032]

[0033] where (x, y, z) are the positions of the GEO and LEO satellites in the absolute coordinate system, the subscript GEO represents the GEO satellite, and LEO represents the LEO satellite.

[0034] In the cooperation of high-low orbit satellites, the fast movement of LEO satellites provides high-dynamic data, while GEO satellites compensate for the coverage blind area. Considering the time delay is the primary task. The signal transmission delay between the two can be calculated by accumulating the path delay and processing delay of ground-LEO-GEO. The total transmission time delay T total is:

[0035] T total = T ground-LEO + T LEO-GEO + T pro (9)

[0036] where each sub-item is calculated based on the distance and processing time. T ground-LEO is the time delay from the ground to the low-orbit satellite, T LEO-GEO is the time delay from the low-orbit satellite to the high-orbit satellite, and T pro is the processing time delay.

[0037] Step 4: Build a lateral control model for autonomous driving vehicles. Specifically as follows:

[0038] First, according to the characteristic of only being able to adjust the longitudinal force of the tires, the present invention establishes a vehicle model considering two degrees of freedom of vehicle side and yaw:

[0039]

[0040] where m is the vehicle mass, a y represents the lateral acceleration, and I zdenotes the moment of inertia, δ f denotes the front wheel steering angle, denotes the yaw angle, l f ,l r is the wheelbase between the front and rear axles, θ vf ,θ vr are the sideslip angles of the front and rear wheels of the vehicle, is the yaw angular velocity, c f ,c r are the sideslip stiffnesses of the front and rear wheels, F yf 、F yr are the lateral forces of the front and rear wheels. In Equation (10), only θ vf ,θ vr are unknown. For the planar motion of a rigid body, the velocity of a point is equal to the translational velocity of the rigid body plus the velocity of rotation about a reference point, that is, with the translational motion about the reference point and the rotational motion about the reference point, namely:

[0041]

[0042] In the case where θ vf ,θ vr is extremely small, θ vf = tanθ vf ,θ vr = tanθ vr . Substituting into Equation (11):

[0043]

[0044] Similarly, we get:

[0045]

[0046] Taking as the state variables, which are the lateral displacement, lateral velocity, yaw angle, and yaw angular velocity respectively. In the case where the sideslip angle is very small, the yaw angle is equal to the course angle, then the above equation can be rewritten in matrix form U is the vehicle steering angle vector, which is the front wheel steering angle in the two-degree-of-freedom dynamic model. Among them:

[0047]

[0048] Step 5: Design the LQR controller in the present invention. The design objective of the LQR controller's objective function is to minimize the error between the state quantity and the expected value. The goal of LQR is to find an optimal control input quantity such that the state of the model system follows a linear dynamic model and, subject to certain constraints, minimize a cost function. This cost function is a quadratic form with respect to the system state and the control input. In autonomous driving, the error value is the difference between the actual state quantity of the vehicle and the planned state quantity. Meanwhile, minimizing the output of the controller and considering the constraints of the system on the objective function, the following LQR controller is obtained:

[0049] (1) Define the error state equation:

[0050]

[0051] The error state quantity e rr = X - X r , where X is the actual positioning state of the vehicle and X r is the planned positioning state of the vehicle. U is the feedback steering angle of the vehicle's front wheels.

[0052] (2) Discretize the error state equation. The present invention uses the midpoint Euler method and the forward Euler method to discretize the error state equation. The result is:

[0053]

[0054] is the coefficient matrix after discretization of formula (15), e rr (k) represents the error state quantity at time k, and U(k) represents the feedback steering angle of the vehicle's front wheels at time k.

[0055] (3) Solve the Riccati equation to obtain the feedback coefficient K:

[0056]

[0057] In formula (17), Q = diag(30, 1, 5, 1), R is the output weight with a value of 6, and P represents an intermediate quantity.

[0058] (4) Solve for the final feedback quantity U(k) = -Ke rr (k).

[0059] Step 6: If only LQR is used for control, no matter what value K takes, the error state quantity e rr and its rate of change cannot be zero simultaneously, that is, the error state quantity e rr cannot always be zero, resulting in a steady-state error in the vehicle simulation. To avoid this result, the present invention introduces feedforward control on the basis of LQR control, aiming to select an appropriate feedforward quantity δf , namely the front wheel steering angle, such that the error state variable e rr = 0.

[0060] After the system stabilizes, that is Use the software Mathematica to solve for e rr , and obtain:

[0061]

[0062] k 1 , k 3 are feedback coefficients. When , the lateral error of the vehicle and the yaw error of the vehicle are not affected by the feedforward quantity δ f and the feedback coefficients k 1 , k 3 . Regardless of the value of δ f , k 1 , k 3 , Regarding this invention will not be specifically discussed.

[0063] Step Seven, establish a preview mechanism for the low-orbit satellite and the planned road on the road surface. In a dynamic driving system, preview refers to calculating the current position, speed, road geometric characteristics (such as curvature, slope, etc.), and the target trajectory of the vehicle to estimate the possible movement of the vehicle within a certain future time period. Based on this information, the control system can adjust the vehicle's acceleration, braking, steering and other behaviors in advance to ensure that the vehicle can drive smoothly and accurately. This invention designs a preview mechanism for the planned road, projects the vehicle speed vector onto the lane center coordinates, finds the point with the minimum curvature by traversing the road planning points, and extends the intersection of the tangent vector and the road as the matching point of the prediction vector at the current moment, achieving road tracking to a certain extent.

[0064] Step Eight, integrate the above high- and low-orbit satellite frequency shift constant speed module, positioning module, autonomous driving lateral control module, LQR control module, feedforward control module, and preview module of the satellite and the planned road on the road surface, and deploy them in the equipment of the autonomous driving vehicle to achieve real-time path tracking of the vehicle.

[0065] The beneficial effects of this invention:

[0066] (1) The present invention proposes a vehicle positioning method based on the Doppler frequency shift of high and low orbit satellites, innovatively applying low orbit satellite technology to vehicle speed measurement. By utilizing the Doppler frequency shift effect and modifying the pseudo-range equation of a single satellite, a satellite frequency shift speed determination equation is constructed. In view of the three-dimensional characteristics of the vehicle speed vector and the influence of time delay in the satellite signal transmission process, the present invention increases the number of observed satellites to 4, forming a multi-satellite joint frequency shift vehicle speed determination equation set, and finally realizes navigation services and basic communication through high orbit satellites. This method effectively improves the accuracy of vehicle speed measurement and provides technical support for precise positioning and control in high dynamic scenarios.

[0067] (2) The present invention proposes a control model composed of LQR and speed feedforward. The final output feedback quantity is a linear combination of the LQR feedback coefficient and the speed feedforward control quantity, which to a certain extent avoids the steady-state error of the lateral error and improves the stability of lateral control.

[0068] (3) The present invention designs a preview mechanism for low orbit satellites and planned roads on the road surface. The vehicle speed vector is projected onto the lane center coordinates. By traversing the road planning points to find the point with the minimum curvature and extending the intersection of the tangent vector and the road as the matching point of the prediction vector at the current moment, the tracking ability of the vehicle chassis for the upper layer planned trajectory is improved. Description of the Drawings

[0069] Figure 1 is the structural diagram of road planning;

[0070] Figure 2 is the overall structural diagram of vehicle frequency shift positioning with high and low orbit satellite network fusion;

[0071] Figure 3 is the structural diagram of low orbit satellite joint vehicle frequency shift speed determination;

[0072] Figure 4 is the schematic diagram of lateral control for autonomous driving;

[0073] Figure 5 is the preview approximation schematic diagram of low orbit satellites and road planning points; Detailed Embodiments

[0074] The following further describes the present invention in conjunction with the drawings. However, the protection scope of the present invention is not limited thereto.

[0075] (1) Road planning modeling: The present invention establishes a planned road model in the CarSim road module, as Figure 1 shown. It includes a closed road with 11 straight lines and simple arcs. The specific parameters are shown in Table 1, and the total length is 212 meters, as Figure 1As shown in the figure. To achieve precise path planning and vehicle control, 50 discrete points are planned for each path segment, and the entire road model consists of 550 points. For each path point, its steering angle and heading angle are recorded in detail to reflect the geometric characteristics and driving direction of the road. In addition, when the path is an arc, the curvature of each point is also recorded to describe the degree of bending of the arc; for a straight path, the curvature value is set to zero. This planned road model can not only provide a detailed reference path for vehicle motion simulation, but also provide a reliable basis for subsequent vehicle dynamic control and autonomous driving algorithm testing.

[0076] Serial number Road type Length Angle Cut-off position 1 Straight line 20m 20.000 2 Arc 10m 90deg 35.708m 3 Arc -10m 90deg 51.416m 4 Arc 10m 180deg 82.832m 5 Arc 5m 90deg 90.686m 6 Arc -5m 180deg 106.394m 7 Arc 5m 180deg 122.102m 8 Arc -5m 180deg 137.810m 9 Arc 10m 180deg 169.226m 10 Straight line 20m 189.226m 11 Arc 15m 90deg 212.788m

[0077] (2) Overall model building process: Figure 2 It is the overall structure diagram of vehicle frequency shift positioning for the integration of high and low orbit satellite networks, which mainly consists of 5 parts: 1. Low orbit satellite Doppler frequency shift vehicle constant speed module, which realizes high-precision measurement of vehicle speed through multi-low orbit satellite frequency shift information. 2. LQR optimization control module, which optimizes the control input of the vehicle and improves the dynamic response performance. 3. Feedforward compensation control module, which eliminates the steady-state error of vehicle state variables and improves the tracking accuracy and stability of the system. 4. Preview path planning and planning point approximation module, which perceives path changes in advance and optimizes the projection point matching mechanism. 5. A and B matrix calculation module and state error and curvature calculation module based on the vehicle two-degree-of-freedom model.

[0078] (3) Build the low orbit satellite Doppler frequency shift vehicle constant speed module. Considering the three-dimensional characteristics of the vehicle's velocity vector, in order to accurately describe the vehicle's velocity state, the present invention further expands to a multi-satellite observation scheme and uses at least 4 satellites to construct a joint equation set. By fusing the frequency shift observation values of multiple satellites, the accuracy and robustness of vehicle speed calculation are significantly improved. Figure 3 It is the schematic diagram of low orbit satellite joint vehicle frequency shift constant speed. Project the relative speed difference between the low orbit satellite and the vehicle onto the unit observation vector to construct the constant speed principle equation of a single satellite, laying a foundation for subsequent high orbit satellite joint positioning:

[0079]

[0080] To solve the vehicle velocity vector, the number of satellites needs to be increased to 4, and thus the satellite frequency shift constant speed equation set (such as Equation 6) is obtained, and the Newton iteration method is used to solve it. According to the Newton iteration method, the four-element equation set is linearized at the estimated solution V k =[v k,x ,v k,y ,v k,z ,δ k,t T The linearized equation set at this point is: ​

[0081]

[0082] △v x , △v y , △v z , △δ t represents the differences in velocity and clock drift between the k-th iteration and the (k + 1)-th iteration;

[0083] The matrix G is expanded based on Equation (7) and is called the Jacobian matrix:

[0084]

[0085] (x k , y k , z k ) represents the position coordinates of the vehicle calculated in the k-th iteration.

[0086] G is only related to the geometric positions of the satellites and the receiver. b is called the pseudorange residual, which is the difference between the observed pseudorange and the pseudorange estimated in the k-th iteration. After obtaining the linearized equations, the equations can be solved using the least squares method to obtain:

[0087]

[0088] Furthermore, the iterative estimated value is obtained:

[0089]

[0090] Iterate repeatedly until V k = [v k,x , v k,y , v k,z , δ k,t T meets the accuracy requirements and the Newton iteration method stops. The solved result is the velocity and clock drift of the receiver.

[0091] Finally, by utilizing the low-latency advantage of LEO satellites and Doppler frequency shift information, and collaborating with GEO signals for optimization, the positioning accuracy can be significantly improved.

[0092] (4) Based on the two-degree-of-freedom lateral control model, the present invention constructs an LQR control module and a feedforward control module to achieve precise control of the vehicle's lateral movement. The specific structure of the lateral autonomous driving model is as shown in Appendix Figure 4 shown. The core design includes the conversion between three coordinate systems, namely the absolute coordinate system, the vehicle body coordinate system, and the path coordinate system. Through the association of these coordinate systems, the lateral movement state of the vehicle can be accurately described. First, establish the state vectors from the absolute coordinate to the vehicle body coordinate and the path coordinate ​Projecting the velocity vector in the vehicle body coordinate system onto the path coordinate system can obtain the deviation information between the current motion state of the vehicle and the target path. The lateral error between the two is d, which reflects the offset between the vehicle center point and the path center line and satisfies:

[0093]

[0094] is the lateral unit vector of the shortest distance projection point on the path coordinate. Traverse in matlab to find the point closest to . The sequence of this point is denoted as d min ,

[0095] Transform Equation (24) to get Take the derivative of it to obtain the lateral control equation:

[0096]

[0097] is the modulus of the velocity vector in the vehicle body coordinate, θ - θ r is the heading error, θ is the vehicle heading angle in the vehicle coordinate system, θ min is its minimum value; θ r is the vehicle heading angle of the projection point in the path coordinate system, and d is the lateral displacement of the vehicle.

[0098] Define the state error quantity, e d is the lateral error, is the lateral velocity error, is the yaw angle error, is the yaw rate error:

[0099]

[0100] Combined with the two-degree-of-freedom equation, transform it into an error differential equation as:

[0101]

[0102] Let δ f = u, and the error equation is transformed into:

[0103]

[0104] In Equation (28), is the feedforward control, C = (b 1 , b 2 , 0, 0) T , b 1 , b 2is the vehicle feedforward control coefficient. According to the above method, the state error and curvature module can be obtained. For the detailed steps of the LQR problem, refer to equations (13)-(15). In the Matlab program, the discrete algebraic Riccati equation can be solved using the "dare" command, or the discrete LQR problem can be directly solved using the "dlqr" command.

[0105] (5) Build the preview module for the road surface planned path and approximate the planned points. In Matlab, the design of the road surface planned points is discretized, that is, the continuous path curve is represented by a series of discrete planned points. However, due to the certain deviation between the actual driving path of the vehicle and the planned points, the points projected onto the path coordinate system in the vehicle body coordinate system usually do not exactly coincide with the discrete planned points. This deviation is caused by the discretization of the planned road and the change of path curvature. Therefore, relying only on the shortest distance point may not guarantee the global optimal path tracking effect. To achieve high-precision path tracking control, an algorithm is used to find the planned point closest to the projection point near the discrete planned points as the target point. The specific schematic diagram is as Figure 5 shown.

[0106]

[0107] θ m 、x m 、y m 、 respectively represent the heading angle, longitudinal displacement, lateral displacement, and state vector of the matching point.

[0108] In Figure 5 use point a 1 to replace point a 2 , and are the transverse and longitudinal unit vectors of the matching point.

[0109] Due to the certain time delay between the vehicle speed vector measured from the satellite and the actual planned path of the vehicle, this time delay is mainly caused by satellite signal transmission, data processing, and calculation processes. That is, when the error vector e rr is zero, the speed feedforward is not necessarily zero. In other words, in this case, the vehicle's steering wheel may not generate the corresponding steering angle adjustment, resulting in the vehicle being unable to respond promptly to path changes and thus deviating from the road. The present invention designs a preview mechanism for the planned road, which will predict the optimal steering demand at a certain planned point in advance according to the path curvature and vehicle dynamic characteristics before the vehicle reaches the point, so as to adjust the steering wheel angle in advance to ensure that the vehicle can smoothly follow the path. Let the advance prediction time be t s , then the following results are obtained:

[0110]

[0111] All predicted values are in path coordinates, x pre , y pre , v x_pre , v y_pre , which are respectively the predicted longitudinal position, lateral position, yaw angle, longitudinal speed, lateral speed, and yaw rate of the vehicle.

[0112] The series of detailed descriptions listed above are only specific descriptions of the feasible implementation manners of the present invention, and they are not intended to limit the protection scope of the present invention. Any equivalent manners or modifications that do not depart from the technical creation of the present invention should be included within the protection scope of the present invention.

Claims

1. A vehicle frequency shift positioning method based on the fusion of high and low orbit satellite networks, characterized in that: include: S1. Establish a low-orbit satellite joint frequency shift constant speed module to obtain Doppler constant speed; S2. Modify the satellite positioning equation, establish a high-orbit and low-orbit satellite fusion positioning mechanism, and obtain the vehicle's precise positioning and speed information; S3. Establish a collaborative transmission model between high-orbit and low-orbit satellites to achieve collaborative processing of high-orbit and low-orbit satellite information.

2. The vehicle frequency shift positioning method based on high-orbit and low-orbit satellite network fusion according to claim 1 is characterized in that: The S1 specifically includes: First, a single low-orbit satellite vehicle positioning system is established. A satellite navigation receiver is built into the vehicle. The relative position (x, y, z) of the vehicle can be accurately determined through triangulation or four-sided positioning algorithms. The relative position of the satellite is (x s ,y s ,z s ), define the Doppler frequency shift value f d is the difference between the receiver receiving frequency and the satellite signal transmitting frequency, let v = [v x ,v y ,v z ] T is the relative velocity vector between the receiver and the satellite, e s =[e s,x ,e s,y ,e s,z ] T is the unit observation vector from the receiver to the satellite, λ is the carrier wavelength, α is the angle between the velocity vector and the unit observation vector, then:

3. The vehicle frequency shift positioning method based on high-orbit and low-orbit satellite network fusion according to claim 1 or 2 is characterized in that: The S2 specifically includes: In a vehicle driving environment, according to the transmission time t of the satellite signal s The time t when the receiver receives the signal u By obtaining the propagation time of the signal, the relative distance from the receiver to the satellite can be obtained. Considering the clock drift between the satellite and the receiver, as well as the influence of other factors, this relative distance is not the real distance, which is called pseudorange ρ = c(t u -t s ), c represents the speed of light; Let the real distance be r s , the clock difference between the receiver and GPS time is δ t , the clock difference between the satellite and GPS time is δ t,s , the signal will also be delayed when passing through the atmosphere during propagation. Assuming that the delay caused by the atmospheric ionosphere is I, the delay caused by the atmospheric troposphere is T, and the delay caused by various other factors and noise that are not considered is ε, then: r s =c[(t u -d t )-(t s -d t,s )-TI-e] (2) The GPS positioning equation contains three vehicle position parameters and one delay parameter. Then the basic GPS positioning equation is: Derivative of formula (3): in, According to the Doppler equation, the delay I caused by the atmospheric ionosphere and the delay T caused by the atmospheric troposphere do not change significantly in a short period of time. and Zero, delay parameter is the unknown coefficient, It can be obtained through satellite navigation messages, and then formula (4) can be rewritten as: The relative speed between the satellite and the vehicle (v s -v) Projected to the unit observation vector e s The GPS positioning equation is used to reversely solve the speed of the vehicle receiver, that is, the speed of the vehicle while driving; Expanding equation (5) to multi-satellite observation and organizing it into a matrix form, we can obtain the satellite frequency shift constant speed equations: G·V=b+n d (6) in: Since the vehicle is in the space coordinate system, V = [v x ,v y ,v z ,δ t ] T , to solve the unknowns in the constant velocity equation, at least 4 low-orbit satellites are needed to solve the constant velocity equations. Design a positioning system with 4 satellites. The relative speed of the satellites is v i (i=1,2,3,4), each with its own unit observation vector e i =[e i,x ,e i,y ,e i,z ], i = 1, 2, 3, 4, the clock drift delay of the four low-orbit satellites is δ t,i (i=1,2,3,4);n d =[n d,1 ,n d,2 ,n d,3 ,n d,4 ] T , is the error noise term that is not considered; this constant speed equation can be solved by Newton iteration or least squares method, and the result of the solution is the speed and delay of the receiver.

4. The vehicle frequency shift positioning method based on high-orbit and low-orbit satellite network fusion according to claim 1 or 3 is characterized in that: The S3 specifically includes: Through the network of ground stations and space stations, GEO and LEO signal data are integrated to correct positioning errors and signal delays. Through the observation of multiple LEO satellites, the high-precision speed of the target obtained by Doppler frequency shift calculation is transmitted to the upper-level high-orbit satellite. The GEO satellite integrates and distributes this information; The distance between high and low orbit satellites can be calculated using the Euclidean distance formula of their orbital coordinates: Where (x, y, z) is the position of GEO and LEO satellites in the absolute coordinate system; In the collaboration between high and low orbit satellites, the rapid movement of LEO satellites provides highly dynamic data, and GEO satellites compensate for coverage blind spots. Considering time delay is the primary task. The signal transmission delay between the two can be calculated by accumulating the path delay and processing delay of ground-LEO-GEO. The total transmission delay is: T total =T ground-LEO +T LEO-GEO +T pro (9) Each of these sub-items is calculated based on distance and processing time, T ground-LEO is the time delay from the ground to the low-orbit satellite, T LEO-GEO is the time delay from the low-orbit satellite to the high-orbit satellite, T pro Processing time delay.

5. The vehicle control method of the vehicle frequency shift positioning method based on the fusion of high and low orbit satellite networks according to claim 1 is characterized in that: The position and speed information of the vehicle obtained by the positioning method of claim 1 is used as the input of the vehicle model, and the vehicle trajectory is accurately solved by the vehicle model through LQR control, feedforward control and preview control to complete the vehicle control, which specifically includes the following: S4. Build a lateral control model for autonomous driving vehicles; S5. Design LQR controller to optimize model control input and steering angle error; S6. Design a feedforward controller to further optimize the feedforward angle δ f , so that the error is zero; S7. Establish a preview mechanism between low-orbit satellites and planned roads to achieve accurate road tracking of vehicles.

6. The vehicle control method according to claim 5, characterized in that: The S4 specifically includes: Establish a vehicle model with two degrees of freedom: lateral and yaw: in c f ,c r is the cornering stiffness of the front and rear wheels. In equation (10), only θ vf ,θ vr Unknown, for the plane motion of a rigid body, the velocity of a point is equal to the translation velocity of the rigid body plus the velocity of rotation around the base point, that is, the translation around the base point and the rotation around the base point, that is: In θ vf ,θ vr In the case of extremely small vf =tanθ vf ,θ vr =tanθ vr ; then: Similarly: Pick are state variables, y represents the lateral displacement, Indicates the lateral speed, represents the yaw angle, Indicates the yaw rate. When the sideslip angle is very small, the yaw angle is equal to the heading angle. The above formula can be rewritten into matrix form U is the vehicle steering angle vector, which is the front wheel steering angle in the two-degree-of-freedom dynamics model; where:

7. The vehicle control method according to claim 5, characterized in that: The LQR controller of S5 specifically includes: S5.1 defines the error state equation: e rr =XX r , X is the actual positioning state of the vehicle, X r is the vehicle planning positioning state, U is the vehicle front wheel feedback angle; S5.2 discretizes the error state equation; the midpoint Euler method and the forward Euler method are used to discretize the error state equation, and the result is: S5.3 Solve the Riccati equation and obtain the feedback coefficient K: In formula (17), Q = diag (30, 1, 5, 1), R is the output weight, and P is the intermediate quantity; S5.4 Solve the front wheel steering angle feedback U(k) = -Ke rr (k).

8. The vehicle control method according to claim 5, characterized in that: The S6 specifically includes: After the vehicle system is stable, Right now For rr Solving, we get: k1, k3 are feedback coefficients. When the vehicle lateral error Not affected by the feedforward amount δ f And the influence of feedback coefficient k.

9. The vehicle control method according to claim 5, characterized in that: The preview mechanism of S7 specifically includes: The optimal steering requirement at this point is predicted based on the path curvature and the dynamic characteristics of the vehicle. The vehicle velocity vector is projected onto the lane center coordinates. The point with the smallest curvature is found by traversing the road planning points. The intersection of the tangent vector and the road is extended as the matching point of the prediction vector at the current moment. Let the advance prediction time be t s , the following prediction results are obtained: x pre ,y pre , v x_pre ,v y_pre , They are the predicted vehicle longitudinal position, lateral position, yaw angle, longitudinal velocity, lateral velocity, and yaw angular velocity respectively.

10. A vehicle device, comprising a processor and a memory, characterized in that: The memory is configured with program codes of the control method of claim 5, and when the processor reads instructions from the memory, the control method can be executed.

Images