Navigation method based on RTK laser vision fusion

Abstract

The invention relates to the technical field of navigation, in particular to a navigation method based on RTK laser vision fusion, which comprises the following steps: checking RTK positioning accuracy and a signal transmission condition with an upper computer; acquiring internal parameters of the camera and the laser radar, and calculating to obtain a coordinate conversion relation between the camera and the laser radar; the longitude and latitude of the current position of the robot in the satellite map are obtained, and an operator delimits a rough sparse path for the robot in a dotting mode according to actual operation requirements; the robot runs from the starting point according to a sparse path, pose information is obtained in real time, and in the running process, the laser radar and the camera start to scan the local environment; relative distances between the dynamic obstacles and the robot and between the static obstacles and the robot are calculated; when an obstacle is detected, calculating an evading course; and after obstacle avoidance, returning to the trunk path to continue driving to complete all operations. The driving route is more accurate in agriculture, frontier defense and the like, and the using effect is better.

Description

Technical Field

[0001] The present invention relates to the field of navigation technology, and in particular to a navigation method based on RTK laser vision fusion. Background Art

[0002] Mobile robots have broad application prospects in the fields of military, industry, agriculture, commerce, transportation, logistics, etc. At present, a core issue restricting the widespread use of mobile robots is the autonomous navigation capability of mobile robots.

[0003] According to the type of sensor used, the current mainstream technology can be divided into two major directions: laser navigation and visual navigation. Among them, laser navigation uses laser radar as the main sensor, combined with inertial sensors, etc., to build a three-dimensional point cloud map of the environment by recording the laser point cloud information collected during the movement of the robot. Visual navigation uses the camera as the main sensor, and builds a point cloud or semantic map by detecting the feature point information in the color image in the field of view of the camera during movement. Laser sensors are more stable in acquiring signals such as depth in regular environments, but the cost is high and can only acquire point clouds. Visual sensors can extract feature point information in the environment based on color changes regardless of whether they are in regular or irregular environments. They are low-cost, but cannot directly acquire depth information and are easily affected by light, while binocular or multi-eye cameras have a large computational burden.

[0004] In terms of application scenarios, current navigation technology is mainly used in relatively regular scenarios such as indoors and streets. Common products include sweepers and inspection robots, but autonomous navigation technology has broader application prospects, such as agriculture and border defense. Compared with the former, the latter has an irregular environment and unclear feature points. The use of lasers or visual sensors alone cannot achieve good mapping and positioning effects. Due to the irregular ground, the robot is prone to deviate from the established route when driving. Summary of the invention

[0005] The purpose of the present invention is to provide a navigation method based on RTK laser vision fusion, which solves the problem that when using mobile robots in agriculture, border defense and other places, due to the irregular environment and unclear feature points, the single use of laser or visual sensor cannot obtain good mapping and positioning effects, and due to the irregular ground, the robot is easy to deviate from the established route when driving.

[0006] To achieve the above object, the present invention provides a navigation method based on RTK laser vision fusion, comprising the following steps:

[0007] Connect the data signal line with the industrial computer to check the RTK positioning accuracy and the signal transmission with the host computer;

[0008] Obtain the internal parameters of the camera and lidar, and calculate the coordinate transformation relationship between the camera and lidar;

[0009] The RTK system obtains the latitude and longitude of the robot's current position in the satellite map, and the operator uses the dot method to mark a rough sparse path for the robot based on actual operation requirements;

[0010] The robot starts from the starting point and drives along a sparse path, acquiring position information in real time. During the driving process, the laser radar and camera begin to scan the local environment;

[0011] By scanning the local environment information, the relative distance between each dynamic obstacle and static obstacle and the robot is calculated;

[0012] When an obstacle is detected, the robot obtains the relative position of the target obstacle and the robot, and calculates the heading for avoidance;

[0013] The robot avoids obstacles in adjacent sparse path nodes according to the calculated avoidance heading, then returns to the main path to continue driving and completes the entire task.

[0014] The internal parameters of the camera and the laser radar are obtained, and the coordinate transformation relationship between the camera and the laser radar is calculated. The steps also include:

[0015] Prepare a chessboard calibration board, use a camera to take 20 pictures from different distances and angles, calculate the corner points after correcting the pictures, and then use Zhang Zhengyou calibration method to solve the camera's intrinsic parameter matrix.

[0016] The internal parameters of the camera and the laser radar are obtained, and the coordinate transformation relationship between the camera and the laser radar is calculated. The steps also include:

[0017] Place the robot in a wide area and place a wooden board directly in front of the lidar. Obtain the coordinates of the flat ground and the normal vector of the plane. Then use the normal vector to determine the pitch and roll angles of the lidar. Finally, calculate the yaw angle from the actual measured distance of the wooden board and the roll angle.

[0018] The internal parameters of the camera and the laser radar are obtained, and the coordinate transformation relationship between the camera and the laser radar is calculated. The steps also include:

[0019] The same chessboard is detected by camera and lidar respectively, the position of the same feature point in the coordinates of the two sensors is calculated, and joint calibration is performed to obtain the coordinate conversion relationship between the camera and lidar.

[0020] The relative distances between the dynamic obstacles and the static obstacles and the robot are calculated by scanning the local environment information, and the steps further include:

[0021] The camera collects image information in the environment, uses deep learning to detect dynamic obstacles and segment static obstacles. After obtaining the color area information of each object, the depth value information of the corresponding point cloud is calculated through the lidar, thereby calculating the relative distance between each dynamic obstacle and static obstacle and the robot.

[0022] The present invention discloses a navigation method based on RTK laser vision fusion. According to the RTK technology, the position and longitude and latitude of the robot in the satellite map can be directly obtained, so as to obtain the coordinates of the robot in the world coordinate system and the satellite cloud map of the surrounding environment, and provide stable and reliable environmental map information from a macro level. The method is matched with a laser sensor and a visual sensor, and the rich color information of the visual sensor and the stable point cloud information of the laser radar are used to detect the changes in the robot's surrounding environment in real time from a micro level, but it does not affect the global satellite map. While ensuring that the robot travels in a predetermined general direction, it can also enable the robot to avoid irregular obstacles around in real time. RTK, laser and visual sensors are used at the same time. Among them, RTK can provide the robot with a global satellite map and coordinates from a macro level, directly realize large-area perception and positioning from the prior art, and is more convenient and efficient than the method in which the robot needs to travel and build maps one by one in the traditional navigation algorithm. At the same time, the present invention uses laser and vision to detect the local environment of the robot in real time, which is used to detect dynamic and static obstacles that are easy to interfere with the robot's travel, and can adjust the course according to the position of the obstacle while maintaining the overall main direction of travel. The robot's travel route in agriculture, border defense and other places is more accurate and the use effect is better. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] 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 or the description of the prior art are briefly introduced below.

[0024] Figure 1 It is an application scenario diagram of the present invention.

[0025] Figure 2 It is a flow chart of the use of the navigation method based on RTK laser vision fusion of the present invention.

[0026] Figure 3 It is the chessboard calibration plate of the present invention.

[0027] Figure 4 It is a schematic diagram of the combined calibration of the present invention.

[0028] Figure 5 It is a schematic diagram of an obstacle of the present invention.

[0029] Figure 6 It is a schematic diagram of the steps of the navigation method based on RTK laser vision fusion of the present invention. DETAILED DESCRIPTION

[0030] Embodiments of the present invention are described in detail below. Examples of the embodiments are shown in the accompanying drawings. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to be used to explain the present invention, but should not be construed as limiting the present invention.

[0031] The first embodiment of the present application is:

[0032] See also Figures 1 to 6 ,in, Figure 1 It is an application scenario diagram of the present invention. Figure 2 It is a flow chart of the use of the navigation method based on RTK laser vision fusion of the present invention. Figure 3 It is the chessboard calibration plate of the present invention. Figure 4 It is a schematic diagram of the combined calibration of the present invention. Figure 5 It is a schematic diagram of an obstacle of the present invention. Figure 6 The present invention provides a navigation method based on RTK laser vision fusion, which includes the following steps:

[0033] S100: Connect the data signal line with the industrial computer to check the RTK positioning accuracy and the signal transmission with the host computer;

[0034] S200: Obtain the internal parameters of the camera and the laser radar, and calculate the coordinate transformation relationship between the camera and the laser radar;

[0035] S300: The RTK system obtains the latitude and longitude of the robot's current position in the satellite map. The operator uses the dot method to mark a rough sparse path for the robot based on actual operation requirements.

[0036] S400: The robot starts to drive along a sparse path from the starting point and obtains position information in real time. During the driving process, the laser radar and camera begin to scan the local environment;

[0037] S500: Calculate the relative distance between each dynamic obstacle and static obstacle and the robot through the scanned local environment information;

[0038] S600: When an obstacle is detected, the robot obtains the relative position between the target obstacle and the robot, and calculates the avoidance heading;

[0039] S700: The robot avoids obstacles in the adjacent sparse path nodes according to the calculated avoidance heading, and then returns to the main path to continue driving, completing the entire task.

[0040] Specifically, the first step is to configure the RTK system, assemble the hardware and match it with the 4G module, connect the data signal line with the industrial computer, and check the RTK positioning accuracy and the signal transmission with the host computer through the APP and serial port software respectively. The second step is to calibrate the binocular camera, prepare the chessboard calibration board, use the camera to take 20 pictures from different distances and angles, calculate the corner points after correcting the pictures, and then use Zhang Zhengyou's calibration method to solve the camera's internal parameter matrix. The third step is to calibrate the laser radar, place the robot in a wide area, place a wooden board in front of the laser radar, obtain the coordinates of the flat ground and the normal vector of the plane, and then use the normal vector to determine the pitch angle and roll angle of the laser radar, and finally calculate the yaw angle by actually measuring the distance of the wooden board and the roll angle. The fourth step is to obtain the internal parameters of the camera and laser radar through the second and third steps. Use the camera and laser radar to detect the same chessboard respectively, calculate the position of the same feature point in the coordinates of the two sensors, and perform joint calibration to obtain the coordinate conversion relationship between the camera and the laser radar, which is used for the subsequent calculation of the depth information corresponding to the feature points. Step 5. Use the RTK system to obtain the longitude and latitude of the robot's current position in the satellite map. The operator will mark a rough sparse path for the robot based on the actual operational needs of the farm. Step 6. The robot starts to drive along the sparse path from the starting point and obtains position information in real time. During the driving process, the lidar and camera begin to scan the local environment. Step 7. The camera collects image information in the environment, uses deep learning to detect moving obstacles such as people and animals, and segments fruit trees through strategies such as threshold segmentation and morphological filtering. After obtaining the color area information of each object, the lidar calculates the depth value information of the corresponding point cloud, thereby calculating the relative distance between each dynamic and static obstacle and the robot. Step 8. If the robot detects an obstacle in an adjacent sparse path node, it can obtain the relative position and posture of the target obstacle and the robot, and calculate the avoidance heading based on geometric principles. Such as Figure 5 As shown in the figure, the blue represents the robot's position, and the green blocks represent obstacles on both sides of the forward direction. The robot calculates the appropriate obstacle avoidance heading as shown in Table 1.

[0041] Table 1 Obstacle avoidance heading calculation table

[0042]

[0043] Step 9. The robot avoids obstacles in adjacent sparse path nodes according to the heading angle calculated in the previous step, then returns to the main path to continue driving, completing all operations in the orchard.

[0044] The present invention combines the advantages of laser radar and visual sensors, and combines RTK technology to achieve accurate autonomous navigation technology in irregular outdoor environments. According to the core technology analysis of mobile robots, it is divided into three modules: environmental perception, path planning, and visual obstacle avoidance.

[0045] Environmental perception module: Based on RTK technology, we can directly obtain the robot's position and longitude and latitude in the satellite map, thereby obtaining the robot's coordinates in the world coordinate system and the satellite cloud map of the surrounding environment, providing stable and reliable environmental map information from a macro level. Due to the presence of irregular obstacles such as fruit trees in the environment, the environmental layout of the upper and lower layers of the tree canopy is inconsistent with the satellite map, so the robot cannot drive completely according to the satellite map during actual operation.

[0046] To address this problem, the present invention combines a laser sensor and a visual sensor, using the rich color information of the visual sensor and the stable point cloud information of the lidar to detect changes in the robot's surrounding environment in real time from a microscopic level without affecting the global satellite map. While ensuring that the robot travels in the predetermined direction, it can also enable the robot to avoid irregular obstacles around it in real time.

[0047] Path planning module: The present invention combines satellite positioning, so the global path can be manually specified by the robot's driving path nodes according to actual operation requirements, and the longitude and latitude coordinates of the robot's necessary driving nodes under the global satellite map can be recorded by manual dotting, which serves as the global path of navigation and plays an overall guiding role. Due to the uneven ground and factors such as the robot's hardware control, the robot is prone to deviate from the designated route when driving at two adjacent points. At this time, local path planning technology is needed to adjust the robot's heading so that it drives in the direction of the global path.

[0048] Visual obstacle avoidance module: In order to solve the above-mentioned local path problem, the present invention introduces lasers and visual sensors, combines the stability of laser detection of depth information and the richness of visual perception of color, detects obstacles around the robot in real time, detects which areas on the ground the robot can drive on, avoids the robot colliding with fruit trees, and always keeps walking on the relative center line of the road.

[0049] The present invention uses RTK, laser and visual sensors at the same time. RTK can provide the robot with global satellite maps and coordinates from a macro level, directly realizing large-area perception and positioning from the existing technology, which is more convenient and efficient than the traditional navigation algorithm that requires the robot to travel and build maps one by one. At the same time, the present invention uses laser and vision to detect the local environment of the robot in real time, which is used to detect dynamic and static obstacles that are easy to interfere with the robot's travel, and can adjust the course according to the position of the obstacle while maintaining the overall main direction of travel. The robot's travel route in agriculture, border defense and other places is more accurate and the use effect is better.

[0050] What is disclosed above is only one or more preferred embodiments of the present application, and cannot be used to limit the scope of rights of the present application. Ordinary technicians in this field can understand that all or part of the processes of implementing the above embodiments and equivalent changes made according to the claims of the present application are still within the scope covered by the present application.

Claims

1. A navigation method based on RTK laser vision fusion, characterized in that: The following steps are involved: Connect the data signal line with the industrial computer to check the RTK positioning accuracy and the signal transmission with the host computer; Obtain the internal parameters of the camera and lidar, and calculate the coordinate transformation relationship between the camera and lidar; The RTK system obtains the latitude and longitude of the robot's current position in the satellite map, and the operator uses the dot method to mark a rough sparse path for the robot based on actual operation requirements; The robot starts from the starting point and drives along a sparse path, acquiring position information in real time. During the driving process, the laser radar and camera begin to scan the local environment; By scanning the local environment information, the relative distance between each dynamic obstacle and static obstacle and the robot is calculated; When an obstacle is detected, the robot obtains the relative position of the target obstacle and the robot, and calculates the heading for avoidance; The robot avoids obstacles in adjacent sparse path nodes according to the calculated avoidance heading, then returns to the main path to continue driving and completes the entire task.

2. The navigation method based on RTK laser vision fusion as claimed in claim 1, characterized in that: Obtaining the internal parameters of the camera and the laser radar, and calculating the coordinate transformation relationship between the camera and the laser radar, the step also includes: Prepare a chessboard calibration board, use a camera to take 20 pictures from different distances and angles, calculate the corner points after correcting the pictures, and then use Zhang Zhengyou calibration method to solve the camera's intrinsic parameter matrix.

3. The navigation method based on RTK laser vision fusion as claimed in claim 2, characterized in that: Obtaining the internal parameters of the camera and the laser radar, and calculating the coordinate transformation relationship between the camera and the laser radar, the step also includes: Place the robot in a wide area and place a wooden board directly in front of the lidar. Obtain the coordinates of the flat ground and the normal vector of the plane. Then use the normal vector to determine the pitch and roll angles of the lidar. Finally, calculate the yaw angle from the actual measured distance of the wooden board and the roll angle.

4. The navigation method based on RTK laser vision fusion as claimed in claim 3, characterized in that: Obtaining the internal parameters of the camera and the laser radar, and calculating the coordinate transformation relationship between the camera and the laser radar, the step also includes: The same chessboard is detected by camera and lidar respectively, the position of the same feature point in the coordinates of the two sensors is calculated, and joint calibration is performed to obtain the coordinate conversion relationship between the camera and lidar.

5. The navigation method based on RTK laser vision fusion as claimed in claim 4, characterized in that: By using the scanned local environment information, the relative distances between each dynamic obstacle and the static obstacle and the robot are calculated. The steps further include: The camera collects image information in the environment, uses deep learning to detect dynamic obstacles and segment static obstacles. After obtaining the color area information of each object, the depth value information of the corresponding point cloud is calculated through the lidar, thereby calculating the relative distance between each dynamic obstacle and static obstacle and the robot.

Images