[0082] Please refer to Figure 1 , Figure 3 Sum Figure 4 Embodiments of the present invention one as:
[0083] An unmanned avoidance driving method based on laser radar and UWB array, including:
[0084] S1, start the laser radar, acquire real-time positioning information based on the area positioning coordinate system established based on the preset UWB array, and control the vehicle along the pre-acquired path trajectory according to the real-time positioning information;
[0085] In this embodiment, the real-time positioning information includes real-time positioning coordinates and real-time navigation angles, and the regional positioning coordinate system created based on the preset UWB array is specifically: obtaining the vehicle and various described in accordance with preset intervals. The distance between the UWB base station acquires the base station coordinates of the nearest four of the UWB base stations, select two of the base station coordinates in which the longitudinal sites are small as the first base station coordinate and the second base station coordinate, wherein the first base station coordinate The abscissa is smaller than the abscissa of the second base station coordinate, the first distance A of the vehicle and the first base station coordinate, the second distance C of the vehicle and the second base station coordinate C, the first base station The third distance B of the coordinates and the second base station coordinate bums into the following formula:
[0086] P = (A + B + C) / 2;
[0087] S = √ [p * (p-a) * (p-b) * (p-c)]
[0088] S = h * b / 2
[0089] a 2 = H 2 + T 2
[0090] The first coordinates of the first base station coordinates (X, Y) of the vehicle are calculated, and the longitudinal difference h of the first base station coordinate (X, Y) is calculated to obtain real-time positioning coordinates (X + T, Y + H);
[0091] The vehicle traveling vector Vc = (X1-X0, Y1-Y0) is calculated based on the positioning coordinates (X0, Y0) of the position (X0, Y0) (X0, Y0) (X0, Y0) before the preset time interposed coordinates (X1, Y1) and the preset time interval; and by y The positive vector VY = (0, 1) of the axis is according to the following formula:
[0092] θ = Arccos (VC * VY / (| VC | * | VY |)
[0093] When calculating the angle θ, if X1-X0> 0, the real-time gantry angle is θ, if X1-X0 <0, the real-time gantry angle is π-θ, if X1-x0 = 0 and Y1-Y0> = 0, Then the head is 0, if X1-X0 = 0 and Y1-Y0 <0, the real-time navigation angle is 180 °.
[0094] According to the calculated real-time positioning coordinates and real-time navigation angles to control the vehicle along the pre-acquired path trajectory, that is, according to real-time positioning coordinates, real-time navigation angle, and path trajectory in real time, it can be obtained by real-time coordinates and target trajectory points. The target navigation angle, and controls the steering of the vehicle to adjust the current real-time navigation angle to the target navigation angle, thereby controlling the vehicle steering, traveling along the path trajectory.
[0095] S2, the obstacle recognition is performed according to the point cloud data of the laser radar, and the three-dimensional data of the obstacle and the obstacle laser radar coordinates are obtained, and the obstacle UWB coordinates are calculated according to the obstacle laser radar coordinates, and according to the obstacle UWB coordinates And the three-dimensional data is determined whether or not avoiding avoidance, and when the avoidance is required, the process proceeds to step S3, otherwise it will continue along the path trajectory;
[0096] Wherein, the point cloud data according to the laser radar is recognized according to the point cloud data of the laser radar, and the three-dimensional data of the obstacle and the obstacle laser radar coordinate are specifically:
[0097] Traverse the point cloud data of the laser radar, the point distance between the calculation point and the point, the two points smaller than the preset threshold, each of which is included in each obstacle. All points of point;
[0098] In the coordinates of all points of the point set, the length L of the obstacle is calculated based on the maximum horizontal coordinate and the minimum abscissa, calculate the width W of the obstacle according to the maximum ordinate and the minimum abscissa, according to the maximum vertical coordinates The height h of the obstacle is calculated from the minimum vertical coordinates to obtain three dimensions of the obstacle;
[0099] The obstacle laser radar coordinates are calculated according to the clustering algorithm, the obstacle laser radar coordinates including radar center point coordinates and radar edge point coordinates;
[0100] Wherein, the step S2 calculates the obstacle the UWB coordinates according to the obstacle laser radar coordinate.
[0101] Getting the current real-time positioning information of the vehicle includes real-time positioning coordinates (X, Y), and real-time navigation angle θ, laser radar coordinates for the obstacle (X ob Y ob ) After entering the following formula:
[0102] X '= x ob Cos θ-y ob sinθ
[0103] Y '= x ob SIN θ + Y ob Cos θ
[0104] The first coordinate (x ', y') can be obtained, and can be obtained according to the current real-time positioning coordinates (X, Y) of the vehicle:
[0105] x ob '= X + x'
[0106] y ob '= Y + y'
[0107] To get an obstacle UWB coordinate (X ob ', Y ob ');
[0108] In this embodiment, according to the laser radar point cloud data, the clustering algorithm and the distance between the obstacles in the laser radar (ie, the obstacle laser radar coordinate), including the center point coordinates and the edge point coordinates. (Ie radar center point coordinates and radar edge point coordinates), and calculate the three-dimensional information of the obstacle, and the real-time positioning information is obtained, and the real-time positioning coordinates, real-time navigation angles, and obstacles laser radar coordinates into formulas and further calculate, The obstacle UWB coordinates can be obtained, including the UWB center point coordinates of the calculated obstacle by the radar center point coordinates and the UWB edge point coordinates.
[0109] Wherein, the obstacle UWB coordinates and the three-dimensional data judgment need to be avoided:
[0110] According to the obstacle UWB coordinates, it is determined whether there is a point (x_t, y_t) and the distance of the path trajectory less than the preset hazardous distance and H is greater than the preset hazard height. If it is necessary to avoid, it is necessary to avoid ;
[0111] Where X min ≤ x_t≤x max Y min ≤Y_t≤y max , Where x min , X max Y min Sum max The minimum horizontal coordinates, maximum horizontal coordinates, minimum ordinate, maximum longitudinal coordinates, respectively, the UWB edge point coordinates of the obstacle UWB coordinates, respectively;
[0112] In this embodiment, according to the obstacle GPS coordinate, we can determine whether the obstacle and path trajectory information is conflicted based on the regional positioning coordinate system established based on the UWB array, ie, whether it is in the dangerous range of the path trajectory, that is, the preset dangerous distance. In this embodiment, the preset hazard is considered according to the vehicle width, for example, the dangerous distance of 4 meters in the vehicle width is 2 meters. In other equivalents, the preset hazard distance can be appropriately increased or decreased on the basis of the vehicle width and vehicle length according to the safety consideration.
[0113] Specifically, according to the maximum smallest coordinates in the UWB edge point coordinates, it is determined whether there is a little horizontal coordinate between the maximum horizontal coordinates and the minimum abscissa, the ordinate between the maximum ordinate and the minimum longitudinal coordinate, and with the path The distance of the trajectory is smaller than the preset dangerous distance, and the height of the obstacle (the three-dimensional coordinates of the obstacle is calculated, and the height information h) is higher than the preset hazard height. If it is necessary to avoid the avoidance, it is necessary to avoid the avoidance, Drive along the original trajectory. In this embodiment, the preset hazard height is half of the height of the vehicle chassis. In other equivalents, the preset hazard height can also be pre-set, or according to the height of the chassis.
[0114] S3, when the distance of the obstacle is preset, the avoidance route is generated according to the preset avoidance algorithm, and travels along the avoidance route until the obstacle and returns to the predetermined channel trajectory. ;
[0115] The path trajectory contains a plurality of ordered target trajectory points, combined Figure 4 It can be seen that the step S3 is specifically:
[0116] The maximum horizontal coordinates in the edge point in the obstacle laser radar coordinates are used as the value obtained by the preset safety spacing as the maximum value of the avoidance. m;
[0117] When the obstacle is preset during the vehicle, according to the radar center point coordinates and the radar edge point coordinates, the spacing is in the horizontal housing range (0, x m ] Take the horizontal coordinate X1 in the following formula, respectively.
[0118] (x1-r) 2 + Y1 2 = R * r
[0119] Thereby, a plurality of turn radar trajectories (X1, Y1) are determined, wherein R is a preset turning radius of the vehicle, the preset distance is greater than the vehicle turn radius R plus preset safety distance;
[0120] Turning radar trajectory point b by the horizontal coordinate m Y b ), Determine a point C (X m Y c ), Where Y c The maximum longitudinal coordinates in the radar edge point coordinates are added to the preset safety spacing, and a point d after the obstacle is greater than the preset hazardous distance, thus controlling the vehicle along the turn radar trajectory to the turn radar trajectory. Point B and the path along B-> C-> D runs to the D point after the current GPS information and the path trajectory continue to perform unmanned tracking;
[0121] like Figure 4 As shown in the present embodiment, if the result of avoiding the need is needed to avoid, the avoidance treatment is required based on the laser radar. According to the turning radius of the vehicle, the preset turn radius, the resulting point O (R, 0), and the largest horizontal coordinate of the radar edge point coordinate of the obstacle laser radar coordinate (the maximum value of the horizontal) m ), Thereby calculating a plurality of turning radar trajectory points, and determines point B, c, d, control the vehicle along the rotary radar trajectory point B, while traveling along the route B-> C-> D can bypass the obstacle At this time, it can be real-time according to the area positioning coordinate system established by the UWB array, thereby controlling the vehicle to continue to enter and drive unmanned by the path trajectory.
