Fusion Vehicle Positioning Method and System

Abstract

This invention discloses a fusion vehicle positioning method and system. The method is executed on a processor electrically connected to an autonomous vehicle controller and multiple electronic systems. The fusion vehicle positioning method performs delay correction and fusion processing on a first real-time coordinate, a second real-time coordinate, lane identification real-time data, multiple vehicle dynamic parameters, and multiple vehicle information received from the multiple electronic systems, combined with weight values, to generate a fusion positioning coordinate. It also performs a confidence level assessment and outputs the delay-corrected first real-time coordinate, the second real-time coordinate, the lane identification real-time data, the fusion positioning coordinate, and the confidence level information to the autonomous vehicle controller, so that the autonomous vehicle controller can implement the operation of the autonomous vehicle.

Description

Technical Field

[0001] This invention relates to a vehicle positioning method and system, and more particularly to a fusion vehicle positioning method and system. Background Technology

[0002] Self-driving vehicles are one of the main tools of future transportation, and localization technology for autonomous vehicles is a popular research and development target. For example, to implement Level 3 and above autonomous driving control as defined by the Society of Automotive Engineers (SAE), most current systems use Simultaneous Localization and Mapping (SLAM). However, SLAM systems may have shortcomings in the face of complex and ever-changing road environments, for the following reasons:

[0003] 1. The vehicle positioning information provided by the SLAM system has a positioning update frequency of less than 10Hz, which leads to problems such as slow positioning update speed and positioning delay.

[0004] 2. The road environment may have issues such as excessively consistent objects such as roadside trees and buildings, insufficient features due to detection occlusion, or sudden changes in environmental features, which can cause discrepancies in SLAM comparison feature information. This can lead to temporary failures and drifting of the SLAM system's positioning signal, making it impossible to locate immediately.

[0005] 3. As mentioned above, the positioning signal of the SLAM system may experience temporary failures and drifting issues. In other words, its positioning signal may be an incorrect positioning signal, which may cause the autonomous vehicle to drive on the wrong path, resulting in a car accident or emergency braking. Summary of the Invention

[0006] In view of this, the main objective of the present invention is to provide a fusion vehicle positioning method and system to overcome the problems of slow positioning update speed, positioning delay and inability to address immediately in the prior art.

[0007] The fusion vehicle localization method of the present invention is executed in a processor electrically connected to an autonomous vehicle controller and multiple electronic systems. The fusion vehicle localization method includes:

[0008] The system receives a first real-time coordinate, a second real-time coordinate, lane identification real-time data, multiple vehicle dynamic parameters, and multiple vehicle information from the multiple electronic systems.

[0009] As time progresses, multiple displacement values ​​are calculated and stored based on the multiple vehicle dynamic parameters and multiple vehicle information received successively.

[0010] Based on these multiple displacement values, delay corrections are made for the first real-time coordinate, the second real-time coordinate, and the lane identification real-time data;

[0011] Determine the levels of the first real-time coordinate and the second real-time coordinate;

[0012] A dynamic predicted coordinate is calculated based on the better grade among the multiple displacements and the first and second real-time coordinates.

[0013] Based on the better one of the first real-time coordinates and the second real-time coordinates, a lateral position data is generated by combining map information, and it is determined whether the error between the lateral position data and the lane identification real-time data is within the allowable value. If so, a lateral auxiliary coordinate is calculated based on the lane identification real-time data or the better one of the first real-time coordinates and the second real-time coordinates.

[0014] A fused positioning coordinate is generated based on the first real-time coordinate and its weight value, the second real-time coordinate and its weight value, the dynamically estimated coordinate and its weight value, and the lateral auxiliary coordinate and its weight value;

[0015] Determine the confidence levels of the first real-time coordinate, the second real-time coordinate, the dynamically estimated coordinate, and the lateral auxiliary coordinate, respectively; and

[0016] The delayed-corrected first real-time coordinates, the second real-time coordinates, the lane identification real-time data, the fused positioning coordinates, and the confidence information are output to the autonomous vehicle controller, so that the autonomous vehicle controller can implement the operation of the autonomous vehicle.

[0017] The integrated vehicle positioning system of this invention includes:

[0018] An autonomous vehicle controller;

[0019] Multiple electronic systems output a first real-time coordinate, a second real-time coordinate, lane recognition real-time data, multiple vehicle dynamic parameters, and multiple vehicle information; and

[0020] A processor is electrically connected to the autonomous vehicle controller and the multiple electronic systems. The processor receives the first real-time coordinate, the second real-time coordinate, the lane recognition real-time data, the multiple vehicle dynamic parameters and the multiple vehicle information from the multiple electronic systems respectively, and calculates and stores multiple displacements based on the multiple vehicle dynamic parameters and the multiple vehicle information received successively.

[0021] The processor performs delay correction on the first real-time coordinate, the second real-time coordinate, and the lane identification real-time data based on the multiple displacements;

[0022] The processor determines the level of the first real-time coordinate and the second real-time coordinate, calculates a dynamic estimated coordinate based on the better level of the multiple displacements and the first real-time coordinate and the second real-time coordinate, generates lateral position data based on the first real-time coordinate or the second real-time coordinate and map information, and determines whether the error between the lateral position data and the lane recognition real-time data is within the allowable value. If so, it calculates a lateral auxiliary coordinate based on the lane recognition real-time data or the better level of the lateral position data and the first real-time coordinate and the second real-time coordinate.

[0023] The processor generates a fused positioning coordinate based on the first real-time coordinate and its weight value, the second real-time coordinate and its weight value, the dynamically estimated coordinate and its weight value, and the lateral auxiliary coordinate and its weight value, and determines the confidence information of the first real-time coordinate, the second real-time coordinate, the dynamically estimated coordinate and the lateral auxiliary coordinate respectively.

[0024] The processor outputs the delayed-corrected first real-time coordinates, the second real-time coordinates, the lane identification real-time data, the fused positioning coordinates, and the confidence information to the autonomous vehicle controller, so that the autonomous vehicle controller can implement the operation of the autonomous vehicle.

[0025] The integrated vehicle positioning method and system according to the present invention achieve the following effects:

[0026] 1. By utilizing vehicle dynamic parameters, which are data generated by an inertial measurement unit (IMU), this invention can update the displacement at a frequency of approximately 50Hz. Therefore, by superimposing and integrating the displacement into the first real-time coordinate or the second real-time coordinate, the positioning update speed of the autonomous vehicle can be improved, and the positioning delay can be compensated.

[0027] 2. Even if the positioning signal experiences a brief failure or drift, the present invention can still obtain the fused positioning coordinates by superimposing the displacement amount, and provide them to the autonomous vehicle controller to achieve the effect of immediate addressing.

[0028] 3. By providing the confidence information of the first real-time coordinate, the second real-time coordinate, the dynamically estimated coordinate, and the lateral auxiliary coordinate to the autonomous vehicle controller, the robustness of the control system can be improved. When the processor determines that the confidence level is low, it can take safety protection actions in advance. Attached Figure Description

[0029] Figure 1 : Block diagram of the integrated vehicle positioning system of the present invention.

[0030] Figure 2A , Figure 2B : Flowchart of the integrated vehicle positioning method of the present invention.

[0031] Figure 3 : A simplified flowchart of the integrated vehicle positioning method of the present invention.

[0032] Figure 4 : Block diagram of an embodiment of the integrated vehicle positioning system of the present invention.

[0033] Figure 5 This invention provides a schematic diagram of the latitude and longitude coordinate system and the coordinate system of the autonomous vehicle.

[0034] Figure 6 In this invention, the first real-time coordinate and displacement are shown in a time-series diagram.

[0035] Figure 7 This invention illustrates the generation of lateral position data using map information and its comparison with real-time lane recognition data. Detailed Implementation

[0036] The following, in conjunction with the accompanying drawings and preferred embodiments of the present invention, further illustrates the technical means employed by the present invention to achieve its intended purpose.

[0037] This invention relates to a fusion-based vehicle positioning method and system; please refer to [the relevant documentation]. Figure 1 The system of the present invention may include an autonomous vehicle controller 21, a plurality of electronic systems 22 and a processor 10. The method of the present invention is executed on the processor 10. The autonomous vehicle controller 21, the plurality of electronic systems 22 and the processor 10 are provided in an autonomous vehicle 20. The processor 10 and the autonomous vehicle controller 21 may be integrated circuit (IC) chips. The processor 10 is electrically connected to the autonomous vehicle controller 21 and the plurality of electronic systems 22 for data transmission. The processor 10, the autonomous vehicle controller 21 and the plurality of electronic systems 22 may be interconnected through a data bus 23. The data bus 23 may be (but is not limited to) a Controller Area Network Bus (CAN Bus). The autonomous vehicle controller 21 is also connected to a steering system 24 (e.g., steering wheel transmission mechanism), a power system 25 (e.g., engine or motor) and a braking system (not shown) of the autonomous vehicle 20 through the data bus 23 to implement autonomous driving operations such as vehicle speed control, braking control and heading control. This invention involves the processor 10 receiving positioning data and sensing data from the plurality of electronic systems 22, and fusing them to generate the positioning information and confidence level of the autonomous vehicle 20. Please refer to [reference needed]. Figure 2A , 2Band Figure 3 The flowchart shown below illustrates the embodiments of the present invention.

[0038] Step S01: Data collection. The processor 10 receives a first real-time coordinate S1, a second real-time coordinate S2, lane recognition real-time data S3, multiple vehicle dynamic parameters S4, and multiple vehicle information S5 from the multiple electronic systems 22. For example, please refer to... Figure 4 The multiple electronic systems 22 include a first positioning system 221, a second positioning system 222, a lane recognition system 223, an inertial measurement system 224, and a vehicle information system 225. The first positioning system 221 outputs the first real-time coordinates S1, the second positioning system 222 outputs the second real-time coordinates S2, the lane recognition system 223 outputs the lane recognition real-time data S3, the inertial measurement system 224 outputs the multiple vehicle dynamic parameters S4, and the inertial measurement system 224 can also be called an inertial measurement unit (IMU), and the vehicle information system 225 outputs the multiple vehicle information S5.

[0039] In embodiments of the present invention, the first positioning system 221 may be a Simultaneous Localization and Mapping (SLAM) system. Therefore, the first real-time coordinate S1 is a SLAM absolute coordinate, which includes location information such as longitude and latitude. The first positioning system 221 also outputs SLAM auxiliary information corresponding to the SLAM absolute coordinate. The SLAM auxiliary information includes a SLAM computation time, a score, and a number of iterations. The SLAM computation time includes the total time from signal acquisition, signal transmission, signal processing to output of the SLAM absolute coordinate by the first positioning system 221. The second positioning system 222 can be a Real-Time Kinematic-Global Positioning System (RTK-GPS) system. Therefore, the second real-time coordinate S2 is an RTK-GPS absolute coordinate, which includes location information such as longitude and latitude. The second positioning system 222 also simultaneously outputs RTK-GPS auxiliary information corresponding to the RTK-GPS absolute coordinate. This RTK-GPS auxiliary information includes an RTK-GPS computation time, an RTK status, and an Inertial Navigation System (INS) status. The RTK-GPS computation time includes the total time from signal acquisition, signal transmission, and signal processing by the second positioning system 222 to output the RTK-GPS absolute coordinate. Please refer to the reference section. Figure 7The lane recognition system 223 outputs real-time lane recognition data S3, which includes a left lane distance S3L, a right lane distance S3R, and a lane recognition time. The lane recognition system 223 can detect a left lane line 31 and a right lane line 32 of the lane where the autonomous vehicle 20 is located using image recognition. The left lane distance S3L is the shortest distance from the autonomous vehicle 20 to the left lane line 31, and the right lane distance S3R is the shortest distance from the autonomous vehicle 20 to the right lane line 32. The lane recognition time includes the total time from signal acquisition, signal transmission, and signal processing by the lane recognition system 223 to output the real-time lane recognition data S3. The multiple vehicle dynamic parameters S4 output by the inertial measurement system 224 may include the heading angle. The sideslip angle β, yaw rate ω, longitudinal acceleration Ax, and lateral acceleration g are specified, where the longitudinal acceleration Ax refers to the forward acceleration of the autonomous vehicle 20. The vehicle information system 225 may include a steering angle detector (not shown) and a wheel speed detector (not shown). The steering angle detector detects the steering wheel angle of the autonomous vehicle 20, and the wheel speed detector detects the vehicle speed. Therefore, the plurality of vehicle information S5 output by the vehicle information system 225 includes the steering wheel angle θ. steering With the vehicle speed V.

[0040] In embodiments of the present invention, such as Figure 3 As shown, the processor 10 performs a vehicle dynamic parameter determination (step S01a). When the autonomous vehicle 20 starts from a stationary state, the processor 10 first processes the received multiple vehicle dynamic parameters S4 (including: vehicle speed V, yaw rate ω, heading angle) The sideslip angle β is first corrected to zero based on the baseline deviation. The autonomous vehicle 20 also uses a Kalman filter for correction during operation. On the other hand, the processor 10 calculates an estimated yaw rate ω. E And determine the estimated slew rate ω. E The difference between the estimated yaw rate ω and a certain threshold value is checked to see if it exceeds a yaw rate threshold, which is the default value. When the estimated yaw rate ω... E If the difference between the estimated yaw rate ω and the threshold value is not greater than (i.e., less than or equal to) the threshold value, then the estimated yaw rate ω is used for subsequent calculations. E It is expressed as follows:

[0041]

[0042] In the above formula, δ rw V represents the front wheel angle of the autonomous vehicle 20; V represents the vehicle speed; L represents the wheelbase of the autonomous vehicle 20, which is the default value; Kus The value represents the understeer coefficient, which is the default value; g is the lateral acceleration; and the front wheel angle δ is the value. rw It is expressed as follows:

[0043] δ rw = (VSR) × θ steering

[0044] In the above formula, VSR represents the ratio of the steering wheel angle to the front wheel angle of the autonomous vehicle 20, which is the default value; θ steering This refers to the steering wheel angle.

[0045] In summary, the first positioning system 221, the second positioning system 222, the lane recognition system 223, the inertial measurement system 224, and the vehicle information system 225, as described above, can adopt known systems. Therefore, the way they output the first real-time coordinates S1, the second real-time coordinates S2, the lane recognition real-time data S3, the multiple vehicle dynamic parameters S4, and the multiple vehicle information S5 is common knowledge in the relevant technical field. The present invention performs further calculations based on the data output by the multiple electronic systems 22, as detailed below.

[0046] It should be noted that the multiple vehicle dynamic parameters S4 and the multiple vehicle information S5 are time-varying. The multiple vehicle dynamic parameters S4 and the multiple vehicle information S5 mentioned in this article can also be represented by time "t", representing the parameters or information obtained at a certain moment "t". For example, the heading angle can be represented as... The sideslip angle is denoted as β(t), the yaw rate as ω(t), and the vehicle speed as V(t). On the other hand, the processor 10 performs a coordinate transformation between the first real-time coordinate S1 and the second real-time coordinate S2. Please refer to [reference needed]. Figure 5 The coordinate system is transformed from a latitude and longitude coordinate system to an autonomous vehicle coordinate system. The autonomous vehicle coordinate system includes a longitudinal axis x and a lateral axis y. The longitudinal axis x is the axis of forward movement of the autonomous vehicle 20 (i.e., heading). The lateral axis y is perpendicular to the longitudinal axis x. Therefore, after the coordinate transformation, the first real-time coordinate S1 and the second real-time coordinate S2 each contain a longitudinal coordinate value and a lateral coordinate value.

[0047] Step S02: As time progresses, the processor 10 calculates and stores a plurality of displacement amounts based on the plurality of vehicle dynamic parameters S4 and the plurality of vehicle information S5 received successively. In this step, as described above, the processor 10 updates the plurality of vehicle dynamic parameters S4 from the inertial measurement system 224 at a relatively high frequency, and the processor 10 can request the vehicle information system 225 to read the plurality of vehicle information S5 at any time. Therefore, the present invention calculates the displacement amount of the autonomous vehicle 20 between the two successive time points based on the plurality of vehicle dynamic parameters S4 and the plurality of vehicle information S5 received at the two successive time points. As time progresses, the processor 10 can sequentially store the plurality of displacement amounts when the autonomous vehicle 20 moves. For the convenience of description, each displacement amount includes a longitudinal component Shift_x(t) and a lateral component Shift_y(t). The longitudinal component Shift_x(t) represents the amplitude of the forward or backward movement of the autonomous vehicle 20 between two successive time points, and the lateral component Shift_y(t) represents the amplitude of the lateral movement of the autonomous vehicle 20 between two successive time points.

[0048] In an embodiment of the present invention, the processor 10 converts the vehicle speed V(t) into a corrected vehicle speed V1(t) according to the magnitude of the longitudinal acceleration Ax and a plurality of threshold values, and then calculates the displacement amount by using the corrected vehicle speed V1(t). Among them, the processor 10 stores a first threshold value th1, a second threshold value th2, a third threshold value th3, a fourth threshold value th4, and a fifth threshold value th5, and th1 < th2 < th3 < th4 < th5, and stores a first adjustment value ad1, a second adjustment value ad2, a third adjustment value ad3, a fourth adjustment value ad4, a fifth adjustment value ad5, and a sixth adjustment value ad6. The first threshold value to the fifth threshold value th1~th5 and the first adjustment value to the sixth adjustment value ad1~sd6 are default values and are all real numbers. When the processor 10 determines that th4 > Ax ≥ th3, calculate V1(t) = V(t) × ad4; when the processor 10 determines that th5 > Ax ≥ th4, calculate V1(t) = V(t) × ad5; when the processor 10 determines that Ax ≥ th5, calculate V1(t) = V(t) × ad6; when the processor 10 determines that th3 > Ax ≥ th2, calculate V1(t) = V(t) × ad3; when the processor 10 determines that th2 > Ax ≥ th1, calculate V1(t) = V(t) × ad2; when the processor 10 determines that th1 > Ax, calculate V1(t) = V(t) × ad1. In an embodiment of the present invention, th1 = -1, th2 = -0.4, th3 = 0, th4 = 0.2, th5 = 0.5, ad1 = 1, ad2 = 1.03, ad3 = 1.03, ad4 = 1.06, ad5 = 1.16, ad6 = 1.1.

[0049] After calculating the corrected vehicle speed V1(t), the processor 10 determines whether the latest yaw rate ω is greater than or equal to a yaw rate threshold value thw, which can be, for example, 0.05 (rad / second). If ω ≧ thw, the processor 10 calculates the longitudinal component Shift_x(t) and the lateral component Shift_y(t) as follows:

[0050]

[0051]

[0052] Where V1(t) is the corrected vehicle speed, and ω(t) is the latest yaw rate. Let ω(t) be the heading angle, and Δt be the time difference between the received yaw rate ω(t).

[0053] If ω is not greater than or equal to thw, the processor 10 calculates the vertical component Shift_x(t) and the horizontal component Shift_y(t) as follows:

[0054]

[0055]

[0056] Where V1(t) is the corrected vehicle speed, Let ω(t) be the heading angle, β(t) be the sideslip angle, and Δt be the time difference between receiving the yaw rate ω(t) or the sideslip angle β(t) successively.

[0057] In summary, as time progresses, the processor 10 calculates and stores multiple displacements based on the multiple vehicle dynamic parameters S4 and the multiple vehicle information S5 received successively. Each displacement includes a longitudinal component Shift_x(t) and a lateral component Shift_y(t) at two consecutive time points.

[0058] Step S03: Delay Correction. The processor 10 performs delay correction on the first real-time coordinates S1, the second real-time coordinates S2, and the lane recognition real-time data S3 based on the multiple displacements. It is understood that the functions of the first positioning system 221, the second positioning system 222, the lane recognition system 223, and the inertial measurement system 224 are different, and their data processing times are also different. Therefore, their data update frequencies to the processor 10 are different. Generally speaking, the data update frequency of the inertial measurement system 224 is higher than that of the first positioning system 221, the second positioning system 222, and the lane recognition system 223. For example, the data update frequencies of the SLAM system, the RTK-GPS system, and the lane recognition system 223 are all less than or equal to 10Hz, while the data update frequency of the inertial measurement system 224 is approximately 50Hz. For example, please refer to the reference... Figure 6 The processor 10 receives a first real-time coordinate S1 with coordinates (x0, y0). The processor 10 can calculate a time difference t1 between the first real-time coordinate S1 and a previous first real-time coordinate S1', where t1 is approximately 100ms. The processor 10 stores a displacement value approximately every 20ms. Therefore, when the processor 10 receives the latest first real-time coordinate S1, multiple displacement values ​​SHn-1, SHn-2, SHn-3, SHn-4, etc., have already been stored. Thus, the processor 10 superimposes these multiple displacement values ​​SHn-1, SHn-2, SHn-3, SHn-4 onto the first real-time coordinate S1 to estimate the position between the first real-time coordinate S1 and the previous first real-time coordinate S1'. The following example illustrates the superposition of the first real-time coordinate S1 and the previous displacement value SHn-1, with the resulting coordinates being (x(t), y(t)), as shown below:

[0059] x(t) = x0 + Shift_x(t)

[0060] y(t) = y0 + Shift_y(t)

[0061] In the aforementioned context, the Δt in Shift_x(t) and Shift_y(t) depends on the SLAM computation time of the first positioning system 221. That is, the SLAM computation time is substituted into the Δt in Shift_x(t) and Shift_y(t). Therefore, the longer the SLAM computation time of the first positioning system 221, the larger the longitudinal component Shift_x(t) and the lateral component Shift_y(t) become. Similarly, when the processor 10 receives the second real-time coordinates S2 and the lane identification real-time data S3, it also performs delay corrections based on the longitudinal component Shift_x(t) and the lateral component Shift_y(t).

[0062] Step S04: The processor 10 determines the level of the first real-time coordinate S1 and the second real-time coordinate S2. In an embodiment of the present invention, the processor 10 can determine the level of the first real-time coordinate S1 based on the score and the number of iterations in the SLAM auxiliary information. A smaller level value indicates a better level. The smaller the values ​​of the score and the number of iterations, the higher the level of the first real-time coordinate S1. The table below shows an example of how the processor 10 determines the level of the first real-time coordinate S1, where each judgment value in the table is a default value.

[0063] The first real-time coordinate level Score Number of iterations 0 Less than 1 Less than 6 times 1 Less than 2 Less than 15 times 2 Less than 3 Less than 15 times

[0064] Similarly, the processor 10 can determine the level of the second real-time coordinate S2 based on the RTK status and INS status in the RTK-GPS auxiliary information. A smaller level value indicates a better level. The table below shows an example of how the processor 10 determines the level of the second real-time coordinate S2.

[0065] The level of the second real-time coordinate RTK status Instagram status 0 Fixed Good 1 Float Good 2 other Poor (NG)

[0066] Step S05: The processor 10 calculates a dynamically estimated coordinate based on the preferred value among the plurality of displacements and the first real-time coordinate S1 and the second real-time coordinate S2. In this step, when the processor 10 receives the first real-time coordinate S1 and the second real-time coordinate S2, the autonomous vehicle 20 is still in motion. Therefore, the position of the autonomous vehicle 20 changes before receiving the next first real-time coordinate S1 or the next second real-time coordinate S2. The processor 10 can store multiple new displacements. Therefore, to ensure positioning accuracy, the processor 10 only uses the first real-time coordinate S1 or the second real-time coordinate S2 with the preferred value and superimposes the plurality of new displacements onto the first real-time coordinate S1 or the second real-time coordinate S2 with the preferred value to form the dynamically estimated coordinate. This allows the dynamically estimated coordinate to be obtained before receiving the next first real-time coordinate S1 or the next second real-time coordinate S2. Please refer to [reference needed]. Figure 6 Taking the first real-time coordinate S1 as an example where the first real-time coordinate S1 has a better level, the new displacement SHn+1 can be superimposed on the first real-time coordinate S1 to obtain the dynamically estimated coordinate.

[0067] Step S06: The processor 10 generates lateral position data based on the better-ranked of the first real-time coordinates S1 and the second real-time coordinates S2, combined with map information 30. It then determines whether the error between the lateral position data and the lane recognition real-time data S3 is within acceptable limits. If so, it calculates a lateral auxiliary coordinate based on the lane recognition real-time data S3 or the better-ranked of the lateral position data and the first real-time coordinates S1 and the second real-time coordinates S2. In this step, please refer to... Figure 4The map information 30 can be stored in a memory 11, and the processor 10 is electrically connected to the memory 11 to read the map information 30. Please refer to [reference needed]. Figure 7 The image shows a portion of the map information 30. Map information 30 includes programming code for a left lane line 31 and programming code for a right lane line 32. Therefore, taking the first real-time coordinate S1 with a better rating as an example, the processor 10 can calculate a left lateral distance CL from the first real-time coordinate S1 to the left lane line 31, and a right lateral distance CR from the first real-time coordinate S1 to the right lane line 32. The lateral position data includes the left lateral distance CL and the right lateral distance CR. The processor 10 determines whether the difference between the left lateral distance CL and the left interval distance S3L in step S01 is less than a left lateral tolerance value, and determines whether the difference between the right lateral distance CR and the right interval distance S3R in step S01 is less than a right lateral tolerance value. When the difference between the left lateral distance CL and the left gap distance S3L is less than the left lateral allowable value, and the difference between the right lateral distance CR and the right gap distance S3R is less than the right lateral allowable value, it means that both the lateral position data and the lane recognition real-time data S3 are usable. The processor 10 can calculate the lateral auxiliary coordinates based on either the lateral position data or the lane recognition real-time data S3.

[0068] Step S07: The processor 10 generates a fused positioning coordinate based on the first real-time coordinate S1 and its weight value, the second real-time coordinate S2 and its weight value, the dynamically estimated coordinate and its weight value, and the lateral auxiliary coordinate and its weight value. In this step, the fused positioning coordinate is represented as follows:

[0069] Fusion positioning coordinates

[0070] = [(First real-time coordinate * A1% + Second real-time coordinate * A2%) + Dynamically estimated coordinate * A3%] + Horizontal auxiliary coordinate * A4%

[0071] In the above formula, A1 is the weight value of the first real-time coordinate S1, and A2 is the weight value of the second real-time coordinate S2. Therefore, absolute position weight allocation is implemented by setting A1 and A2. A3 is the weight value of the dynamically estimated coordinate, and A4 is the weight value of the lateral auxiliary coordinate. The processor 10 can set A1, A2, A3 and A4 according to different conditions. The table below is an example. The conditions and A1 to A4 are data stored in the processor 10 and can be adjusted as needed. That is, the processor sets A1, A2, A3 and A4 according to the level of the SLAM absolute coordinate and the level of the RTK-GPS absolute coordinate.

[0072]

[0073] Step S08: The processor 10 determines the confidence information of the first real-time coordinate S1, the second real-time coordinate S2, the dynamically estimated coordinate, and the lateral auxiliary coordinate. Since this invention employs multiple systems, including the first positioning system 221, the second positioning system 222, the lane recognition system 223, and the inertial measurement system 224, confidence level determination is performed on the data generated by each system. If the confidence level determination result of a certain electronic system 22 is poor, it indicates that the electronic system 22 may require calibration or repair, as explained below.

[0074] (1) Confidence in absolute position

[0075] The processor 10 determines whether the error between the latest received first real-time coordinate S1 and the second real-time coordinate S2 is less than or equal to a first set value, which is a default value. The processor 10 then outputs a first confidence level information based on the determination result, as shown in the table below.

[0076]

[0077]

[0078] (2) Dynamic positioning confidence level

[0079] After calculating the dynamically estimated coordinates, the processor 10 determines whether the error between the fused positioning coordinates and the dynamically estimated coordinates is less than a second preset value within a threshold time, and outputs a second confidence level based on the determination result. The threshold time and the second preset value are both preset values.

[0080]

[0081] (3) Confidence in lateral position

[0082] As mentioned above, in step S06, the processor 10 has generated the lateral position data in conjunction with the map information 30, and in step S07, the processor 10 has generated the fused positioning coordinates. Therefore, the processor 10 determines whether the error between the fused positioning coordinates and the lateral position data is less than or equal to a third set value, which is a preset value. The processor 10 then outputs a third confidence level information based on the determination result, as shown in the table below.

[0083]

[0084] In summary, in step S08, the confidence information determined by the processor 10 includes the first confidence information, the second confidence information, and the third confidence information.

[0085] Step S09: Output the delayed-corrected first real-time coordinates S1, the second real-time coordinates S2, the lane recognition real-time data S3, the fused positioning coordinates, and the confidence information to the autonomous vehicle controller 21, so that the autonomous vehicle controller 21 can implement the operation of the autonomous vehicle. The processor 10, through the aforementioned steps S01 to S08, generates information including the delayed-corrected first real-time coordinates S1, the second real-time coordinates S2, the lane recognition real-time data S3 (according to step S02), the fused positioning coordinates (according to step S07), and the confidence information (according to step S08), and therefore provides the aforementioned data to the autonomous vehicle controller 21 for autonomous driving operation.

[0086] In summary, when the autonomous vehicle controller 21 performs autonomous driving operations based on the data, it can improve the accuracy of controlling the autonomous vehicle 20 and reduce the phenomenon of temporary failure of the autonomous vehicle 20 in receiving positioning information. Furthermore, after receiving information with low confidence, the autonomous vehicle controller 21 can take safety precautions in advance, such as controlling the steering system 24 and the power system 25 of the autonomous vehicle 20 to make the autonomous vehicle 20 decelerate or pull over to the side of the road, thus avoiding accidents or emergency braking situations.

[0087] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any way. Although the present invention has been disclosed above with reference to preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art can make some modifications or alterations to the above-disclosed technical content to create equivalent embodiments without departing from the scope of the technical solution of the present invention. Any simple modifications, equivalent changes and alterations made to the above embodiments based on the technical essence of the present invention without departing from the content of the technical solution of the present invention shall still fall within the scope of the technical solution of the present invention.

Claims

1. A fusion-based vehicle positioning method, characterized in that, Executed on a processor electrically connected to an autonomous vehicle controller and multiple electronic systems, the fused vehicle localization method includes: Step (a) receives a first real-time coordinate, a second real-time coordinate, lane identification real-time data, multiple vehicle dynamic parameters, and multiple vehicle information from the multiple electronic systems respectively; Step (b) proceeds over time, and calculates and stores multiple displacements based on the multiple vehicle dynamic parameters and multiple vehicle information received successively. Step (c) performs delay correction on the first real-time coordinate, the second real-time coordinate, and the lane identification real-time data based on the multiple displacements; Step (d) determines the levels of the first real-time coordinate and the second real-time coordinate; Step (e) Calculate a dynamically estimated coordinate based on the better grade among the multiple displacements and the first real-time coordinate and the second real-time coordinate; Step (f) Generate a lateral position data by combining the better of the first real-time coordinates and the second real-time coordinates with map information, and determine whether the error between the lateral position data and the lane identification real-time data is within the allowable value. If so, calculate a lateral auxiliary coordinate based on the lane identification real-time data or the better of the lateral position data and the first real-time coordinates and the second real-time coordinates. Step (g) generates a fused positioning coordinate based on the first real-time coordinate and its weight value, the second real-time coordinate and its weight value, the dynamically estimated coordinate and its weight value, and the lateral auxiliary coordinate and its weight value; Step (h) determines the confidence information of the first real-time coordinate, the second real-time coordinate, the dynamically estimated coordinate, and the horizontal auxiliary coordinate, respectively. as well as Step (i) outputs the delayed-corrected first real-time coordinates, the second real-time coordinates, the lane identification real-time data, the fused positioning coordinates, and the confidence information to the autonomous vehicle controller, so that the autonomous vehicle controller can implement the operation of the autonomous vehicle.

2. The fusion vehicle positioning method as described in claim 1, characterized in that, In step (a): The first real-time coordinate is a Simultaneous Localization and Mapping (SLAM) absolute coordinate; The second real-time coordinate is an absolute coordinate of a Real-Time Dynamic Global Positioning System (RTK-GPS); The real-time lane recognition data includes a left-side clearance distance and a right-side clearance distance; These multiple vehicle dynamic parameters include a heading angle, a side slip angle, a yaw rate, a longitudinal acceleration, and a lateral acceleration; This vehicle information includes steering wheel angle and vehicle speed.

3. The fusion vehicle positioning method as described in claim 1, characterized in that, The processor calculates an estimated yaw rate and determines whether the difference between the estimated yaw rate and a yaw rate among the plurality of vehicle dynamic parameters is greater than a yaw rate threshold; if not, the yaw rate is used for calculation.

4. The fusion vehicle positioning method as described in claim 1, characterized in that, In step (b), each displacement includes a longitudinal component Shift_x(t) and a lateral component Shift_y(t); When the processor determines that the latest yaw rate among the multiple vehicle dynamic parameters is greater than or equal to a yaw rate threshold, the longitudinal component Shift_x(t) and the lateral component Shift_y(t) are represented as follows: Where V1(t) is a corrected vehicle speed, and ω(t) is the latest yaw rate. Let ω(t) be one of the multiple vehicle dynamic parameters, and let Δt be the time difference between the successive receipt of the yaw rate ω(t). When the processor determines that the latest yaw rate among the multiple vehicle dynamic parameters is less than the yaw rate threshold, the longitudinal component Shift_x(t) and the lateral component Shift_y(t) are represented as follows: Where V1(t) is a corrected vehicle speed, Let ω(t) be a heading angle among the multiple vehicle dynamic parameters, β(t) be a side slip angle among the multiple vehicle dynamic parameters, and Δt be the time difference between receiving the yaw rate ω(t) or the side slip angle β(t) successively. The processor converts a vehicle speed from the multiple vehicle information into the corrected vehicle speed based on a longitudinal acceleration from the multiple vehicle dynamic parameters and multiple threshold values.

5. The fusion vehicle positioning method as described in claim 1, characterized in that, In step (c), the processor superimposes multiple displacement values ​​onto the first real-time coordinate, the second real-time coordinate, and the lane identification real-time data for delay correction.

6. The fusion vehicle positioning method as described in claim 1, characterized in that, In step (f), the map information includes equation data for a left lane and equation data for a right lane; the processor calculates a left lateral distance from the better of the first real-time coordinates and the second real-time coordinates to the left lane, and calculates a right lateral distance from the better of the first real-time coordinates and the second real-time coordinates to the right lane, wherein the lateral position data includes the left lateral distance and the right lateral distance; The processor determines whether the difference between the left lateral distance and the left interval distance is less than a left lateral tolerance value, and determines whether the difference between the right lateral distance and the right interval distance is less than a right lateral tolerance value; When the processor determines that the difference between the left lateral distance and the left gap distance is less than the left lateral tolerance value, and the difference between the right lateral distance and the right gap distance is less than the right lateral tolerance value, the processor calculates the lateral auxiliary coordinates based on one of the lateral position data and the lane identification real-time data.

7. The fusion vehicle positioning method as described in claim 1, characterized in that, In step (g), the fused positioning coordinates are represented as follows: Fusion positioning coordinates = [(First real-time coordinate * A1% + Second real-time coordinate * A2%) + Dynamically estimated coordinate * A3%] + Lateral auxiliary coordinate * A4% In the above formula, A1, A2, A3 and A4 are weight values. The processor sets A1, A2, A3 and A4 according to the level of the first real-time coordinate and the level of the second real-time coordinate. The first real-time coordinate is the Simultaneous Localization and Mapping (SLAM) absolute coordinate, and the second real-time coordinate is the Real-Time Dynamic Global Positioning System (RTK-GPS) absolute coordinate.

8. The fusion vehicle positioning method as described in claim 1, characterized in that, In step (h): The processor determines whether the error between the latest received first real-time coordinate and the second real-time coordinate is less than or equal to a first set value, and outputs a first confidence information based on the determination result. The first confidence information is a text message or code. After the processor calculates the dynamically estimated coordinates, it determines whether the error between the fused positioning coordinates and the dynamically estimated coordinates is less than a second set value within a threshold time, and outputs a second confidence information based on the judgment result. The second confidence information is a text message or code. The processor determines whether the error between the fused positioning coordinates and the lateral position data is less than or equal to a third preset value, and outputs a third confidence information based on the determination result. The third confidence information is a text message or code.

9. A fusion vehicle positioning system, characterized in that, Include: An autonomous vehicle controller; Multiple electronic systems output a first real-time coordinate, a second real-time coordinate, lane recognition real-time data, multiple vehicle dynamic parameters, and multiple vehicle information respectively; as well as A processor is electrically connected to the autonomous vehicle controller and the multiple electronic systems. The processor receives the first real-time coordinate, the second real-time coordinate, the lane recognition real-time data, the multiple vehicle dynamic parameters and the multiple vehicle information from the multiple electronic systems respectively, and calculates and stores multiple displacements based on the multiple vehicle dynamic parameters and the multiple vehicle information received successively. The processor performs delay correction on the first real-time coordinate, the second real-time coordinate, and the lane identification real-time data based on the multiple displacements; The processor determines the level of the first real-time coordinate and the second real-time coordinate, calculates a dynamic estimated coordinate based on the better level of the multiple displacements and the first real-time coordinate and the second real-time coordinate, generates lateral position data based on the first real-time coordinate or the second real-time coordinate and map information, and determines whether the error between the lateral position data and the lane recognition real-time data is within the allowable value. If so, it calculates a lateral auxiliary coordinate based on the lane recognition real-time data or the better level of the lateral position data and the first real-time coordinate and the second real-time coordinate. The processor generates a fused positioning coordinate based on the first real-time coordinate and its weight value, the second real-time coordinate and its weight value, the dynamically estimated coordinate and its weight value, and the lateral auxiliary coordinate and its weight value, and determines the confidence information of the first real-time coordinate, the second real-time coordinate, the dynamically estimated coordinate and the lateral auxiliary coordinate respectively. The processor outputs the delayed-corrected first real-time coordinates, the second real-time coordinates, the lane identification real-time data, the fused positioning coordinates, and the confidence information to the autonomous vehicle controller, so that the autonomous vehicle controller can implement the operation of the autonomous vehicle.

10. The integrated vehicle positioning system as described in claim 9, characterized in that, These multiple electronic systems include: A first positioning system, which is a Simultaneous Localization and Mapping (SLAM) system, outputs the first real-time coordinates; A second positioning system is a real-time dynamic global positioning (RTK-GPS) system, which outputs the second real-time coordinates; The lane recognition system outputs real-time lane recognition data. An inertial measurement system outputs these multiple vehicle dynamic parameters; and A vehicle information system outputs information about these multiple vehicles; The map information is stored in memory, and the processor is electrically connected to the memory to read the map information.

Images