Automatic parking route planning evaluation method and device

Abstract

The present application discloses an automatic parking route planning evaluation method and device, which relates to the field of route planning. The method includes: constructing a grid occupancy map of a target parking lot; planning a parking route according to the grid occupancy map, a starting point and a target point to obtain a reference route; collecting the actual route of a test vehicle during automatic parking according to the reference route; determining global factors and local factors for automatic parking evaluation according to the reference route and the actual route; the global factors include actual total time and actual route length, and the local factors include actual route deviation area, maximum route deviation length, actual average speed and number of collisions; and evaluating the automatic parking of the test vehicle according to the global factors and the local factors. The present application improves the effectiveness of automatic parking evaluation.

Description

Technical Field

[0001] The present application relates to the technical field of path planning, and in particular to an automatic parking route planning and evaluation method and device. Background Art

[0002] With the rapid development of big data and artificial intelligence, the development of autonomous driving technology is in full swing. Among them, automatic parking has become the first application scenario for the implementation of autonomous driving due to its relatively fixed scenes and low driving speed and has been widely used. In order to accelerate the development of the field of intelligent car parking and cultivate high-level young talents familiar with this field, various intelligent car parking competitions have emerged. These competitions generally evaluate the driving status of intelligent cars during the competition according to the rules of the competition and give the final scoring results through manual referees. The evaluation criteria only consider the performance of a certain aspect of the intelligent car, and do not analyze and evaluate it in many aspects, which affects the effectiveness of the automatic parking evaluation. Summary of the invention

[0003] The purpose of this application is to provide an automatic parking route planning evaluation method and device, which can improve the effectiveness of automatic parking evaluation.

[0004] To achieve the above-mentioned purpose, the present application provides the following solutions: In the first aspect, the present application provides an automatic parking route planning evaluation method, comprising: constructing a grid occupancy map of a target parking lot; planning a parking path according to the grid occupancy map, a starting point and a target point to obtain a reference path; collecting an actual path of a test vehicle during automatic parking according to the reference path; determining global factors and local factors for automatic parking evaluation according to the reference path and the actual path; the global factors include actual total time and actual path length, and the local factors include actual path deviation area, maximum path deviation length, actual average speed and number of collisions; and evaluating the automatic parking of the test vehicle according to the global factors and the local factors.

[0005] In a second aspect, the present application provides an automatic parking route planning evaluation device, comprising: a grid occupancy map construction module, used to construct a grid occupancy map of a target parking lot; a reference path determination module, used to plan a parking path according to the grid occupancy map, a starting point and a target point to obtain a reference path; an actual path acquisition module, used to acquire an actual path of a test vehicle during automatic parking according to the reference path; an evaluation factor determination module, used to determine global factors and local factors for automatic parking evaluation according to the reference path and the actual path; the global factors include actual total time and actual path length, and the local factors include actual path deviation area, maximum path deviation length, actual average speed and number of collisions; an evaluation module, used to evaluate the automatic parking of the test vehicle according to the global factors and the local factors.

[0006] According to the specific embodiments provided by the present application, the present application discloses the following technical effects: the present application determines the global factors and local factors for automatic parking evaluation based on the reference path and the actual path; the global factors include the actual total time and the actual path length, and the local factors include the actual path deviation area, the maximum path deviation length, the actual average speed and the number of collisions. The real-time path of the test vehicle is detected and compared with the global planned path analysis, the real-time parking path is evaluated using the actual deviation area, and the path planning is scored based on the test vehicle's own factors, such as the actual average speed and the number of collisions. This achieves a multi-faceted evaluation from the global to the local of the automatic parking path, thereby improving the effectiveness of the automatic parking evaluation. BRIEF DESCRIPTION OF THE DRAWINGS

[0007] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0008] Figure 1 A schematic diagram of a flow chart of an automatic parking route planning and evaluation method provided in one embodiment of the present application.

[0009] Figure 2 A schematic diagram of the overall process of automatic parking route planning provided in one embodiment of the present application.

[0010] Figure 3 A schematic diagram of a grid mapping provided in an embodiment of the present application.

[0011] Figure 4 A schematic diagram of the Ackerman steering geometry principle provided for one embodiment of the present application.

[0012] Figure 5 A schematic diagram of a vehicle kinematic model provided in accordance with an embodiment of the present application.

[0013] Figure 6 A schematic diagram of a vehicle single circle and double circle detection model provided in one embodiment of the present application.

[0014] Figure 7 A schematic diagram of a bidirectional search provided in an embodiment of the present application.

[0015] Figure 8 A schematic diagram showing a Cartesian coordinate system reference trajectory provided in an embodiment of the present application.

[0016] Fig. 9 A schematic diagram showing a reference trajectory of a Frenet coordinate system provided in an embodiment of the present application.

[0017] Fig.10 A schematic diagram of the conversion between the Cartesian coordinate system and the Frenet coordinate system provided in an embodiment of the present application.

[0018] Fig.11 A schematic diagram of the intrusion relationship between a dynamic interferer and a reference path provided in an embodiment of the present application.

[0019] Fig.12 A projection diagram of a dynamic interferer on an ST diagram provided in an embodiment of the present application.

[0020] Fig.13 A schematic diagram of a multi-stage optimization search provided in an embodiment of the present application.

[0021] Fig.14 A schematic diagram of dynamic programming provided for an embodiment of the present application.

[0022] Fig.15 This is a rendering of a secondary planning speed curve provided in an embodiment of the present application.

[0023] Fig.16 A schematic diagram of the dual PID control information transmission process provided in one embodiment of the present application.

[0024] Fig.17 A schematic diagram of a parking lot environment detection process based on a YOLO8 detection model provided in an embodiment of the present application.

[0025] Fig.18 A schematic diagram of a unilateral bidirectional ranging method provided in one embodiment of the present application.

[0026] Fig.19 A schematic diagram of a partial intersection area between an actual path and a reference path provided in an embodiment of the present application. DETAILED DESCRIPTION

[0027] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

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

[0029] This application provides an automatic parking route planning evaluation method, such as Figure 1 As shown, the automatic parking route planning and evaluation method includes steps 101 to 105.

[0030] Step 101: Construct a grid occupancy map of the target parking lot.

[0031] Step 102: Perform parking path planning according to the grid occupancy map, the starting point and the target point to obtain a reference path.

[0032] Step 103: Collecting the actual path of the test vehicle during the automatic parking process according to the reference path.

[0033] Step 104: Determine global factors and local factors for automatic parking evaluation based on the reference path and the actual path; the global factors include actual total time and actual path length, and the local factors include actual path deviation area, maximum path deviation length, actual average speed and number of collisions.

[0034] Step 105: Evaluate the automatic parking of the test vehicle according to the global factors and the local factors.

[0035] The layout of the parking lot targeted by this application is mainly based on the common vertical parking scenario. Parking spaces, parked vehicles and multiple sensors for detecting the status of the parking lot are arranged in the parking lot according to the real parking lot scenario: multiple cameras, ultra-wide band (UWB) base stations, UWB anchor points and a lidar.

[0036] The camera uses a visual detection algorithm to detect the status of the test site in real time to ensure that there are no interference anomalies in the test site. If parking interference is found, the terminal will be given a warning or the test will be terminated. UWB base stations and UWB anchor points are wireless positioning facilities that rely on distance measurement. They are used to determine the distance or other data information between unknown UWB anchor points and known indoor UWB base stations in parking lots, and then use positioning algorithms and the data obtained to resolve the exact location of the anchor point. LiDAR is used to work normally in complex and various light scenes and collect information about the surrounding environment through excellent ranging capabilities.

[0037] like Figure 2 As shown, the present application provides a point cloud map through a perception module, and forms a lanelet2 map based on the point cloud map. The point cloud map, lanelet2 map and UWB anchor point positioning information together form a smart parking lot map in the form of a grid occupancy map, and trajectory planning and tracking control are performed based on the smart parking lot map.

[0038] Among them, step 101 specifically includes step 1011 to step 1013.

[0039] Step 1011: Use the Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping (LIO-SAM) framework based on smoothing and mapping to collect a point cloud map of the target parking lot. The point cloud map is a data set of points in a specific coordinate system, which contains a large amount of environmental data, such as three-dimensional coordinates, color, intensity, and time. The point cloud data is obtained by the LiDAR, where each point represents the coordinate information of the surface of the object obtained by the LiDAR sensor. These point cloud data can be used to build high-precision maps, as well as in the fields of autonomous driving, environmental information systems, and environmental monitoring. The smart parking lot can use data collection vehicles or data collection systems to collect the corresponding data of the smart parking lot. Ubuntu22.04 and Robot Operating System (ROS) 2 are used for data collection. The ROS2 command: ros2 bag record / topic1 / topic2 / topic3 ​​is used to complete the collection of point cloud data, Inertial Measurement Unit (IMU) data and Global Navigation Satellite System (GNSS) data. The tightly coupled LiDAR inertial odometry framework based on smoothing and mapping adopted in this application can realize the construction of high-precision point cloud maps.

[0040] Step 1012: Divide the point cloud map into driving areas and parking areas to obtain a Lanelet2 map. The observed elements of the Lanelet2 map are the physical layer of the map, including road signs, traffic lights, traffic signs, parking spaces, etc., which are suitable for defining the internal elements of the smart parking lot. The default format of Lanelet2 is the Open Street Map (OSM) format in the form of eXtensible Markup Language (XML). The OSM map editor is used for editing and visualization, and Lanelet2 assumes that all elements in the map can be projected onto a plane. The Lanelet2 map includes six types of elements: points, lines, polygons, lanes, areas, and control elements. The six types of elements fully meet the elements required for the construction of the smart parking lot map, and are produced using a vector map builder. Specifically, the point cloud map file is imported into the vector map builder, and each area in the map is functionally divided, mainly into driving areas and parking areas, to obtain a Lanelet2 map.

[0041] Step 1013: Grid mapping the Lanelet2 map and adding the positioning information of the UWB anchor point to generate a grid occupancy map. Grid mapping divides the environment into multiple grids and stores occupancy or elevation information in each grid. Existing grid maps can be divided into two-dimensional grids, elevation grids and three-dimensional grids according to the different data stored in the grids. Each grid of the grid occupancy map stores a probabilistic occupancy information. The color of the two-dimensional grid cell determines the various states of the map. Figure 3 As shown in the figure, dark gray indicates occupied areas, that is, areas with obstacles; light gray indicates idle areas, that is, areas without obstacles. Figure 3 Part (b) is for Figure 3 The raster map after raster mapping in part (a). Figure 3 The rectangular box in part (a) is an obstacle. The subsequent steps are to analyze the trajectory planning algorithm based on this type of grid occupancy map.

[0042] The positioning information of the UWB anchor point is integrated with the point cloud map and the Lanelet2 map to form a grid occupancy map of the UWB-based smart parking lot. In the grid occupancy map of the UWB-based smart parking lot, the occupancy value of the area with obstacles is 1, and the occupancy value of the area without obstacles is 0. There is a UWB anchor point with known coordinate information, and the coordinate information of the UWB anchor point is determined by the surrounding UWB base stations through the signal time of flight (TOF) positioning method. At this point, the grid occupancy map required for subsequent trajectory planning has been constructed.

[0043] The obstacles in the path planning of this application are divided into static obstacles and dynamic obstacles, so the scenes for parking path planning are divided into static obstacle scenes and dynamic obstacle scenes. The test vehicle in this application is a smart car.

[0044] In a static obstacle scenario, step 102 specifically includes steps 10201 to 10203.

[0045] Before step 10201, a vehicle model is first established, and the vehicle model takes into account the vehicle steering and kinematics model.

[0046] The steering devices of modern vehicles are designed based on the Ackermann steering geometry principle. Figure 4 As shown in the figure, this structural design can ensure that the instantaneous rotation center of the four wheels is the same. During parking, the vehicle is traveling at a low speed and the wheels can be simplified to a state of no slip and no side deviation. Figure 4 The geometric relationship in can be derived as follows: and .in, is the vehicle wheelbase, is the vehicle wheelbase, is the outer turning angle of the front axle of the vehicle, is the inner turning angle of the front axle of the vehicle. Based on the two-degree-of-freedom bicycle model, equivalent wheels are set at the center of the front and rear axles, where the front wheel is used as the steering wheel. The following is derived based on geometric principles: . is the vehicle turning radius; is the equivalent steering angle. The relationship between the equivalent steering angle, the inner turning angle of the front wheel, the outer turning angle of the front wheel, the radius, the wheelbase and the track is: , and .

[0047] The vehicle kinematics model refers to the study of the vehicle motion law from a geometric perspective, mainly analyzing the change of the vehicle's spatial posture information and speed information over time. When parking, the car is traveling at a low speed, and usually there is no need to consider handling stability and other dynamic issues, so the vehicle kinematics model is used for subsequent research, such as Figure 5 As shown, A, B, C and D constitute the circumscribed rectangle of the vehicle. In order to ensure the safety of subsequent path planning and avoid collision with obstacles in the parking environment, it is necessary to perform collision detection on the vehicle. This application uses the improved equivalent double circle method to construct the vehicle model as shown in Figure 6 As shown in (b), the single circle model is Figure 6 As shown in (a). Figure 6 middle, is the center of the circumscribed circle of the vehicle contour rectangle, and the radius of the circle is The double-disc contour model is composed of two intersecting circles spliced ​​along the center axis of the vehicle. The centers of the two circles are and , and They are The horizontal and vertical coordinates of and They are The horizontal and vertical coordinates of is the heading angle of the vehicle.

[0048] According to the position coordinates of the vehicle's rear axle and the geometric relationship, we can get: Among them, the radii of the circumscribed circles of the rear axle and the front axle are and , , , Indicates the front overhang distance, Indicates the rear overhang distance. and They are t time The horizontal and vertical coordinates of and They are t time The horizontal and vertical coordinates of .

[0049] The equivalent double-circle model simplifies the collision detection between the vehicle and other obstacles into the collision detection between the circle and other shaped contours. It only needs to solve the minimum distance from the center of the circle to the contour. The improved equivalent double-circle model is more adaptable to models with inconsistent front and rear overhangs. While reducing the amount of calculation, it also increases the scope of application. The global optimal path planning in the subsequent steps will be based on the improved equivalent double-circle model.

[0050] Step 10201: Expand each obstacle in the grid occupancy map into a circumscribed circle of the outline of each obstacle to obtain a grid occupancy map.

[0051] Step 10202: On the grid occupancy map, the test vehicle is used as a mass point and a bidirectional A The search algorithm searches for a first path between a starting point and a UWB anchor point, and a second path between the UWB anchor point and a target point. The first path and the second path are connected to form a first global path. The UWB anchor point is set at a set position in a target parking lot. When the coordinate information of the UWB anchor point is unknown, the coordinate information of the UWB anchor point is determined based on the UWB base station through a TOF positioning method.

[0052] This application is based on the UWB-based smart parking lot grid occupancy map, improves the search strategy, and integrates the UWB anchor point information and cost function optimization to complete the final parking path planning.

[0053] This application is to solve the traditional A The low efficiency of path search algorithm in parking lot is a problem for traditional A The algorithm optimizes the search strategy and proposes a two-way A A new search strategy for the search algorithm. This method considers the target point as the initial node and performs a synchronous two-way search with the initial node, which improves the efficiency of path planning while ensuring that the path conforms to the actual situation.

[0054] In addition, UWB anchor points are set up inside the parking lot, and UWB anchor points are placed at appropriate locations inside the parking lot to provide bidirectional A The path search algorithm is further optimized. The search strategy becomes a two-way search from the starting point to the UWB anchor point and a two-way search from the UWB anchor point to the target point, such as Figure 7 shown.

[0055] The cost function is A The core of the algorithm. The quality of the cost function design determines the The number of search nodes when the path search algorithm searches determines the search efficiency of the algorithm.

[0056] This application is based on a two-way A The algorithm adds a new bidirectional search for the estimated cost of the current node, assuming that the starting point coordinates are , the current node in the forward search process The coordinates are , the current node in the reverse search process The coordinates are , the target point is , the improved two-way A The cost function of the path search algorithm is: , .

[0057] is the cost function of the adjacent nodes during the forward search process; is the cost function of the adjacent nodes in the reverse search process; is the cost from the starting point to the current node during the forward search; is the cost from the starting point to the current node during the reverse search; is the estimated cost from the current node to the target point during the forward search; is the estimated cost from the current node to the target point during the reverse search process; is the estimated cost of searching from the current node to the current node in the forward search process; The estimated cost of searching forward from the current node to the current node during a backward search.

[0058] Build 800 800 grids occupy the map, and the traditional A Path search algorithm, bidirectional A Path search algorithm, bidirectional A based on UWB anchor point Search Algorithm and Improved Cost Function for UWB Bidirectional A The algorithm is compared and analyzed. Taking the same starting point and target point, the comparison and analysis are conducted from two dimensions: the number of nodes traversed by the algorithm and the time consumed by the algorithm search: 1. The number of nodes traversed during the algorithm search process is related to the algorithm search efficiency; 2. The time consumed by the algorithm search is the core indicator of whether the algorithm is improved or not. Bidirectional A based on UWB The number of nodes traversed by the search algorithm is 1340. The algorithm is similar, but the search time is reduced to 16.165 seconds, which speeds up the search; UWB bidirectional A with improved cost function The search algorithm adds an estimated cost for the current node in a two-way search, which induces both parties to move closer to the current search point during the two-way search. The number of nodes traversed by the search is 144, and the search time is 1.141 seconds, which increases the search efficiency. Even when the grid occupies a map with a large number of road network nodes, the path search can be completed quickly and efficiently.

[0059] Step 10203: Use multiple quintic Bezier curves to smooth and splice the first global path. If the number of control points in the last segment of the first global path is less than 6, use a quartic Bezier curve to smoothly splice to obtain a reference path.

[0060] The first global path obtained in step 10202 is not smooth and may not meet the motion constraints of the autonomous driving vehicle during parking. It is necessary to use multiple quintic Bezier curves to smooth and splice the planned path.

[0061] Using the path discrete control points obtained in step 10202, let the number of control points be , generally satisfied , while a single quintic Bezier curve can only use 6 control points. Therefore, the smoothing of the entire path needs to be performed through a combination of multiple quintic Bezier curves.

[0062] At the connection points of these curves, several conditions must be met: first, the position must be continuous to ensure seamless connection; second, the tangent direction needs to remain consistent to ensure a smooth transition of the curve; finally, the curvature of the curve also needs to be continuous to ensure a natural transition of the turning point to facilitate vehicle tracking control.

[0063] Take the splicing between the first quintic Bezier curve and the second quintic Bezier curve as an example, assuming that the control point of the first Bezier curve is , the control points of the latter Bezier curve are For position continuity, it is necessary to ensure that the end point of the previous curve is completely aligned with the subsequent curve start point; for the continuity of the first derivative of the curve, it is necessary to require and On the same line; for the continuity of the curve curvature, an auxiliary control point is added where two Bezier curves are adjacent , and the auxiliary control point Position constraint and between, express and is the same point. According to the tangent vector and its second-order derivative characteristics of the Bezier curve, the problem of solving the optimal position of the auxiliary control point can be described as an optimization problem: , , represents the constraints, n is a constant.

[0064] Based on the above optimization method, the best additional auxiliary control points can be determined ,and , so the previous scattered path point The next four points serve as the remaining four control points for the next five Bezier curves. , considering that the number of scattered points in the entire planning path may not meet the number of control points required by fitting several quintic Bezier curves. Therefore, for this situation, the number of points left at the end of the planning path scattered points is different, and it is necessary to handle it separately. The last segment of the Bezier curve can be determined according to the number of different remaining points. For example, the number of points left at the end of the planning path scattered points is 3, plus the auxiliary control point And the end control point of the previous Bezier curve , there are five control points in total, and finally a quartic Bezier curve will be used to complete the smooth splicing.

[0065] Improved cost function for UWB bidirectional A The path obtained by the search algorithm is resampled, and 10 points are selected from it to verify the effectiveness of the smooth optimization algorithm for splicing multiple Bezier curves to obtain a smooth path.

[0066] Calculate the curvature of the optimized path curve. The entire path is a smooth and continuous curve, and the curvature changes continuously, which meets the condition of continuous curvature of the parking path. During the whole process, the curvature of the optimized path changes in a small range. Through calculation, the maximum curvature value of the smooth path based on the splicing of multiple quintic Bezier curves is , which is conducive to the continuous change of vehicle turning angle and reduces the difficulty of subsequent control. Moreover, the path after smooth optimization of multiple segments of quintic Bezier curves still satisfies the properties of first-order derivative continuity and second-order derivative continuity.

[0067] In a dynamic obstacle scenario, the first global path is smoothed using a quintic Bezier curve path smoothing algorithm. After obtaining a reference path, parking path planning is performed based on the grid occupancy map, the starting point, and the target point to obtain a reference path, which specifically includes steps 10204 to 10210.

[0068] Step 10204: Convert the reference path as the initial reference path into the Frenet coordinate system.

[0069] This application uses the smooth parking path planned above, that is, the reference path, as the reference line of the Frenet coordinate system, and uses the tangent vector and normal vector of the reference line to establish a coordinate system. Compared with Cartesian coordinates, the Frenet coordinate system simplifies the path planning problem. Based on the Frenet coordinate system, the position state of the vehicle at each moment is decomposed into directions to describe, which can reduce the work of processing coordinate information when fitting the trajectory curve. In the Cartesian coordinate system, the reference path is often a curved curve such as Figure 8 As shown, Figure 8 (a) is a schematic diagram of vehicle driving. Figure 8 (b) is a schematic diagram of the path. The curved path can be converted into Axis straight line, such as Fig. 9 As shown, use The axis represents the distance traveled by the curved path, and The axis represents the degree of deviation from the curve reference path. Fig. 9 (a) is a schematic diagram of vehicle driving. Fig. 9 (b) is a schematic diagram of the path. Therefore, compared with the Cartesian coordinate system, the Frenet coordinate system can reduce the difficulty of vehicle trajectory planning and make it easier to determine the relative position of other interference objects in the parking environment to the reference path. The motion state of the vehicle in the Cartesian coordinate system at any time can be described as: .in, is the position vector of the vehicle in the Cartesian coordinate system; is the vehicle heading angle; is the curvature; for speed; is the acceleration.

[0070] Transform the variables in the Cartesian coordinate system into the Frenet coordinate system and use the following quantities to describe the motion state of the vehicle: .in, is the longitudinal coordinate in the Frenet coordinate system; is the tangential speed of the vehicle; is the tangential acceleration of the vehicle; is the lateral coordinate in the Frenet coordinate system; is the normal velocity of the vehicle; is the normal acceleration of the vehicle; is the first derivative of the horizontal coordinate with respect to the vertical coordinate; The dot on the letter expression in this application represents the derivative, one dot represents the first-order derivative, and two dots represent the second-order derivative.

[0071] according to Fig.10 Expand and derive the conversion formula between the Frenet coordinate system and the Cartesian coordinate system. According to the definition of the orthogonal basis, we can get: .

[0072] in, is the heading angle of point Q; is the heading angle of point P; and is the unit orthogonal vector of point Q; and is the unit orthogonal vector of point P. represents the reference trajectory position vector, let for , vehicle current location Projection point with reference line Distance d It can be calculated by the following formula: .

[0073] like ,but Take a positive sign; otherwise Take the negative sign, x is the horizontal coordinate of the vehicle in the Cartesian coordinate system, and y is the vertical coordinate of the vehicle in the Cartesian coordinate system.

[0074] Lateral displacement , let the parameter , from which the lateral velocity can be calculated: .

[0075] Depend on and Relationship , taking the derivatives on both sides of the equation and rearranging them, we get: .in , Represents the curvature of the projected point.

[0076] right The derivative is: .

[0077] according to definition, The expression is: , T Indicates transpose.

[0078] according to Define, and Substitute it into the calculation expression: , after simplification, we can get: .

[0079] Computable for: . Combining the formulas, we get: , . Represents the first derivative of the curvature at the projected point.

[0080] In summary, the vehicle status information in the Frenet coordinate system Vehicle status information in Cartesian coordinate system The conversion equation between the two can be determined by the formula. The conversion formula from the Cartesian coordinate system to the Frenet coordinate system is: .

[0081] Represents the cumulative arc length of the reference path, that is, the distance from the starting point of the reference path to the current point.

[0082] The conversion formula from Frenet coordinate system to Cartesian coordinate system is: .

[0083] Step 10205: Define moving targets in the target parking lot whose moving speed is greater than a set threshold as dynamic interference objects; moving targets include pedestrians and cars.

[0084] Step 10206: Project all dynamic interference objects into the displacement time diagram.

[0085] The displacement time (ST) graph is a graph that describes the movement of vehicles or other objects in space and time. The ST graph clearly shows the parking trajectory and the possible trajectory interference of surrounding interference objects in the future. When screening dynamic interference objects, pedestrians, vehicles or other objects with a moving speed greater than the set threshold are defined as dynamic interference objects. The dynamic interference objects are projected into the Frenet coordinate system established based on the reference path. When processing dynamic interference objects, this application represents all dynamic interference objects with rectangular bounding boxes. The upper-level prediction module predicts the possible subsequent actions of the obstacle through the environment recognized by the perception module. The upper-level prediction module is used to avoid dynamic obstacles in path planning. Based on the upper-level prediction module, the trajectory of the dynamic interference object is predicted and its rectangular contour points are found to be projected to When the shortest distance between the vehicle contour point and the projection point is less than the automatic parking path safety distance , it means that the dynamic interference object invades the initial reference path of the vehicle's automatic parking (from the starting point To the destination ),like Fig.11 As shown, Fig.11 The dotted lines on both sides indicate the safe driving range of the vehicle. Time has come The time is the time period when the dynamic interferer invades the initial reference path. Time is the intrusion start time, The time is the end time of the intrusion. The dynamic disturbance object is located on the initial reference path at the moment To displacement Department, The dynamic disturbance is located at the upper displacement of the initial reference path at time To displacement At this time, the dynamic interference is projected into the ST diagram, such as Fig.12 As shown. Through the above analysis, we can conclude that designing a curve in the ST graph to avoid dynamic interference objects can ensure that the vehicle will not collide during parking. Therefore, the essence of speed planning is to find a curve in the ST graph that will not cause collision.

[0086] This application uses YOLOv8 to identify interference objects, OpenCV frame difference method to calculate speed, and OpenCV library to detect moving objects, which uses the differences between video frames to identify moving objects.

[0087] Step 10207: Discretize the displacements in the displacement-time graph into equal distances to obtain discrete nodes of each displacement.

[0088] Step 10208: Create a state transition equation between every two adjacent displacement discrete nodes; the state transition equation is a state transition equation for the path position and velocity; wherein the path position and velocity are the path position and velocity of the test vehicle in the Frenet coordinate system, respectively.

[0089] Step 10209: Based on the set constraints, the state transition equations of every two adjacent displacement discrete nodes are optimized to obtain the speed of each displacement discrete node, and the speeds of each displacement discrete node constitute a first speed curve; the set constraints include speed limit constraints, test vehicle physical constraints, and overlap constraints between the test vehicle and dynamic interference objects.

[0090] First, the original problem is divided into sub-problems of smaller dimensions. Once each sub-problem reaches the number of elements of the original problem, the optimal value of the objective function can be found. The purpose of this application is to find a curve in the ST projection graph on which there is no risk of collision with dynamic interference objects and make corresponding driving decisions. Fig.12 The middle quadrilateral area is the projection of the dynamic interference on the ST diagram. The main vehicle trajectory should avoid this area. The horizontal and vertical axes of the ST diagram are discretized, and the state transfer equation is established. The boundary conditions are set according to the physical constraints of the vehicle and the regional constraints of the dynamic interference. Finally, the objective function is determined. The process is as follows: Fig.13 First, the ST graph is preprocessed and the horizontal axis is With vertical axis Direction is discretized using Will Direction equidistant discretization, expressed as displacement ,use Will The direction is discretized with equal distance, expressed as , ST diagram after preprocessing, is the number of discretized displacements, is the number of discretized moments, such as Fig.14 As shown in the figure, the green feasible speed curve can be finally obtained through the optimization results of each stage, where n is an integer. Fig.14 The medium grey areas are obstacles.

[0091] In the stage, As a state variable, here and They represent the first The phase path position and velocity. In addition, the acceleration Defined as a decision variable, the state transition equation is: . For the The state variables of the phase, and Respectively Stage path positions and velocities, For the k The control variables of the stage.

[0092] In the ST projection graph, the optimal speed curve is searched. In addition to avoiding dynamic interferences, each discrete node also needs to meet the efficiency requirements of the parking process. Therefore, two sub-objective functions of speed and interference are defined. The expressions are shown in the formula: .in, is the expected speed in the parking lot; is the speed deviation penalty factor; is the dynamic disturbance sub-objective function; is the expected speed cost weight coefficient; is the interference cost weight coefficient, is the total objective function value, for The first derivative of .

[0093] The constraints are mainly for the speed limit requirements in the parking lot, the physical constraints of the vehicle, and the overlap constraints of dynamic interference objects (the overlap constraints between the test vehicle and the dynamic interference objects). The constraints are set as shown in the formula: .in, The maximum speed at which the test vehicle is allowed to travel; The maximum acceleration allowed for the test vehicle; To test the vehicle's speed at the starting point; To test the acceleration of the vehicle at the starting point; To test the speed of the vehicle at the target point; To test the acceleration of the vehicle at the target point, is the initial state velocity, is the final state velocity, is the initial state acceleration, is the final state acceleration, is a set of integers.

[0094] When the initial state information of parking is confirmed, the reverse solution can be used to perform dynamic programming calculation and solution. Starting from the target state position, reverse to the starting point and gradually find the optimal solution, and finally recursively return to the initial state point. The basic recursive formula of the reverse solution is: .

[0095] in, For the The indicator function of the stage, Define the termination condition and recursive relation of the recursion, For the The indicator function of the stage, For the The indicator function of the stage, is the optimal solution of the cost function, An optional set of points.

[0096] Step 10210: Based on the smoothness of the speed curve and the deviation between the speed curve and the reference speed curve, perform secondary programming on the first speed curve to obtain a second speed curve; and use the path generated according to the second speed curve as a reference path.

[0097] The optimal speed planning based on dynamic programming is solved on the discretized ST diagram, which is often affected by factors such as the size of the interval division. The planned speed curve may be discontinuous or non-differentiable, which makes it impossible to use it directly in the control system. When the degree of discretization is too small, it may lead to a huge amount of calculation, affecting the planning efficiency. Therefore, it is necessary to perform secondary smoothing on the above-mentioned first speed curve.

[0098] The standard form of the secondary smoothing of the first velocity curve is defined as: .

[0099] in, is the array of variables to be solved; is the gradient vector; H is the Hessian matrix; and All are equality constraint matrices; and is the inequality constraint matrix.

[0100] In this application, the secondary parking speed planning problem is: is the time interval, for t The location information at the moment Discretization processing was performed, and the relationship between the variables is: is the vehicle trajectory position information at point i, is the vehicle speed information at point i, is the vehicle acceleration information at point i, is the vehicle acceleration information between two points (point i+1 and point i), and is always a constant.

[0101] Will and Carry out the second-order and third-order Taylor expansion respectively, and we can get the formula: , .

[0102] Converting the above equation into matrix form, the continuity equality constraint is as follows: . Among them, the matrix ,matrix .

[0103] The equality constraints for the parking start and end states are: , .

[0104] in, They are the trajectory position, velocity, and acceleration information of the initial state respectively; They are respectively the trajectory position, velocity, and acceleration information of the terminal state.

[0105] The equality constraints of the objective function have been determined. For the inequality constraints, the continuity at the state nodes and the vehicle dynamic constraints are mainly considered. The speed optimization constraints of the quadratic programming (QP) are as follows: .in, Maximum deceleration allowed for the test vehicle; The maximum acceleration allowed for the test vehicle.

[0106] When designing the objective function of quadratic programming, the smoothness of the speed curve and the deviation from the reference speed curve are mainly considered: the first part It is the weighted sum of the integral of the acceleration and the square of the jerk. This sub-objective function ensures that the vehicle's driving speed is smooth and changes steadily during driving. , To test the vehicle acceleration weight coefficient; It is the acceleration weight coefficient of the test vehicle.

[0107] Part 2 It is a sub-objective function that takes into account the deviation from the reference speed curve, and ensures that the vehicle and the reference speed curve should fit as closely as possible. , is the reference speed weight coefficient; It is the reference position obtained by dynamic planning at this moment.

[0108] In summary, the quadratic programming objective function is: Among them, the quadratic programming optimization variables are: .

[0109] According to the above boundary conditions and objective function , using the built-in quadprog solver of MATLAB software to calculate, a speed curve that satisfies both smoothness and safety is obtained, namely the second speed curve, the effect of the quadratic programming speed curve, such as Fig.15 shown.

[0110] In the dynamic obstacle scenario, when the test vehicle performs automatic parking according to the reference path, the process specifically includes: providing a reference speed and a reference acceleration through a second speed curve, and using a PID control algorithm to perform dual PID control on the speed and acceleration of the test vehicle.

[0111] The PID control algorithm consists of three parts: proportional (P), integral (I) and derivative (D). It calculates the target control amount based on the difference between the system input and output. Enter a value for the system. is the system output value, is the deviation between them, and then the deviation is input into the controller. After the combination operation of the three parts, the control quantity can be obtained , thereby correcting the output value of the controlled object and controlling the deviation , the PID operation expression is: .in, is the error between the system output and the reference value; is the proportionality coefficient; is the integral gain coefficient; is the differential gain coefficient.

[0112] The longitudinal speed control of the controlled vehicle, i.e. the test vehicle, is to operate the accelerator and brake pedal of the vehicle, and obtain the control amount according to the deviation between the actual vehicle speed and the expected vehicle speed. The speed and acceleration with and without dynamic interferences have been planned in the automatic parking speed planning section under dynamic interference. Therefore, this application takes the speed and acceleration dual-loop PID as the core and the speed planning module as the input to design the longitudinal speed controller and acceleration controller, such as Fig.16 shown.

[0113] The vehicle trajectory tracking control algorithm is simulated and verified by Simulink and Carsim software. The simulation results show that the vehicle trajectory tracking control algorithm is relatively stable during the parking process, and both the lateral and longitudinal displacements can be tracked smoothly. The maximum lateral error is 0.047m, and the maximum heading error is 0.0171 radians, both of which meet the driving requirements. In summary, the vehicle trajectory tracking control algorithm designed in this application can accurately and smoothly track the target path.

[0114] Among them, step 103 specifically includes: according to the UWB positioning anchor point located on the test vehicle, based on the UWB base station through the signal-based TOF positioning method, real-time positioning is performed during the automatic parking process of the test vehicle to obtain the actual path.

[0115] During the test vehicle's automatic parking process according to the reference path, multiple laser radars are used to collect the test vehicle's position information. Since the target to be detected is relatively small, and the quality of the point cloud data obtained by the laser radar is greatly affected by the installation height, which affects the accuracy of the subsequent detection model, it is necessary to analyze the deployment height of the laser radar. Complete the deployment of multiple laser radars and determine the deployment position of multiple laser radars on the smart car competition track.

[0116] The height above the ground of the laser radar in the multi-laser radar at the target parking lot is: .in, is the height of the laser radar from the ground, This is the bottom angle of the laser beam that the LiDAR projects onto the bottom of the test vehicle. It is the shortest distance from the ground projection point of the lidar to the center point of the test vehicle.

[0117] The top angle of the laser beam of the multi-lidar on the top of the test vehicle is: The vertical angle range of multiple laser radars on the test vehicle is: .

[0118] in, This is the top angle of the laser beam that the laser radar projects onto the top of the smart car. To test the actual height of the vehicle, is the horizontal distance between the laser radar and the test vehicle, represents the actual length of the test vehicle, Vertical angle range of multiple lidars projected onto the test vehicle.

[0119] Since the volume of the detection object is relatively small compared to a normal vehicle, the laser beam must be dense enough in the vertical direction to obtain high-quality point cloud data. This requires that the placement height cannot be too high. However, a too low height will cause the occlusion of obstacles to be particularly serious, resulting in a long period of time when the target cannot be detected. and The reference position of the laser radar can be determined, and different positions can be selected according to different tracks. The detection object is the dynamic and static obstacles that need to be tested during parking, and cone barrels are one of them.

[0120] When the test vehicle performs automatic parking according to the reference path, the process specifically includes: step 201 to step 207 .

[0121] Step 201: Perform time synchronization and space synchronization processing on multiple laser radars.

[0122] After determining the placement of multiple laser radars, the multi-lidar reference time is obtained through time and space synchronization of the multiple laser radars, and accurate time synchronization is performed. The space synchronization of multiple laser radars is actually the mutual conversion of coordinate systems between multiple laser radars, which includes rotation and translation of coordinate systems.

[0123] Time synchronization of multi-sensor data can effectively solve the problem that the data of different sensors cannot be correctly aligned in the time dimension due to different start-up times or sampling frequencies. For example, when two laser radars are not accurately time synchronized, the "ghost" phenomenon of the target will appear, affecting the accuracy of the subsequent detection model. Therefore, in order to fully utilize the advantages of multiple laser radars, they need to be accurately time synchronized.

[0124] The calculation formula for time synchronization is as follows: , where is the exact time of the data packet, It is the Coordinated Universal Time (UTC) time provided by the current GPS timing module to the laser radar. is a constant of 1 second, It is the internal timestamp information of the laser radar carried by the frame header in the data packet, that is, the reception time of the first laser beam of the first data block of the data packet. After determining the exact time of each data packet, the exact time of each laser point can be obtained according to the formula. By analyzing the timestamp of each laser point and combining the formula, the exact time of each laser point of multiple laser radars can be obtained, that is, the data after multi-laser radar time synchronization. The time offset of the laser point in the data packet is .in, is the time offset of the laser point in the data packet; The time it takes for all the lasers in the lidar to complete one round of firing and charging is generally between 50 and 60 microseconds. is the emission time interval between two lasers; Seq is the channel number of the laser; Ind is the number of a complete round of laser data in the data packet.

[0125] The spatial synchronization of multiple laser radars is actually the conversion of coordinate systems between multiple laser radars, which includes the rotation and translation of coordinate systems. is a point in the laser radar coordinate system P, then the conversion formula for point p in the coordinate system L to the coordinate system S is: The coordinates in coordinate system S after transformation are .

[0126] In the formula, is the displacement of the origin of coordinate system L to P; R is a 3×3 rotation matrix.

[0127] .

[0128] in, is the pitch angle, is the rolling angle, is the yaw angle.

[0129] Step 202: When the test vehicle is performing automatic parking, the original point cloud is collected in real time by using multiple laser radars after time synchronization and space synchronization processing.

[0130] This application filters a large number of invalid point clouds in the original point cloud to obtain a valid point cloud range. The redundant parts in the XY plane are removed, and the valid range is selected. For this valid range, the redundant Z-axis point cloud is filtered out using a straight-through filter that takes into account the height difference. This is equivalent to obtaining a valid xyz three-dimensional space and removing the rest of the redundant ones.

[0131] Step 203: for a frame of original point cloud: the original point cloud is clipped according to the polygonal area formed by a plurality of preview points to obtain a three-dimensional point cloud of the polygonal area; each preview point is set in the target parking lot.

[0132] Among them, step 203 is to read a frame of original point cloud, first use conditional filtering based on polygonal area to determine the effective range of the point cloud in the XY plane, specifically including: according to the location and size of the smart car competition venue and the offset relative to the laser radar coordinate system, determine 6-10 preview points and crop the original point cloud data according to the polygonal area formed by the preview points.

[0133] Step 204: Use a straight-through filter to filter the set part of the point cloud in the vertical direction in the three-dimensional point cloud of the polygonal area to obtain the filtered point cloud, specifically including: using a straight-through filter that takes into account the height difference to filter the point cloud on the Z axis of the point cloud for the three-dimensional point cloud of the polygonal area to obtain a valid XYZ three-dimensional space point cloud, more specifically including: using the height difference between the test vehicle and the cone barrel to obtain part of the point cloud data representing the cone barrel, and using a suitable threshold range to filter the point cloud data that does not meet the requirements. The calculation formula is: . To test the height difference between the vehicle and the cone, is the height of the cone, is the height of the test vehicle. The threshold range is The pass-through filter filters out the point cloud data that does not meet the requirements.

[0134] Step 205: Calculate the distance measurement value of each laser point in the filtered point cloud, and update each laser point with each distance measurement value.

[0135] For the valid XYZ three-dimensional space point cloud obtained in step 204, an adaptive radius filter based on a distance metric is used to remove noise points. Specifically, a frame of point cloud data is read, and the distance from each point to the origin of each laser radar coordinate is calculated and normalized. The formula is as follows: .

[0136] in, ' is the distance metric; and They are the distances from a certain point to the origins of two radar coordinates, in meters; is the weight factor. Since the number of point clouds of low-line-count lidar is relatively low compared to the total point clouds, the weight factor is set to 0.3 to reduce its impact. Norm is the normalization constant.

[0137] Step 206: Map each updated laser point to the grid occupation map, traverse the number of laser points in each grid in the grid occupation map, and retain if the number is greater than the point cloud number threshold, otherwise remove the point cloud in the grid to obtain the point cloud information after preprocessing the original point cloud of the frame.

[0138] Steps 201 to 206 synchronize the time and space of the placed multiple laser radars, pre-process the data collected by the multiple laser radars, first select a valid xy area, and then select a suitable z-axis area in the xy area, so that a valid xyz three-dimensional space is found. Since there is still a noise point cloud in it, the final method is needed to remove the noise points, and finally a frame of pre-processed point cloud information is generated.

[0139] Step 207: Identify the position information of the test vehicle and the cones during the parking process based on the pre-processed point cloud information of each frame, and determine whether the test vehicle collides with the cones based on the position information of the test vehicle and the cones.

[0140] While collecting the actual path of the test vehicle during automatic parking according to the reference path, the automatic parking route planning evaluation method also includes: using multiple cameras in combination with a target detection model to detect the status of parking spaces in the target parking lot in real time; the target detection model is obtained by training the YOLO8 detection model, and the parking space status is idle or occupied. When there is no idle parking space, an alarm message is issued to ensure the safety of the target parking lot while meeting the test evaluation requirements.

[0141] like Fig.17 As shown, multiple cameras are used in combination with a target detection model to detect the status of parking spaces in a target parking lot in real time, specifically including steps 301 to 307.

[0142] Step 301, initialize the system: When the system starts, the target detection model is initialized. This step involves loading the pre-trained model weights and configuration, which is usually completed through the Detector class method.

[0143] Step 302, image acquisition (select input source): For example, the system captures real-time images from a surveillance camera. This is achieved through a timer-triggered function that is responsible for communicating with the camera interface and acquiring the latest image.

[0144] Step 303, data preprocessing: The acquired image is first preprocessed, including steps such as resizing and normalization, to meet the input requirements of the model.

[0145] Step 304, Parking space model detection: The preprocessed image is fed into the object detection model. The Detector class is responsible for this process, which takes the image as input and outputs the bounding box and status (free or occupied) of the detected parking space.

[0146] Step 305, result display: The detection results are processed and parsed to determine the exact location and status of each parking space. The output of the object detection model is parsed and mapped to specific parking spaces in the parking lot.

[0147] Step 306, status update and communication: Based on the detection results, the system updates the status of each parking space and provides this information to the system in real time through an interface or application programming interface (API).

[0148] Step 307, cyclic detection: the system repeats the above process at a set frequency to update the parking space status information in real time.

[0149] For UWB positioning in this application, the specific process includes: determining the distance or other data information between the unknown UWB positioning anchor point and the known indoor UWB base stations in the target parking lot, and then using the positioning algorithm and the obtained data to resolve the exact location of the anchor point. The specific positioning algorithm uses the TOF positioning method.

[0150] like Fig.18 As shown, this method records the time when the unknown positioning tag sends a signal to the base station and the time when the base station sends a signal to the unknown positioning tag, which is similar to the two handshakes of a network connection. Therefore, it does not rely on precise time synchronization between the two parts, and only estimates the distance through the relative flight time of the UWB.

[0151] The specific operations of the TOF positioning method include steps 401 to 404 .

[0152] Step 401: The UWB positioning anchor point with unknown position is located at a specific time point Send UWB pulse signal to the positioning base station.

[0153] Step 402: Locate the base station at a time point After receiving the UWB pulse signal time, at a point in time Send a reply signal.

[0154] Step 403: The UWB positioning anchor point with unknown position is at time point Receive the response signal from the positioning base station, the whole process takes , .

[0155] Step 404: The transmission time of the UWB signal between the unknown UWB positioning anchor point and the positioning base station is: , and Providing a local clock can make up for the TOA positioning method's high requirements for time synchronization between the positioning tag to be measured and the positioning base station, which leads to the problem of deviation in positioning information. Compared with one-way ranging, the ranging error is reduced, and the time measurement accuracy does not change significantly with the increase of the test distance, the performance is stable, and the measurement distance is far. In summary, the TOF positioning method is used as the positioning method of the UWB positioning tag of this application.

[0156] In various automatic parking competitions or automatic parking tests of smart cars, the total time of the entire process of automatic parking of smart cars is generally used as the only reference standard, which may not be perfect. This application adds new evaluation criteria on this basis to improve its original scoring system. This application proposes a multi-level evaluation standard, that is, the automatic parking of the test vehicle is evaluated based on global factors, local factors and local factors. Each factor is first given a full score of 100 points. Each factor contains multiple sub-factors, and then based on the sub-factors, points are added or subtracted according to the actual driving conditions to obtain the score of each factor. Finally, all factor scores are multiplied by their corresponding weights to obtain the final score. The formula for evaluating the automatic parking of the test vehicle is expressed as: .in, For the total score, Score the global factor, for The weight of Score for local factors, for The weight of To score other factors, for The scores of other factors are determined based on two test items, namely emergency braking and overtaking test. The scores of other factors can be determined through at least one emergency braking test or at least one overtaking test. The specific number of emergency braking and overtaking tests can be determined based on actual conditions.

[0157] During the test of the automatic parking of the smart car, the total time is the time it takes for the smart car to complete an automatic parking. The total time standard is set as , when the total time is greater than For the total path length, since the smart car used for testing and the smart car for global planning are doing automatic parking planning in the same parking lot, this application uses the ideal path obtained in the global planning as the standard for adding and subtracting points. The length of the ideal path (reference path) is It is obtained by adding up the Euclidean distances between every two points after interpolation. Similar to the total time of the entire journey, the score of the entire path length increases or decreases by one point every 0.1 meter until all points are subtracted.

[0158] The calculation formula for the global factor score is: .in, To set the ideal time, is the length of the reference path, is the actual total time, is the actual path length.

[0159] The local factors of the evaluation criteria include four sub-factors: actual path deviation area, actual path maximum deviation length, maximum average speed, and number of collisions. The actual path deviation area refers to the area between the actual driving path and the ideal path, such as Fig.19 As shown in the figure, the area is obtained by accumulating multiple intersection areas between the actual path and the ideal path; the maximum deviation length of the actual path is the maximum deviation distance between the ideal path point and the actual path point corresponding to its normal line, in meters; the maximum average speed is the maximum average speed during the automatic parking process; the number of collisions refers to the number of collisions between the smart car and static obstacles during the racing process. Fig.19 The coordinate system is the laser radar coordinate system.

[0160] .in, The deviation area standard is 3.2m 2 , is the actual path deviation area, which is the sum of the deviation areas of the reference path and the actual path. is the standard deviation length, is the maximum length of the path deviation between the reference path and the actual path, is the standard average speed, is the actual average speed, To test the number of collisions between the vehicle and the cone. If multiple automatic parkings are performed, It is the maximum value among multiple actual average speeds.

[0161] This application mainly selects emergency braking and overtaking as test items. These two test items can be added or deleted according to actual conditions. 50 points will be deducted when a test item fails. Emergency braking is a test item in which the main model car brakes itself when it finds a stationary obstacle parked in front and cannot avoid it. According to the parking lot object state model proposed in the previous article, the main smart car is judged as passed after the obstacle stops safely for more than 5 seconds, otherwise it will fail; overtaking is a scenario in which the main smart car decides to overtake when it finds an obstacle but can avoid it. When the main model car successfully overtakes and the distance is greater than 1 meter, it is judged as passed, otherwise 50 points will be deducted. The scores of other factors are determined by the emergency braking test items and the overtaking test items. The scores of other factors are expressed as: If the test vehicle passes the overtaking test, ,otherwise ; If the test vehicle passes the emergency brake test ,otherwise .

[0162] The emergency braking test item is a situation where there is a stationary obstacle parked in front of the test vehicle in the direction of travel and it cannot be avoided. If the test vehicle brakes itself without colliding with the stationary obstacle and the braking state exceeds 5 seconds, the test vehicle is judged to have passed the emergency braking test item, otherwise it is judged that the test vehicle has not passed the emergency braking test item. The criterion for the test vehicle not to collide with the stationary obstacle is that the vehicle model of the test vehicle does not overlap with the circumscribed circle of the stationary obstacle; the vehicle model is an equivalent double-circle model, and the two circles in the equivalent double-circle model are the circumscribed circles of the rear axle and the front axle of the test vehicle respectively.

[0163] The overtaking test item is a situation where there is an obstacle parked in front of the test vehicle in the direction of travel and there is space for avoidance. If the test vehicle exceeds the obstacle and the distance of the obstacle is greater than 1 meter, it is judged to have passed the overtaking test item, otherwise it is judged that the test vehicle has not passed the overtaking test item. The criterion for the test vehicle to pass the obstacle is that in the displacement time diagram, the shortest distance between the contour point of the test vehicle and the projection point of the obstacle is greater than the set safety distance.

[0164] This application uses parking lot laser radar, camera, UWB and other equipment to obtain the state of the smart car and the parking lot, the interaction state of the smart car and dynamic and static obstacles, and the state of the smart car in environmental factors to perform a multi-level trajectory evaluation method for the smart car during parking. This application realizes the real-time path detection of the test smart car and the analysis and comparison with the global planning path, calculates the error area for the evaluation of the real-time parking path, and scores the smart car trajectory planning based on the smart car's own state, offset and other parameters. This application can be widely used in the evaluation of smart car automatic parking systems.

[0165] Based on the same inventive concept, an embodiment of the present application also provides an automatic parking route planning evaluation device for implementing the automatic parking route planning evaluation method involved above. The present application provides an automatic parking route planning evaluation device including: a grid occupancy map construction module, which is used to construct a grid occupancy map of a target parking lot. A reference path determination module, which is used to plan a parking path according to the grid occupancy map, a starting point and a target point to obtain a reference path. An actual path acquisition module, which is used to acquire the actual path of a test vehicle during automatic parking according to the reference path. An evaluation factor determination module, which is used to determine global factors and local factors for automatic parking evaluation according to the reference path and the actual path; the global factors include the actual total time and the actual path length, and the local factors include the actual path deviation area, the maximum path deviation length, the actual average speed and the number of collisions. An evaluation module, which is used to evaluate the automatic parking of the test vehicle according to the global factors and the local factors.

[0166] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0167] This article uses specific examples to illustrate the principles and implementation methods of this application. The description of the above embodiments is only used to help understand the method and core ideas of this application. At the same time, for those skilled in the art, according to the ideas of this application, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting this application.

Claims

1. An automatic parking route planning evaluation method, characterized in that: The automatic parking route planning evaluation method comprises: Constructing a grid occupancy map of the target parking lot; Perform parking path planning according to the grid occupancy map, the starting point and the target point to obtain a reference path; collecting an actual path of the test vehicle during automatic parking according to the reference path; Determine global factors and local factors for automatic parking evaluation according to the reference path and the actual path; the global factors include actual total time and actual path length, and the local factors include actual path deviation area, maximum path deviation length, actual average speed and number of collisions; evaluating the automatic parking of the test vehicle according to the global factors and the local factors; In a static obstacle scenario, parking path planning is performed according to the grid occupancy map, the starting point and the target point to obtain a reference path, specifically including: On the grid occupancy map, the test vehicle is taken as a mass point and a bidirectional A The search algorithm determines a first path between the starting point and the UWB anchor point, and a second path between the UWB anchor point and the target point, wherein the first path and the second path are connected to form a first global path; the UWB anchor point is set at a set position of the target parking lot; Smoothing and splicing the first global path using multiple quintic Bezier curves, and if the number of control points in the last segment of the first global path is less than 6, smoothing and splicing using quartic Bezier curves to obtain the reference path; In a dynamic obstacle scenario, multiple sections of quintic Bezier curves are used to smooth and splice the first global path. If the number of control points in the last section of the first global path is less than 6, a quartic Bezier curve is used for smooth splicing. After the reference path is obtained, parking path planning is performed according to the grid occupancy map, the starting point and the target point to obtain a reference path, which specifically includes: Converting the reference path as an initial reference path into a Frenet coordinate system; A moving target whose moving speed in the target parking lot is greater than a set threshold is defined as a dynamic interference object; the moving target includes pedestrians and cars; Projecting the dynamic disturbances into a displacement-time graph; Discretize the displacements in the displacement-time graph into equal distances to obtain discrete nodes of each displacement; Creating a state transition equation between every two adjacent displacement discrete nodes; the state transition equation is a state transition equation for path position and velocity; wherein the path position and velocity are the path position and velocity of the test vehicle in the Frenet coordinate system, respectively; Based on set constraints, the speed of the test vehicle is optimized for the state transition equation of each two adjacent displacement discrete nodes to obtain the speed of each displacement discrete node, and the speeds of each displacement discrete node constitute a first speed curve; the set constraints include speed limit constraints, test vehicle physical constraints, and overlap constraints between the test vehicle and dynamic interference objects; Based on the smoothness of the speed curve and the deviation between the speed curve and the reference speed curve, performing secondary programming on the first speed curve to obtain a second speed curve; and using a path generated according to the second speed curve as the reference path; The formula for evaluating the automatic parking of the test vehicle according to the global factors and the local factors is expressed as: ； in, For the total score, Score the global factor, for The weight of Score the local factor, for The weight of To score other factors, for The weight of ； in, To set the ideal time, is the length of the reference path, is the actual total time, is the actual path length; ； in, is the deviation area standard, is the actual path deviation area, which is the sum of the areas of the deviations between the reference path and the actual path. is the standard deviation length, is the maximum length of the path deviation between the reference path and the actual path, is the standard average speed, is the actual average speed, is the number of collisions between the test vehicle and the cone barrel; The other factor scores are determined by the emergency braking test item and the overtaking test item, and the other factor scores are expressed as: ; If the test vehicle passes the overtaking test item, ,otherwise If the test vehicle passes the emergency brake test item, ,otherwise ; The emergency braking test item is a situation where there is a stationary obstacle parked in front of the test vehicle in the direction of travel and it cannot be avoided. If the test vehicle brakes itself without colliding with the stationary obstacle and the braking state lasts for more than 5 seconds, it is determined that the test vehicle has passed the emergency braking test item; otherwise, it is determined that the test vehicle has not passed the emergency braking test item; The judgment criterion that the test vehicle does not collide with the stationary obstacle is that the vehicle model of the test vehicle does not overlap with the circumscribed circle of the stationary obstacle; the vehicle model is an equivalent double-circle model, and the two circles in the equivalent double-circle model are the circumscribed circles of the rear axle and the front axle of the test vehicle respectively; The overtaking test item is a situation where there is an obstacle parked in front of the test vehicle in the direction of travel and there is a space for avoidance. If the test vehicle exceeds the obstacle and the distance of exceeding the obstacle is greater than 1 meter, it is determined that the overtaking test item has been passed, otherwise it is determined that the test vehicle has not passed the overtaking test item; The judgment criterion for the test vehicle to exceed the obstacle is that in the displacement time diagram, the shortest distance between the contour point of the test vehicle and the projection point of the obstacle is greater than the set safety distance.

2. The automatic parking route planning and evaluation method according to claim 1, characterized in that: Construct a grid occupancy map of the target parking lot, including: A tightly coupled LiDAR inertial odometry framework based on smoothing and mapping is used to collect a point cloud map of the target parking lot; Divide the point cloud map into a driving area and a parking area to obtain a Lanelet2 map; The Lanelet2 map is grid mapped and the positioning information of the UWB anchor point is added to generate the grid occupancy map.

3. The automatic parking route planning and evaluation method according to claim 1, characterized in that: In a dynamic obstacle scenario, the test vehicle performs automatic parking according to the reference path, specifically including: The reference speed and reference acceleration are provided by the second speed curve, and a PID control algorithm is used to perform dual PID control on the speed and acceleration of the test vehicle.

4. The automatic parking route planning and evaluation method according to claim 1, characterized in that: Collecting the actual path of the test vehicle during the automatic parking process according to the reference path specifically includes: According to the UWB positioning anchor point located on the test vehicle, based on the UWB base station through the TOF positioning method, real-time positioning is performed during the automatic parking process of the test vehicle to obtain the actual path.

5. The automatic parking route planning and evaluation method according to claim 1, characterized in that: When the test vehicle performs automatic parking according to the reference path, the process specifically includes: Perform time and space synchronization for multiple laser radars; During the automatic parking process of the test vehicle, the original point cloud is collected in real time by using multiple laser radars after time synchronization and space synchronization processing; For a frame of the original point cloud: the original point cloud is clipped according to a polygonal area formed by a plurality of preview points to obtain a three-dimensional point cloud of the polygonal area; each preview point is set in the target parking lot; Using a straight-through filter to filter a set portion of point clouds in a vertical direction in the three-dimensional point cloud of the polygonal area to obtain a filtered point cloud; Calculating the distance measurement value of each laser point in the filtered point cloud, and updating each laser point with each distance measurement value; Mapping each updated laser point to the grid occupation map, traversing the number of laser points in each grid in the grid occupation map, retaining if the number is greater than the point cloud number threshold, otherwise discarding the point cloud in the grid, and obtaining the point cloud information of the original point cloud preprocessed in the frame; The position information of the test vehicle and the cone barrel during the parking process is identified based on the pre-processed point cloud information of each frame to determine whether the test vehicle collides with the cone barrel.

6. The automatic parking route planning and evaluation method according to claim 1, characterized in that: While collecting the actual path of the test vehicle during the automatic parking process according to the reference path, the automatic parking route planning evaluation method further includes: A plurality of cameras are used in combination with a target detection model to detect the status of parking spaces in the target parking lot in real time; the target detection model is obtained by training a YOLO8 detection model, and the status of the parking spaces is either free or occupied. When there are no free parking spaces, an alarm message is issued.

7. An automatic parking route planning and evaluation device, characterized in that: The automatic parking route planning and evaluation device comprises: A grid occupancy map construction module, used to construct a grid occupancy map of a target parking lot; A reference path determination module, configured to plan a parking path according to the grid occupancy map, a starting point and a target point, and obtain a reference path; An actual path acquisition module, used to acquire an actual path of the test vehicle during the automatic parking process according to the reference path; an evaluation factor determination module, configured to determine global factors and local factors for automatic parking evaluation according to the reference path and the actual path; the global factors include actual total time and actual path length, and the local factors include actual path deviation area, maximum path deviation length, actual average speed, and number of collisions; An evaluation module, configured to evaluate the automatic parking of the test vehicle according to the global factors and the local factors; In a static obstacle scenario, parking path planning is performed according to the grid occupancy map, the starting point and the target point to obtain a reference path, specifically including: On the grid occupancy map, the test vehicle is taken as a mass point and a bidirectional A The search algorithm determines a first path between the starting point and the UWB anchor point, and a second path between the UWB anchor point and the target point, wherein the first path and the second path are connected to form a first global path; the UWB anchor point is set at a set position of the target parking lot; Smoothing and splicing the first global path using multiple quintic Bezier curves, and if the number of control points in the last segment of the first global path is less than 6, smoothing and splicing using quartic Bezier curves to obtain the reference path; In a dynamic obstacle scenario, multiple sections of quintic Bezier curves are used to smooth and splice the first global path. If the number of control points in the last section of the first global path is less than 6, a quartic Bezier curve is used for smooth splicing. After the reference path is obtained, parking path planning is performed according to the grid occupancy map, the starting point and the target point to obtain a reference path, which specifically includes: Converting the reference path as an initial reference path into a Frenet coordinate system; A moving target whose moving speed in the target parking lot is greater than a set threshold is defined as a dynamic interference object; the moving target includes pedestrians and cars; Projecting the dynamic disturbances into a displacement-time graph; Discretize the displacements in the displacement-time graph into equal distances to obtain discrete nodes of each displacement; Creating a state transition equation between every two adjacent displacement discrete nodes; the state transition equation is a state transition equation for path position and velocity; wherein the path position and velocity are the path position and velocity of the test vehicle in the Frenet coordinate system, respectively; Based on set constraints, the speed of the test vehicle is optimized for the state transition equation of each two adjacent displacement discrete nodes to obtain the speed of each displacement discrete node, and the speeds of each displacement discrete node constitute a first speed curve; the set constraints include speed limit constraints, test vehicle physical constraints, and overlap constraints between the test vehicle and dynamic interference objects; Based on the smoothness of the speed curve and the deviation between the speed curve and the reference speed curve, performing secondary programming on the first speed curve to obtain a second speed curve; and using a path generated according to the second speed curve as the reference path; The formula for evaluating the automatic parking of the test vehicle according to the global factors and the local factors is expressed as: ； in, For the total score, Score the global factor, for The weight of Score the local factor, for The weight of To score other factors, for The weight of ； in, To set the ideal time, is the length of the reference path, is the actual total time, is the actual path length; ； in, is the deviation area standard, is the actual path deviation area, which is the sum of the areas of the deviations between the reference path and the actual path. is the standard deviation length, is the maximum length of the path deviation between the reference path and the actual path, is the standard average speed, is the actual average speed, is the number of collisions between the test vehicle and the cone barrel; The other factor scores are determined by the emergency braking test item and the overtaking test item, and the other factor scores are expressed as: ; If the test vehicle passes the overtaking test item, ,otherwise If the test vehicle passes the emergency brake test item, ,otherwise ; The emergency braking test item is a situation where there is a stationary obstacle parked in front of the test vehicle in the direction of travel and it cannot be avoided. If the test vehicle brakes itself without colliding with the stationary obstacle and the braking state lasts for more than 5 seconds, it is determined that the test vehicle has passed the emergency braking test item; otherwise, it is determined that the test vehicle has not passed the emergency braking test item; The judgment criterion that the test vehicle does not collide with the stationary obstacle is that the vehicle model of the test vehicle does not overlap with the circumscribed circle of the stationary obstacle; the vehicle model is an equivalent double-circle model, and the two circles in the equivalent double-circle model are the circumscribed circles of the rear axle and the front axle of the test vehicle respectively; The overtaking test item is a situation where there is an obstacle parked in front of the test vehicle in the direction of travel and there is a space for avoidance. If the test vehicle exceeds the obstacle and the distance of exceeding the obstacle is greater than 1 meter, it is determined that the overtaking test item has been passed, otherwise it is determined that the test vehicle has not passed the overtaking test item; The judgment criterion for the test vehicle to exceed the obstacle is that in the displacement time diagram, the shortest distance between the contour point of the test vehicle and the projection point of the obstacle is greater than the set safety distance.

Images