Method for realizing automatic driving system of low-speed sweeper

By using Viobot mapping, YOLO model detection, and AprilTag tagging, combined with Bézier curve path planning, the problems of high cost and insufficient obstacle avoidance performance of low-speed sweepers have been solved, achieving low-cost autonomous obstacle avoidance and charging functions.

CN118963342APending Publication Date: 2024-11-15TIANJIN UNIVERSITY OF TECHNOLOGY
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202411018112.X
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-07-29
Publication Date
2024-11-15

AI Technical Summary

Technical Problem

Traditional autonomous driving systems are too expensive for low-speed cleaning robot vehicles, making large-scale commercialization difficult, and pure vision systems have insufficient obstacle avoidance performance in complex environments.

Method used

The system uses Viobot for mapping and path recording, combines YOLO models for obstacle and road edge detection, uses Bézier curves to plan obstacle avoidance paths, and enables autonomous reversing charging through AprilTag tags. It reduces costs and improves obstacle avoidance capabilities by integrating computer vision and sensors.

Benefits of technology

It enables low-speed sweepers to autonomously avoid obstacles and recharge themselves, reducing system costs while maintaining high obstacle avoidance performance in complex environments.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118963342A_ABST
    Figure CN118963342A_ABST
Patent Text Reader

Abstract

A method for realizing an automatic driving system of a low-speed sweeper comprises the following steps: S1, after key frames of a path needing to be swept are recorded through Zichuan viobot, mapping and track pre-recording are carried out; the coordinate points in the local coordinate system are stored, quaternion is solved through an arc tangent function to obtain an Euler angle, and a yaw angle is recorded; s2, tracing is carried out through the built graph and the track, distance matching is carried out on a current point and a recorded point, and then the angular velocity is published through the difference value of the distance from the point to the straight line and the yaw angle; and S3, performing obstacle avoidance and obstacle stopping and track correction of visual detection on the obstacles and the road side lines through the trained model, converting image pixel coordinates of the centers of the obstacles and the side lines into actual three-dimensional world coordinates by calculating a normalized plane, and encapsulating the obtained distance and depth information into an ROS topic form to be published. The Bezier curve is used for obstacle avoidance to plan an obstacle avoidance path; and S4, performing autonomous reversing charging by using an AprilTag tag, and performing calculation by using an Euler angle obtained in real time by using a trigonometric function and a detection tag and a three-dimensional coordinate point to obtain an angular velocity. According to the invention, the sensor cost of the motor sweeper can be reduced, and the complexity of algorithm implementation is reduced.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention relates to the technical field of computer vision, path planning, model training detection and coordinate system conversion, and in particular to a sweeper control system that realizes automatic driving and obstacle avoidance. Background Art

[0002] In recent years, autonomous driving technology has received widespread attention in road sweeping and has shown great potential for application. Autonomous driving technology aims to enable vehicles to travel safely and efficiently without human intervention by combining multiple sensors and intelligent algorithms. The core goal of this technology is to improve traffic efficiency, reduce accidents caused by human errors, and provide society with a more environmentally friendly and convenient way of travel. Although this technology has made significant progress, it still faces many challenges.

[0003] Traditional autonomous driving systems mainly rely on high-precision sensors such as lidar and cameras, which can provide rich environmental data. Lidar can generate high-resolution three-dimensional maps to help vehicles accurately locate and avoid obstacles; cameras can capture information such as road signs, traffic lights and pedestrians. However, the high cost of these devices makes the price of the entire system high, making it difficult to achieve large-scale commercial applications. Especially in specific scenarios, such as low-speed sweeping robot cars on fixed roads, these high-precision sensors are too expensive and impractical.

[0004] To address the cost issue, we use a pure vision approach to achieve autonomous driving through cameras and computer vision algorithms. This approach not only significantly reduces costs, but also makes full use of existing visual processing technology. The pure vision autonomous driving system uses deep learning algorithms to analyze images captured by the camera in real time to identify road conditions, obstacles, and traffic signs. Although the performance of the pure vision system in handling complex environments may not be as good as that of LiDAR, its cost-effectiveness makes it an ideal choice for low-speed cleaning robot cars. Summary of the invention

[0005] The purpose of the present invention is to make up for the deficiencies of the prior art and provide a method for realizing an automatic driving system for a low-speed road sweeper. To this end, the present invention specifically adopts the following technical solutions:

[0006] S1: After recording the key frames of the path to be cleaned by Zichuan Viobot, a map is constructed. On the basis of the constructed map, the position and posture information output by Zichuan Viobot is recorded, the coordinate points in the local coordinate system are saved, and the quaternion is solved by the inverse tangent function to obtain the Euler angle, and the yaw angle is recorded.

[0007] S2: Use the established graph and trajectory to find the track, use the current point to match the distance with the recorded point, and then find the previous point and the next point adjacent to it in the recorded file to form a straight line.

[0008] The angular velocity is published by the difference between the distance from the point to the line and the yaw angle;

[0009] S3: Obstacle avoidance, stopping and trajectory correction are performed by visually detecting obstacles and road edges through the trained YOLO model. In addition, the image pixel coordinates of the center of obstacles and edges are converted into actual 3D world coordinates by calculating the normalized plane, and the acquired distance and depth information are encapsulated into a ROS topic for publication. The obstacle avoidance path is planned using Bezier curves, and a set of control points can be defined to generate a smooth obstacle avoidance path.

[0010] S4: Use AprilTag to obtain the position and posture of the tag in three-dimensional space. According to the identified tag position and the current position of the vehicle, plan the vehicle's reverse path for autonomous reverse charging. By obtaining the Euler angle and three-dimensional coordinate points of the tag in real time, the angular velocity is obtained through basic calculation.

[0011] The hardware used in the present invention is composed as follows:

[0012] Zichuan VIOBOT PRO module × 1

[0013] Bibikaryon binocular RGBD camera × 1;

[0014] Ruiche robot chassis × 1;

[0015] Edge computing host Lingxi HS140×1. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings of the embodiments will be briefly introduced below. Obviously, the drawings in the following description only relate to some embodiments of the present invention, but are not intended to limit the present invention.

[0017] Figure 1 : Flow chart of automatic driving system of low-speed road sweeper.

[0018] Figure 2 :Device hardware composition. DETAILED DESCRIPTION

[0019] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with specific embodiments and with reference to the accompanying drawings.

[0020] Reference Figure 1 , a method for realizing an automatic driving system of a low-speed sweeper, the method comprising the following steps:

[0021] Step S1: Record key frames of the path to be cleaned by Zichuan Viobot, build a map and pre-record the trajectory;

[0022] Step S11: Start the SLAM algorithm of Zichuan VIOBOT, the three-dimensional coordinate points and yaw angles in the / pr_loop / odometry_rect topic.

[0023] Step S12: Write a ROS node that records the trajectory and save the three-dimensional coordinate points and yaw angles into the info.txt file.

[0024] Step S2: Use the established graph and trajectory to find the closest point x to the current point after matching the distance between the current point and the recorded points 1 Select a two-dimensional coordinate point x whose forward and backward distances are both 0.6m 0 With x 2 The plane straight line can be obtained;

[0025] Step S21: Find the current point and the obtained plane line, and then combine the PID control algorithm, multiply the distance from the point to the line by a coefficient obtained based on empirical values, and use the obtained result as the angular velocity released by the car.

[0026] Step S22: When the vehicle is turning, the angular velocity obtained in step S21 has a certain lag, which is not effective during the turning process. Therefore, the yaw angle can be used to better obtain the angle that the vehicle should turn. Only the difference in the yaw angle can be used to obtain the angular velocity that the car should turn.

[0027] Step S23: After the vehicle is tracked and loop detection is performed, only the current point is compared with the nearest point, which may cause the focus to be mistakenly judged as the starting point. To avoid this situation, a sliding window method is used to select the first twenty points and the last twenty windows for comparison in turn, thereby avoiding misjudgment.

[0028] Step S3: Obstacle avoidance, stopping and trajectory correction are performed by visually detecting obstacles and road edges through the trained model. Obstacle avoidance uses a third-order Bezier curve:

[0029] B(t)=(1-t) 3 p 0 +3t(1-t) 2 p 1 +3t 2 (1-t)p 2 +t 3 p 3

[0030] After obtaining a trajectory using the above formula, the translation matrix and rotation matrix are used to change the coordinate system, and the obstacle avoidance trajectory is spliced ​​onto the tracking trajectory, thereby realizing the obstacle avoidance path planning.

[0031] Step S31: Collect image data of common obstacles on the road, including people, cars, bicycles, motorcycles, cats, dogs, and birds. Collect image data of white edges of the road.

[0032] Step S32: Use the labeling tool LabelImg to label the collected images

[0033] Step S33: Divide the labeled data set into a training set and a validation set, and organize the data structure according to the requirements of YOLOv5. Train two models separately. Use the trained model to infer the image to obtain the detection result, use non-maximum suppression to retain the best detection result, and filter out the detection result with higher confidence based on the detection confidence. Finally, the detected data is combined with the depth distance obtained by the depth camera to encapsulate it into a ROS topic and publish it.

[0034] Step S34: Calculate the normalized plane coordinates of the obstacle center and the roadside center in the image:

[0035] x=(uc x ) / f x

[0036] y=(uc x ) / f x

[0037] Undistort the coordinate points: x, y = undistort(x, y)

[0038] Calculate the distance based on the depth and scale information: X = x·Depth·Scale

[0039] Step S35: Subscribe to the topic of obstacle detection, and select control points based on the size of the sweeper itself, draw a Bezier curve, and transform the generated Bezier curve into a coordinate system through the rotation matrix and translation vector, and connect the obstacle avoidance trajectory to the trajectory point of the current coordinate system of the car.

[0040] Step S4: Use the AprilTag tag to perform autonomous reverse charging, and use trigonometric functions and the Euler angles obtained from the detection tag and the three-dimensional coordinate points to calculate the angular velocity.

[0041] Step S41: Park the sweeper at a distance of 2-3 meters from the charging head, and start the rear camera of the sweeper to detect the tag.

[0042] Step S42: If the camera does not detect the tag, the angular velocity is published until the tag is detected. After the tag is detected, the position and posture of the sweeper relative to the charging pile are obtained, including the three-dimensional position point and the attitude angle.

[0043] Step S43: Use the obtained distance and angle to calculate the angular velocity that needs to be published using trigonometric functions until the vehicle and the charging head are in a straight line. Then stop publishing the angular velocity and only publish the linear velocity to allow the sweeper to get close to the charging head until the charging signal is obtained and the node stops running.

[0044] The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art can easily think of various equivalent modifications or substitutions within the technical scope disclosed by the present invention, and these modifications or substitutions should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention shall be based on the protection scope of the claims.

Claims

1. A method for realizing an automatic driving system for a low-speed road sweeper, characterized in that: The method comprises the following steps: S1: After recording the key frames of the path to be cleaned by Zichuan Viobot, a map is constructed. On the basis of the constructed map, the position and posture information output by Zichuan Viobot is recorded, the coordinate points in the local coordinate system are saved, and the quaternion is solved by the inverse tangent function to obtain the Euler angle, and the yaw angle is recorded. S2: Use the established graph and trajectory to find the track. Use the current point to match the distance with the recorded point, find the previous point and the next point adjacent to it in the record file to form a straight line, and publish the angular velocity through the difference between the distance from the point to the straight line and the yaw angle; S3: Obstacle avoidance, stopping and trajectory correction are performed by visually detecting obstacles and road edges through the trained YOLO model. In addition, the image pixel coordinates of the center of obstacles and edges are converted into actual 3D world coordinates by calculating the normalized plane, and the acquired distance and depth information are encapsulated into a ROS topic for publication. The obstacle avoidance path is planned using Bezier curves, and a set of control points can be defined to generate a smooth obstacle avoidance path. S4: Use AprilTag to obtain the position and posture of the tag in three-dimensional space. According to the identified tag position and the current position of the vehicle, plan the vehicle's reverse path for autonomous reverse charging. By obtaining the Euler angle and three-dimensional coordinate points of the tag in real time, the angular velocity is obtained through basic calculation.

2. A method for realizing an automatic driving system for a low-speed road sweeper according to claim 1, characterized in that: In step S1, when recording key frames, use the node records provided by Zichuan viobot and write them into the device, and create a bag-of-words map for quick retrieval of map data and loop detection. When pre-recording the trajectory, subscribe to the three-dimensional coordinate points in the topic / pr_loop / odometry_rect published by the Zichuan camera module, record a point every 0.3m, and write it into the trajectory file. After parsing the quaternion angle to calculate the Euler angle, the yaw angle is also written into the trajectory file. The data in the trajectory file is divided into four columns and saved in the file, namely: xyz yaw. For subsequent tracking.

3. A method for realizing an automatic driving system for a low-speed road sweeper according to claim 1, characterized in that: In step S2, the current point is compared and matched with the trajectory point and then the tracking navigation is performed. This process includes calculating the minimum distance between the current point and all the points in the trajectory, obtaining the trajectory point x1 closest to the current point, finding x0 that is two points away in the forward direction and x2 that is two points away in the backward direction, connecting the two-dimensional coordinate points x0 and x2 into a plane straight line, and then using the distance from x1 to the straight line multiplied by a proportional coefficient kp as the release of the angular velocity, and the proportional coefficient is obtained based on the empirical value. At the same time, the difference between the yaw angles of points x1 and x2 is used as a supplement to the release of the angular velocity.

4. A method for realizing an automatic driving system for a low-speed road sweeper according to claim 1, characterized in that: In step S3, yolov5 is used to train the road obstacles and road sidelines that need to be detected to achieve the functions of obstacle avoidance and trajectory correction. First, the image data is labeled for model training. By segmenting the extracted obstacles and sideline features, the yolov5 model and image processing algorithm are used to extract and identify the features, and the loss function is defined for model training. Perform model reasoning and post-processing: perform NMS on the detected bounding boxes to remove overlapping boxes and retain the best detection results. When the IoU is greater than the set threshold, delete the bounding boxes with larger overlaps. Use the polynomial fitting method to fit the detected road sideline points to obtain continuous sidelines. Fitting formula: y=ax 2 +bx+c. Finally, continue to adapt the model and recompile the model to convert it into a structure supported by Lingxi. In terms of obstacle avoidance, after using vision to detect the bounding box of the obstacle, the depth information of the bounding box can be obtained through the bicam. In this way, the lateral and longitudinal distances (Length, Depth) between the sweeper and the obstacle can be obtained, plus the current position of the vehicle (x, y), and then combined with the length and width of the vehicle (Car lengt ,Car width ). The coordinates of the obstacle avoidance points can be obtained: (x,y)(x+Car length +Depth,y+Car width +Length)(x+2*Car length +2*Depth,y) Set these three points as p1, p2, and p3 respectively, and let (0≤t≤1) represent the points on the curve. Substitute into the third-order Bezier curve formula: B(t)=(1-t) 3 p0+3t(1-t) 2 p1+3t 2 (1-t)p2+t 3 p3 By drawing Bezier curves as control points, a smooth obstacle avoidance trajectory can be obtained.

5. A method for realizing an automatic driving system for a low-speed road sweeper according to claim 1, characterized in that: In step S4, the tag is used for automatic reversing and recharging. The AprilTag tag is placed above the charging pile. When the vehicle approaches the charging pile area, the camera captures the tag image and identifies the position and posture of the tag through the AprilTag detection algorithm. The position and size of the tag in the image are combined with the internal parameters of the camera to calculate the position and direction of the tag in three-dimensional space. Then the path of the vehicle's reversing is planned, and the position and posture of the vehicle are continuously adjusted through real-time recognition of the tag.