Automatic navigation control method and device and teaching experiment mobile platform system

Abstract

The invention provides an automatic navigation control method and device and a teaching experiment mobile platform system. The method comprises the following steps: carrying out key frame tracking, local map construction and loopback detection processing based on image data of a visual sensor to obtain heterogeneous data, and carrying out fusion processing on the heterogeneous data to obtain an environment perception map; performing pose estimation processing on a visual sensor on the teaching mobile platform based on the original measurement data to determine a pose state; positioning the teaching mobile platform in the environment perception map, determining positioning information and a current pose state of the teaching mobile platform in the environment perception map, and performing path planning search on a moving path of the teaching mobile platform to obtain a target path which meets a path planning constraint condition and can reach an end point; trajectory tracking control is carried out, and autonomous decision navigation is completed. The method provided by the invention can effectively improve the accuracy and robustness of automatic navigation control.

Description

Technical Field

[0001] The present invention relates to the field of intelligent control technology, and in particular to an automatic navigation control method, device and teaching experiment mobile platform system, and also to an electronic device, a non-transitory computer-readable storage medium and a computer program product. Background Art

[0002] Autonomous driving means that the vehicle perceives the surrounding environment and makes decisions and controls through various sensors arranged on the vehicle body, and drives the vehicle without the need for driver operation. At present, the industry has achieved L2 level autonomous driving. Autonomous driving technology mainly includes branches such as intelligent perception, autonomous positioning, autonomous navigation, and autonomous decision-making. It has shown broad application prospects in many fields such as civil and education. At the same time, the teaching experiment mobile platform system related to autonomous driving is more and more widely used, but the current teaching mobile platform system has high limitations, resulting in poor automatic navigation control accuracy and robustness of the teaching mobile platform system. Therefore, how to design an automatic navigation control scheme based on the teaching mobile platform system has become a technical problem that needs to be solved urgently. Summary of the invention

[0003] The present invention provides an automatic navigation control method and device, which are used to solve the defects in the prior art that the teaching mobile platform system has high limitations, resulting in poor automatic navigation control accuracy and robustness of the teaching mobile platform system.

[0004] The present invention provides an automatic navigation control method, comprising: obtaining image data collected by a visual sensor preset on a teaching mobile platform, performing key frame tracking, local map construction and loop detection processing based on the image data, obtaining perceived heterogeneous data, and fusing the heterogeneous data to obtain an environmental perception map; obtaining raw measurement data collected by an inertial measurement sensor preset on the teaching mobile platform, performing posture estimation processing on the visual sensor on the teaching mobile platform based on the raw measurement data, and determining the posture state of the visual sensor in the teaching mobile platform; obtaining the teaching information collected by a positioning system preset on the teaching mobile platform The teaching mobile platform is positioned in the environment perception map based on the posture state and the position information, and the positioning information and current posture state of the teaching mobile platform in the environment perception map are determined; based on the positioning information and current posture state of the teaching mobile platform in the environment perception map, and using preset path planning constraints, a path planning search is performed on the moving path of the teaching mobile platform to obtain a target path that meets the path planning constraints and can reach the end point; based on the target path, the teaching mobile platform is tracked and controlled so that the teaching mobile platform can complete autonomous decision-making navigation.

[0005] According to the automatic navigation control method provided by the present invention, the key frame tracking, local map construction and loop detection processing are performed based on the image data to obtain perceived heterogeneous data, and the heterogeneous data are fused to obtain an environmental perception map, including: feature extraction based on the image data to obtain feature point information; map initialization of the visual sensor based on the feature point information to obtain the corresponding three-dimensional space coordinates, and re-pose estimation and tracking of the current key frame based on the key reference frame to obtain the re-estimated current key frame initial pose; local map tracking is performed based on the re-estimated current key frame initial pose to update the observation information of the map points in the local map, and by counting the number of relevant feature points, it is determined whether the tracking is successful. If the tracking is successful, a corresponding key frame is created; the key frames in the key frame queue are processed in sequence to obtain map points that correspond to the key frames; the map points are screened according to the recall rate and the number of observations to eliminate low-quality map points to obtain screened map points, and new map points are generated based on the screened map points, and the new map points are merged into the corresponding key frame; the map point coordinates and the visual sensor pose in the key frame are locally optimized by minimizing the reprojection error or the photometric error to obtain an optimized second key frame; the second key frame is added to the loop detection queue for loop detection to remove redundant key frames, obtain a target key frame after removing redundant key frames, and obtain an environment perception map based on the target key frame.

[0006] According to the automatic navigation control method provided by the present invention, the adding of the second key frame to the loop detection queue for loop detection to remove redundant key frames, obtain the target key frame after removing the redundant key frames, and obtain the environment perception map based on the target key frame, including: adding the second key frame to the loop detection queue for closed-loop detection processing to obtain a closed-loop detection result; performing closed-loop correction processing based on the closed-loop detection result to remove redundant key frames, obtain the target key frame after removing the redundant key frames, and obtain the environment perception map based on the target key frame; wherein, the adding of the second key frame to the loop detection queue for loop detection to remove redundant key frames, obtain the target key frame after removing the redundant key frames, and obtain the environment perception map based on the target key frame. The loop detection queue performs closed-loop detection processing to obtain a closed-loop detection result, including: adding the second key frame to the loop detection queue to perform database query and similarity calculation to obtain a closed-loop detection result; the closed-loop correction processing is performed based on the closed-loop detection result to remove redundant key frames, obtain a target key frame after removing redundant key frames, and obtain an environment perception map based on the target key frame, including: performing closed-loop fusion and essential graph optimization processing based on the closed-loop detection result to achieve removal of redundant key frames, and obtain a target key frame after removing redundant key frames, and obtain an environment perception map based on the target key frame.

[0007] According to the automatic navigation control method provided by the present invention, the posture estimation processing of the visual sensor on the teaching mobile platform based on the original measurement data to determine the posture state of the visual sensor in the teaching mobile platform includes: acquiring feature point information from the image data based on the original measurement data; performing posture estimation processing on the visual sensor on the teaching mobile platform based on the feature point information and a perspective projection model to obtain the posture state of the visual sensor in the teaching mobile platform; The method of performing posture estimation processing on the visual sensor on the teaching mobile platform based on the perspective projection model to obtain the posture state of the visual sensor in the teaching mobile platform includes: when performing posture estimation processing on the visual sensor on the teaching mobile platform based on the perspective projection model, using the bundle adjustment method to optimize the error generated in the posture estimation process, and constructing a least squares problem with the goal of minimizing the reprojection error to obtain the optimized posture state of the visual sensor in the teaching mobile platform.

[0008] According to the automatic navigation control method provided by the present invention, based on the positioning information and current posture state of the teaching mobile platform in the environmental perception map, and using preset path planning constraints, a path planning search is performed on the moving path of the teaching mobile platform to obtain a target path that meets the path planning constraints and can reach the end point. Specifically, it includes: based on the positioning information and current posture state of the teaching mobile platform in the environmental perception map, and using a preset time-space hybrid algorithm model, under the path planning constraints, a path planning iterative search is performed on the moving path of the teaching mobile platform until a path that meets the path planning constraints and can reach the end point is found as the target path; wherein the path planning constraints include: the teaching mobile platform moves within a preset moving area, and the teaching mobile platform moves to avoid obstacles under dynamic obstacle constraints.

[0009] The present invention also provides an automatic navigation control device, including: an environmental perception module, which is used to obtain image data collected by a visual sensor preset on the teaching mobile platform, perform key frame tracking, local map construction and loop detection processing based on the image data, obtain perceived heterogeneous data, perform fusion processing on the heterogeneous data, and obtain an environmental perception map; an autonomous positioning module, which is used to obtain raw measurement data collected by an inertial measurement sensor preset on the teaching mobile platform, perform posture estimation processing on the visual sensor on the teaching mobile platform based on the raw measurement data, and determine the posture state of the visual sensor in the teaching mobile platform; obtain the teaching data collected by the positioning system preset on the teaching mobile platform The teaching mobile platform comprises: a position information of the mobile platform; positioning the teaching mobile platform in the environmental perception map based on the posture state and the position information, and determining the positioning information and the current posture state of the teaching mobile platform in the environmental perception map; a path planning module, which is used to search for the moving path of the teaching mobile platform based on the positioning information and the current posture state of the teaching mobile platform in the environmental perception map, and to obtain a target path that meets the path planning constraints and can reach the end point; an intelligent control module, which is used to perform trajectory tracking control on the teaching mobile platform based on the target path, so that the teaching mobile platform can complete autonomous decision-making navigation.

[0010] According to the automatic navigation control device provided by the present invention, the environment perception module is specifically used for: Based on the image data, feature extraction is performed to obtain feature point information; based on the feature point information, a map of the visual sensor is initialized to obtain corresponding three-dimensional space coordinates, and based on the key reference frame, a re-pose estimation and tracking of the current key frame is performed to obtain a re-estimated initial pose of the current key frame; based on the re-estimated initial pose of the current key frame, local map tracking is performed to update the observation information of the map points in the local map, and by counting the number of relevant feature points, it is determined whether the tracking is successful, and if the tracking is successful, a corresponding key frame is created; the key frames in the key frame queue are processed in sequence to obtain the key points that are respectively related to the key frame. The map points have corresponding relationships in the frame; the map points are screened according to the recall rate and the number of observations to eliminate low-quality map points to obtain screened map points, and new map points are generated based on the screened map points, and the new map points are merged into the corresponding key frame; the coordinates of the map points and the visual sensor pose in the key frame are locally optimized by minimizing the reprojection error or the photometric error to obtain an optimized second key frame; the second key frame is added to the loop detection queue for loop detection to remove redundant key frames, to obtain a target key frame after removing the redundant key frames, and an environment perception map is obtained based on the target key frame.

[0011] The present invention also provides a teaching experiment mobile platform system for implementing any one of the automatic navigation control methods described above, comprising: a host computer, a remote control communication module, an unmanned vehicle, and an expansion module; the unmanned vehicle is provided with a visual sensor and an inertial measurement sensor; the expansion module is provided with a photoelectric detection module or a thermal imaging module for performing an over-temperature detection task; the remote control communication module is used to control a terminal for data communication; the host computer is used to implement the automatic navigation control method, and the automatic navigation control method comprises the following steps: Obtain image data collected by a visual sensor preset on the teaching mobile platform, perform key frame tracking, local map construction and loop detection processing based on the image data, obtain perceived heterogeneous data, perform fusion processing on the heterogeneous data, and obtain an environment perception map; Obtaining raw measurement data collected by an inertial measurement sensor preset on the teaching mobile platform, performing posture estimation processing on the visual sensor on the teaching mobile platform based on the raw measurement data, and determining the posture state of the visual sensor in the teaching mobile platform; obtaining the position information of the teaching mobile platform collected by a positioning system preset on the teaching mobile platform; locating the teaching mobile platform in the environmental perception map based on the posture state and the position information, and determining the positioning information and current posture state of the teaching mobile platform in the environmental perception map; Based on the positioning information and current posture state of the teaching mobile platform in the environmental perception map, and using preset path planning constraints, a path planning search is performed on the moving path of the teaching mobile platform to obtain a target path that satisfies the path planning constraints and can reach the end point; The teaching mobile platform is tracked and controlled based on the target path so that the teaching mobile platform can complete autonomous decision-making and navigation.

[0012] The present invention also provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the automatic navigation control method as described in any one of the above items when executing the program.

[0013] The present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the automatic navigation control method as described in any one of the above items.

[0014] The present invention also provides a computer program product, comprising a computer program, wherein when the computer program is executed by a processor, the automatic navigation control method as described in any one of the above items is implemented.

[0015] The automatic navigation control method provided by the present invention obtains image data collected by a visual sensor preset on a teaching mobile platform, performs key frame tracking, local map construction and loop detection processing based on the image data, obtains perceived heterogeneous data, performs fusion processing on the heterogeneous data to obtain an environment perception map, obtains original measurement data collected by an inertial measurement sensor preset on the teaching mobile platform, performs posture estimation processing on the visual sensor on the teaching mobile platform based on the original measurement data, determines the posture state of the visual sensor in the teaching mobile platform, and obtains the position information of the teaching mobile platform collected by a positioning system preset on the teaching mobile platform; locates the teaching mobile platform in the environment perception map based on the posture state and the position information, determines the positioning information and the current posture state of the teaching mobile platform in the environment perception map, and uses the positioning information and the current posture state of the teaching mobile platform in the environment perception map to perform path planning search on the moving path of the teaching mobile platform based on the positioning information and the current posture state of the teaching mobile platform in the environment perception map and the preset path planning constraint conditions, obtains a target path that meets the path planning constraint conditions and can reach the end point, and performs trajectory tracking control on the teaching mobile platform based on the target path, so that the teaching mobile platform completes autonomous decision-making navigation, which can effectively improve the accuracy and robustness of automatic navigation control. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0017] Figure 1 It is a flow chart of the automatic navigation control method provided by the present invention.

[0018] Figure 2 It is a complete flow chart of the automatic navigation control method provided by the present invention.

[0019] Figure 3 It is a schematic diagram of the Ackerman steering model provided by the present invention.

[0020] Figure 4 It is a schematic diagram of the key point extraction principle provided by the present invention.

[0021] Figure 5 It is a schematic diagram of the Bundle Adjustment optimization principle provided by the present invention.

[0022] Figure 6 It is a schematic diagram of the effect of the SLAM algorithm provided by the present invention.

[0023] Figure 7It is a schematic diagram of the pre-integration principle provided by the present invention.

[0024] Figure 8 It is a processing schematic diagram of the spatiotemporal hybrid algorithm model provided by the present invention.

[0025] Fig. 9 It is a flowchart of an example experimental task provided by the present invention.

[0026] Fig.10 It is a diagram of an exemplary experimental structure provided by the present invention.

[0027] Fig.11 It is an eight-bit image for fire and smoke detection provided by the present invention.

[0028] Fig.12 It is a schematic diagram of the binary smoke and fire detection provided by the present invention.

[0029] Fig.13 It is a schematic diagram of the smoke and fire detection process provided by the present invention.

[0030] Fig.14 It is a schematic diagram of the co-viewing diagram provided by the present invention.

[0031] Fig.15 It is a structural schematic diagram of the automatic navigation control device provided by the present invention.

[0032] Fig.16 It is a structural schematic diagram of the teaching mobile platform provided by the present invention.

[0033] Fig.17 It is a structural schematic diagram of the electronic device provided by the present invention. DETAILED DESCRIPTION

[0034] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be clearly and completely described below in conjunction with the drawings in the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention. Figure 1-Figure 17 The automatic navigation control method and device of the present invention are described, and the embodiments thereof are described in detail. Based on the automatic navigation control method of the present invention, the embodiments thereof are described in detail. Figure 1 As shown, it is a flow chart of the automatic navigation control method provided by the present invention, and the specific implementation process includes the following steps: Step 101: obtain image data collected by a preset visual sensor on the teaching mobile platform, perform key frame tracking, local map construction and loop detection processing based on the image data, obtain perceived heterogeneous data, fuse the heterogeneous data, and obtain an environment perception map.

[0035] In an embodiment of the present invention, feature extraction is performed on image data collected by a preset visual sensor on a teaching mobile platform to obtain feature point information, map initialization of the visual sensor is performed based on the feature point information to obtain corresponding three-dimensional space coordinates, and re-pose estimation and tracking of the current key frame is performed based on the key reference frame to obtain a re-estimated initial pose of the current key frame; local map tracking is performed based on the re-estimated initial pose of the current key frame to update the observation information of the map points in the local map, and by counting the number of relevant feature points, it is determined whether the tracking is successful. If the tracking is successful, a corresponding key frame is created; the key frames in the key frame queue are sequentially tracked. The map points are processed to obtain map points that correspond to the key frames respectively; the map points are screened according to the recall rate and the number of observations to eliminate low-quality map points to obtain screened map points, and new map points are generated based on the screened map points, and the new map points are merged into the corresponding key frames; the map point coordinates and the visual sensor pose in the key frame are locally optimized by minimizing the reprojection error or the photometric error to obtain an optimized second key frame; the second key frame is added to the loop detection queue for loop detection to remove redundant key frames, obtain a target key frame after removing the redundant key frames, and obtain an environment perception map based on the target key frame. Among them, when adding the second key frame to the loop detection queue for loop detection to remove redundant key frames and obtain the target key frame after removing the redundant key frames, and obtaining the environment perception map based on the target key frame, the second key frame can be first added to the loop detection queue for closed-loop detection processing to obtain a closed-loop detection result, and then a closed-loop correction processing is performed based on the closed-loop detection result to remove redundant key frames, obtain the target key frame after removing the redundant key frames, and obtain the environment perception map based on the target key frame. Among them, the adding the second key frame to the loop detection queue for closed-loop detection processing to obtain a closed-loop detection result includes: adding the second key frame to the loop detection queue for database query and similarity calculation to obtain a closed-loop detection result; the closed-loop correction processing based on the closed-loop detection result to remove redundant key frames, obtain the target key frame after removing the redundant key frames, and obtain the environment perception map based on the target key frame, including: performing closed-loop fusion and essential graph optimization processing based on the closed-loop detection result to achieve the removal of redundant key frames, and obtain the target key frame after removing the redundant key frames, and obtain the environment perception map based on the target key frame.

[0036] It should be noted that the unmanned vehicle (i.e., teaching mobile platform) used in the present invention adopts the motion mode of a rear-wheel drive vehicle with front-wheel steering and rear-wheel drive in the application scenario, and adopts the Ackerman steering model to more completely describe the vehicle's steering motion and acceleration and deceleration motion and other motion processes, such as Figure 3 As shown. Among them, the state of the unmanned vehicle can be expressed as .in Represents the position information of the rear axle center of the unmanned vehicle in the global coordinate system. represents the yaw angle of the unmanned vehicle; They represent the current speed and acceleration of the unmanned vehicle in the Frenet coordinate system, represents the steering angle of the front wheels of the unmanned vehicle, is the wheelbase of the front and rear wheels along the vehicle body direction. When , the moving trajectory of the unmanned vehicle can be approximately expressed as a moving arc, and the arc radius is expressed as .

[0037] Steering angle of unmanned vehicle and its steering angular velocity There is the following kinematic relationship between them: The present invention defines control input , by discretizing the integral sampling time , the unit time step can be calculated The robot state at can be expressed by the following differential equations: Control input is conditioned , Constraints, Indicates the sampling time; represents the acceleration of the unmanned vehicle in the Frenet coordinate system; T represents the sampling time; Represents the state of the unmanned vehicle at time t-1.

[0038] In this step, the unmanned vehicle realizes intelligence, and environmental perception is a prerequisite. Through the collaborative work of multiple sensors, such as visual sensors (such as binocular vision sensors) and lidar sensors, the unmanned vehicle can perceive the surrounding complex environment in real time, accurately and robustly, and obtain key information such as pedestrians, vehicles, obstacles, etc. On this basis, the unmanned vehicle uses simultaneous localization and mapping (SLAM) technology to intelligently fuse heterogeneous data perceived by multiple sensors to generate a semantically rich and high-precision environmental perception map, laying a solid foundation for subsequent positioning, path planning and decision-making control. The present invention deploys the ORB-SLAM2 (Oriented FAST and Rotated BRIEF-SLAM 2) algorithm on the host computer, and supports monocular, binocular, and RGB-D cameras. Figure 2 As shown in the figure, the Intel RealSense Depth Camera D435 depth camera is equipped to realize the environmental perception and map construction of unmanned vehicles. ORB-SLAM2 is a visual SLAM algorithm based on feature points. By extracting the ORB (Oriented FAST and Rotated BRIEF) feature point information in the image and combining graph optimization and loop detection techniques, it realizes real-time and robust camera pose estimation and sparse environment reconstruction. Since the input of the map construction system is a grayscale image and a depth map, the initialization can be completed through the first key frame, that is, the initial map point and the current key frame pose are constructed.

[0039] In the process of key frame tracking based on the image data of the present invention, the main goal of the tracking module is to estimate the position of the camera (such as a visual sensor) in real time, screen key video frames (i.e., key frames), and create visual mileage without the need for global optimization. The processing flow of the tracking module can be divided into the following five steps: (1) Feature extraction. The ORB feature extraction algorithm provided by OpenCV is used to extract key points and descriptors from the input grayscale image through the ORB-SLAM system. Key points are generally points in the image (i.e., key frames) where the grayscale has obvious changes in the horizontal and vertical directions. The descriptor is used to describe each feature point in the form of a vector as the unique feature of each feature point. In the ORB-SLAM system, key point extraction adopts an improved FAST corner detection algorithm, such as Figure 4As shown in the figure, the calculation of the descriptor is based on the BRIEF (Binary Robust Independent Elementary Features) algorithm. (2) Camera initialization. The depth map obtained by the RGB-D camera (such as a binocular vision sensor) and the extracted feature point information are used to generate the corresponding three-dimensional space coordinates. Unlike monocular or binocular cameras, after the RGB-D camera extracts the feature points in the first frame image (i.e., the first key frame), the absolute coordinates of the feature points in the three-dimensional space can be directly calculated, thereby completing the camera initialization process. Before the camera is initialized, the camera parameters must be calibrated using the Zhang Zhengyou calibration method. The specific algorithm of the Zhang Zhengyou calibration method is as follows: 1. Prepare the calibration board: Select a black and white chessboard as the calibration board, and the grid points on the black and white chessboard will be extracted as feature points by the algorithm. 2. Take images: Use the camera to be calibrated to take pictures of the chessboard from multiple directions and distances. 3 Extract image points: For each image, determine the corner points on the calibration board and record the coordinates. 4. Extract world points: According to the size of the calibration board, calculate the world coordinates of each corner point. 5. Calibrate the camera: Use the correspondence between the image points and the world points to solve the camera's intrinsic and extrinsic parameters. The algorithm for solving the intrinsic parameters is: by minimizing the reprojection error, using the camera's intrinsic parameter matrix and distortion coefficients, project the world coordinates onto the image plane, and compare with the actual observed image points, and adjust the intrinsic parameters to minimize the error. The algorithm for solving the extrinsic parameters is: using the known intrinsic parameters, transform the world coordinates to the camera coordinate system, and then solve the camera's rotation matrix and translation vector by minimizing the error between the camera coordinates and the image coordinates. 6. Evaluate the calibration results: Evaluate the calibration results, usually including the calculation of the reprojection error and the analysis of the distortion parameters. Finally, the parameters of the input camera are obtained for subsequent calculations. In addition, the current frame pose is estimated based on the previous frame pose. After the camera is successfully initialized, first check the validity of the constant speed motion model. The constant speed motion model is used to predict the camera pose and guide the matching of the map points observed in the previous frame. If the match is successful, the camera motion speed is considered constant; otherwise, the pose needs to be re-estimated using the key reference frame. The key reference frame is the common viewpoint with the current key frame (i.e., the relevant feature point or inlier point, such as Fig.14(as shown in the figure). If the match with the key reference frame still fails, the relocalization module is triggered to estimate the pose. The relocalization process includes: screening candidate key frames, calculating the number of common words, constructing the PnP problem and solving it using the RANSAC algorithm. If the number of inliers meets the threshold condition, the relocalization is considered successful. (4) Local map construction and optimization. After successfully estimating the initial pose of the current frame, update the local map based on the pose. Project some map points to the current frame (i.e., key frame or video frame), use Bundle Adjustment (BA) to optimize the pose, as shown in the figure, and update the observation information of the map points. By counting the number of inliers, it is determined whether the tracking is successful, as shown in the figure. Figure 5 As shown, - are the interior points from different perspectives, , , are different positions of the unmanned vehicle. (5) Key frame creation. The key frame selection strategy will directly affect the real-time performance of the system. The map point is the spatial position point of the relevant feature point or internal point in the coordinate system corresponding to the environment perception map.

[0040] In the process of constructing a local map based on the image data, the main task of the local map module is to update the motion model and optimize the local map at the same time after successfully tracking the current frame. The processing flow of this module can be divided into the following steps: (1) Key frame insertion. Check whether there is a key frame in the buffer queue passed by the previous module. If so, process the first key frame in the queue and obtain its corresponding map point relationship; if not, the current thread pauses for 3 milliseconds and then starts the next round of query. (2) Eliminate low-quality map points. Filter according to the recall rate and number of observations of the map points. The elimination principle is that the recall rate is lower than 0.25 and the number of observations in three consecutive key frames is less than 2. On the contrary, if the map point is not eliminated in three consecutive key frames, it is regarded as a high-quality map point. (3) Generate new map points. Perform feature matching on the current key frame with the top 10 key frames with the highest degree of co-viewing. At the same time, fuse the map points of the current key frame with the co-viewing key frames to increase the number of map points. There are two cases for map point fusion: if there is no corresponding point at the back-projected position of the map point, it is added directly; if there is a corresponding point, the two map points are merged into the keyframe with more observations. (4) Local Bundle Adjustment (BA) optimization. By minimizing the reprojection error or photometric error, the camera pose and local map point coordinates are optimized to improve the accuracy of the local map. (5) Remove redundant keyframes and add the current keyframe to the loop detection queue. Whether a keyframe is redundant depends on whether 90% of its map points can be observed by 3 or more other keyframes.

[0041] The main task of loop detection based on the image data is to solve the problem of estimating the globally consistent trajectory and map position over time. Before the loop is closed, the module continuously detects new key frames. The processing flow of the loop detection module can be divided into the following steps: (1) Database query. Query potential closed-loop candidate key frames based on the bag-of-words vector of the current key frame. The principle of selecting closed-loop candidate key frames is that they are close to the direction of the current key frame but have no direct connection. (2) Similarity calculation. If the current frame and the closed-loop candidate key frame are matched according to the bag-of-words number results, the SE(3) transformation is solved and optimized to obtain the similarity transformation matrix between the two frames to minimize the projection error. (3) Closed-loop fusion. After completing the first two steps, the system enters a closed-loop state and starts to fuse repeated point cloud data. At this time, the current frame and the key frame matched by the connected closed loop are fused. All key frames in the closed loop and the map points observed by each key frame will be globally optimized to make the fusion of point cloud data more effective. (4) Essential graph optimization. By introducing fixed poses as constraints, all map points and keyframe poses are optimized. The main purpose of essential map optimization is to disperse the accumulated errors to the entire closed-loop trajectory and improve global consistency. Through the collaboration of the above three threads. ORB-SLAM algorithm realizes high-precision, real-time monocular SLAM functions. Among them, the effective combination of key technologies such as feature extraction and matching, camera initialization, motion estimation, local mapping optimization, loop detection and global optimization ensures the robustness and accuracy of the algorithm in various scenarios. Figure 6 As shown in the figure, it is the effect diagram of SLAM algorithm. Figure 6 The point cloud shown can more accurately identify the shape of the object, build map information, and obtain environmental perception information.

[0042] Step 102: obtain original measurement data collected by an inertial measurement sensor preset on the teaching mobile platform, perform posture estimation processing on the visual sensor on the teaching mobile platform based on the original measurement data, and determine the posture state of the visual sensor in the teaching mobile platform; obtain the position information of the teaching mobile platform collected by a positioning system preset on the teaching mobile platform; locate the teaching mobile platform in the environmental perception map based on the posture state and the position information, and determine the positioning information and current posture state of the teaching mobile platform in the environmental perception map.

[0043] In the implementation of the present invention, after obtaining the original measurement data collected by the inertial measurement sensor preset on the teaching mobile platform, feature point information can be obtained from the image data based on the original measurement data; based on the feature point information, the visual sensor on the teaching mobile platform is subjected to posture estimation processing based on the perspective projection model to obtain the posture state of the visual sensor in the teaching mobile platform. Among them, the posture estimation processing of the visual sensor on the teaching mobile platform based on the perspective projection model to obtain the posture state of the visual sensor in the teaching mobile platform, the corresponding specific implementation process includes: when the posture estimation processing of the visual sensor on the teaching mobile platform is performed based on the perspective projection model, the error generated in the posture estimation processing is optimized using the bundle adjustment method, and the least squares problem is constructed with the goal of minimizing the reprojection error to obtain the optimized posture state of the visual sensor in the teaching mobile platform.

[0044] Specifically, in the process of positioning the teaching mobile platform in the environmental perception map based on the posture state and the position information, and determining the positioning information and current posture state of the teaching mobile platform in the environmental perception map, centimeter-level positioning accuracy can be achieved by integrating a high-precision global positioning system, an inertial sensor (Inertial Measurement Unit, IMU) and a variety of vehicle-mounted sensors. The present invention is based on IMU and depth camera sensors, and adopts a visual inertial navigation fusion positioning algorithm, aiming to improve the high-precision positioning performance of unmanned vehicles in complex environments by optimizing the fusion of multi-source sensor data. The inertial sensor is an inertial measurement unit. When positioning, the posture estimation can be achieved through IMU pre-integration or based on the perspective projection model (Perspective-n-Point, PnP).

[0045] During the IMU pre-integration process, the operating frequency of the camera (i.e., visual sensor) is usually in the range of tens of hertz, while the operating frequency of the inertial navigation system (INS) can reach hundreds of hertz. When coupling the data generated by these two different devices, the data format must be unified to ensure accurate fusion of information. For the data measured by the inertial navigation system, pre-integration processing is necessary to enhance the data consistency and accuracy of the overall system. Figure 7 This is the IMU pre-integration schematic.

[0046] From the measured angular velocity and acceleration There are two aspects to model the motion of gyroscopes and accelerometers. Angular velocity and acceleration The measured values ​​of are composed of their respective true values, deviations and white noise superposition: Among them, b represents deviation; n represents white noise; the subscript gyr represents angular velocity; and acc represents acceleration.

[0047] Expressing the above formula in the world coordinate system and the IMU coordinate system, the pre-integration processing can be obtained: In the formula, They are The measured values ​​of angular velocity and acceleration at the moment; and Respectively represent The bias error of the gyroscope and accelerometer at this moment; and Respectively represent Gaussian errors of gyroscopes and accelerometers at discrete moments; and Represents a rotation matrix: and is the translation vector: is the acceleration due to gravity: and The speed at the corresponding moment: is the sampling time; b represents the deviation; n represents white noise; the subscript gyr represents the angular velocity; and acc represents the acceleration.

[0048] The pose estimation process based on PnP includes: when performing pose estimation based on the perspective projection model (Perspective-n-Point, PnP), matched feature point pairs have been obtained. Using these feature point pairs, the PnP method can be used to solve the pose of the camera in the robot body. After that, the bundle adjustment (BA) method is used to optimize the errors generated in the solution process. By constructing a least squares problem, the goal is to minimize the reprojection error. In order to improve the real-time performance of the system and reduce the amount of calculation, the optimization process is limited to a fixed time window, and the sliding window algorithm is used to control the scale of the bundle adjustment. In this strategy, only the $N$ key frames closest to the current moment and the $M$ landmarks in the window are retained. At this time, the system state variables to be optimized in the sliding window can be expressed as: Each state variable Representative The state of the keyframe, Represents the state of the kth landmark; It can be further broken down into: In the above representation, represents the rotation from the robot body coordinate system to the world coordinate system, Represents speed, represents translation, and $ Represent the deviations of the accelerometer and gyroscope in the IMU coordinate system respectively. The present invention is based on the prior terms obtained by marginalization processing, which uses historical information to provide a constrained reference. IMU measurement residual terms, which include adjusting the estimate of the system state by the difference between the IMU measurement value and the predicted value. Visual reprojection residual terms, which adjust the pose estimate by comparing the observed feature point positions with the feature point projection positions calculated based on the current pose estimate.

[0049] By iteratively optimizing this cost function, a high-precision estimation of the camera pose can be achieved, while also optimizing the system’s understanding of the environment and enhancing the robot’s ability to navigate in three-dimensional space.

[0050] Step 103: Based on the positioning information and current posture state of the teaching mobile platform in the environmental perception map, and using preset path planning constraints, a path planning search is performed on the moving path of the teaching mobile platform to obtain a target path that meets the path planning constraints and can reach the destination.

[0051] In an embodiment of the present invention, based on the positioning information and current posture state of the teaching mobile platform in the environmental perception map, and using a preset spatiotemporal hybrid algorithm model, the moving path of the teaching mobile platform can be iteratively searched for path planning under the path planning constraint conditions until a path that satisfies the path planning constraint conditions and can reach the end point is found as the target path. The path planning constraint conditions include: the teaching mobile platform moves within a preset moving area, and the teaching mobile platform moves to avoid obstacles under dynamic obstacle constraints.

[0052] In this step, the present invention, based on the perception and positioning technology implemented in the previous text, uses the point cloud map generated by perception SLAM and the unmanned vehicle positioning data determined by sensor fusion to generate a collision-free safe path for the unmanned vehicle. In order to make the path meet all the constraints on the current unmanned vehicle in the current node, the present invention uses a spatiotemporal hybrid A The algorithm (space-time hybrid algorithm model) is used as a path planner to search for a target path that satisfies kinematic and dynamic constraints for the unmanned vehicle. Figure 8 As shown, it is a space-time mixture A Schematic diagram of the algorithm (i.e., the time-space hybrid algorithm model). The algorithm maintains a set and a SET, on which path finding and state expansion are based. Each node in the underlying search contains: (1) the time point of the trajectory planned by the unmanned vehicle at the current node. (2) The current motion state of the unmanned vehicle at the current node. The state is represented by the definition in the previous text as The present invention takes into account the continuity of the unmanned vehicle workspace and the discreteness of time, and refers to the concept of motion primitives

[14] , discretizes the control space, selects the acceleration and angular velocity of the unmanned vehicle as input control quantities, and limits the input amplitude. The algorithm calls the expand child node function (Get_Neighbors) to expand the current node through the control space. And the state of the child nodes of the current node can be obtained by the following calculation. For nodes with search status The state of its child nodes can be expressed as ,in It can be calculated as follows: Among them, the s vector represents the state information of the node; are the current position and angle respectively, is the position and angle of the previous moment; in the coefficient matrix A, B is the correction coefficient of the control information input at the corresponding moment; Represents the control input of the current node.

[0053] (3) Current node The actual cost of driverless vehicles .in, The arrival time and the cost of controlling smoothness are expressed as follows: in, represents the cost of controlling smoothness, Represents the control input of the current node. Considering the motion characteristics of the unmanned vehicle, the present invention is designed To make the generated path more satisfying of motion characteristics, it is expressed as follows: The present invention expects the unmanned vehicle to drive forward to the target point as quickly as possible and to perform steering, reversing and direction changes as little as possible, so two penalty factors are used: , Punishment for the above actions.

[0054] besides, represents the cost of steering smoothness, Represents the angle of the current node; It represents the time cost required to reach the current node, reflecting the cost of arrival time. These three costs together constitute the actual cost of path planning. , , are the correction coefficients of the three parameters.

[0055] (4) Current node Heuristic cost of autonomous vehicles .in, The function is a heuristic function, which consists of two parts. The nonholonomic constraint heuristic cost without obstacles and The cost of the complete constraint heuristic with obstacles. Only the kinematic constraints of the vehicle are considered, while obstacles in the environment are ignored. To the end state The shortest path.

[0056] Specifically, it returns the length cost of the Reeds-Shepp curve from the current point to the end point and the Euclidean distance from the current point to the end point The maximum value of is expressed as follows: Complete Constraint Heuristic By using the traditional A The algorithm obtains the shortest distance from the node to the end point as its cost value. The function is expressed as follows: (5) Current node Total path cost of unmanned vehicle .in, The function represents the total path cost of the current node, expressed as and sum.

[0057] Astar search first pops the set with the minimum Value Node Next, the algorithm determines the current node Status Is it close to the end point? If so, the algorithm will call the RS curve generation function (RS_Curve_Expansion) to calculate a Reeds-Shepp curve path from the current point to the end point without considering environmental static obstacles. This route is feasible and no environmental collision will occur. Otherwise, the current node Expand and call the conflict detection module to check each child node Whether there is a conflict with static obstacles and dynamic constraints in the environment. For feasible child nodes, the algorithm further calculates the node of , , The cost value of In the collection. Select In the collection The smallest node is the current node, and the node is added In the collection.

[0058] Finally, the search is repeated until a path that satisfies the constraints and reaches the end point is found as the goal.

[0059] For the generated child nodes , the algorithm calls the conflict detection module and uses the collision detection function (Check_Collision) to determine whether the state meets two constraints (i.e. path planning constraints): one is whether the body is in the feasible area The second is whether the body meets the dynamic obstacle constraint conditions. Specifically, it is necessary to judge the current node state Feasible, the state needs to meet the following conditions: ,in represents the dynamic obstacle constraint at the current moment; and .

[0060] Step 104: performing trajectory tracking control on the teaching mobile platform based on the target path, so that the teaching mobile platform can complete autonomous decision-making navigation.

[0061] In the embodiment of the present invention, in addition to automatically controlling the unmanned vehicle after autonomous decision-making and navigation based on the environmental information obtained by SLAM, the unmanned vehicle can also be controlled based on the keyboard information obtained by the host computer. The development based on the host computer control method is mainly based on ROS development, using the keyboard input information as the basis for controlling the vehicle. Through the keyboard input information, all control functions of the vehicle are realized, and the autonomous navigation control development is subsequently carried out on this basis.

[0062] In addition, the teaching mobile platform of the present invention can also realize the communication interconnection. The remote control communication module of the teaching mobile platform is mainly responsible for providing the user of the teaching mobile platform with the ability to remotely operate and debug the mobile photoelectric detection platform (i.e., the teaching mobile platform provided with a photoelectric detection module). Whether it is equipment debugging, remote control of the movable body, selection of detection targets, or observation of detection results, these operations will not be performed directly on the body computing core, but through the remote control communication module, remote operation of the SBC, such a mode can well simulate the use scenario of mobile detection equipment, so that students can more intuitively understand the development logic of the system in practical applications. Rich remote control communication schemes can mobilize students to use wireless communication and network-related knowledge and increase the integration of knowledge in various subjects. The remote control communication module provides two communication modes: Internet and radio. Mode 1, the computing core accesses the Internet through the 5G module Quectel RM520N-GL, and accesses the same virtual local area network with the control terminal through the FRP intranet penetration service provided by the server, and transmits instructions and data using the TCP network communication protocol. Mode 2: The computing core connects its own HDMI video signal to the Siyi HM30 image transmission module, and the receiving end connects the HDMI signal to the KVM module of the control terminal. At the same time, the USB control command of the KVM is converted into a TTL signal through the USB2UART adapter module and transmitted to the computing core through radio. The control terminal selects a GPDPocket3 portable computer with a KVM module and rich Internet access capabilities. The simpler way is to realize the interaction completely locally in the computing core. Using the radio image transmission mode, the user experience itself is equivalent to separating the computer's display and keyboard and mouse, and using them at a distance. Using the Internet, you can also use the remote desktop to access the computing core. These two control methods and interaction logics are basically not difficult to control, and students can easily get started. The more complex method is suitable for those who have spare time or are interested in network development. Using the Internet access mode, the request response of the computing core is transferred to the server, and then obtained through the client. This mode is closer to the actual production environment, and this mode not only uses the FRP service of the server, but also makes full use of the server's computing power and the 5G access capability of the computing core to complete more complex AI services such as autonomous driving control. Expand students' ability to apply knowledge related to network communication.

[0063] The photoelectric detection module described in the present invention refers to a system that detects the optical quantity to be measured or the optical quantity converted from the non-optical physical quantity to be measured by photoelectric conversion and circuit processing. Photoelectric detection technology is an important component of various detection technologies. The teaching experiment mobile platform system described in the present invention can be provided with a photoelectric detection module or a thermal imaging module for over-temperature detection tasks on the expansion module according to the actual teaching experiment needs. The photoelectric detection module can be an Intel D435 that assists unmanned vehicles in environmental perception. In this part, the present invention can use other photoelectric detection modules with environmental perception capabilities, such as laser radar, TOF camera, etc.; the thermal imaging module can be a Hikvision thermal imaging gimbal that completes special over-temperature detection tasks. The whole system application based on Intel D435 and the SLAM results have been described above. The following description is mainly based on the photoelectric detection module that completes the special task. The present invention provides users with full choices through the expansion module, and basically provides all devices commonly used in photoelectric detection on the market, including monocular cameras, binocular cameras, thermal imaging gimbals, depth cameras, laser radars, etc., as well as new photoelectric detection devices that the laboratory itself focuses on, various cameras, etc.

[0064] For example, an example experiment can be designed for an unmanned vehicle based on a thermal imaging module (such as Hikvision's thermal imaging gimbal). The main task flow is as follows: first, when the machine is turned on, the control rotation of the gimbal and the acquisition of gimbal information need to be completed in the handheld terminal. Then, based on the host computer, the unmanned vehicle collects environmental information, builds and saves maps, sets target points, and enters cruise mode. If no fire is found, the gimbal keeps patrolling and the car patrols the route in a fixed mode. If a fire is found, the host computer processes the information, triggers an alarm, stops on the spot and sends positioning information, so that the terminal operator can deal with the fire in time. Fig. 9 Example experimental task flow chart. Based on the example experimental objectives of the present invention, the experimental structure diagram is designed as follows: The terminal is connected to the host computer through the network. Since a graphical interface is required, the host computer is directly accessed using VNC. The host computer is connected to the unmanned vehicle through USB for control. The Hikvision gimbal is connected to the host computer through a network cable for data transmission, and the overall communication link is unobstructed. Fig.10 Example experiment structure diagram. The main goal of the example experiment of the present invention is to detect over-temperature, mainly using the smoke and fire detection algorithm, and making judgments by analyzing the thermal imaging image information; according to the formula T = B + C (100–S + 1) is binarized; B: background gray value in the current image; C: constant, which varies with different detectors; : Germanium window transmittance, 0.87; : Atmospheric transmittance; : Sensitivity, used to compensate for the difference between atmospheric transmittance and actual value. Fig.11As shown, it is an eight-bit image of fireworks detection; Fig.12 The following is the corresponding binarized image. The suspicious position is highlighted by binarization. If the number of pixels is >4, it is judged as a suspicious target and then subjected to subsequent inspection. The main process of the final design of fireworks detection is as follows: Fig.13 The figure shows a flow chart for fire and smoke detection. In the example experiment of the present invention, the pan / tilt head is placed in patrol mode and rotates regularly, which is often used to find and detect fire points in high-risk locations. In the experimental design of the present invention, since fire and smoke detection cannot be performed indoors, the present invention uses a high-temperature object to compare with the ambient temperature, and realizes the experimental design by discovering over-temperature. In this experiment, the present invention selects a bottle of 80-degree hot water and performs detection at room temperature of 22 degrees. The over-temperature range is set to 45 degrees. First, it can be seen that the pan / tilt head can accurately identify over-temperature objects. The temperature of the hot water is 71.3 degrees, which is out of range, triggering an alarm message and automatically popping up a window.

[0065] It should be noted that the new ORB-SLAM system designed by the present invention uses the same features to complete all SLAM tasks: tracking, mapping, relocalization and closed-loop correction. A survival of the fittest strategy is used to select the reconstruction of points and keyframes, resulting in excellent robustness and generating a compact and trackable map that grows only when the scene content changes, allowing lifelong operation. The present invention is exhaustively evaluated on 27 sequences from the most popular datasets. ORB-SLAM achieves unprecedented performance when compared with other state-of-the-art monocular SLAM methods. Bundle Adjustment (BA) is known to provide accurate camera positioning estimates as well as sparse geometric reconstruction, as long as a strong matching network and a good initial guess are provided. For a long time, this method has been considered unsuitable for real-time applications such as visual simultaneous positioning and mapping (visual SLAM). The goal of visual SLAM is to estimate the camera trajectory while reconstructing the environment. Now, the present invention knows that to achieve accurate results at an acceptable computational cost, the real-time SLAM algorithm must provide the following conditions to BA: corresponding observations of scene features (map points) in a subset of selected frames (keyframes). As the number of keyframes increases, the complexity grows accordingly, and their selection should avoid unnecessary redundancy. A strong network configuration of keyframes and points that produces accurate results, i.e., a set of well-distributed keyframes, observations of points with significant parallax, and a large number of closed-loop matches. An initial estimate of the keyframe poses and point positions for nonlinear optimization. A local map that is optimized during exploration to achieve scalability. The ability to perform fast global optimization (e.g., a pose graph) to close the loop in real time. Based on the main ideas of PTAM, the present invention designs an ORB-SLAM system, a novel monocular SLAM system, whose main contributions are as follows: Use the same features for all tasks: tracking, map building, relocalization, and loop closure. This makes the system of the present invention more efficient, simple, and reliable. The present invention uses ORB features, which allow real-time performance without a GPU, providing good invariance to viewpoint and lighting changes. Runs in real time in large environments. Due to the use of co-viewing, tracking and mapping are concentrated in a local co-viewing area, regardless of the size of the global map. Real-time loop closure based on the pose graph optimization called Essential Graph of the present invention. It is built from systematically maintained spanning trees, loop closure links, and strong edges in the co-visual map. Real-time camera relocalization with significant viewpoint and illumination invariance. This allows recovery from tracking failures and enhances map reuse. A new automatic and robust initialization procedure, based on model selection, allows creation of initial maps for both planar and non-planar scenes. A survival of the fittest approach to selecting map points and keyframes that is generous in generation but strict in culling.This policy improves the robustness of tracking and enhances lifetime performance because redundant keyframes are discarded. In place recognition, we propose a bag-of-vocabulary place recognizer based on DBoW2, using ORB. ORB is a binary feature that is invariant to rotation and scale (within a certain range), resulting in a very fast recognizer with good invariance to viewpoint. We demonstrate the high recall and robustness of the recognizer in four different datasets, requiring less than 39 milliseconds (including feature extraction) to retrieve loop candidates from a database of 10K images. In this study, we use an improved version of that place recognizer that uses co-visual information and returns multiple hypotheses instead of just the best match when querying the database. In keyframe selection, the most cost-effective approach is to retain as many points as possible while retaining only non-redundant keyframes. The approach using PTAM is very cautious in inserting keyframes to avoid excessive growth in computational complexity. This restrictive keyframe insertion strategy causes tracking to fail under difficult exploration conditions. Our survival of the fittest strategy achieves unprecedented robustness in difficult scenarios by inserting keyframes as quickly as possible and removing redundant frames afterwards to avoid additional costs.

[0066] The automatic navigation control method provided by the present invention obtains image data collected by a visual sensor preset on a teaching mobile platform, performs key frame tracking, local map construction and loop detection processing based on the image data, obtains perceived heterogeneous data, performs fusion processing on the heterogeneous data to obtain an environment perception map, obtains original measurement data collected by an inertial measurement sensor preset on the teaching mobile platform, performs posture estimation processing on the visual sensor on the teaching mobile platform based on the original measurement data, determines the posture state of the visual sensor in the teaching mobile platform, and obtains the position information of the teaching mobile platform collected by a positioning system preset on the teaching mobile platform; locates the teaching mobile platform in the environment perception map based on the posture state and the position information, determines the positioning information and the current posture state of the teaching mobile platform in the environment perception map, and uses the positioning information and the current posture state of the teaching mobile platform in the environment perception map to perform path planning search on the moving path of the teaching mobile platform based on the positioning information and the current posture state of the teaching mobile platform in the environment perception map and the preset path planning constraint conditions, obtains a target path that meets the path planning constraint conditions and can reach the end point, and performs trajectory tracking control on the teaching mobile platform based on the target path, so that the teaching mobile platform completes autonomous decision-making navigation, which can effectively improve the accuracy and robustness of automatic navigation control.

[0067] The automatic navigation control device provided by the present invention is described below. The automatic navigation control device described below and the automatic navigation control method described above can be referred to in correspondence with each other. Fig.15As shown, it is a schematic diagram of the structure of the automatic navigation control device provided by the present invention. The automatic navigation control device of the present invention specifically includes the following parts: The environmental perception module 1501 is used to obtain image data collected by a preset visual sensor on the teaching mobile platform, perform key frame tracking, local map construction and loop detection processing based on the image data, obtain perceived heterogeneous data, and fuse the heterogeneous data to obtain an environmental perception map.

[0068] The autonomous positioning module 1502 is used to obtain the original measurement data collected by the inertial measurement sensor preset on the teaching mobile platform, perform posture estimation processing on the visual sensor on the teaching mobile platform based on the original measurement data, and determine the posture state of the visual sensor in the teaching mobile platform; obtain the position information of the teaching mobile platform collected by the positioning system preset on the teaching mobile platform; locate the teaching mobile platform in the environmental perception map based on the posture state and the position information, and determine the positioning information and current posture state of the teaching mobile platform in the environmental perception map.

[0069] The path planning module 1503 is used to perform a path planning search on the moving path of the teaching mobile platform based on the positioning information and current posture state of the teaching mobile platform in the environmental perception map, and use preset path planning constraints to obtain a target path that meets the path planning constraints and can reach the destination.

[0070] The intelligent control module 1504 is used to perform trajectory tracking control on the teaching mobile platform based on the target path, so that the teaching mobile platform can complete autonomous decision-making navigation.

[0071] The present invention covers aspects such as environmental perception, autonomous positioning, path planning and intelligent control. Taking Segway Robotics PANDA with Ackerman model as the unmanned vehicle platform, the paper elaborates on how to realize the intelligence of unmanned vehicles from four perspectives: environmental perception, autonomous positioning, path planning and intelligent control. The block diagram of unmanned vehicles realizing autonomous navigation is shown in the figure. Figure 2 As shown. This block diagram shows the key modules and technical links involved in the autonomous navigation process of the unmanned vehicle. These include an environment perception module, a path planning module, a trajectory tracking control module, and an actuator control module. The automatic navigation control device provided by the present invention can effectively improve the accuracy and robustness of automatic navigation control.

[0072] The teaching experiment mobile platform system for implementing any one of the automatic navigation control methods described above provided by the present invention is described below. The teaching experiment mobile platform system for implementing any one of the automatic navigation control methods described above described below and the automatic navigation control method described above can be referred to each other. Fig.16 As shown, it is a structural schematic diagram of the teaching experiment mobile platform system provided by the present invention for implementing the automatic navigation control method as described in any one of the above. The teaching experiment mobile platform system for implementing the automatic navigation control method as described in any one of the above, the system is the teaching mobile platform, specifically including the following parts: a host computer, a remote control communication module, an unmanned vehicle, and an extension module; the unmanned vehicle is provided with a visual sensor and an inertial measurement sensor; the extension module is provided with a photoelectric detection module or a thermal imaging module for performing over-temperature detection tasks; the remote control communication module is used to control the terminal for data communication; the host computer is used to implement the automatic navigation control method, and the automatic navigation control method includes the following steps: Obtain image data collected by a visual sensor preset on the teaching mobile platform, perform key frame tracking, local map construction and loop detection processing based on the image data, obtain perceived heterogeneous data, perform fusion processing on the heterogeneous data, and obtain an environment perception map; Obtaining raw measurement data collected by an inertial measurement sensor preset on the teaching mobile platform, performing posture estimation processing on the visual sensor on the teaching mobile platform based on the raw measurement data, and determining the posture state of the visual sensor in the teaching mobile platform; obtaining the position information of the teaching mobile platform collected by a positioning system preset on the teaching mobile platform; locating the teaching mobile platform in the environmental perception map based on the posture state and the position information, and determining the positioning information and current posture state of the teaching mobile platform in the environmental perception map; Based on the positioning information and current posture state of the teaching mobile platform in the environmental perception map, and using preset path planning constraints, a path planning search is performed on the moving path of the teaching mobile platform to obtain a target path that satisfies the path planning constraints and can reach the end point; The teaching mobile platform is tracked and controlled based on the target path so that the teaching mobile platform can complete autonomous decision-making and navigation.

[0073] Fig.17 An example of a physical structure diagram of an electronic device is shown in FIG. Fig.17As shown, the electronic device (i.e., a real-time monitoring system for a power grid or a real-time monitoring device for a power grid) may include: a processor 1701, a communication interface 1704, a memory 1702, and a communication bus 1703, wherein the processor 1701, the communication interface 1704, and the memory 1702 communicate with each other through the communication bus 1703. The processor 1701 may call the logic instructions in the memory 1702 to execute the automatic navigation control method, which includes: obtaining image data collected by a visual sensor preset on a teaching mobile platform, performing key frame tracking, local map construction, and loop detection processing based on the image data, obtaining perceived heterogeneous data, and performing fusion processing on the heterogeneous data to obtain an environmental perception map; obtaining raw measurement data collected by an inertial measurement sensor preset on the teaching mobile platform, performing posture estimation processing on the visual sensor on the teaching mobile platform based on the raw measurement data, and determining the posture state of the visual sensor in the teaching mobile platform; obtaining the preset inertial measurement sensor on the teaching mobile platform The teaching mobile platform is located in the environment perception map based on the posture state and the position information, and the positioning information and the current posture state of the teaching mobile platform in the environment perception map are determined; based on the positioning information and the current posture state of the teaching mobile platform in the environment perception map, and using preset path planning constraints, a path planning search is performed on the moving path of the teaching mobile platform to obtain a target path that meets the path planning constraints and can reach the end point; based on the target path, the teaching mobile platform is tracked and controlled to enable the teaching mobile platform to complete autonomous decision-making navigation.

[0074] In addition, the logic instructions in the above-mentioned memory 1702 can be implemented in the form of a software functional unit and can be stored in a computer-readable storage medium when it is sold or used as an independent product. Based on this understanding, the technical solution of the present application can be essentially or partly embodied in the form of a software product that contributes to the prior art. The computer software product is stored in a storage medium, including several instructions to enable a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk, etc. Various media that can store program codes.

[0075] On the other hand, the present application also provides a computer program product, which includes a computer program. The computer program can be stored on a computer-readable storage medium. When the computer program is executed by a processor, the computer can execute the automatic navigation control method provided by the above-mentioned methods. The method includes: obtaining image data collected by a visual sensor preset on the teaching mobile platform, performing key frame tracking, local map construction and loop detection processing based on the image data to obtain perceived heterogeneous data, and fusing the heterogeneous data to obtain an environmental perception map; obtaining raw measurement data collected by an inertial measurement sensor preset on the teaching mobile platform, performing posture estimation processing on the visual sensor on the teaching mobile platform based on the raw measurement data, and determining the position of the visual sensor. The method comprises the steps of: determining the posture state of the teaching mobile platform by a sensor on the teaching mobile platform; obtaining the position information of the teaching mobile platform collected by a preset positioning system on the teaching mobile platform; locating the teaching mobile platform in the environment perception map based on the posture state and the position information, and determining the positioning information and current posture state of the teaching mobile platform in the environment perception map; performing path planning search on the moving path of the teaching mobile platform based on the positioning information and current posture state of the teaching mobile platform in the environment perception map and using preset path planning constraints, and obtaining a target path that meets the path planning constraints and can reach the end point; and performing trajectory tracking control on the teaching mobile platform based on the target path, so that the teaching mobile platform can complete autonomous decision-making navigation.

[0076] On the other hand, the present application also provides a computer-readable storage medium, which includes a stored program, wherein the automatic navigation control method provided by the above-mentioned methods is executed when the program is run, and the method includes: obtaining image data collected by a visual sensor preset on the teaching mobile platform, performing key frame tracking, local map construction and loop detection processing based on the image data, obtaining perceived heterogeneous data, and fusing the heterogeneous data to obtain an environmental perception map; obtaining raw measurement data collected by an inertial measurement sensor preset on the teaching mobile platform, performing posture estimation processing on the visual sensor on the teaching mobile platform based on the raw measurement data, and determining the position of the visual sensor in the teaching mobile platform. posture state; obtaining the position information of the teaching mobile platform collected by the positioning system preset on the teaching mobile platform; locating the teaching mobile platform in the environment perception map based on the posture state and the position information, and determining the positioning information and current posture state of the teaching mobile platform in the environment perception map; based on the positioning information and current posture state of the teaching mobile platform in the environment perception map, and using preset path planning constraints, performing path planning search on the moving path of the teaching mobile platform, and obtaining a target path that meets the path planning constraints and can reach the end point; performing trajectory tracking control on the teaching mobile platform based on the target path, so that the teaching mobile platform can complete autonomous decision-making navigation.

[0077] The device embodiments described above are merely illustrative, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. Ordinary technicians in this field can understand and implement it without paying creative labor.

[0078] Through the description of the above implementation methods, those skilled in the art can clearly understand that each implementation method can be implemented by means of software plus a necessary general hardware platform, and of course, can also be implemented by hardware. Based on this understanding, the above technical solution is essentially or the part that contributes to the prior art can be embodied in the form of a software product, and the computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a disk, an optical disk, etc., including a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment or some parts of the embodiments.

[0079] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present application, rather than to limit it. Although the present application has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present application.

Claims

1. An automatic navigation control method, characterized in that: include: Obtain image data collected by a visual sensor preset on the teaching mobile platform, perform key frame tracking, local map construction and loop detection processing based on the image data, obtain perceived heterogeneous data, perform fusion processing on the heterogeneous data, and obtain an environment perception map; Obtaining raw measurement data collected by an inertial measurement sensor preset on the teaching mobile platform, performing posture estimation processing on the visual sensor on the teaching mobile platform based on the raw measurement data, and determining the posture state of the visual sensor in the teaching mobile platform; Obtaining the location information of the teaching mobile platform collected by a positioning system preset on the teaching mobile platform; Positioning the teaching mobile platform in the environment perception map based on the posture state and the position information, and determining the positioning information and current posture state of the teaching mobile platform in the environment perception map; Based on the positioning information and current posture state of the teaching mobile platform in the environmental perception map, and using preset path planning constraints, a path planning search is performed on the moving path of the teaching mobile platform to obtain a target path that satisfies the path planning constraints and can reach the end point; The teaching mobile platform is tracked and controlled based on the target path, so that the teaching mobile platform can complete autonomous decision-making and navigation.

2. The automatic navigation control method according to claim 1, characterized in that: The method of performing key frame tracking, local map construction and loop detection processing based on the image data to obtain perceived heterogeneous data, and performing fusion processing on the heterogeneous data to obtain an environment perception map includes: Perform feature extraction based on the image data to obtain feature point information; perform map initialization of the visual sensor based on the feature point information to obtain corresponding three-dimensional space coordinates, and perform re-pose estimation and tracking on the current key frame based on the key reference frame to obtain a re-estimated initial pose of the current key frame; perform local map tracking based on the re-estimated initial pose of the current key frame to update the observation information of the map points in the local map, and determine whether the tracking is successful by counting the number of relevant feature points, and if the tracking is successful, create a corresponding key frame; The key frames in the key frame queue are processed in sequence to obtain map points that correspond to the key frames respectively; the map points are screened according to the recall rate and the number of observations to eliminate low-quality map points to obtain screened map points, and new map points are generated based on the screened map points, and the new map points are merged into the corresponding key frames; the map point coordinates and the visual sensor pose in the key frame are locally optimized by minimizing the reprojection error or the photometric error to obtain an optimized second key frame; the second key frame is added to the loop detection queue for loop detection to remove redundant key frames, obtain a target key frame after removing redundant key frames, and obtain an environmental perception map based on the target key frame.

3. The automatic navigation control method according to claim 2, characterized in that: The step of adding the second key frame to a loop detection queue for loop detection to remove redundant key frames, obtaining a target key frame after removing the redundant key frames, and obtaining an environment perception map based on the target key frame specifically includes: Adding the second key frame to a loop detection queue for loop detection processing to obtain a loop detection result; performing loop correction processing based on the loop detection result to remove redundant key frames to obtain a target key frame after removing the redundant key frames, and obtaining an environment perception map based on the target key frame; Among them, the adding the second key frame to the loop detection queue for closed-loop detection processing to obtain a closed-loop detection result includes: adding the second key frame to the loop detection queue for database query and similarity calculation to obtain a closed-loop detection result; the closed-loop correction processing based on the closed-loop detection result to remove redundant key frames, obtain the target key frame after removing the redundant key frames, and obtain the environment perception map based on the target key frame, including: performing closed-loop fusion and essential graph optimization processing based on the closed-loop detection result to achieve the removal of redundant key frames, and obtain the target key frame after removing the redundant key frames, and obtain the environment perception map based on the target key frame.

4. The automatic navigation control method according to claim 1, characterized in that: The performing posture estimation processing on the visual sensor on the teaching mobile platform based on the original measurement data to determine the posture state of the visual sensor in the teaching mobile platform includes: Based on the original measurement data, feature point information is acquired from the image data; based on the feature point information and based on the perspective projection model, a posture estimation process is performed on the visual sensor on the teaching mobile platform to obtain the posture state of the visual sensor in the teaching mobile platform; The performing posture estimation processing on the visual sensor on the teaching mobile platform based on the perspective projection model to obtain the posture state of the visual sensor in the teaching mobile platform includes: When the posture estimation of the visual sensor on the teaching mobile platform is performed based on the perspective projection model, the error generated in the posture estimation process is optimized using the bundle adjustment method. By constructing a least squares problem, the goal is to minimize the reprojection error to obtain the optimized posture state of the visual sensor in the teaching mobile platform.

5. The automatic navigation control method according to claim 1, characterized in that: The method of performing path planning search on the moving path of the teaching mobile platform based on the positioning information and the current posture state of the teaching mobile platform in the environmental perception map and using preset path planning constraints to obtain a target path that satisfies the path planning constraints and can reach the end point specifically includes: Based on the positioning information and current posture state of the teaching mobile platform in the environmental perception map, and using a preset spatiotemporal hybrid algorithm model, under the path planning constraints, an iterative search is performed on the moving path of the teaching mobile platform until a path that satisfies the path planning constraints and can reach the end point is found as the target path; wherein the path planning constraints include: the teaching mobile platform moves within a preset moving area, and the teaching mobile platform moves while avoiding obstacles under dynamic obstacle constraints.

6. An automatic navigation control device, characterized in that: include: The environment perception module is used to obtain image data collected by a preset visual sensor on the teaching mobile platform, perform key frame tracking, local map construction and loop detection processing based on the image data, obtain perceived heterogeneous data, and perform fusion processing on the heterogeneous data to obtain an environment perception map; An autonomous positioning module, used to obtain raw measurement data collected by an inertial measurement sensor preset on the teaching mobile platform, perform posture estimation processing on the visual sensor on the teaching mobile platform based on the raw measurement data, and determine the posture state of the visual sensor in the teaching mobile platform; Obtaining the location information of the teaching mobile platform collected by a positioning system preset on the teaching mobile platform; Positioning the teaching mobile platform in the environment perception map based on the posture state and the position information, and determining the positioning information and current posture state of the teaching mobile platform in the environment perception map; A path planning module, which is used to perform path planning search on the moving path of the teaching mobile platform based on the positioning information and current posture state of the teaching mobile platform in the environmental perception map and using preset path planning constraints to obtain a target path that satisfies the path planning constraints and can reach the end point; The intelligent control module is used to perform trajectory tracking control on the teaching mobile platform based on the target path, so that the teaching mobile platform can complete autonomous decision-making navigation.

7. A teaching experiment mobile platform system for implementing the automatic navigation control method according to any one of claims 1 to 5, characterized in that: include: A host computer, a remote control communication module, an unmanned vehicle, and an extension module; the unmanned vehicle is provided with a visual sensor and an inertial measurement sensor; the extension module is provided with a photoelectric detection module or a thermal imaging module for over-temperature detection tasks; the remote control communication module is used to control the terminal for data communication; the host computer is used to implement the automatic navigation control method, and the automatic navigation control method includes the following steps: Obtain image data collected by a visual sensor preset on the teaching mobile platform, perform key frame tracking, local map construction and loop detection processing based on the image data, obtain perceived heterogeneous data, perform fusion processing on the heterogeneous data, and obtain an environment perception map; Obtaining raw measurement data collected by an inertial measurement sensor preset on the teaching mobile platform, performing posture estimation processing on the visual sensor on the teaching mobile platform based on the raw measurement data, and determining the posture state of the visual sensor in the teaching mobile platform; Obtaining the location information of the teaching mobile platform collected by a positioning system preset on the teaching mobile platform; Positioning the teaching mobile platform in the environment perception map based on the posture state and the position information, and determining the positioning information and current posture state of the teaching mobile platform in the environment perception map; Based on the positioning information and current posture state of the teaching mobile platform in the environmental perception map, and using preset path planning constraints, a path planning search is performed on the moving path of the teaching mobile platform to obtain a target path that satisfies the path planning constraints and can reach the end point; The teaching mobile platform is tracked and controlled based on the target path, so that the teaching mobile platform can complete autonomous decision-making and navigation.

8. An electronic device comprising a memory, a processor, and a computer program stored in the memory and running on the processor, characterized in that: When the processor executes the program, the automatic navigation control method according to any one of claims 1 to 5 is implemented.

9. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the automatic navigation control method according to any one of claims 1 to 5 is implemented.

10. A computer program product, comprising a computer program, characterized in that When the computer program is executed by a processor, the automatic navigation control method according to any one of claims 1 to 5 is implemented.

Images