A speed planning method and medium for unmanned driving in underground mines based on V2X

Abstract

The present application discloses a speed planning method and medium for unmanned driving in underground mining based on V2X, which relates to the field of path planning, including: receiving the signal light status information and road traffic information broadcast by the roadside unit RSU through the V2X module, obtaining the traffic light status, remaining time and recommended speed of the road, combining the vehicle current position information of the positioning module, the vehicle surrounding environment information of the perception module and the road information of the map module, judging whether there is a signal light and the status of the signal light, detecting whether there is a front vehicle in the lane, matching the front vehicle and the stop line position of the lane, and generating a speed control curve to keep a safe distance from the front vehicle or stop before the stop line by comparing the position of the front vehicle and the stop line position, using the Sigmod speed planning algorithm. In view of the fact that the unmanned driving vehicles in underground mining in the prior art cannot reasonably plan the speed when passing through a signal light intersection due to the lack of traffic light information, the present application reasonably plans the speed in advance when passing through the intersection.

Description

Technical Field

[0001] The present application relates to the field of path planning, and in particular to a V2X-based unmanned driving speed planning method and medium for underground mining. Background Art

[0002] In open-pit mines and underground mines, unmanned trucks, mining vehicles and other large equipment are gradually replacing manual driving to realize the automation of mining and ore transportation operations. Compared with the urban road environment faced by passenger cars, the road environment in mining areas is more complex and changeable, with slopes, curves, mud, dust and other harsh working conditions, which puts higher requirements on the perception, decision-making and control capabilities of the unmanned driving system in underground working conditions.

[0003] In order to ensure the safety and efficiency of unmanned vehicles in mining areas, it is necessary to study key technologies that adapt to mining conditions, such as all-weather environmental perception, high-precision vehicle positioning, complex terrain path planning, vehicle formation control, etc. Among them, the speed planning of vehicles when passing through mining intersections is particularly critical, which directly affects the vehicle's traffic efficiency and fuel economy.

[0004] At present, unmanned vehicles in mining areas mainly use on-board sensors (such as lidar, cameras, etc.) to achieve environmental perception and obstacle detection, and perform local path planning and speed control based on the perception results and preset maps. However, this vehicle-based perception solution is difficult to obtain global intersection traffic information, such as traffic light status, traffic flow, etc., resulting in a lack of reasonable speed planning when vehicles pass through intersections, and prone to unstable behaviors such as sudden braking and acceleration, which reduces traffic efficiency and ride comfort.

[0005] To overcome the above problems, some researchers have begun to explore unmanned driving control methods based on vehicle-road collaboration. By deploying roadside units (RSUs) at intersections, collecting data such as traffic lights and traffic flows and broadcasting them to surrounding vehicles through the V2X network, the vehicles can optimize speed planning and trajectory tracking based on the received intersection traffic information, thereby achieving information interaction and collaborative control between vehicles and traffic infrastructure. The speed planning method based on V2X can significantly improve the smoothness of vehicles passing through intersections.

[0006] However, existing V2X speed planning methods are mainly aimed at urban road traffic, and few studies have paid special attention to the application scenarios in underground mining areas. Compared with urban environments, the traffic at intersections in underground mining areas has the characteristics of low traffic density, sparse traffic lights, and a single type of vehicle. Developing a dedicated V2X speed planning method for unmanned vehicles in underground mining areas can reduce computing overhead and improve real-time performance.

[0007] In addition, in the narrow and winding sections of the underground mining area, in addition to obeying the traffic light instructions, the unmanned vehicle also needs to maintain a safe distance from the vehicle in front of it. In particular, when the vehicle in front is stationary or driving at a low speed, it needs to slow down smoothly or even stop to avoid the risk of collision. Therefore, how to consider the status of the traffic light and the vehicle in front at the same time and formulate a speed planning strategy that takes into account traffic rules and vehicle safety is the key to ensuring smooth and safe unmanned driving in underground mining areas. Summary of the invention

[0008] In view of the problem in the prior art that unmanned vehicles cannot reasonably plan the speed when passing through a traffic light intersection due to lack of traffic light information, the present application provides a speed planning method and medium for unmanned driving in underground mining based on V2X. The signal light status information and road traffic information broadcast by the roadside unit RSU are received through the V2X module to obtain the traffic light status, remaining time and recommended travel speed of the road. The vehicle current position information of the positioning module, the vehicle surrounding environment information of the perception module and the road information of the map module are combined to determine whether there is a traffic light and the status of the traffic light, detect whether there is a leading vehicle in the lane, match the leading vehicle and the stop line position in the lane, and generate a speed control curve for keeping a safe distance from the leading vehicle or stopping before the stop line by comparing the leading vehicle position and the stop line position, using the Sigmod speed planning algorithm. This method can enable unmanned vehicles in underground mining to reasonably plan the speed in advance when passing through an intersection based on real-time traffic light information and the leading vehicle position information.

[0009] The purpose of this application is achieved through the following technical solutions.

[0010] One aspect of the present application provides a speed planning method for unmanned driving in underground mines based on V2X, which is characterized by comprising: S1, receiving and processing the vehicle current position information of the positioning module, the vehicle surrounding environment information of the perception module, the road information of the map module, and the traffic light status information and recommended speed information of the V2X module; S2, judging whether there is a traffic light on the current road section according to the information received by the V2X module in step S1, if there is no traffic light, executing step S9, if there is a traffic light, executing step S3; S3, judging the traffic light status according to the traffic light status information received by the V2X module in step S1; if the traffic light is red or yellow, executing step S4, if the traffic light is green, executing Step S9; S4, based on the vehicle surrounding environment information obtained by the perception module in step S1, determine whether there is a CIPV target on the way in the current lane. If there is a CIPV target, execute step S5; if there is no CIPV target, execute step S12; S5, based on the vehicle current position information obtained by the positioning module in step S1 and the CIPV target position information obtained by the perception module, match the position index of the CIPV target on the vehicle trajectory planned by the unmanned vehicle; S6, based on the vehicle current position information obtained by the positioning module in step S1 and the stop line position information obtained by the map module, match the position index of the midpoint of the stop line on the trajectory; S7, compare the position indexes calculated in step S5 and step S6 If the stop line position index is greater than or equal to the CIPV target position index, then execute step S8, otherwise execute step S14; S8, according to the CIPV target motion state information obtained by the perception module in step S1, determine whether the CIPV target is in motion; if the CIPV target is in motion, use the Sigmod speed planning algorithm to plan a speed sequence that maintains a preset distance with the dynamic CIPV target as the final expected speed sequence of this cycle; if the CIPV target is in a stationary state, use the Sigmod speed planning algorithm to plan a speed sequence that maintains a preset distance before reaching the stationary CIPV target as the final expected speed sequence of this cycle; S9, according to step S2 and step S14, determine whether the CIPV target is in motion; if the CIPV target is in motion, use the Sigmod speed planning algorithm to plan a speed sequence that maintains a preset distance before reaching the stationary CIPV target as the final expected speed sequence of this cycle; S15, according to step S2 and step S16, determine whether the CIPV target is in motion; if the CIPV target is in motion, use the Sigmod speed planning algorithm to plan a speed sequence that maintains a preset distance before reaching the stationary CIPV target as the final expected speed sequence of this cycle; S17, according to step S18, determine whether the CIPV target is in motion; if the CIPV target is in motion, use the Sigmod speed planning algorithm to plan a speed sequence that maintains a preset distance before reaching the stationary CIPV target According to the judgment result of step S3, if there is no traffic light or a green light, step S4 is executed to determine whether there is a CIPV target in the lane; if there is a CIPV target in the lane, step S5 is executed to calculate the CIPV target position index, and step S8 is executed to calculate the speed sequence; if there is no CIPV target in the lane, step S10 is executed; S10, according to the recommended speed information received by the V2X module in step S1, the road speed limit information obtained by the map module and the speed limit of the unmanned vehicle itself, the minimum value of the three is taken as the expected speed; S11, using the Sigmod speed planning algorithm, with the expected speed calculated in step S10 as the target, a smooth speed sequence is planned as the final expected speed sequence of this cycle;S12, according to the judgment result of step 4, if there is no CIPV target in the lane, then execute step S6 to calculate the stop line position index, and execute step S10 to calculate the expected speed, and then execute step S13; S13, estimate whether the traffic light has switched to green when the unmanned vehicle drives to the intersection at the expected speed calculated in step S10; if the traffic light has switched to green, execute step S11 to plan the speed sequence; if the traffic light has not switched to green, execute step S14; S14, use the Sigmod speed planning algorithm to plan the speed sequence for stopping before the stop line at the traffic light intersection as the final expected speed sequence of this cycle. ;

[0011] Among them, the CIPV (Closest In-Path Vehicle) target refers to the vehicle closest to the vehicle on the planned driving path of the unmanned vehicle. The concept of CIPV target originates from the adaptive cruise control (ACC) system, which is used to describe the target vehicle that the vehicle needs to follow or avoid. In the V2X-based mine unmanned driving speed planning method of this application, the identification and tracking of CIPV targets is one of the important inputs of speed planning. Specifically, the perception module (such as laser radar, camera, etc.) of the unmanned vehicle detects and tracks obstacles around the vehicle in real time, and judges whether the obstacle is a vehicle in the lane based on the position, speed, acceleration and other state information of the obstacle. If there is a vehicle in the lane, the vehicle closest to the vehicle is marked as a CIPV target. The real-time position (such as GPS coordinates) of the CIPV target is obtained through the perception module, and it is compared with the current position of the vehicle to calculate the position index of the CIPV target on the planned path of the vehicle. The position index represents the distance of the CIPV target from the starting point of the vehicle's path, which is used for subsequent speed planning. The real-time speed information of the CIPV target is obtained through the perception module to judge whether the CIPV target is in a stationary state or a moving state. If the CIPV target is stationary, it means that there may be a red light or congestion ahead, and the vehicle needs to slow down and stop; if the CIPV target is moving, the vehicle needs to keep a safe distance from it and adjust the vehicle speed according to its speed. According to the position index and motion state of the CIPV target, the Sigmod speed planning algorithm is used to generate a smooth speed control sequence. When the CIPV target is stationary, the goal of the speed sequence is to stop before reaching the CIPV target; when the CIPV target is moving, the goal of the speed sequence is to maintain a preset safe distance from the CIPV target. The generated speed sequence is passed to the lower-level control module for execution as the final expected speed of the unmanned vehicle. When the unmanned vehicle detects the CIPV target and the traffic light at the same time, it is necessary to determine the relative positions of the two. If the traffic light stop line is closer to the vehicle, it will respond to the red light signal first and stop before the stop line; if the CIPV target is closer to the vehicle, it will keep a safe distance from the CIPV target first to avoid collision.

[0012] Furthermore, matching the position index of the CIPV target on the vehicle trajectory planned by the unmanned vehicle in S5 includes: calculating the Euclidean distance from each trajectory point to the CIPV target based on the current position information of the unmanned vehicle obtained by the positioning module and the CIPV target position information obtained by the perception module. , take the Euclidean distance The trajectory point index corresponding to the minimum value is used as the matching position index of the CIPV target on the trajectory, where The calculation formula is:

[0013] { d i = ( x i − x ob ) 2 + ( y i − y ob ) 2 min( d 1 d 2 ... d i ... d n ) i <= n i , n ∈ [ 0 , path _ poin t _ size ] ;

[0014] in: represents the Euclidean distance from the i-th trajectory point to the CIPV target; , represents the coordinates of the i-th trajectory point; , represents the coordinates of the CIPV target; Represents the index value; Indicates the total number of track points of the current track.

[0015] Furthermore, matching the position index of the midpoint of the stop line on the trajectory in step S6 includes: substituting the coordinates of the midpoint of the stop line into the CIPV target coordinates in step S5 according to the current position information of the unmanned vehicle obtained by the positioning module and the stop line position information obtained by the map module, and using the coordinates of the midpoint of the stop line in step S5. The calculation formula is used to calculate the matching position index of the midpoint of the stop line on the trajectory.

[0016] Furthermore, in S8, the Sigmod function is used to generate a speed sequence that maintains a preset distance from the CIPV target. The expression of the Sigmod function is as follows: { x i = s i s end basic i = 1 exp( p b − p a × x i ) desire _ speed i = v c + ( v tar − v c ) × basic i i ∈ [ 0 , path _ poin t _ size ] ;

[0017] Where: Represents the normalized value of the distance from the i-th trajectory point to the center of the rear axle of the unmanned vehicle, bask_link; Indicates the distance from the i-th trajectory point to the center of the rear axle of the unmanned vehicle, bask_link; Indicates the distance from the last trajectory point to the center of the rear axle of the unmanned vehicle, bask_link; Indicates the difference factor between the expected speed and the current speed; Represents the parameters used by the Sigmod speed planner; represents the expected speed corresponding to the i-th trajectory point; Indicates the current speed of the unmanned vehicle; Represents the target vehicle speed used by the Sigmod speed planner.

[0018] Further, in S13, estimating whether the traffic light has switched to green when the unmanned vehicle arrives at the intersection at the expected speed includes: calculating the distance required for the unmanned vehicle to accelerate from the current position to the expected speed and maintain the expected speed to the stop line based on the current position information of the unmanned vehicle obtained by the positioning module, the stop line position information obtained by the map module and the expected speed calculated in step S10. , calculate the distance from the current position to the stop line , obtain the signal light status information from the V2X module to determine the remaining time of the red light , when satisfied Less than or equal to When the driverless car reaches the intersection at the expected speed, the red light has switched to green. The calculation formula is:

[0019] ;

[0020] Among them, when When , it means driving to the stop line; when When , the red light is switched to green light, where: It indicates the sum of the distance from the current position to the target speed and the countdown time of the red light when the target speed is reached; Indicates the distance from the current position of the unmanned vehicle to the stop line; Indicates the time when the red light of the traffic light switches to green; if Less than , it is considered that when the driverless car drives to the intersection at the expected speed, the red light has switched to green.

[0021] Furthermore, the perception module uses at least one of a laser radar, a millimeter-wave radar, and a visual sensor to perceive the vehicle's surrounding environment.

[0022] Furthermore, the positioning module uses at least one of GNSS positioning, IMU inertial navigation positioning and wheel speed odometer positioning to determine the current position of the vehicle.

[0023] Furthermore, the V2X module communicates with the roadside unit RSU through the unmanned vehicle OBU, receives the traffic light status information and road traffic information broadcast by the RSU, and obtains the traffic light status, remaining time and recommended travel speed of the road.

[0024] Another aspect of the present application also provides a computer-readable storage medium, which stores computer instructions. When the computer instructions are executed by a processor, a V2X-based unmanned driving speed planning method for underground mining is implemented in the present application.

[0025] Compared with the prior art, the advantages of this application are:

[0026] The V2X module receives the traffic light status information broadcast by the roadside unit RSU, obtains the traffic light status and remaining time, and combines the positioning information and map information to determine whether there is a traffic light ahead and the current traffic light status. The traffic light status can be known in advance, providing a basis for the speed planning of the unmanned vehicle, which helps the unmanned vehicle to make speed planning in advance according to the traffic light status, ensuring that the unmanned vehicle passes through the intersection safely and smoothly.

[0027] The perception module uses sensors such as lidar, millimeter-wave radar and visual sensors to perceive the vehicle's surrounding environment in real time, determine whether there is a vehicle in front of the lane, and obtain the position and motion status information of the vehicle in front, providing important reference information for the speed planning of the unmanned vehicle, so that the unmanned vehicle can perceive the status of the vehicle in front in time, maintain a safe distance from the vehicle in front, and avoid rear-end collision accidents.

[0028] By comparing the matching position index of the preceding vehicle in this lane and the stop line position index, it is determined whether the preceding vehicle is stopped in front of the stop line, and the stop line is used as an important reference position for speed planning. When the position index of the preceding vehicle is greater than or equal to the stop line position index, that is, the preceding vehicle is stopped behind the stop line, the position of the preceding vehicle is used as the target position for speed planning; when the position index of the preceding vehicle is less than the stop line position index, that is, the stop line is located between the preceding vehicle and this vehicle, the stop line position is used as the target position for speed planning. By introducing the stop line position, it is possible to effectively prevent unmanned vehicles from invading the intersection due to improper parking position of the preceding vehicle.

[0029] For both stationary and moving states of the leading vehicle, the Sigmod function is used to generate a speed control curve that maintains a safe distance from the leading vehicle. When the leading vehicle is stationary, the unmanned vehicle approaches the leading vehicle at a constant speed to a preset distance; when the leading vehicle is moving, the unmanned vehicle and the leading vehicle move together while maintaining a preset distance; when there is no leading vehicle in front, the unmanned vehicle travels at the highest speed allowed by the road, and a smooth speed control curve is generated through the Sigmod function to improve the smoothness and safety of the unmanned vehicle and the leading vehicle following each other.

[0030] By estimating the time required for the unmanned vehicle to reach the stop line at the expected speed and comparing it with the remaining time of the traffic light, it can be determined whether the unmanned vehicle can pass through the intersection within the green light period. This can prevent the unmanned vehicle from forcing its way through the intersection during the red light period due to starting too late or driving too slowly, and ensure that the unmanned vehicle complies with traffic light rules. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] The present application will be further described in the form of exemplary embodiments, which will be described in detail by the accompanying drawings. These embodiments are not restrictive, and in these embodiments, the same number represents the same structure, wherein:

[0032] Figure 1 is an exemplary flow chart of a V2X-based unmanned driving speed planning method for underground mining according to some embodiments of the present application;

[0033] Figure 2 It is a schematic diagram of the hardware connection structure shown in some embodiments of the present application. DETAILED DESCRIPTION

[0034] The method and system provided in the embodiments of the present application are described in detail below with reference to the accompanying drawings.

[0035] Figure 1It is an exemplary flow chart of a speed planning method for unmanned driving in underground mines based on V2X according to some embodiments of the present application, including: S1, receiving and processing the vehicle current position information of the positioning module, the vehicle surrounding environment information of the perception module, the road information of the map module, and the traffic light status information and recommended speed information of the V2X module; S2, judging whether there is a traffic light on the current road section according to the information received by the V2X module in step S1, if there is no traffic light, executing step S9, if there is a traffic light, executing step S3; S3, judging the traffic light status according to the traffic light status information received by the V2X module in step S1; if the traffic light is red or yellow, executing step S4, if the traffic light is green , then execute step S9; S4, according to the vehicle surrounding environment information obtained by the perception module in step S1, determine whether there is a CIPV target on the way in the current lane. If there is a CIPV target, execute step S5, if there is no CIPV target, execute step S12; S5, according to the vehicle current position information obtained by the positioning module in step S1 and the CIPV target position information obtained by the perception module, match the position index of the CIPV target on the vehicle trajectory planned by the unmanned vehicle; S6, according to the vehicle current position information obtained by the positioning module in step S1 and the stop line position information obtained by the map module, match the position index of the midpoint of the stop line on the trajectory; S7, compare the calculations of step S5 and step S6 If the stop line position index is greater than or equal to the CIPV target position index, then execute step S8, otherwise execute step S14; S8, according to the CIPV target motion state information obtained by the perception module in step S1, determine whether the CIPV target is in motion; if the CIPV target is in motion, use the Sigmod speed planning algorithm to plan a speed sequence that maintains a preset distance from the dynamic CIPV target as the final expected speed sequence of this cycle; if the CIPV target is in a stationary state, use the Sigmod speed planning algorithm to plan a speed sequence that maintains a preset distance before reaching the stationary CIPV target as the final expected speed sequence of this cycle; S9, according to step S2 and the judgment result of step S3, if there is no traffic light or there is a green light, then execute step S4 to determine whether there is a CIPV target in the lane; if there is a CIPV target in the lane, execute step S5 to calculate the CIPV target position index, and execute step S8 to calculate the speed sequence; if there is no CIPV target in the lane, execute step S10; S10, according to the recommended speed information received by the V2X module in step S1, the road speed limit information obtained by the map module and the speed limit of the unmanned vehicle itself, take the minimum value of the three as the expected speed; S11, use the Sigmod speed planning algorithm, take the expected speed calculated in step S10 as the target, and plan a smooth speed sequence as the final expected speed sequence of this cycle;S12, according to the judgment result of step 4, if there is no CIPV target in the lane, then execute step S6 to calculate the stop line position index, and execute step S10 to calculate the expected speed, and then execute step S13; S13, estimate whether the traffic light has switched to green when the unmanned vehicle drives to the intersection at the expected speed calculated in step S10; if the traffic light has switched to green, execute step S11 to plan the speed sequence; if the traffic light has not switched to green, execute step S14; S14, use the Sigmod speed planning algorithm to plan the speed sequence for stopping before the stop line at the traffic light intersection as the final expected speed sequence of this cycle. ;

[0036] Figure 2It is a schematic diagram of the hardware connection structure shown in some embodiments of the present application. Perception module: LiDAR: One or more LiDARs are installed on the top of the vehicle. By emitting laser beams and receiving reflected signals, a 3D point cloud map around the vehicle is generated. By segmenting, clustering, classifying, and other processing of the point cloud map, the position, size, speed, and other information of the obstacle can be detected. Millimeter wave radar: Multiple millimeter wave radars are installed on the front and rear bumpers of the vehicle. By emitting and receiving high-frequency electromagnetic waves, the distance, relative speed, and azimuth between the vehicle and the obstacle are measured. Millimeter wave radar is less affected by weather and can make up for the shortcomings of LiDAR in rainy and foggy weather. Visual sensor: Multiple high-definition cameras are installed behind the front windshield of the vehicle. Through image processing algorithms such as target detection, semantic segmentation, and optical flow analysis, the type of obstacles (pedestrians, vehicles, traffic signs, etc.) and motion state (stationary, same direction, lateral, etc.) are judged. Sensor fusion: The perception module feeds the data of LiDAR, millimeter wave radar, and visual sensor into the fusion algorithm, and generates a unified environmental perception model through methods such as spatiotemporal registration, tracking association, and decision layer fusion. The fused perception results are more accurate and robust. The perception module needs to output the perception results containing CIPV target information to provide the necessary decision-making basis for the subsequent steps. Track the CIPV target and obtain its status information in real time, mainly including: CIPV target ID: used to uniquely identify a CIPV target, usually using a unique number generated by the target detection and tracking algorithm. CIPV target type: indicates the category to which the CIPV target belongs, such as cars, trucks, buses, etc. Obtained by the target recognition algorithm. CIPV target position coordinates (x, y, z): indicates the spatial position of the CIPV target in the vehicle coordinate system. Among them, x represents the front-to-back (longitudinal) distance, y represents the left-to-right (lateral) distance, and z represents the vertical height. Obtained by the sensor fusion positioning algorithm. CIPV target speed (vx, vy): indicates the movement speed of the CIPV target in the vehicle coordinate system. Among them, vx represents the longitudinal speed and vy represents the lateral speed. Obtained by the target tracking algorithm. CIPV target acceleration (ax, ay): indicates the movement acceleration of the CIPV target in the vehicle coordinate system, which is similar to the speed. Obtained by the target tracking algorithm. CIPV target movement direction and steering angular velocity: indicates the movement direction and steering change rate of the CIPV target. Obtained through target tracking algorithm. CIPV target size (L, W, H): Indicates the length, width, height and other geometric parameters of the CIPV target. Obtained through sensor measurement or querying the target type database.

[0037] The positioning module includes: GNSS positioning: one or more GNSS receiving antennas are installed on the top of the vehicle to calculate the absolute position (latitude, longitude and elevation) of the vehicle by receiving the L1 / L2 frequency navigation signals of GPS, Beidou, GLONASS and other satellites. RTK (Real-Time Kinematic) technology is used to communicate with the ground base station and receive differential correction data, which can improve the GNSS positioning accuracy to the centimeter level. In environments where GNSS signals are blocked or interfered with (such as tunnels, under elevated roads, etc.), other positioning methods such as inertial navigation and wheel speed meter are used as supplements. IMU inertial navigation positioning: high-precision, low-drift fiber optic gyro IMU is selected as the inertial navigation unit. By integrating the data of the three-axis accelerometer and the three-axis gyroscope, the position, speed and attitude change of the vehicle relative to the starting point are calculated. The IMU is installed at the center of mass of the vehicle and aligned with the vehicle coordinate system to reduce the influence of installation errors and lever arm effects. Zero-speed correction, static correction and other technologies are used to regularly correct the zero bias and scale factor errors of the IMU to suppress the accumulation of IMU errors. Wheel speed odometer positioning: A high-resolution wheel speed sensor (such as a Hall sensor) is installed on each wheel of the vehicle. By measuring the wheel speed and combining the wheel radius parameter, the vehicle's travel distance and speed are estimated. A high-precision steering angle sensor is installed on the front wheel to measure the steering angle of the front wheel in real time to infer the vehicle's heading change. Through the mileage calculation method (such as the dead algorithm), the wheel speed and steering angle data are integrated to obtain the vehicle's 2D displacement and heading angle relative to the starting point. The wheel speed odometer is easily disturbed by factors such as wheel slip and tire pressure changes, and needs to be complemented by sensors such as IMU and GNSS. Positioning fusion: The data of GNSS, IMU and wheel speed odometer are input into a centralized sensor fusion algorithm, such as the extended Kalman filter (EKF) and the unscented Kalman filter (UKF). The fusion algorithm takes the IMU as the core, takes the GNSS position and wheel speed odometer displacement as the observed quantity, dynamically estimates the error state of the IMU (such as zero bias, scale factor, etc.), and corrects the IMU integration result. When the GNSS signal is good, the fusion algorithm trusts the GNSS position more and uses the GNSS information to correct the errors of the IMU and the wheel speed meter; when the GNSS signal is missing, the fusion algorithm relies on the calculation of the IMU and the wheel speed meter to maintain the continuity of the positioning output. The state vector of the fusion algorithm generally includes the vehicle position, speed, attitude, IMU error parameters, etc., and the observation vector includes the GNSS position, wheel speed meter displacement, etc. The optimal estimation of the positioning result is achieved through the recursion of the state equation and the observation equation. The output frequency of the fusion positioning can reach 50-100Hz, and the positioning accuracy is at the centimeter level, which can meet the needs of unmanned driving. The positioning module needs to obtain the current position and attitude information of the unmanned vehicle to provide a reference for subsequent trajectory matching and speed planning.The positioning algorithm outputs the current position and attitude information of the vehicle, mainly including: Current position (x, y): represents the plane position coordinates of the vehicle in the global coordinate system. Among them, x represents the east-west direction (longitude) coordinate, and y represents the north-south direction (latitude) coordinate. Coordinate systems such as UTM and Gauss-Krüger are usually used. Current height z: represents the vertical height coordinates of the vehicle in the global coordinate system. Ellipsoid height or normal height is usually used. Roll angle roll: represents the rotation angle of the vehicle around the driving direction axis (x-axis). When the angle is positive, the vehicle tilts to the right. Pitch angle pitch: represents the rotation angle of the vehicle around the lateral axis (y-axis). When the angle is positive, the vehicle tilts upward. Yaw angle yaw: represents the rotation angle of the vehicle around the vertical axis (z-axis), also known as the heading angle. When the angle is positive, the vehicle deflects to the left. Current speed (vx, vy): represents the plane motion speed of the vehicle in the global coordinate system. Among them, vx represents the east-west speed, and vy represents the north-south speed. Current acceleration (ax, ay): represents the acceleration of the vehicle's plane motion in the global coordinate system, similar to velocity. Current angular velocity (ωx, ωy, ωz): represents the angular velocity of the vehicle's rotation around the three coordinate axes. Among them, ωx represents the rolling angular velocity, ωy represents the pitch angular velocity, and ωz represents the yaw angular velocity.

[0038] Map module: The map module stores a high-precision map with environmental elements such as roads, lane lines, traffic signs, and buildings marked. The map module receives the output of the positioning module and queries the relative position of the vehicle's current position in the map and the semantic information of the road. The map module can also provide services such as global path planning, lane-level navigation, and traffic rule judgment. The V2X module includes: V2X communication unit: Dedicated short-range communication technologies such as DSRC (Dedicated Short Range Communication) or C-V2X (Cellular V2X) are used to achieve direct communication between vehicles and roadside facilities and other vehicles. DSRC is based on the IEEE 802.11p standard and operates in the 5.9GHz frequency band. The communication range can reach 300-1000m, and the communication delay is less than 100ms, which is suitable for safety applications. C-V2X is based on cellular network technology (such as 4G / 5G) and realizes V2X communication through the Uu interface (cellular link) and PC5 interface (direct link). The communication range and capacity are larger and can support more types of applications. The map module needs to output lane and stop line information related to the current vehicle position. According to the current position and posture of the vehicle, query the lane where the vehicle is located in the map. The specific steps are as follows: Project the vehicle position coordinates (x, y) to the map coordinate system. According to the projected position and posture of the vehicle, search for one or several lane centerlines closest to the vehicle. Calculate the lateral distance and heading deviation from the vehicle projection point to the centerline of each lane. Select the lane with the smallest lateral distance and heading deviation within the threshold as the candidate lane. If there are multiple candidate lanes, then optimize according to factors such as vehicle trajectory and speed to obtain the final current lane. According to the current lane obtained by matching, output the following information: Current lane ID: ID number used to uniquely identify a lane. Current lane type: indicates the functional attributes of the lane, such as through lane, left turn lane, right turn lane, tidal lane, etc. Current lane speed limit: indicates the maximum speed limit of the lane, in km / h or m / s. If there is no clear mark on the map, it can be estimated based on factors such as road grade and area type. Current lane geometry: indicates geometric parameters such as the centerline and boundary line of the lane, usually expressed in the form of a discrete point sequence. Check whether there is a stop line within a certain distance range (such as 50-100m) in front of the current lane. If it exists, the following information is output: Stop line ID: The ID number used to uniquely identify a stop line. Stop line position (x, y, z): Indicates the three-dimensional spatial position of the center point of the stop line in the map coordinate system. Stop line geometry: Indicates the geometric parameters of the stop line, such as width and line type. Traffic light ID associated with the stop line: Indicates the ID number of the traffic light group to which the stop line belongs.

[0039] The V2X communication unit includes hardware modules such as RF transceiver, baseband processor, security chip, and software modules such as V2X protocol stack and application layer software. Information reception and processing: The V2X module receives infrastructure information such as signal light status, traffic signs, road construction, etc. from the roadside unit (RSU) through V2I communication, and assists the unmanned vehicle in making decisions and planning under well working conditions. The V2X module receives motion status information such as position, speed, acceleration, steering, and other motion status information from other vehicles through V2V communication, as well as behavioral intention information such as braking, lane change, and emergency obstacle avoidance. The V2X module receives global information such as traffic flow, road network events, and weather conditions from the traffic management center through V2N (Vehicle-to-Network) communication, and optimizes the navigation path of the unmanned vehicle under well working conditions. The V2X module performs pre-processing such as decoding, parsing, and filtering on the massive heterogeneous data received, extracts structured information that is valuable to unmanned driving, and performs time-space synchronization and coordinate conversion. The V2X module integrates the processed V2X information with the local information perceived by the on-board sensors to build a more comprehensive and longer-range environmental perception model, reducing the blind spots and misjudgments of a single sensor. Information broadcast and interaction: The V2X module periodically collects the self-status information (such as position, speed, heading, etc.) of the unmanned vehicle under well conditions, and broadcasts it to surrounding vehicles through V2V communication, so that other vehicles can perceive the movement status of the vehicle. When performing operations such as changing lanes and overtaking, the V2X module sends behavioral intention information to related vehicles in advance, negotiates and optimizes decision-making plans, and improves collaborative efficiency and safety. When the V2X module detects an emergency situation (such as collision warning, road obstacles, etc.), it immediately broadcasts event information to surrounding vehicles and the traffic control center to trigger a regional collaborative emergency mechanism. The V2X module responds to requests from other vehicles or the traffic control center, provides information such as the vehicle's sensor data, decision logs, and participates in tasks such as traffic accident responsibility determination and road condition analysis. The V2X module ensures the confidentiality, integrity, and verifiability of information interaction through security mechanisms such as authentication and encryption to prevent information leakage and tampering.

[0040] The map module has built-in high-precision map information, including the geometry of the road, lane lines, traffic signs, traffic light positions, etc. The map module can provide lane-level map data matching the current position of the vehicle, providing a reference for unmanned driving decisions. The V2X module includes an on-board unit OBU and a roadside unit MEC+RSU, which realizes vehicle-road collaboration through wireless communication technologies such as dedicated short-range communication DSRC or cellular vehicle networking C-V2X. The roadside unit RSU is installed on a signal light pole or on the side of the road. It collects real-time traffic flow information and signal light status on the road through sensors and sends them to the OBU through broadcast or on-demand. The on-board unit OBU receives the signal light information package of the RSU and parses the traffic light status and remaining time of the signal light. The OBU can also receive road traffic information sent by the RSU, such as road congestion, road section speed limit, recommended speed, etc. Integrated in the autonomous driving controller, the autonomous driving controller is connected to the wire-controlled mining truck through the auxiliary controller. The autonomous driving controller receives data collected by the camera, lidar and millimeter-wave radar, and receives data from the OBU and MEC+RSU. Among them, MEC is the abbreviation of Multi-access Edge Computing, which means multi-access edge computing. It is a network architecture concept that aims to deploy computing and storage resources at the edge of the network close to the user side to provide low-latency, high-bandwidth, and real-time services. In the V2X scenario, MEC is usually deployed together with the road side unit RSU (Road Side Unit) to form an edge computing node.

[0041] The decision-making and planning module of the unmanned driving system in the well working condition receives the vehicle current position information of the positioning module, the obstacle information around the vehicle fused by the perception module, the lane-level map information of the map module, and the signal light status and traffic flow recommended speed information of the V2X module through the CAN bus, comprehensively analyzes the driving environment of the unmanned vehicle under the well working condition, and formulates a local path and speed planning strategy. In step S2, after the V2X module of the unmanned vehicle receives the signal light status information and recommended speed information broadcast by the roadside unit RSU through the OBU, it needs to determine whether there is a traffic light in the road ahead. In specific implementation, the RSU can include the location information of the traffic light (such as GPS coordinates) in the broadcast information. After receiving the location information, the OBU matches it with the current location information of the vehicle and the map information, and determines whether the distance between the current location of the vehicle and the traffic light is within the set threshold range. If the distance is less than the threshold, it is considered that there is a traffic light in the road ahead, and step S3 is executed; otherwise, it is considered that there is no traffic light in the road ahead, and step S9 is executed.

[0042] In step S3, if step S2 determines that there is a traffic light on the road ahead, the V2X module determines whether the current traffic light is in a red, yellow or green state based on the traffic light status information sent by the RSU. Generally, the traffic light status information sent by the RSU includes the red, yellow and green light status and remaining time of each traffic light. After receiving the information, the OBU can determine the status of the current traffic light based on the light color status when the remaining time is greater than 0. If the current traffic light is red or yellow, execute step S4; if it is green, execute step S9.

[0043] In step S4, if step S3 determines that the current traffic light is in red or yellow state, the unmanned vehicle in the well working condition needs to slow down and stop or wait for the green light. At this time, it is also necessary to determine whether there are other vehicles in front of the lane to avoid rear-end collision. In specific implementation, the perception module fuses the data collected by the laser radar, millimeter wave radar and visual sensor, identifies the obstacles in the lane, and determines the type of obstacles according to the position, speed, acceleration and other information of the obstacles, and obtains the CIPV (Closest In-Path Vehicle) of the nearest front vehicle target in the lane. If there is a CIPV target in the lane, step S5 is executed to calculate the relative position and speed difference between the vehicle and the CIPV, and generate a speed curve that maintains a safe distance from the CIPV; if there is no CIPV target in the lane, step S12 is executed to calculate the speed curve to the stop line.

[0044] S5, according to the vehicle current position information obtained by the positioning module in step S1 and the CIPV target position information obtained by the perception module, match the position index of the CIPV target on the vehicle trajectory planned by the unmanned vehicle; the positioning module obtains the current position coordinates of the unmanned vehicle through multi-sensor fusion The perception module obtains the location coordinates of the CIPV target through sensor data processing and target recognition. .

[0045] The reference trajectory of the unmanned vehicle is composed of a series of trajectory points where i is the index of the trajectory point, ranging from [0, N-1], and N is the total number of trajectory points. The reference trajectory is generally given by the decision planning module, or directly taken from the discrete points of the lane centerline in the map module.

[0046] Traverse each trajectory point of the reference trajectory and calculate the trajectory point To CIPV target point Euclidean distance : All distances obtained in step 4 Find the minimum value in , and its corresponding trajectory point index i is the matching position index of the CIPV target on the reference trajectory: , i=0, 1, ..., N-1, where argmin represents the variable value corresponding to the minimum value. The CIPV target matching position index obtained in step 5 is Saved for subsequent vehicle speed planning, the above calculation process can be implemented in the decision-making planning module of the well-working unmanned driving system. The decision-making planning module subscribes to the vehicle position information of the positioning module, subscribes to the obstacle information output by the perception module, and updates the reference trajectory data in time, so that the CIPV matching position can be calculated once in each control cycle. Matching the position of the CIPV target on the reference trajectory can, on the one hand, determine whether the CIPV target is within the driving path of the unmanned vehicle under well conditions; on the other hand, knowing the position of the CIPV target on the reference trajectory, the unmanned vehicle under well conditions can plan its own speed curve relative to the position of the CIPV target according to constraints such as safety distance, and pass through the intersection in a safe and smooth manner such as following or parking.

[0047] S6, according to the vehicle current position information obtained by the positioning module in step S1 and the stop line position information obtained by the map module, match the position index of the midpoint of the stop line on the trajectory; the positioning module obtains the current position coordinates of the unmanned vehicle through multi-sensor fusion The map module stores high-precision map information, which includes the stop line position coordinates of each intersection. Generally, the stop line is a line segment across the lane, which can be obtained by the starting coordinates of the line segment. and the end point coordinates To indicate that the map module matches the vehicle's current position to the nearest stop line ahead, and sends its starting and ending coordinates to the decision-making planning module.

[0048] After receiving the coordinates of the start and end points of the stop line, the decision-making planning module first calculates the coordinates of the midpoint of the stop line : , .

[0049] The reference trajectory of the unmanned vehicle is composed of a series of trajectory points where i is the index of the trajectory point, ranging from [0, N-1], and N is the total number of trajectory points. The reference trajectory is generally given by the decision planning module, or directly taken from the discrete points of the lane centerline in the map module. Substitute the CIPV target coordinates in step S5 , using the same method as step S5, calculate the Euclidean distance from each trajectory point to the midpoint of the stop line : , i=0,1,...,N-1 where all the distances obtained in step 5 are Find the minimum value in , and its corresponding trajectory point index i is the matching position index of the midpoint of the stop line on the reference trajectory: , i=0, 1,..., N-1, where argmin represents the variable value corresponding to the minimum value.

[0050] Match the stop line midpoint obtained in step 6 to the position index Save it for subsequent vehicle speed planning. The above calculation process can be implemented in the decision-making planning module of the well-working unmanned driving system. The decision-making planning module subscribes to the vehicle position information of the positioning module, accesses the map module to obtain the stop line position information, and updates the reference trajectory data in time, so that the midpoint matching position of the stop line can be calculated once in each control cycle. The purpose of matching the position of the midpoint of the stop line on the reference trajectory is to determine the position where the unmanned vehicle under well conditions needs to stop and wait. Only by knowing the position of the stop line can the current relative position and distance between the unmanned vehicle under well conditions and the stop line be determined, so as to plan a suitable deceleration or parking curve. At the same time, when passing through an intersection, the stop line can be used as an important waypoint for the unmanned vehicle under well conditions. The unmanned vehicle under well conditions needs to slow down and observe the intersection before passing the stop line to ensure safety through the intersection.

[0051] In step S7, it is necessary to compare the CIPV target matching position index calculated in step S5 with the stop line midpoint matching position index calculated in step S6 to determine whether the unmanned vehicle under well working conditions should slow down and stop to let the CIPV in front pass the stop line first. Step S5 calculates the matching position index of the CIPV target on the reference trajectory. Step S6 calculates the matching position index of the midpoint of the stop line on the reference trajectory , these two index values ​​represent the projection positions of the CIPV target point and the midpoint of the stop line on the reference trajectory. The larger the index value, the farther it is from the current position of the vehicle.

[0052] Compare and The size relationship: If , indicating that the stop line is in front of the CIPV target, that is, the CIPV target vehicle has not crossed the stop line. At this time, the unmanned vehicle under the well working condition should slow down and stop, waiting for the front vehicle to pass the intersection first, and execute step S8. , indicating that the stop line is behind the CIPV target, that is, the CIPV target vehicle has crossed the stop line. At this time, the unmanned vehicle under the well working condition does not need to stop and wait, but can continue to follow the vehicle and execute step S14, passing the comparison result to step S8 or step S14 for corresponding speed planning.

[0053] S8, according to the CIPV target motion state information obtained by the perception module in step S1, determine whether the CIPV target is in motion; if the CIPV target is in motion, use the Sigmod speed planning algorithm to plan a speed sequence that keeps a preset distance from the dynamic CIPV target as the final expected speed sequence of this cycle; if the CIPV target is in a stationary state, use the Sigmod speed planning algorithm to plan a speed sequence that keeps a preset distance before reaching the stationary CIPV target as the final expected speed sequence of this cycle; in S8, use the Sigmod function to generate a speed sequence that keeps a preset distance from the CIPV target, and the Sigmod function expression is as follows:

[0054] { x i = s i s end basic i = 1 exp( p b − p a × x i ) desire _ speed i = v c + ( v tar − v c ) × basic i i ∈ [ 0 , path _ poin t _ size ] ;

[0055] Where: It represents the normalized value of the distance from the i-th trajectory point to the center of the rear axle of the unmanned vehicle under the well condition, bask_link; represents the distance from the i-th trajectory point to the center of the rear axle of the unmanned vehicle under the well condition, bask_link; Indicates the distance from the last trajectory point to the center of the rear axle of the unmanned vehicle in the well condition, bask_link; Indicates the difference factor between the expected speed and the current speed; Represents the parameters used by the Sigmod speed planner; represents the expected speed corresponding to the i-th trajectory point; Indicates the current speed of the unmanned vehicle under well conditions; Represents the target vehicle speed used by the Sigmod speed planner. Through Sigmod speed planning, the unmanned vehicle under well working conditions can generate a smooth deceleration or following curve according to the relative position with the front vehicle. When the front vehicle is stationary, the unmanned vehicle under well working conditions will decelerate to stop; when the front vehicle is moving, the unmanned vehicle under well working conditions will keep a safe distance from it. This speed planning method can take into account both driving safety and riding comfort.

[0056] In step S9, the unmanned vehicle under well conditions needs to comprehensively consider the status of the signal light and the position of the vehicle in front to decide whether to slow down or stop. According to the judgment result of step S2, determine whether there is a signal light in the road section ahead of the unmanned vehicle under well conditions: if there is no signal light, execute step S4; if there is a signal light, continue to execute step 2. According to the judgment result of step S3, determine the color status of the signal light: if the signal light is green, execute step S4; if the signal light is red or yellow, execute step S4. Execute step S4 to determine whether there is a CIPV target of the vehicle in front in the lane where the unmanned vehicle under well conditions is located: if there is a CIPV target, execute step 5; if there is no CIPV target, execute step 10. If there is a CIPV target in the lane, execute step S5 to calculate the matching position index of the CIPV target on the reference trajectory. . Then execute step S8, use the Sigmod function to plan a speed curve that maintains a safe distance from the CIPV target, and obtain the expected speed sequence for this cycle. If there is no CIPV target in the lane, execute step S10, and use the dynamic programming algorithm to calculate the optimal speed sequence through the intersection. It can be seen that the execution logic of step S9 is divided into two branches: one is that the traffic light is green or there is no traffic light, at this time the unmanned vehicle under the well condition can pass; the other is that the traffic light is red or yellow, at this time the unmanned vehicle under the well condition needs to stop and wait. In each branch, different speed planning methods are selected according to the presence or absence of the CIPV target.

[0057] When there is a CIPV target, the speed planning of the unmanned vehicle under the well working condition is mainly to follow the vehicle, and the goal is to keep a safe distance from the vehicle in front. At this time, the Sigmod function is used to generate a smooth following speed curve, which can avoid rear-end collisions and ensure riding comfort. When there is no CIPV target, the speed planning of the unmanned vehicle under the well working condition is mainly to pass through the intersection, and the goal is to pass through the intersection safely at a faster speed. At this time, the dynamic programming algorithm is used to solve the optimal speed sequence, and the travel time is minimized under the constraints of safety and comfort. Through the comprehensive judgment of step S9, the unmanned vehicle under the well working condition can autonomously select the appropriate speed control strategy according to the traffic lights and the position of the vehicle in front, so as to improve the safety and efficiency of intersection traffic.

[0058] In step S10, the unmanned vehicle under well working conditions needs to comprehensively consider various speed limits and determine a safe, legal and efficient expected speed; the V2X module receives the recommended speed information broadcast by the roadside unit (RSU) or other vehicles through the Internet of Vehicles. The recommended speed is usually generated dynamically based on factors such as road conditions, weather, and traffic flow, and represents the recommended speed for the current road section.

[0059] The map module queries the high-precision map database based on the current position of the unmanned vehicle under well conditions to obtain the road speed limit information of the current section. The speed limit information is usually set by the traffic management department and represents the maximum speed of the current road section. The well-condition unmanned driving system sets a maximum speed limit for the vehicle itself based on the vehicle dynamics parameters and safety constraints. , which is usually lower than the physical speed limit of the vehicle to ensure that the unmanned vehicle in well conditions can drive safely under various conditions.

[0060] Take the minimum value of the above three speed values ​​as the expected speed of the unmanned vehicle in the current section under well conditions : Where min represents the minimum value function. Passed to the speed planning module as the target speed of the dynamic planning algorithm. The above calculation process can be implemented in the decision-making planning module of the mine working condition unmanned driving system. The decision-making planning module subscribes to the recommended speed information of the V2X module, accesses the map module to obtain the road speed limit information, and reads the vehicle's own speed limit parameters from the configuration file to calculate the expected speed. By integrating speed limits from multiple sources, the unmanned vehicle under the mine working condition can adaptively adjust its own speed and drive safely, compliantly and efficiently under different roads and traffic environments. Among them, the recommended speed reflects the dynamic guidance of the speed by the traffic management department, which helps to improve traffic efficiency and safety; the road speed limit reflects the mandatory constraints on the speed of the traffic regulations, ensuring the legality of the unmanned vehicle under the mine working condition; and the vehicle's own speed limit is the bottom line of the safety of the unmanned vehicle under the mine working condition, preventing the unmanned vehicle under the mine working condition from losing control under extreme conditions.

[0061] In step S11, the unmanned vehicle under well working condition uses the Sigmod function to convert the expected vehicle speed obtained in step S10 into a smooth speed curve as the speed control target of this cycle; assuming that the expected vehicle speed calculated by the unmanned vehicle under well working condition in step S10 is , the current vehicle speed is .

[0062] For each trajectory point i on the reference trajectory, calculate its normalized position on the trajectory : ,in, is the distance from trajectory point i to the center of the rear axle of the unmanned vehicle under well conditions, is the distance from the end point of the trajectory to the center of the rear axle of the unmanned vehicle under well conditions. Use the Sigmod function to calculate the expected speed corresponding to each trajectory point : , where k and a are the parameters of the Sigmod function, which control the smoothness of the speed change. As the final desired speed sequence of this cycle, it is used for subsequent speed tracking control.

[0063] The core idea of ​​Sigmod speed planning is to calculate the speed at the starting point and the current speed. Approaching, vehicle speed at the end point and target speed Approaching, with a smooth transition in the middle section, by adjusting the two parameters k and a, the steepness of the speed change and the starting position of the change can be controlled. Sigmod function is a commonly used speed planning method, which is applicable to various road scenarios. On a road without obstacles, the Sigmod function can be used to smoothly accelerate the unmanned vehicle under well conditions to the desired speed; on a road with obstacles, the Sigmod function can be used to smoothly decelerate the unmanned vehicle under well conditions to a safe speed.

[0064] In step S12, the unmanned vehicle under the well working condition needs to plan a smooth speed curve according to the stop line position and the expected speed in the absence of a leading vehicle, so that the vehicle can pass the intersection safely and comfortably; according to the judgment result of step S4, if there is no CIPV target in the lane where the unmanned vehicle under the well working condition is located, step S6 is executed to calculate the matching position index of the midpoint of the stop line on the reference trajectory .

[0065] Execute step S10 to calculate the expected speed of the unmanned vehicle on the current road section under the well working condition The expected speed takes into account the recommended speed of the V2X module, the road speed limit of the map module, and the speed limit of the vehicle itself. Step S13 is executed, using a dynamic programming algorithm to index the stop line position. and expected speed As a constraint, a speed curve is planned from the current position to the stop line.

[0066] The goal of the dynamic programming algorithm is to minimize the acceleration change of the entire speed curve while satisfying the following constraints: The starting speed of the speed curve is equal to the current speed of the unmanned vehicle under well conditions ; The end speed of the speed curve is equal to the expected vehicle speed ; The end point of the speed curve is equal to the stop line position ; The speed at any point on the speed curve does not exceed the road speed limit ; The acceleration at any point on the speed curve does not exceed the comfortable acceleration ; The acceleration change rate at any point on the speed curve does not exceed the comfortable acceleration change rate . The dynamic programming algorithm divides the entire speed curve into several discrete time steps, each time step corresponds to a speed variable. By optimizing these speed variables, an optimal speed curve that meets the constraints can be obtained. The speed curve obtained in step 5 is used as the expected speed sequence of this cycle for subsequent speed tracking control. Unlike Sigmod speed planning, dynamic programming speed planning can consider multiple constraints at the same time, such as speed limit, acceleration limit, following distance limit, etc., so it can generate a safer, more comfortable and efficient speed curve.

[0067] The core of dynamic programming speed planning is to discretize the continuous speed curve and transform it into a multi-stage decision problem. Each stage corresponds to a time step, and each decision corresponds to a speed variable. Through reverse recursion, the optimal solution of the entire speed curve can be solved under the premise of satisfying the constraints. In unmanned driving, dynamic programming speed planning is usually used in scenes such as intersection passing, lane change, overtaking, etc., which can enable unmanned vehicles under well conditions to drive smoothly and efficiently in complex traffic environments. Compared with other speed planning methods, dynamic programming speed planning has a large amount of calculation and poor real-time performance, so it is necessary to reserve sufficient calculation time in the decision planning module.

[0068] S13, estimating whether the traffic light has switched to green when the unmanned vehicle arrives at the intersection at the expected speed calculated in step S10; if the traffic light has switched to green, executing step S11 to plan the speed sequence; if the traffic light has not switched to green, executing step S14; based on the current position information of the unmanned vehicle obtained by the positioning module, the stop line position information obtained by the map module and the expected speed calculated in step S10, calculating the distance required for the unmanned vehicle under well working conditions to accelerate from the current position to the expected speed and maintain the expected speed to the stop line , calculate the distance from the current position to the stop line , obtain the signal light status information from the V2X module to determine the remaining time tred of the red light. Less than or equal to When the unmanned vehicle under the well working condition drives to the intersection at the expected speed, the red light has switched to green, among which, The calculation formula is: , among which, when When , it means driving to the stop line; when When , the red light is switched to green light, where: It indicates the sum of the distance from the current position to the target speed and the countdown time of the red light when the target speed is reached; It indicates the distance from the current position to the stop line of the unmanned vehicle in the well working condition; Indicates the time when the red light of the traffic light switches to green; if Less than , it is considered that when the unmanned vehicle under well working conditions drives to the intersection at the expected speed, the red light has switched to green. This decision-making method based on spatiotemporal prediction can significantly improve the efficiency of unmanned vehicles under well working conditions in passing through signal light intersections. Traditional unmanned vehicles under well working conditions usually slow down and stop directly when detecting a red light, even if the red light is about to switch to green. Step S13 allows the unmanned vehicle under well working conditions to accelerate in advance when the red light is about to end, so that it can pass through the intersection as soon as the green light comes on, reducing unnecessary stopping and waiting.

[0069] In step S14, the unmanned vehicle under the well working condition needs to plan a smooth deceleration curve under the red light state so that the vehicle stops just when it reaches the stop line. Assume that the current speed of the unmanned vehicle under the well working condition is , the stop line position index is , for each trajectory point i on the reference trajectory, calculate its normalized position on the trajectory : ,in, is the distance from trajectory point i to the center of the rear axle of the unmanned vehicle under well conditions, It is the distance from the stop line to the center of the rear axle of the unmanned vehicle under well working conditions.

[0070] Use the Sigmod function to calculate the expected speed corresponding to each trajectory point : , where k and a are the parameters of the Sigmod function, controlling the smoothness of the speed change. In particular, for the trajectory point at the stop line position, its expected speed is set to 0: v [ index _ stop ] = 0 . The series of expected speeds obtained in steps 3 and 4 are As the final expected speed sequence of this cycle, it is used for subsequent speed tracking control. Similar to the Sigmod speed planning of step S11, the Sigmod speed planning of step S14 is also to generate a smooth speed curve so that the unmanned vehicle under well conditions can comfortably decelerate to the stop line. The difference is that the speed curve of step S14 must end with 0 speed to ensure that the unmanned vehicle under well conditions stops completely before the stop line. In addition to the Sigmod function, other forms of deceleration curves can also be used, such as polynomial curves, exponential curves, etc. As long as the requirements of starting speed, terminal speed and smoothness are met, they can be used as deceleration control strategies for unmanned vehicles under well conditions.

Claims

1. A speed planning method for unmanned driving in underground mines based on V2X, characterized in that: include: S1, receiving and processing the vehicle's current location information from the positioning module, the vehicle's surrounding environment information from the perception module, the road information from the map module, and the traffic light status information and recommended speed information from the V2X module; S2, judging whether there is a traffic light on the current road section according to the information received by the V2X module in step S1, if there is no traffic light, executing step S9, if there is a traffic light, executing step S3; S3, judging the state of the traffic light according to the traffic light state information received by the V2X module in step S1; if the traffic light is red or yellow, executing step S4; if the traffic light is green, executing step S9; S4, judging whether there is a CIPV target in the current lane according to the vehicle surrounding environment information obtained by the perception module in step S1, if there is a CIPV target, executing step S5, if there is no CIPV target, executing step S12; S5, matching the position index of the CIPV target on the vehicle trajectory planned by the unmanned vehicle according to the vehicle current position information obtained by the positioning module in step S1 and the CIPV target position information obtained by the perception module; S6, matching the position index of the midpoint of the stop line on the trajectory according to the current position information of the vehicle obtained by the positioning module in step S1 and the stop line position information obtained by the map module; S7, comparing the position indexes calculated in step S5 and step S6, if the stop line position index is greater than or equal to the CIPV target position index, executing step S8, otherwise executing step S14; S8, judging whether the CIPV target is in motion according to the motion state information of the CIPV target obtained by the perception module in step S1; if the CIPV target is in motion, using the Sigmod speed planning algorithm to plan a speed sequence that keeps a preset distance from the dynamic CIPV target as the final desired speed sequence of this cycle; if the CIPV target is in a stationary state, using the Sigmod speed planning algorithm to plan a speed sequence that keeps a preset distance before reaching the stationary CIPV target as the final desired speed sequence of this cycle; S9, according to the judgment results of step S2 and step S3, if there is no traffic light or there is a green light, step S4 is executed to determine whether there is a CIPV target in the lane; if there is a CIPV target in the lane, step S5 is executed to calculate the CIPV target position index, and step S8 is executed to calculate the speed sequence; If there is no CIPV target in the lane, execute step S10; S10, taking the minimum value of the recommended speed information received by the V2X module in step S1, the road speed limit information obtained by the map module, and the speed limit of the unmanned vehicle itself as the expected speed; S11, using the Sigmod speed planning algorithm, taking the desired vehicle speed calculated in step S10 as the target, planning a smooth speed sequence as the final desired speed sequence of this cycle; S12, according to the judgment result of step 4, if there is no CIPV target in the lane, execute step S6 to calculate the stop line position index, execute step S10 to calculate the expected vehicle speed, and then execute step S13; S13, estimating whether the traffic light has switched to green when the driverless vehicle reaches the intersection at the expected speed calculated in step S10; if the traffic light has switched to green, executing step S11 to plan the speed sequence; if the traffic light has not switched to green, executing step S14; S14, using the Sigmod speed planning algorithm, plans a speed sequence for stopping at the stop line at the traffic light intersection as the final expected speed sequence of this cycle; The position index of the matched CIPV target on the vehicle trajectory planned by the unmanned vehicle in S5 includes: According to the current position information of the unmanned vehicle obtained by the positioning module and the CIPV target position information obtained by the perception module, the Euclidean distance d from each trajectory point to the CIPV target is calculated. i , take the Euclidean distance d i The trajectory point index corresponding to the minimum value is used as the matching position index of the CIPV target on the trajectory, where d i The calculation formula is: Where: d i represents the Euclidean distance from the i-th trajectory point to the CIPV target; x i ,y i represents the coordinates of the i-th trajectory point; x ob ,y ob Indicates the coordinates of the CIPV target; i, n indicate index values; path_point_size indicates the total number of trajectory points of the current trajectory; The position index of the midpoint of the stop line on the trajectory is matched in step S6, including: According to the current position information of the unmanned vehicle obtained by the positioning module and the stop line position information obtained by the map module, the coordinates of the midpoint of the stop line are substituted into the CIPV target coordinates in step S5, and the coordinates of the stop line midpoint are substituted into the CIPV target coordinates in step S5. i The calculation formula is used to calculate the matching position index of the midpoint of the stop line on the trajectory; In S8, the Sigmod function is used to generate a speed sequence that maintains a preset distance from the CIPV target. The expression of the Sigmod function is as follows: Where: x i represents the normalized value of the distance from the i-th trajectory point to the center of the rear axle of the unmanned vehicle, bask_link; s i Indicates the distance from the i-th trajectory point to the center of the rear axle of the unmanned vehicle, bask_link; s end Indicates the distance from the last track point to the center of the rear axle of the unmanned vehicle, bask_link; basic i Indicates the difference factor between the expected speed and the current speed; p a ,p b Desire_speed represents the parameters used by the Sigmod speed planner; desire_speed i represents the expected velocity corresponding to the i-th trajectory point; v c Indicates the current speed of the unmanned vehicle; v tar Represents the target vehicle speed used by the Sigmod speed planner; In S13, estimating whether the traffic light has switched to green when the driverless vehicle drives to the intersection at the expected speed includes: According to the current position information of the unmanned vehicle obtained by the positioning module, the stop line position information obtained by the map module and the expected speed calculated in step S10, the distance s required for the unmanned vehicle to accelerate from the current position to the expected speed and maintain the expected speed to the stop line is calculated. total , calculate the distance s from the current position to the stop line to_stop_line , obtain the signal light status information from the V2X module to determine the remaining time t of the red light red , when s is satisfied total Less than or equal to s to_stop_line When the driverless car reaches the intersection at the expected speed, the red light has switched to green, where s total The calculation formula is: Among them, when s total >s to_stop_line When s total ≤s to_stop_line When the red light turns to green; Where: s total It indicates the sum of the distance from the current position to the target speed and the countdown time of the red light when the target speed is reached; s to_stop_line Indicates the distance from the current position of the unmanned vehicle to the stop line; t red Indicates the time when the red light of the traffic light switches to green; if s total Less than s to_stop_line , it is considered that when the driverless car drives to the intersection at the expected speed, the red light has switched to green; a represents the parameter of the Sigmod function.

2. The V2X-based unmanned driving speed planning method for underground mines according to claim 1 is characterized in that: The perception module uses at least one of a laser radar, a millimeter-wave radar, and a visual sensor to perceive the vehicle's surrounding environment.

3. The V2X-based unmanned driving speed planning method for underground mining according to claim 2 is characterized in that: The positioning module uses at least one of GNSS positioning, IMU inertial navigation positioning and wheel speed odometer positioning to determine the current position of the vehicle.

4. The V2X-based unmanned driving speed planning method for underground mining according to claim 3 is characterized in that: The V2X module communicates with the roadside unit RSU through the unmanned vehicle OBU, receives the traffic light status information and road traffic information broadcast by the RSU, and obtains the traffic light status, remaining time and recommended travel speed of the road.

5. A computer-readable storage medium storing computer instructions, which implement the method according to any one of claims 1 to 4 when the computer instructions are executed by a processor.

Images