A method for determining relative position of a net-connected vehicle on a curve

Abstract

The present application relates to the technical field of networked vehicles, and relates to a method for judging the relative position of a networked vehicle on a curve, comprising the following steps: S1: calculating the radius of curvature of the driving track of the vehicle at the current time; S2: judging the stability of the driving track at the current time; S3: calculating the center of the driving track of the vehicle at the current time; S4: judging whether the included angle between the line connecting the vehicle and the center of the driving track of the vehicle at the current time and the line connecting the remote vehicle and the center of the driving track of the vehicle at the current time is greater than a preset included angle threshold value, if the included angle is greater than the preset included angle threshold value, the driving track of the vehicle at the current time is not adopted, otherwise, the driving track of the vehicle at the current time is adopted and the next step is executed; S5: dividing the lane according to the shortest distance from the remote vehicle to the driving track of the vehicle at the current time, and judging which lane the remote vehicle is located in on the driving section of the vehicle at the current time, the present method avoids the situation that the roadside unit cannot judge the position of the vehicle due to failure.

Description

Technical Field

[0001] This invention relates to the field of connected vehicle technology, and more specifically, to a method for determining the relative position of a connected vehicle on a curve. Background Technology

[0002] C-V2X technology enhances a vehicle's perception capabilities through vehicle-to-vehicle, vehicle-to-pedestrian, and vehicle-to-roadside connectivity. Vehicle-to-vehicle communication occurs between onboard units. Current methods for determining relative vehicle position on curves rely on lidar and image sensing, which are susceptible to weather and obstructions. Another approach utilizes roadside unit-generated maps; the vehicle communicates with these units to determine its position within the map, thus establishing its relative position on curves. However, this method is dependent on the roadside units; malfunctions in these units can affect the determination of vehicle position. Summary of the Invention

[0003] To address the shortcomings and deficiencies of existing technologies, this invention provides a method for determining the relative position of connected vehicles on curves.

[0004] To achieve the above objectives, the present invention provides a method for determining the relative position of a connected vehicle on a curve, the method comprising the following steps:

[0005] S1: Obtain the yaw rate of the vehicle at the current moment, the yaw rate of the previous moment, the current speed, the curvature of the previous moment's driving trajectory after filtering, the curvature of the driving trajectory before the previous moment after filtering, the yaw rate of the previous moment after filtering, the yaw rate of the previous moment after filtering, calculate the curvature of the vehicle's driving trajectory at the current moment and filter the curvature of the driving trajectory at the current moment, and then calculate the radius of curvature of the vehicle's driving trajectory at the current moment.

[0006] S2: Determine the stability of the driving trajectory at the current moment. If the driving trajectory at the current moment is unstable, issue a low confidence report and do not adopt the driving trajectory at the current moment. Otherwise, adopt the driving trajectory at the current moment and proceed to the next step.

[0007] S3: Calculate the center of the current vehicle's trajectory.

[0008] S4: Determine whether the angle between the line connecting the current vehicle and the center of its current trajectory and the line connecting the distant vehicle and the center of the current trajectory of the current vehicle is greater than a preset angle threshold. If the angle is greater than the preset angle threshold, the current trajectory of the current vehicle is not adopted; otherwise, the current trajectory of the current vehicle is adopted and the next step is executed.

[0009] S5: Divide the lanes according to the shortest distance from the distant vehicle to the current vehicle's current travel trajectory, and determine which lane the distant vehicle is in on the current travel segment of the road.

[0010] In S1, the curvature of the vehicle's current trajectory is calculated as follows:

[0011] Curvature = Current yaw rate (radians / second) / Current vehicle speed (meters / second)

[0012] The curvature of the vehicle is filtered by a discrete second-order low-pass filter, and the filtering formula is as follows:

[0013]

[0014] Where ω0=2πf0, f0 is the cutoff frequency, ζ is the damping coefficient, and T s For sampling time, y n Let y be the curvature of the current driving trajectory after filtering. n-1 The curvature of the driving trajectory after filtering at the previous moment, y n-2 The curvature of the driving trajectory after filtering is μ. n The curvature of the trajectory at the current moment.

[0015] The radius of curvature of the vehicle's current trajectory is calculated as follows: radius of curvature = 1 / curvature.

[0016] In step S2, the yaw rate of the vehicle at the current moment is filtered by a differential discrete second-order low-pass filter. Based on the filtered yaw rate of the vehicle at the current moment, the corresponding confidence level is found in the confidence lookup table. If the confidence level is less than the preset confidence threshold, the driving trajectory is determined to be unstable, a low confidence report is issued and the driving trajectory is not adopted. Otherwise, the driving trajectory is determined to be stable, the driving trajectory is adopted and the next step is executed.

[0017] The filtering formula for the yaw rate of this vehicle by the differential discrete second-order low-pass filter is as follows:

[0018]

[0019] Where ω0=2πf0, f0 is the cutoff frequency, ζ is the damping coefficient, and T s For sampling time, y n y is the filtered yaw rate at the current moment. n-1 y is the filtered yaw rate from the previous moment. n-2 The yaw rate is the filtered yaw rate of the previous time step, μ. n Let μ be the yaw rate at the current moment. n-1This represents the yaw rate at the previous moment.

[0020] In S3, the center of the vehicle's trajectory lies on the y-axis in the vehicle's coordinate system. If the vehicle turns right, the center is on the negative half-axis of y, with coordinates (0, -r); if the vehicle turns left, the center is on the positive half-axis of y, with coordinates (0, r), where r is the radius of curvature of the vehicle's trajectory at the current moment.

[0021] S4 includes the following steps:

[0022] S41: Calculate the coordinates of the remote vehicle in the vehicle coordinate system (RV) x RV y );

[0023] S42: Calculate the coordinates of the intersection point between the center of the line connecting the current trajectory of the distant vehicle and the center of the current trajectory of the local vehicle. x intersection y );

[0024] S43: Calculate the distance s from the intersection point coordinates to the vehicle:

[0025]

[0026] S44: Calculate the angle θ between the line connecting the current vehicle to the center of its current trajectory and the line connecting the distant vehicle to the center of the current trajectory of the current vehicle.

[0027]

[0028] If θ>Π, then θ=2Π–θ,

[0029] If the included angle is greater than the preset included angle threshold, the current driving trajectory of the vehicle will not be adopted; otherwise, the current driving trajectory of the vehicle will be adopted and the next step will be executed.

[0030] S5 includes the following steps:

[0031] S51: Calculate the shortest distance from the distant vehicle to the current vehicle's trajectory:

[0032] If this vehicle is turning right, then the shortest distance from the distant vehicle to this vehicle at the current moment is:

[0033]

[0034] If this vehicle is turning left, then the shortest distance from the distant vehicle to this vehicle at the current moment is:

[0035]

[0036] S52: Lanes are divided based on the shortest distance from the distant vehicle to the current vehicle's trajectory.

[0037] If this vehicle turns right, then

[0038]

[0039] If this vehicle turns left, then

[0040]

[0041] Among them, w lane This refers to the lane width, which is 3-4 meters.

[0042] S53: Determine which lane the distant vehicle is in at the current moment on the road segment in which this vehicle is currently traveling.

[0043] The beneficial effects of this invention are:

[0044] This invention provides a method for determining the relative position of a connected vehicle on a curve. By calculating the radius of curvature of the vehicle at the current moment based on the curvature of its current trajectory and the yaw rate, the future driving path of the vehicle can be predicted. Furthermore, based on the relative position of a distant vehicle to the current vehicle, the method determines which lane the distant vehicle is located in. Compared with existing methods for determining the relative position of a vehicle on a curve, this method avoids situations where the roadside unit malfunctions and cannot determine the vehicle's position, and it is less affected by weather or obstructions. Attached Figure Description

[0045] Figure 1 This is a step diagram of a method for determining the relative position of a connected vehicle on a curve according to the present invention. Detailed Implementation

[0046] The present invention will now be described in further detail with reference to the accompanying drawings, which are simplified schematic diagrams.

[0047] The method of this invention is to calculate the filtered radius of curvature of the vehicle's trajectory based on the vehicle's current speed and yaw rate at the moment of the curve, and to predict the future trajectory of the vehicle based on the radius of curvature. This trajectory is used as the baseline of the vehicle's current lane, and the distance of the distant vehicle from the current lane line is used to determine which relative lane the distant vehicle belongs to.

[0048] like Figure 1 As shown, the present invention provides a method for determining the relative position of a connected vehicle on a curve, the method comprising the following steps:

[0049] S1: Obtain the yaw rate of the vehicle at the current moment, the yaw rate of the previous moment, the current speed, the curvature of the previous moment's driving trajectory after filtering, the curvature of the driving trajectory before the previous moment after filtering, the yaw rate of the previous moment after filtering, the yaw rate of the previous moment after filtering, calculate the curvature of the vehicle's driving trajectory at the current moment and filter the curvature of the driving trajectory at the current moment, and then calculate the radius of curvature of the vehicle's driving trajectory at the current moment.

[0050] S2: Determine the stability of the driving trajectory at the current moment. If the driving trajectory at the current moment is unstable, issue a low confidence report and do not adopt the driving trajectory at the current moment. Otherwise, adopt the driving trajectory at the current moment and proceed to the next step.

[0051] S3: Calculate the center of the current vehicle's trajectory.

[0052] S4: Determine whether the angle between the line connecting the current vehicle and the center of its current trajectory and the line connecting the distant vehicle and the center of the current trajectory of the current vehicle is greater than a preset angle threshold. If the angle is greater than the preset angle threshold, the current trajectory of the current vehicle is not adopted; otherwise, the current trajectory of the current vehicle is adopted and the next step is executed.

[0053] S5: Divide the lanes according to the shortest distance from the distant vehicle to the current vehicle's current travel trajectory, and determine which lane the distant vehicle is in on the current travel segment of the road.

[0054] Both the remote vehicle and the local vehicle need to be equipped with information transmission devices. The remote vehicle sends its GPS and heading angle data to the local vehicle through these devices. The local vehicle receives the remote vehicle's GPS and heading angle data through the same devices to determine the remote vehicle's location and uses the aforementioned method for determining the relative position of a connected vehicle on curves to identify which lane the remote vehicle is currently in. In this embodiment, the information transmission device is an onboard T-BOX. The local vehicle can also use this device to send its own GPS and heading angle data to other vehicles to ensure their safe driving.

[0055] In S1, the curvature of the vehicle's current trajectory is calculated as follows:

[0056] Curvature = Current yaw rate (radians / second) / Current vehicle speed (meters / second)

[0057] The curvature of the vehicle is filtered by a discrete second-order low-pass filter, and the filtering formula is as follows:

[0058]

[0059] Where ω0=2πf0, f0 is the cutoff frequency, ζ is the damping coefficient, and T s For sampling time, y n Let y be the curvature of the current driving trajectory after filtering. n-1 The curvature of the driving trajectory after filtering at the previous moment, y n-2 ζ represents the curvature of the driving trajectory at the previous time step, after filtering, and μn represents the curvature of the driving trajectory at the current time step. The initial values ​​are set as follows: y1 = μ1, y2 = μ2. ζ can be set to 1, and can also be adjusted according to user needs.

[0060] After calculating the curvature, the signal is filtered by a discrete second-order low-pass filter, which can attenuate unwanted high-frequency noise signals. The discrete second-order low-pass filter can reduce road noise signals, sensor noise signals, and noise signals generated by vehicles swaying left and right in the lane.

[0061] The radius of curvature of the vehicle's current trajectory is calculated as follows: radius of curvature = 1 / curvature.

[0062] Since the yaw rate has positive and negative values, when the yaw rate transitions from a positive value to a negative value near 0, the radius of curvature will oscillate between positive infinity and negative infinity. At this time, when the radius of curvature is used as the input signal of the filter, there will be a large discontinuity. Therefore, the curvature (the reciprocal of the radius) is used as the calculation input for the discrete second-order low-pass filter.

[0063] When the curvature is 0 or close to 0, the value of 1 / curvature is infinite. At this time, a default value is assigned to the radius of curvature, which can be adjusted according to the user's needs.

[0064] If the vehicle speed is less than the calibration threshold, or the calculated radius of curvature is greater than the calibration threshold, then a predefined default value for the radius of curvature will be output. This default value is typically set to 32767m, but can also be 32000m or other values. The vehicle speed calibration threshold is 1m / s, and the radius of curvature calibration threshold is 2500m. Both of these calibration thresholds can be adjusted according to the user's needs.

[0065] In step S2, the yaw rate of the vehicle at the current moment is filtered by a differential discrete second-order low-pass filter. Based on the filtered yaw rate of the vehicle at the current moment, the corresponding confidence level is found in the confidence lookup table. If the confidence level is less than the preset confidence threshold, the driving trajectory is determined to be unstable, a low confidence report is issued and the driving trajectory is not adopted. Otherwise, the driving trajectory is determined to be stable, the driving trajectory is adopted and the next step is executed.

[0066] The filtering formula for the yaw rate of this vehicle by the differential discrete second-order low-pass filter is as follows:

[0067]

[0068] Where ω0=2πf0, f0 is the cutoff frequency, ζ is the damping coefficient, and T s For sampling time, y n y is the filtered yaw rate at the current moment. n-1 y is the filtered yaw rate from the previous moment. n-2 The yaw rate is the filtered yaw rate of the previous time step, μ. n Let μ be the yaw rate at the current moment. n-1 This represents the yaw rate at the previous moment. Initial values ​​are set as follows: y1 = 0, y2 = 0. ζ can be set to 1, and can also be adjusted according to user needs.

[0069] When in a stable state, the radius of curvature calculation for the vehicle's trajectory is relatively accurate, but it becomes less accurate in unstable environments. When path prediction is inaccurate, it's necessary to identify the environmental state and relay it to the vehicle. Confidence information can be determined using filtered yaw rate to identify stable states. When the vehicle's yaw rate changes significantly within a short period, the confidence indicator will issue a low-confidence report. These unstable states may include one or more of the following: lane changes, being at the entrance or exit of a curve, being at a curve transition point, obstacle avoidance, or other high-dynamic driving situations. High-dynamic driving situations are low-confidence driving states.

[0070] The confidence lookup table contains the filtered yaw rate at the current time and its corresponding confidence level.

[0071] The default confidence threshold is typically set to 50, but it can also be set to 60, 70, or 80. The confidence threshold can be adjusted according to the user's needs.

[0072] In S3, the center of the vehicle's trajectory lies on the y-axis in the vehicle's coordinate system. If the vehicle turns right, the center is on the negative half-axis of y, with coordinates (0, -r); if the vehicle turns left, the center is on the positive half-axis of y, with coordinates (0, r), where r is the radius of curvature of the vehicle's trajectory at the current moment.

[0073] S4 includes the following steps:

[0074] S41: Calculate the coordinates of the remote vehicle in the vehicle coordinate system (RV) x RV y );

[0075] S42: Calculate the coordinates of the intersection point between the center of the line connecting the current trajectory of the distant vehicle and the center of the current trajectory of the local vehicle. x intersection y );

[0076] S43: Calculate the distance s from the intersection point coordinates to the vehicle:

[0077]

[0078] S44: Calculate the angle θ between the line connecting the current vehicle to the center of its current trajectory and the line connecting the distant vehicle to the center of the current trajectory of the current vehicle.

[0079]

[0080] If θ>Π, then θ=2Π–θ,

[0081] If the included angle is greater than the preset included angle threshold, the current driving trajectory of the vehicle will not be adopted; otherwise, the current driving trajectory of the vehicle will be adopted and the next step will be executed.

[0082] In this embodiment, the preset included angle threshold is set to 125°. In addition, it can also be 120°, 110° or other angles. The preset included angle threshold can be adjusted according to the user's needs.

[0083] S5 includes the following steps:

[0084] S51: Calculate the shortest distance from the distant vehicle to the current vehicle's trajectory:

[0085] If this vehicle is turning right, then the shortest distance from the distant vehicle to this vehicle at the current moment is:

[0086]

[0087] If this vehicle is turning left, then the shortest distance from the distant vehicle to this vehicle at the current moment is:

[0088]

[0089] S52: Lanes are divided based on the shortest distance from the distant vehicle to the current vehicle's trajectory.

[0090] If this vehicle turns right, then

[0091]

[0092] If this vehicle turns left, then

[0093]

[0094] Among them, w lane This refers to the lane width, which is 3-4 meters.

[0095] S53: Determine which lane the distant vehicle is in at the current moment on the road segment in which this vehicle is currently traveling.

[0096] The application scenario of the method for determining the relative position of a connected vehicle on a curve according to the present invention includes the vehicle itself and multiple remote vehicles. The following embodiment uses the vehicle itself and two remote vehicles as specific examples. In fact, the determination method is applicable to multiple remote vehicles, and its determination method is suitable for two remote vehicles, so it will not be described in detail here.

[0097] If this vehicle makes a left turn, first calculate the radius of curvature of its current trajectory, then determine if the trajectory is stable. If stable, calculate the center of the trajectory. Here, let the radius of curvature of the current trajectory be r = 10m, and the center coordinates be (0, 10). The coordinates of distant vehicle 1 in this vehicle's coordinate system are (0, 5), and the coordinates of distant vehicle 2 in this vehicle's coordinate system are (0, -5). The intersection points of the lines connecting distant vehicle 1, distant vehicle 2, and the center of this vehicle's current trajectory with the current trajectory are all (0, 0), with an included angle of 0°. The shortest distance from distant vehicle 1 to this vehicle's current trajectory is -5m. The lane width is 4 meters. Therefore, distant vehicle 1 is located in the left adjacent lane of this vehicle, and the shortest distance from distant vehicle 2 to this vehicle's current trajectory is 5m, placing distant vehicle 2 in the right adjacent lane of this vehicle.

[0098] If this vehicle is making a right turn, first calculate the radius of curvature of its current trajectory. Then determine if the current trajectory is stable. If stable, calculate the center of the trajectory. Here, let the radius of curvature of the current trajectory be r = 10m, and the center coordinates be (0, -10). The coordinates of distant vehicle 1 in this vehicle's coordinate system are (0, 5), and the coordinates of distant vehicle 2 in this vehicle's coordinate system are (0, -5). The intersection points of the lines connecting distant vehicle 1, distant vehicle 2, and the center of the current trajectory with the current trajectory are all (0, 0), with an included angle of 0°. The shortest distance from distant vehicle 1 to the current trajectory is 5m, and the lane width is 4 meters. Therefore, distant vehicle 1 is located in the left adjacent lane of this vehicle, and the shortest distance from distant vehicle 2 to the current trajectory is -5m, placing distant vehicle 2 in the right adjacent lane of this vehicle.

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

Claims

1. A method for determining the relative position of a connected vehicle on a curve, characterized in that, The method includes the following steps: S1: Obtain the yaw rate of the vehicle at the current moment, the yaw rate of the previous moment, the current speed, the curvature of the previous moment's driving trajectory after filtering, the curvature of the driving trajectory before the previous moment after filtering, the yaw rate of the previous moment after filtering, the yaw rate of the previous moment after filtering, calculate the curvature of the vehicle's driving trajectory at the current moment and filter the curvature of the driving trajectory at the current moment, and then calculate the radius of curvature of the vehicle's driving trajectory at the current moment. S2: Determine the stability of the driving trajectory at the current moment. If the driving trajectory at the current moment is unstable, issue a low confidence report and do not adopt the driving trajectory at the current moment. Otherwise, adopt the driving trajectory at the current moment and proceed to the next step. S3: Calculate the center of the current vehicle's trajectory. S4: Determine whether the angle between the line connecting the current vehicle and the center of its current trajectory and the line connecting the distant vehicle and the center of the current trajectory of the current vehicle is greater than a preset angle threshold. If the angle is greater than the preset angle threshold, the current trajectory of the current vehicle is not adopted; otherwise, the current trajectory of the current vehicle is adopted and the next step is executed. S5: Divide the lanes according to the shortest distance from the distant vehicle to the current vehicle's trajectory, and determine which lane the distant vehicle is in on the current vehicle's current travel segment; In S1, the curvature of the vehicle's current trajectory is calculated as follows: Curvature = Current yaw rate (radians / second) / Current vehicle speed (meters / second) The curvature of the vehicle is filtered by a discrete second-order low-pass filter, and the filtering formula is as follows: Where ω0 = 2πf0, f0 is the cutoff frequency, ζ is the damping coefficient, and T s For sampling time, y n Let y be the curvature of the current driving trajectory after filtering. n-1 The curvature of the driving trajectory after filtering at the previous moment, y n-2 The curvature of the driving trajectory after filtering is μ. n The curvature of the trajectory at the current moment; The radius of curvature of the vehicle's current trajectory is calculated as follows: radius of curvature = 1 / curvature.

2. The method for determining the relative position of a connected vehicle on a curve according to claim 1, characterized in that, In step S2, the yaw rate of the vehicle at the current moment is filtered by a differential discrete second-order low-pass filter. Based on the filtered yaw rate of the vehicle at the current moment, the corresponding confidence level is found in the confidence lookup table. If the confidence level is less than the preset confidence threshold, the driving trajectory is determined to be unstable, a low confidence report is issued and the driving trajectory is not adopted. Otherwise, the driving trajectory is determined to be stable, the driving trajectory is adopted and the next step is executed. The filtering formula for the yaw rate of this vehicle by the differential discrete second-order low-pass filter is as follows: Where ω0 = 2πf0, f0 is the cutoff frequency, ζ is the damping coefficient, and T s For sampling time, y n y is the filtered yaw rate at the current moment. n-1 y is the filtered yaw rate from the previous moment. n-2 The yaw rate is the filtered yaw rate of the previous time step, μ. n Let μ be the yaw rate at the current moment. n-1 This represents the yaw rate at the previous moment.

3. The method for determining the relative position of a connected vehicle on a curve according to claim 2, characterized in that, In S3, the center of the vehicle's trajectory lies on the y-axis in the vehicle's coordinate system. If the vehicle turns right, the center is on the negative half-axis of y, with coordinates (0, -r); if the vehicle turns left, the center is on the positive half-axis of y, with coordinates (0, r), where r is the radius of curvature of the vehicle's trajectory at the current moment.

4. The method for determining the relative position of a connected vehicle on a curve according to claim 3, characterized in that, S4 includes the following steps: S41: Calculate the coordinates of the remote vehicle in the vehicle body coordinate system. ; S42: Calculate the coordinates of the intersection point between the center of the line connecting the current trajectories of the distant vehicle and the current vehicle's trajectory, and the current vehicle's trajectory. ; S43: Calculate the distance s from the intersection point coordinates to the vehicle: S44: Calculate the angle between the line connecting the current vehicle to the center of its current trajectory and the line connecting the distant vehicle to the center of the current trajectory. , If θ > Π, then θ = 2Π – θ. If the included angle is greater than the preset included angle threshold, the current driving trajectory of the vehicle will not be adopted; otherwise, the current driving trajectory of the vehicle will be adopted and the next step will be executed.

5. The method for determining the relative position of a connected vehicle on a curve according to claim 4, characterized in that, S5 includes the following steps: S51: Calculate the shortest distance from the distant vehicle to the current vehicle's trajectory: If this vehicle is turning right, then the shortest distance from the distant vehicle to this vehicle at the current moment is: ； If this vehicle is turning left, then the shortest distance from the distant vehicle to this vehicle at the current moment is: ； S52: Lanes are divided based on the shortest distance from the distant vehicle to the current vehicle's trajectory. If this vehicle turns right, then If this vehicle turns left, then Among them, w lane This refers to the lane width, which is 3-4 meters. S53: Determine which lane the distant vehicle is in at the current moment on the road segment in which this vehicle is currently traveling.

Images