Automatic navigation control methods, devices, and mobile teaching experimental platform systems

Abstract

This invention provides an automatic navigation control method, device, and teaching experiment mobile platform system. The method includes: performing keyframe tracking, local map construction, and loop closure detection processing based on image data from a visual sensor to obtain heterogeneous data; fusing the heterogeneous data to obtain an environmental perception map; performing pose estimation processing on the visual sensor on the teaching mobile platform based on raw measurement data to determine its pose state; locating the teaching mobile platform in the environmental perception map to determine its positioning information and current pose state; and performing path planning search on the platform's movement path to obtain a target path that meets path planning constraints and reaches the destination, thereby performing trajectory tracking control and completing autonomous decision-making navigation. The method provided by this invention can effectively improve the accuracy and robustness of automatic navigation control.

Description

Technical Field

[0001] This 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. It also relates to an electronic device, a non-transitory computer-readable storage medium, and a computer program product. Background Technology

[0002] Autonomous driving refers to vehicles using various sensors mounted on their bodies to perceive their surroundings and make control decisions, allowing them to drive without a driver. Currently, Level 2 autonomous driving has been achieved in the industry. Autonomous driving technology mainly includes branches such as intelligent perception, autonomous localization, autonomous navigation, and autonomous decision-making, and has shown broad application prospects in various fields such as civilian and educational applications. Meanwhile, teaching and experimental mobile platform systems related to autonomous driving are becoming increasingly widely used. However, currently used teaching mobile platform systems have significant limitations, resulting in poor accuracy and robustness in automatic navigation control. Therefore, designing an automatic navigation control scheme based on teaching mobile platform systems has become an urgent technical problem to be solved. Summary of the Invention

[0003] This invention provides an automatic navigation control method and device to address the shortcomings of existing teaching mobile platform systems, which have high limitations, resulting in poor accuracy and robustness of automatic navigation control.

[0004] This invention provides an automatic navigation control method, comprising: obtaining image data collected by a preset visual sensor on a teaching mobile platform; performing keyframe tracking, local map construction, and loop closure detection processing based on the image data to obtain perceived heterogeneous data; performing fusion processing on the heterogeneous data to obtain an environmental perception map; obtaining raw measurement data collected by a preset inertial measurement sensor on the teaching mobile platform; performing pose estimation processing on the visual sensor on the teaching mobile platform based on the raw measurement data to determine the pose state of the visual sensor in the teaching mobile platform; and obtaining the teaching... The location information of the mobile platform; based on the pose state and the location information, the teaching mobile platform is located in the environmental perception map to determine the location information and current pose state of the teaching mobile platform in the environmental perception map; based on the location information and current pose 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 movement path of the teaching mobile platform to obtain a target path that meets the path planning constraints and can reach the destination; based on the target path, trajectory tracking control is performed on the teaching mobile platform to enable the teaching mobile platform to complete autonomous decision-making navigation.

[0005] According to the automatic navigation control method provided by the present invention, the step of performing keyframe tracking, local map construction, and loop closure detection based on the image data to obtain perceived heterogeneous data, and fusing the heterogeneous data to obtain an environmental perception map, includes: extracting features based on the image data to obtain feature point information; initializing the map of the visual sensor based on the feature point information to obtain the corresponding three-dimensional spatial coordinates, and performing re-pose estimation tracking of the current keyframe based on a key reference frame to obtain the re-estimated initial pose of the current keyframe; performing local map tracking based on the re-estimated initial pose of the current keyframe to update the observation information of map points within the local map, and determining whether the tracking is successful by counting the number of relevant feature points. If tracking is successful, a corresponding keyframe is created. Keyframes in the keyframe queue are processed sequentially to obtain map points corresponding to each keyframe. Low-quality map points are filtered based on their recall and observation count to obtain filtered map points. New map points are generated based on these filtered points and integrated into the corresponding keyframes. The coordinates of map points and the pose of the visual sensor in the keyframes are locally optimized by minimizing reprojection error or photometric error to obtain an optimized second keyframe. The second keyframe is added to a loop closure detection queue for loop closure detection to remove redundant keyframes, resulting in a target keyframe after removing redundant keyframes. An environmental perception map is obtained based on the target keyframe.

[0006] According to the automatic navigation control method provided by the present invention, the step of adding the second keyframe to a loop closure detection queue for loop closure detection to remove redundant keyframes, obtaining a target keyframe after removing redundant keyframes, and obtaining an environmental perception map based on the target keyframe includes: adding the second keyframe to a loop closure detection queue for loop closure detection processing to obtain a loop closure detection result; performing loop closure correction processing based on the loop closure detection result to remove redundant keyframes, obtaining a target keyframe after removing redundant keyframes, and obtaining an environmental perception map based on the target keyframe; wherein, the step of adding the second keyframe to a loop closure detection queue for loop closure detection to remove redundant keyframes, obtaining a target keyframe after removing redundant keyframes, and obtaining an environmental perception map based on the target keyframe; wherein, the step of adding the second keyframe to a loop closure detection queue for loop closure detection to remove redundant keyframes, obtaining a target keyframe based on the target keyframe, and obtaining an environmental perception map based on the target keyframe; The loop detection queue performs loop closure detection processing to obtain loop closure detection results, including: adding the second keyframe to the loop closure detection queue for database query and similarity calculation to obtain loop closure detection results; the loop closure correction processing based on the loop closure detection results to remove redundant keyframes and obtain target keyframes after removing redundant keyframes, and obtaining an environment perception map based on the target keyframes, includes: performing loop closure fusion and essence graph optimization processing based on the loop closure detection results to remove redundant keyframes and obtain target keyframes after removing redundant keyframes, and obtaining an environment perception map based on the target keyframes.

[0007] According to the automatic navigation control method provided by the present invention, the step of performing pose estimation processing on the visual sensor on the teaching mobile platform based on the original measurement data to determine the pose state of the visual sensor on the teaching mobile platform includes: obtaining feature point information from the image data based on the original measurement data; and performing pose estimation processing on the visual sensor on the teaching mobile platform based on the feature point information and based on a perspective projection model to obtain the pose state of the visual sensor on the teaching mobile platform.

[0008] The step of performing pose estimation processing on the visual sensor on the teaching mobile platform based on the perspective projection model to obtain the pose state of the visual sensor on the teaching mobile platform includes: when performing pose estimation processing on the visual sensor on the teaching mobile platform based on the perspective projection model, optimizing the error generated in the pose estimation process using the bundle adjustment method, and constructing a least squares problem with the goal of minimizing the reprojection error to obtain the optimized pose state of the visual sensor on the teaching mobile platform.

[0009] According to the automatic navigation control method provided by the present invention, the step of performing path planning search on the movement path of the teaching mobile platform based on the positioning information and current pose 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 destination, specifically includes: based on the positioning information and current pose state of the teaching mobile platform in the environmental perception map, and using a preset spatiotemporal hybrid algorithm model, under the path planning constraints, performing iterative path planning search on the movement path of the teaching mobile platform until a path that satisfies the path planning constraints and can reach the destination is found as the target path; wherein, the path planning constraints include: the teaching mobile platform moves within a preset movement area, and the teaching mobile platform avoids obstacles under dynamic obstacle constraints.

[0010] This invention also provides an automatic navigation control device, comprising: an environment perception module, used to obtain image data collected by a preset visual sensor on a teaching mobile platform, perform keyframe tracking, local map construction, and loop closure detection processing based on the image data to obtain perceived heterogeneous data, and perform fusion processing on the heterogeneous data to obtain an environment perception map; and an autonomous positioning module, used to obtain raw measurement data collected by a preset inertial measurement sensor on the teaching mobile platform, perform pose estimation processing on the visual sensor on the teaching mobile platform based on the raw measurement data, and determine the pose state of the visual sensor in the teaching mobile platform; and obtain the teaching... The system includes: a mobile platform location information module; a path planning module; a path planning search module; and an intelligent control module. The mobile platform is positioned on the environmental perception map based on its pose state and location information, determining its location information and current pose state.

[0011] According to the automatic navigation control device provided by the present invention, the environmental perception module is specifically used for:

[0012] Feature extraction is performed on the image data to obtain feature point information. Based on the feature point information, the visual sensor map is initialized to obtain the corresponding 3D spatial coordinates. The current keyframe is then re-estimated and tracked based on a key reference frame to obtain the re-estimated initial pose of the current keyframe. Local map tracking is performed based on the re-estimated initial pose of the current keyframe to update the observation information of map points within the local map. The success of tracking is determined by counting the number of relevant feature points. If tracking is successful, a corresponding keyframe is created. The keyframes in the keyframe queue are processed sequentially to obtain the corresponding keyframes. The frames contain corresponding map points; low-quality map points are filtered based on the recall rate and number of observations to obtain filtered map points, and new map points are generated based on the filtered map points and fused into the corresponding keyframes; the coordinates of map points and the pose of the visual sensor in the keyframes are locally optimized by minimizing reprojection error or photometric error to obtain an optimized second keyframe; the second keyframe is added to the loop closure detection queue for loop closure detection to remove redundant keyframes, and the target keyframe after removing redundant keyframes is obtained; an environmental perception map is obtained based on the target keyframe.

[0013] This invention also provides a teaching experiment mobile platform system for implementing the automatic navigation control method described in any of the above claims, comprising: a host computer, a remote control communication module, an unmanned vehicle, and an expansion module; the unmanned vehicle is equipped with a visual sensor and an inertial measurement sensor; the expansion module is equipped 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, which includes the following steps:

[0014] Image data collected by a preset visual sensor on a teaching mobile platform is obtained. Based on the image data, keyframe tracking, local map construction, and loop closure detection are performed to obtain perceived heterogeneous data. The heterogeneous data is then fused to obtain an environmental perception map.

[0015] The system obtains raw measurement data collected by a preset inertial measurement sensor on the teaching mobile platform; performs pose estimation processing on the visual sensor on the teaching mobile platform based on the raw measurement data to determine the pose state of the visual sensor on the teaching mobile platform; obtains the location information of the teaching mobile platform collected by a preset positioning system on the teaching mobile platform; and locates the teaching mobile platform in the environmental perception map based on the pose state and the location information of the teaching mobile platform in the environmental perception map and determines the positioning information and current pose state of the teaching mobile platform in the environmental perception map.

[0016] Based on the location information and current pose of the teaching mobile platform in the environmental perception map, and using preset path planning constraints, the movement path of the teaching mobile platform is searched to obtain a target path that meets the path planning constraints and can reach the destination.

[0017] The teaching mobile platform is tracked and controlled based on the target path to enable it to make autonomous decision-making and navigation.

[0018] The present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor, when executing the program, implements the automatic navigation control method as described in any of the preceding claims.

[0019] 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 of the preceding claims.

[0020] The present invention also provides a computer program product, including a computer program that, when executed by a processor, implements the automatic navigation control method as described in any of the preceding claims.

[0021] The automatic navigation control method provided by this invention obtains image data collected by a pre-set visual sensor on a teaching mobile platform. Based on the image data, it performs keyframe tracking, local map construction, and loop closure detection to obtain perceived heterogeneous data. The heterogeneous data is then fused to obtain an environmental perception map. Raw measurement data collected by a pre-set inertial measurement sensor on the teaching mobile platform is also obtained. Based on the raw measurement data, pose estimation processing is performed on the visual sensor on the teaching mobile platform to determine its pose state. The location information of the teaching mobile platform collected by a pre-set positioning system on the teaching mobile platform is then obtained. Based on the pose state and location information, the teaching mobile platform is located in the environmental perception map to determine its positioning information and current pose state. Based on the positioning information and current pose state of the teaching mobile platform in the environmental perception map, and using pre-set path planning constraints, a path planning search is performed on the movement path of the teaching mobile platform to obtain a target path that meets the path planning constraints and can reach the destination. Based on the target path, trajectory tracking control is performed on the teaching mobile platform to enable autonomous decision-making navigation. This effectively improves the accuracy and robustness of automatic navigation control. Attached Figure Description

[0022] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0023] Figure 1 This is a flowchart illustrating the automatic navigation control method provided by the present invention.

[0024] Figure 2 This is a complete flowchart of the automatic navigation control method provided by the present invention.

[0025] Figure 3 This is a schematic diagram of the Ackermann steering model provided by the present invention.

[0026] Figure 4 This is a schematic diagram illustrating the key point extraction principle provided by the present invention.

[0027] Figure 5 This is a schematic diagram illustrating the optimization principle of Bundle Adjustment provided by the present invention.

[0028] Figure 6 This is a schematic diagram illustrating the effect of the SLAM algorithm provided by this invention.

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

[0030] Figure 8 This is a schematic diagram of the spatiotemporal hybrid algorithm model provided by the present invention.

[0031] Figure 9 This is a flowchart of an example experimental task provided by the present invention.

[0032] Figure 10 This is an example experimental structure diagram provided by the present invention.

[0033] Figure 11 This is an eight-bit image for smoke detection provided by the present invention.

[0034] Figure 12 This is a schematic diagram of the binarized smoke detection provided by the present invention.

[0035] Figure 13 This is a schematic diagram of the smoke detection process provided by the present invention.

[0036] Figure 14 This is a schematic diagram of the common view provided by the present invention.

[0037] Figure 15 This is a schematic diagram of the automatic navigation control device provided by the present invention.

[0038] Figure 16 This is a schematic diagram of the structure of the mobile teaching platform provided by the present invention.

[0039] Figure 17 This is a schematic diagram of the structure of the electronic device provided by the present invention. Detailed Implementation

[0040] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention. The following description, in conjunction with... Figures 1-17 This invention describes the automatic navigation control method and apparatus, and provides a detailed description of its embodiments. The following is a detailed description of embodiments based on the automatic navigation control method described in this invention. Figure 1 The diagram shown is a flowchart of the automatic navigation control method provided by the present invention. The specific implementation process includes the following steps:

[0041] Step 101: Obtain image data collected by a preset visual sensor on the teaching mobile platform, perform keyframe tracking, local map construction and loop closure detection based on the image data to obtain perceived heterogeneous data, and perform fusion processing on the heterogeneous data to obtain an environmental perception map.

[0042] In this embodiment of the invention, feature point information is obtained by extracting features from image data collected by a pre-set visual sensor on a teaching mobile platform. Based on this feature point information, the visual sensor's map is initialized to obtain corresponding three-dimensional spatial coordinates. Then, based on a key reference frame, the current keyframe is re-estimated and tracked to obtain a re-estimated initial pose of the current keyframe. Local map tracking is performed based on the re-estimated initial pose of the current keyframe to update the observation information of map points within the local map. The success of the tracking is determined by counting the number of relevant feature points. If the tracking is successful, a corresponding keyframe is created. The keyframes in the keyframe queue are then processed sequentially. The process involves: processing map points corresponding to the keyframes; filtering map points based on their recall and observation count to remove low-quality points, obtaining filtered map points, generating new map points based on these filtered points, and integrating these new map points into the corresponding keyframes; optimizing the map point coordinates and visual sensor poses in the keyframes by minimizing reprojection or photometric errors to obtain optimized second keyframes; adding the second keyframe to a loop closure detection queue to remove redundant keyframes, obtaining the target keyframe after removing redundant keyframes, and obtaining an environmental perception map based on the target keyframe. Specifically, in the process of adding the second keyframe to the loop closure detection queue for loop closure detection to remove redundant keyframes and obtain the target keyframe after removing redundant keyframes, and obtaining the environmental perception map based on the target keyframe, the second keyframe can first be added to the loop closure detection queue for loop closure detection processing to obtain the loop closure detection result, and then loop closure correction processing can be performed based on the loop closure detection result to remove redundant keyframes and obtain the target keyframe after removing redundant keyframes, and the environmental perception map can be obtained based on the target keyframe. The step of adding the second keyframe to the loop closure detection queue for loop closure detection processing to obtain the loop closure detection result includes: adding the second keyframe to the loop closure detection queue for database query and similarity calculation to obtain the loop closure detection result; the step of performing loop closure correction processing based on the loop closure detection result to remove redundant keyframes and obtain the target keyframe after removing the redundant keyframes, and obtaining the environment perception map based on the target keyframe includes: performing loop closure fusion and essence graph optimization processing based on the loop closure detection result to remove redundant keyframes and obtain the target keyframe after removing the redundant keyframes, and obtaining the environment perception map based on the target keyframe.

[0043] It should be noted that the unmanned vehicle (i.e., the teaching mobile platform) used in this invention adopts a rear-wheel drive motion mode with front-wheel steering and rear-wheel drive in the application scenario. An Ackerman steering model is used to more completely describe the vehicle's steering, acceleration, and deceleration movements, such as... Figure 3 As shown. The state of the unmanned vehicle can be represented as... .in This indicates the position information of the rear axle center of the unmanned vehicle in the global coordinate system. Indicates the yaw angle of the unmanned vehicle; These represent the current speed and acceleration of the unmanned vehicle in the Frenet coordinate system, respectively. This indicates the steering angle of the front wheels of the autonomous vehicle. This is the wheelbase for the front and rear wheels along the vehicle's direction. When the steering angle is fixed... At this time, the trajectory of the unmanned vehicle can be approximated as a moving circular arc, with the radius of the arc denoted as... .

[0044] Autonomous vehicle steering angle and its turning angular velocity The following kinematic relationship exists between them:

[0045]

[0046] This invention defines control input By discretizing the integral sampling time It can calculate the unit time step. The robot's state at a given point can be represented by the following system of differential equations:

[0047]

[0048] Control input is conditional , Constraints Indicates the sampling time; The value represents the acceleration of the unmanned vehicle in the Frenet coordinate system; T represents the sampling time. This represents the state of the unmanned vehicle at time t-1.

[0049] In this step, environmental perception is a prerequisite for the intelligentization of the unmanned vehicle. This invention utilizes the collaborative work of multiple sensors, such as visual sensors (e.g., binocular vision sensors) and LiDAR sensors, enabling the unmanned vehicle to perceive its complex surrounding environment in real time, accurately, and robustly, acquiring key information such as pedestrians, vehicles, and obstacles. Based on this, the unmanned vehicle employs Simultaneous Localization and Mapping (SLAM) technology to intelligently fuse heterogeneous data from multiple sensors, generating a semantically rich and highly accurate environmental perception map, laying a solid foundation for subsequent localization, path planning, and decision-making control. This invention deploys the ORB-SLAM2 (Oriented FAST and Rotated BRIEF-SLAM 2) algorithm on a host computer, simultaneously supporting monocular, binocular, and RGB-D cameras. Figure 2 As shown, an Intel RealSense Depth Camera D435 is used to achieve environmental perception and map building for autonomous vehicles. ORB-SLAM2 is a feature-point-based visual SLAM algorithm that extracts ORB (Oriented Fast and Rotated BRIEF) feature point information from images and combines graph optimization and loop closure detection techniques to achieve real-time, robust camera pose estimation and sparse environment reconstruction. Since the input to the map building system is a grayscale image and a depth map, initialization can be completed through the first keyframe, i.e., constructing the initial map points and the pose of the current keyframe.

[0050] In the keyframe tracking process based on the image data of this invention, the main goal of the tracking module is to estimate the position of the camera (e.g., a visual sensor) in real time, select key video frames (i.e., keyframes), 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., keyframes) where the grayscale changes significantly in both the horizontal and vertical directions. Descriptors are used to describe each feature point, and are presented as a vector as the unique feature of each feature point. In the ORB-SLAM system, the key point extraction adopts the improved FAST corner detection algorithm, such as Figure 4As shown, the calculation of the descriptor is based on the BRIEF (Binary Robust Independent Elementary Features) algorithm. (2) Camera initialization. The corresponding three-dimensional spatial coordinates are generated by using the depth map obtained by the RGB-D camera (such as a binocular vision sensor) and the extracted feature point information. 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), it can directly calculate the absolute coordinates of the feature points in the three-dimensional space, thus completing the camera initialization process. Before the camera initialization, the camera parameters need to 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 checkerboard as the calibration board. The grid points on the black and white checkerboard will be extracted as feature points by the algorithm. 2. Take pictures: Use the camera to be calibrated to take pictures of the checkerboard 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: Calculate the world coordinates of each corner point according to the size of the calibration board. 5. Camera Calibration: Utilizing the correspondence between image points and world points, the camera's intrinsic and extrinsic parameters are solved. The intrinsic parameter solution algorithm is as follows: By minimizing the reprojection error, the world coordinates are projected onto the image plane using the camera's intrinsic parameter matrix and distortion coefficients. This is then compared with the actually observed image points, and the intrinsic parameters are adjusted to minimize the error. The extrinsic parameter solution algorithm is as follows: Using the known intrinsic parameters, the world coordinates are transformed to the camera coordinate system. Then, the camera's rotation matrix and translation vector are solved by minimizing the error between the camera coordinates and the image coordinates. 6. Evaluation of Calibration Results: The calibration results are evaluated, typically including the calculation of reprojection error and the analysis of distortion parameters. Finally, the parameters for inputting the camera are obtained for subsequent calculations. Additionally, the current frame pose is estimated based on the pose of the previous frame. After successful camera initialization, the validity of the constant velocity motion model is first checked. The constant velocity motion model is used to predict the camera pose and guide the matching of map points observed in the previous frame. If the matching is successful, the camera's motion speed is considered constant; otherwise, the pose needs to be re-estimated using a key reference frame. The key reference frame is a point that shares the same viewpoint as the current keyframe (i.e., a relevant feature point or interior point, such as...). Figure 14The frame with the most (as shown). If the matching with the key reference frame still fails, the relocalization module is triggered to estimate the pose. The relocalization process includes: screening candidate keyframes, 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, the local map is updated based on the pose. Some map points are projected onto the current frame (i.e., the keyframe or video frame), and the pose is optimized using Bundle Adjustment (BA), as shown in the figure, and the observation information of the map points is updated. By counting the number of inliers, the tracking success is determined, such as Figure 5 As shown, - These are interior points viewed from different perspectives. , , These represent different locations of the unmanned vehicle. (5) Keyframe creation. The keyframe selection strategy directly affects the real-time performance of the system. The map points are the spatial location points of relevant feature points or interior points in the coordinate system corresponding to the environment perception map.

[0051] 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 after successfully tracking the current frame. The processing flow of this module can be divided into the following steps: (1) Keyframe insertion. Check whether there is a keyframe in the buffer queue passed by the previous module. If there is, process the first keyframe in the queue and obtain its corresponding map point relationship; if there is no keyframe, the current thread pauses for 3 milliseconds and then starts the next round of query. (2) Remove low-quality map points. Filter according to the recall rate and the number of observations of the map points. The removal principle is that the recall rate is less than 0.25 and the number of observations in three consecutive keyframes is less than 2. Conversely, if the map point is not removed in three consecutive keyframes, it is considered a high-quality map point. (3) Generate new map points. Perform feature matching between the current keyframe and the top 10 keyframes with the highest co-visibility. At the same time, merge the map points of the current keyframe and the co-visibility keyframes to increase the number of map points. Map point fusion is divided into two cases: if there is no corresponding point at the back projection 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 closure detection queue. Whether a keyframe is redundant depends on whether 90% of its map points can be observed by 3 or more other keyframes.

[0052] The main task of loop closure detection based on the image data is to solve the problem of estimating the position of the globally consistent trajectory and map over time. Before achieving loop closure, the module continuously detects new keyframes. The processing flow of the loop closure detection module can be divided into the following steps: (1) Database query. Based on the bag-of-words vector of the current keyframe, query potential loop closure candidate keyframes. The selection principle of the loop closure candidate keyframe is that it is similar to the current keyframe in direction, but there is no direct connection relationship. (2) Similarity calculation. If the current frame and the loop closure candidate keyframe are matched according to the bag-of-words result, the SE (3) transformation is solved and optimized to obtain the similarity transformation matrix between the two frames, so as to minimize the projection error. (3) Loop closure fusion. After completing the first two steps, the system enters the loop closure state and begins to fuse the repeated point cloud data. At this time, the current frame and the keyframe matched by the connected loop are fused. All keyframes in the loop, as well as the map points observed by each keyframe, will be globally optimized to make the fusion of point cloud data more effective. (4) Essential graph optimization. By introducing stationary pose as a constraint, the poses of all map points and keyframes are optimized. The main purpose of the essential graph optimization is to distribute the accumulated error throughout the closed-loop trajectory, improving global consistency. Through the collaboration of these three threads, the ORB-SLAM algorithm achieves high-precision, real-time monocular SLAM functionality. The effective combination of key technologies such as feature extraction and matching, camera initialization, motion estimation, local mapping optimization, and loop closure detection and global optimization ensures the algorithm's robustness and accuracy in various scenarios. Figure 6 The image shown is a screenshot of the SLAM algorithm. Figure 6 The point cloud shown can accurately identify object shapes, construct map information, and obtain environmental perception information.

[0053] Step 102: Obtain raw measurement data collected by the preset inertial measurement sensor on the teaching mobile platform; perform pose estimation processing on the visual sensor on the teaching mobile platform based on the raw measurement data to determine the pose state of the visual sensor on the teaching mobile platform; obtain the location information of the teaching mobile platform collected by the preset positioning system on the teaching mobile platform; locate the teaching mobile platform in the environmental perception map based on the pose state and the location information to determine the positioning information and current pose state of the teaching mobile platform in the environmental perception map.

[0054] In this invention, after obtaining the raw measurement data collected by the preset inertial measurement sensor on the teaching mobile platform, feature point information can be obtained from the image data based on the raw measurement data. Based on the feature point information and a perspective projection model, pose estimation processing is performed on the visual sensor on the teaching mobile platform to obtain the pose state of the visual sensor on the teaching mobile platform. Specifically, the process of performing pose estimation processing on the visual sensor on the teaching mobile platform based on the perspective projection model to obtain the pose state of the visual sensor on the teaching mobile platform includes: when performing pose estimation processing on the visual sensor on the teaching mobile platform based on the perspective projection model, the bundle adjustment method is used to optimize the errors generated during the pose estimation process. By constructing a least squares problem, the goal is to minimize the reprojection error to obtain the optimized pose state of the visual sensor on the teaching mobile platform.

[0055] Specifically, in the process of locating the teaching mobile platform in the environmental perception map based on the pose state and location information, and determining the positioning information and current pose 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 measurement unit (IMU), and multiple vehicle-mounted sensors. This invention, based on an IMU and a depth camera sensor, employs a visual-inertial navigation fusion positioning algorithm, aiming to improve the high-precision positioning performance of unmanned vehicles in complex environments through optimized fusion of multi-source sensor data. The inertial sensor is an inertial measurement unit. During positioning, pose estimation can be achieved through IMU pre-integration or based on a perspective-n-point (PnP) model.

[0056] In the IMU pre-integration process, the operating frequency of the camera (i.e., the visual sensor) is typically in the tens of hertz range, while the operating frequency of the inertial navigation system (INS) can reach hundreds of hertz. When coupling data from these two different devices, a unified data format must be used to ensure accurate information fusion. Pre-integration processing is necessary for data measured by the inertial navigation system to enhance the overall system's data consistency and accuracy. Figure 7 This is a schematic diagram of the IMU pre-integration principle.

[0057] From the measured angular velocity and acceleration A motion model is established for the gyroscope and accelerometer from two aspects: angular velocity. and acceleration The measured values ​​are composed of their respective true values, biases, and white noise superimposed:

[0058]

[0059] Where b represents deviation; n represents white noise; the subscript gyr represents angular velocity; and acc represents acceleration.

[0060] Expressing the above equation in both the world coordinate system and the IMU coordinate system, and performing pre-integration, we obtain:

[0061]

[0062] In the formula, They are respectively The measured values ​​of angular velocity and acceleration at a given moment; and Represent The bias error of the gyroscope and accelerometer at any given time; and Represent Gaussian errors of gyroscopes and accelerometers under discrete time-major conditions; and Represents the rotation matrix: and For translation vectors: For gravitational acceleration: and Velocity at the corresponding moment: yr represents the sampling time; b represents the bias; n represents white noise; the subscript gyr represents angular velocity; acc represents acceleration.

[0063] The pose estimation process based on Perspective-n-Point (PnP) includes: During pose estimation based on the Perspective-n-Point (PnP) model, matched feature point pairs have already been obtained. Using these feature point pairs, the PnP method can be used to solve for the camera pose within the robot body. Subsequently, the Bundle Adjustment (BA) method is used to optimize the errors generated during the solution process. By constructing a least-squares problem, the goal is to minimize the reprojection error. To improve the system's real-time performance and reduce computational cost, the optimization process is limited to a fixed time window, and a sliding window algorithm is used to control the scale of the Bundle Adjustment. In this strategy, only the N keyframes closest to the current time and the M landmarks within the window are retained. Therefore, the system state variables to be optimized within the sliding window can be expressed as:

[0064]

[0065] Each state variable Representing the The state of the keyframe, This represents the state of the k-th landmark. It can be further subdivided into:

[0066]

[0067] In the above representation, This represents the rotation from the robot's body coordinate system to the world coordinate system. Represents speed, Represents translation. and $ These represent the deviations of the accelerometer and gyroscope in the IMU coordinate system, respectively. This invention is based on prior terms obtained through edge processing, which utilize historical information to provide a constraint reference. The IMU measurement residual term includes adjustments to the system state estimate based on the difference between IMU measurements and predictions. The visual reprojection residual term adjusts the pose estimation by comparing the observed feature point positions with the projected feature point positions calculated based on the current pose estimation.

[0068] By iteratively optimizing this cost function, high-precision estimation of camera pose can be achieved, while also optimizing the system's understanding of the environment and enhancing the robot's navigation capabilities in three-dimensional space.

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

[0070] In this embodiment of the invention, based on the positioning information and current pose 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 for the movement path of the teaching mobile platform is performed until a path that satisfies the path planning constraints and can reach the destination is found as the target path. The path planning constraints include: the teaching mobile platform moves within a preset movement area; and the teaching mobile platform avoids obstacles under dynamic obstacle constraints.

[0071] In this step, based on the perception and localization technologies implemented earlier, this invention utilizes point cloud maps generated by perceptual SLAM and unmanned vehicle localization data determined by sensor fusion to generate collision-free safe paths for unmanned vehicles. To ensure the path satisfies all constraints related to the current unmanned vehicle at the current node, this invention employs a spatiotemporal hybrid A... The algorithm (a spatiotemporal hybrid algorithm model) acts as a path planner, searching for target paths that satisfy kinematic and dynamic constraints for autonomous vehicles. For example... Figure 8 As shown, it is a spatiotemporal hybrid A Schematic diagram of the algorithm (i.e., spatiotemporal hybrid algorithm model). Spatiotemporal hybrid A The algorithm maintains a set and a SET, which are used for path finding and state expansion. Each node in the underlying search contains: (1) the time point of the planned trajectory of the current node's unmanned vehicle; and (2) the current motion state of the current node's unmanned vehicle. The state is represented by the definition above. This invention takes into account the continuity of the unmanned vehicle's workspace and the discreteness of time, and refers to the concept of motion primitives

[14] to discretize the control space, select the unmanned vehicle's acceleration and angular velocity as input control quantities, and limit the input amplitude. The algorithm will call the extended child node function (Get_Neighbors) to expand the current node through the control space. And the state of the current node's child nodes can be obtained by the following calculation. For those with search state The state of a node and its child nodes can be represented as follows: ,in The calculation can be performed as follows:

[0072]

[0073] Here, the s vector represents the state information of the node; These represent the current position and angle, respectively. The previous position and angle; coefficient matrices A and B This is the correction coefficient for the input control information at the corresponding time. This represents the control input for the current node.

[0074] (3) Current node The actual cost of driverless vehicles .in, The cost of arrival time and control smoothness is expressed as follows:

[0075]

[0076] in, This represents the cost of controlling smoothness. This represents the control input for the current node. Considering the motion characteristics of unmanned vehicles, this invention is designed... To make the generated path better satisfy motion characteristics, it is represented as follows:

[0077]

[0078] This invention aims to enable unmanned vehicles to move forward towards the target point as quickly and minimally as possible, involving actions such as turning, reversing, and changing direction. Therefore, two penalty factors are employed. , The above actions will be punished.

[0079] besides, The cost representing steering smoothness, This represents the angle of rotation at the current node; This 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. , , These are the correction coefficients for the three parameters.

[0080] (4) Current node The heuristic cost of autonomous vehicles .in, The function is a heuristic function, consisting of two parts. Nonholonomic constraint heuristic cost and obstacle-free The cost of heuristics involving complete constraints. The cost of heuristics involving nonholonomic constraints. Calculating from the current state only by considering the vehicle's kinematic constraints and ignoring environmental obstacles and other information. To the end state The shortest path.

[0081] Specifically, this means returning the cost of the length of the Reeds-Shepp curve from the current point to the endpoint. Euclidean distance from the current point to the destination The maximum value is represented as follows:

[0082]

[0083] Complete constraint heuristic By using traditional A on the current node The algorithm uses the shortest distance from the node to the destination as its cost. The final... The function is represented as follows:

[0084]

[0085] (5) Current node Total path cost of autonomous vehicles .in, The function represents the total path cost of the current node, expressed as: and sum.

[0086]

[0087] Astar search first pops the value with the smallest value from the OpenSet collection. value node Next, the algorithm will determine the current node. status Is the destination near? 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 destination, ignoring static environmental obstacles. This route is feasible and must not involve environmental collisions. Otherwise, for the current node... Expand the scope and call the conflict detection module to check each child node. Does it conflict with static obstacles and dynamic constraints in the environment? For feasible child nodes, the algorithm further calculates the node's... of , , The value of the node, and add the node to Within the set. Select. In the collection The smallest node is the current node, and this node is added. In the collection.

[0088] Finally, repeat the search until a path that satisfies the constraints and reaches the destination is found as the target.

[0089] For the generated child nodes The algorithm calls the collision detection module and uses the collision detection function (Check_Collision) to determine whether the state meets two constraints (i.e., path planning constraints): first, whether the machine is in a feasible region. First, it involves determining whether the machine body meets the constraints of dynamic obstacles. Specifically, it requires judging the current node state. It is feasible, provided that the state meets the following conditions: ,in This represents the dynamic obstacle constraints at the current moment; and .

[0090] Step 104: 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 and navigation.

[0091] In this embodiment of the invention, in addition to autonomous decision-making and navigation based on environmental information obtained from SLAM, the unmanned vehicle can also be controlled by obtaining keyboard information from a host computer. The development of the host computer-based control method is mainly based on ROS, using keyboard input as the basis for vehicle control. Through keyboard input, all vehicle control functions are achieved, and subsequent development of autonomous navigation control will follow.

[0092] Furthermore, the teaching mobile platform described in this invention can also achieve communication interconnection. The remote control communication module of the teaching mobile platform is mainly responsible for providing users with the ability to remotely operate and debug the mobile photoelectric detection platform (i.e., the teaching mobile platform equipped with a photoelectric detection module). Whether it is equipment debugging, remotely controlling the mobile vehicle body, selecting the detection target, or observing the detection results, these operations are not performed directly on the vehicle body's computing core, but are performed remotely through the remote control communication module to operate the SBC. This mode can effectively simulate the usage scenarios of mobile detection equipment, allowing students to more intuitively understand the development logic of the system in practical applications. The rich remote control communication schemes can mobilize students to apply their knowledge of wireless communication and networks, increasing the integration of knowledge from various subjects. The remote control communication module provides two communication modes: Internet and radio. In mode one, the computing core accesses the Internet through the Quectel RM520N-GL 5G module, and through the FRP intranet penetration service provided by the server, it connects to the same virtual local area network as the control terminal, using the TCP network communication protocol to transmit instructions and data. In Mode 2, the computing core connects its HDMI video signal to the Siyi HM30 image transmission module, while the receiving end connects the HDMI signal to the KVM module of the control terminal. Simultaneously, a USB-to-UART adapter converts the KVM's USB control commands into TTL signals, which are then transmitted wirelessly to the computing core. The control terminal is a GPDPocket3 portable computer, equipped with a KVM module and rich internet access capabilities. A simpler approach involves completely local interaction on the computing core using wireless image transmission. The user experience is akin to having the computer's monitor, keyboard, and mouse independent, allowing for remote use via the internet or remote desktop access. Both control methods and interaction logic are relatively straightforward and easy for students to learn. A more complex approach, suitable for those with extra time or an interest in network development, utilizes internet access to transfer the computing core's request and response to a server, which is then retrieved by the client. This mode more closely resembles a real-world production environment and not only utilizes the server's FRP service but also fully leverages the server's computing power and the computing core's 5G access capabilities to perform more complex AI services such as autonomous driving control. This expands students' ability to apply their knowledge of network communication.

[0093] The photoelectric detection module described in this invention refers to a system that detects optical quantities or optical quantities converted from non-optical physical quantities through photoelectric conversion and circuit processing. Photoelectric detection technology is an important component of various detection technologies. The teaching experiment mobile platform system described in this invention can be equipped with photoelectric detection modules or thermal imaging modules for over-temperature detection tasks on expansion modules according to actual teaching experiment needs. The photoelectric detection module can be an Intel D435 to assist unmanned vehicles in environmental perception. In this part, this invention can use other photoelectric detection modules with environmental perception capabilities, such as LiDAR, TOF cameras, etc.; the thermal imaging module can be a Hikvision thermal imaging gimbal for completing specific over-temperature detection tasks. The application of the entire system based on Intel D435 and the SLAM results have been described above. The following description mainly focuses on the photoelectric detection module for completing specific tasks. This invention provides users with ample choices through expansion modules, basically providing all commonly used photoelectric detection devices on the market, including monocular cameras, binocular cameras, thermal imaging gimbals, depth cameras, LiDAR, etc., as well as new photoelectric detection devices that the laboratory focuses on, such as various cameras.

[0094] For example, an example experiment can be designed as an unmanned vehicle based on a thermal imaging module (such as a Hikvision thermal imaging gimbal). The main task flow is as follows: First, the handheld terminal needs to control the rotation of the gimbal and obtain gimbal information. Then, the host computer enables the unmanned vehicle to collect environmental information, build and save the map, set the target point, and enter the patrol mode. If no fire is detected, the gimbal continues to patrol, and the vehicle patrols the route in a fixed mode. If a fire is detected, the host computer processes the information, triggers an alarm, stops in place, and sends location information, so that the terminal operator can deal with the fire in time. Figure 9 Example experimental task flowchart. Based on the example experimental objective of this invention, the experimental structure diagram is designed as follows: The terminal and the host computer are connected via a network. Since a graphical interface is required, VNC is mainly used to directly access the host computer. The host computer is connected to and controlled by the unmanned vehicle via USB. The Hikvision gimbal is connected to the host computer and transmits data via a network cable. The overall communication link is smooth. Figure 10 Example experimental structure diagram. The main objective of this invention's example experiment is overheat detection, primarily employing a smoke and fire detection algorithm, making a judgment by analyzing thermal imaging information; based on the formula T = B + C. (100–S + 1) is used for binarization; B: background grayscale value in the current image; C: constant, which varies depending on the detector; Germanium window transmittance: 0.87; Atmospheric transmittance; Sensitivity is used to compensate for the difference between atmospheric transmittance and the actual value. Figure 11The image shown is an eight-bit graph for smoke detection; as shown... Figure 12 The image shown is the corresponding binarized image. Binarization is used to highlight suspicious locations; if the number of pixels is greater than 4, the target is judged as suspicious and undergoes further inspection. The final design of the main smoke detection process is as follows: Figure 13 The diagram shows the smoke and fire detection flowchart. In the example experiment of this invention, the pan-tilt unit is in inspection mode, rotating regularly, which is commonly used to detect and detect fire points in high-risk locations. In the experimental design of this invention, since smoke and fire detection cannot be performed indoors, this invention uses a high-temperature object to compare with the ambient temperature. The experiment is designed to detect overheating. In this experiment, this invention selects a bottle of 80-degree hot water and conducts the detection in an environment with a room temperature of 22 degrees Celsius. The overheating range is set to 45 degrees Celsius. First, it can be seen that the pan-tilt unit can accurately identify the overheated object. The temperature of the hot water is 71.3 degrees Celsius, which exceeds the range, triggering an alarm message and automatically displaying a pop-up window.

[0095] It should be noted that the novel ORB-SLAM system designed in this invention uses the same features to complete all SLAM tasks: tracking, map building, relocalization, and loop closure correction. A survival-of-the-fittest strategy is used to select points and keyframes for reconstruction, resulting in excellent robustness and generating a compact and trackable map that only grows as scene content changes, allowing for lifetime operation. This invention has been thoroughly evaluated on 27 sequences from the most popular datasets. ORB-SLAM achieves unprecedented performance compared to other state-of-the-art monocular SLAM methods. Bundle Adjustment (BA) is known to provide accurate camera localization estimation and sparse geometric reconstruction, provided a strong matching network and good initial guesses are provided. This method has long been considered unsuitable for real-time applications such as visual simultaneous localization and mapping (visual SLAM). The goal of visual SLAM is to estimate camera trajectories while reconstructing the environment. Now, this invention shows that to achieve accurate results at an acceptable computational cost, real-time SLAM algorithms must provide the BA with the following: corresponding observations of scene features (map points) in a subset of selected frames (keyframes). As the number of keyframes increases, so does the complexity; their selection should avoid unnecessary redundancy. A robust network configuration of keyframes and points that produces accurate results—a well-distributed set of keyframes, observations of points with significant disparity, and a large number of loop closure matches—is crucial. Initial estimations of keyframe pose and point positions are needed for nonlinear optimization. Local maps optimized during exploration enable scalability. The ability to perform fast global optimizations (e.g., pose graphs) to close loops in real time is essential. Based on the main ideas of PTAM, this invention designs an ORB-SLAM system, a novel monocular SLAM system, whose main contributions are as follows: The same features are used for all tasks: tracking, map building, relocalization, and loop closure. This makes the system more efficient, simple, and reliable. The invention uses ORB features, which allow real-time performance without a GPU and provide good invariance to viewpoint and lighting changes. It operates in real-time in large environments. Due to the use of a common view, tracking and mapping are concentrated within a local common view area, independent of the global map size. Real-time loop closure is achieved based on pose graph optimization, which this invention calls the Essential Graph. It is constructed through a system-maintained spanning tree, closed-loop links, and strong edges in the shared view. Real-time camera relocalization features significant viewpoint and lighting invariance. This allows recovery from tracking failures and enhances map reuse. A novel, automatic, and robust initialization procedure, based on model selection, allows for the creation of initial maps for both planar and non-planar scenes. A survival-of-the-fittest approach to selecting map points and keyframes is very generous during generation but very strict during culling.This policy improves the robustness of tracking and enhances lifetime operation because redundant keyframes are discarded. In location recognition, this invention proposes a bag-of-vocabulary location recognizer based on DPoW2, using ORB. ORB is a binary feature invariant to rotation and scale (within a certain range), resulting in a very fast recognizer with good viewpoint invariance. This invention demonstrates high recall and robustness of the recognizer on four different datasets, requiring less than 39 milliseconds (including feature extraction) to retrieve loop candidates from a 10K image database. In this study, this invention uses an improved version of that location recognizer, using co-view information and returning multiple hypotheses rather than just the best match when querying the database. In keyframe selection, the most cost-effective approach in this invention is to retain as many points as possible while retaining only non-redundant keyframes. The PTAM approach inserts keyframes very cautiously to avoid excessive computational complexity. This restrictive keyframe insertion strategy causes tracking to fail under difficult exploration conditions. The survival-of-the-fittest strategy of this invention achieves unprecedented robustness in difficult scenarios by inserting keyframes as quickly as possible and then removing redundant frames afterward to avoid additional costs.

[0096] The automatic navigation control method provided by this invention obtains image data collected by a pre-set visual sensor on a teaching mobile platform. Based on the image data, it performs keyframe tracking, local map construction, and loop closure detection to obtain perceived heterogeneous data. The heterogeneous data is then fused to obtain an environmental perception map. Raw measurement data collected by a pre-set inertial measurement sensor on the teaching mobile platform is also obtained. Based on the raw measurement data, pose estimation processing is performed on the visual sensor on the teaching mobile platform to determine its pose state. The location information of the teaching mobile platform collected by a pre-set positioning system on the teaching mobile platform is then obtained. Based on the pose state and location information, the teaching mobile platform is located in the environmental perception map to determine its positioning information and current pose state. Based on the positioning information and current pose state of the teaching mobile platform in the environmental perception map, and using pre-set path planning constraints, a path planning search is performed on the movement path of the teaching mobile platform to obtain a target path that meets the path planning constraints and can reach the destination. Based on the target path, trajectory tracking control is performed on the teaching mobile platform to enable autonomous decision-making navigation. This effectively improves the accuracy and robustness of automatic navigation control.

[0097] The automatic navigation control device provided by the present invention is described below. The automatic navigation control device described below can be referred to in correspondence with the automatic navigation control method described above. References Figure 15The diagram shown is a structural schematic 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:

[0098] The environment perception module 1501 is used to obtain image data collected by a preset visual sensor on the teaching mobile platform, perform keyframe tracking, local map construction and loop closure 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.

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

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

[0101] 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 and navigation.

[0102] This invention covers environmental perception, autonomous localization, path planning, and intelligent control. Using the Segway Robotics PANDA with an Ackerman model as an unmanned vehicle platform, it elaborates on how to achieve intelligent unmanned vehicles from these four perspectives: environmental perception, autonomous localization, path planning, and intelligent control. A block diagram illustrating autonomous navigation for unmanned vehicles is shown below. Figure 2 As shown in the diagram, this block diagram illustrates the key modules and technical aspects involved in the autonomous navigation process of an unmanned vehicle. These include an environmental perception module, a path planning module, a trajectory tracking control module, and an actuator control module. The automatic navigation control device provided by this invention can effectively improve the accuracy and robustness of automatic navigation control.

[0103] The following describes a teaching experiment mobile platform system provided by the present invention for implementing the automatic navigation control method described in any of the above claims. The teaching experiment mobile platform system described below for implementing the automatic navigation control method described in any of the above claims can be referred to in correspondence with the automatic navigation control method described above. References Figure 16 The diagram shows a structural schematic of a teaching experiment mobile platform system provided by the present invention for implementing the automatic navigation control method described above. The teaching experiment mobile platform system of the present invention, which implements the automatic navigation control method described above, specifically includes the following components: a host computer, a remote control communication module, an unmanned vehicle, and an expansion module; the unmanned vehicle is equipped with a visual sensor and an inertial measurement sensor; the expansion module is equipped with a photoelectric detection module or a thermal imaging module for performing over-temperature detection tasks; the remote control communication module is used for data communication with the control terminal; the host computer is used to implement the automatic navigation control method, which includes the following steps:

[0104] Image data collected by a preset visual sensor on a teaching mobile platform is obtained. Based on the image data, keyframe tracking, local map construction, and loop closure detection are performed to obtain perceived heterogeneous data. The heterogeneous data is then fused to obtain an environmental perception map.

[0105] The system obtains raw measurement data collected by a preset inertial measurement sensor on the teaching mobile platform; performs pose estimation processing on the visual sensor on the teaching mobile platform based on the raw measurement data to determine the pose state of the visual sensor on the teaching mobile platform; obtains the location information of the teaching mobile platform collected by a preset positioning system on the teaching mobile platform; and locates the teaching mobile platform in the environmental perception map based on the pose state and the location information of the teaching mobile platform in the environmental perception map and determines the positioning information and current pose state of the teaching mobile platform in the environmental perception map.

[0106] Based on the location information and current pose of the teaching mobile platform in the environmental perception map, and using preset path planning constraints, the movement path of the teaching mobile platform is searched to obtain a target path that meets the path planning constraints and can reach the destination.

[0107] The teaching mobile platform is tracked and controlled based on the target path to enable it to make autonomous decision-making and navigation.

[0108] Figure 17 An example of a physical structure diagram of an electronic device is shown below. Figure 17As shown, the electronic device (i.e., the power grid real-time monitoring system or power grid real-time monitoring equipment) may include: a processor 1701, a communication interface 1704, a memory 1702, and a communication bus 1703. The processor 1701, communication interface 1704, and memory 1702 communicate with each other via the communication bus 1703. The processor 1701 can call logical instructions in the memory 1702 to execute an automatic navigation control method. This method includes: obtaining image data collected by a preset visual sensor on the teaching mobile platform; performing keyframe tracking, local map construction, and loop closure detection based on the image data to obtain perceived heterogeneous data; performing fusion processing on the heterogeneous data to obtain an environmental perception map; obtaining raw measurement data collected by a preset inertial measurement sensor on the teaching mobile platform; performing pose estimation processing on the visual sensor on the teaching mobile platform based on the raw measurement data to determine the pose state of the visual sensor on the teaching mobile platform; and obtaining the preset... The positioning system collects the location information of the teaching mobile platform; based on the pose state and the location information, the teaching mobile platform is positioned in the environmental perception map to determine the positioning information and current pose state of the teaching mobile platform in the environmental perception map; based on the positioning information and current pose 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 movement path of the teaching mobile platform to obtain a target path that meets the path planning constraints and can reach the destination; based on the target path, trajectory tracking control is performed on the teaching mobile platform to enable the teaching mobile platform to complete autonomous decision-making and navigation.

[0109] Furthermore, the logical instructions in the aforementioned memory 1702 can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this application. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0110] On the other hand, this application also provides a computer program product, which includes a computer program that 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 methods. The method includes: obtaining image data collected by a preset visual sensor on a teaching mobile platform; performing keyframe tracking, local map construction, and loop closure detection processing based on the image data to obtain perceived heterogeneous data; performing fusion processing on the heterogeneous data to obtain an environmental perception map; obtaining raw measurement data collected by a preset inertial measurement sensor on the teaching mobile platform; performing pose estimation processing on the visual sensor on the teaching mobile platform based on the raw measurement data to determine the pose of the visual sensor. The system detects the pose state of the teaching mobile platform; obtains the location information of the teaching mobile platform collected by a preset positioning system on the teaching mobile platform; locates the teaching mobile platform in the environmental perception map based on the pose state and the location information, and determines the positioning information and current pose state of the teaching mobile platform in the environmental perception map; based on the positioning information and current pose state of the teaching mobile platform in the environmental perception map, and using preset path planning constraints, performs path planning search on the movement path of the teaching mobile platform to obtain a target path that meets the path planning constraints and can reach the destination; and performs trajectory tracking control on the teaching mobile platform based on the target path to enable the teaching mobile platform to complete autonomous decision-making and navigation.

[0111] Furthermore, this application also provides a computer-readable storage medium, which includes a stored program, wherein the program, when running, executes the automatic navigation control method provided by the methods described above. This method includes: obtaining image data collected by a preset visual sensor on a teaching mobile platform; performing keyframe tracking, local map construction, and loop closure detection processing based on the image data to obtain perceived heterogeneous data; performing fusion processing on the heterogeneous data to obtain an environmental perception map; obtaining raw measurement data collected by a preset inertial measurement sensor on the teaching mobile platform; performing pose 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 on the teaching mobile platform. The process involves: obtaining the teaching mobile platform's position information from a preset positioning system; locating the teaching mobile platform in the environmental perception map based on the position information and the position status, determining the platform's location information and current position status in the map; performing path planning search on the teaching mobile platform's movement path based on the location information and current position status in the map, and using preset path planning constraints, obtaining a target path that satisfies the path planning constraints and reaches the destination; and performing trajectory tracking control on the teaching mobile platform based on the target path to enable autonomous decision-making and navigation.

[0112] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units, i.e., they may be located in one location or distributed across multiple network units. Some or all of the modules may be selected based on actual needs to achieve the objectives of the present embodiment. Persons of ordinary skill in the art will be able to understand and implement the present invention without inventive effort.

[0113] Through the above description of the embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus necessary general-purpose hardware platforms, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solutions, in essence or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in the various embodiments or some parts of the embodiments.

[0114] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application.

Claims

1. An automatic navigation control method, characterized in that, include: Image data collected by a preset visual sensor on a teaching mobile platform is obtained. Based on the image data, keyframe tracking, local map construction, and loop closure detection are performed to obtain perceived heterogeneous data. The heterogeneous data is then fused to obtain an environmental perception map. The raw measurement data collected by the preset inertial measurement sensor on the teaching mobile platform is obtained, and the pose estimation processing of the visual sensor on the teaching mobile platform is performed based on the raw measurement data to determine the pose state of the visual sensor in the teaching mobile platform. Obtain the location information of the teaching mobile platform collected by the preset positioning system on the teaching mobile platform; Based on the pose state and the location information, the teaching mobile platform is located in the environmental perception map to determine the location information and current pose state of the teaching mobile platform in the environmental perception map. Based on the location information and current pose of the teaching mobile platform in the environmental perception map, and using preset path planning constraints, the movement path of the teaching mobile platform is searched to obtain a target path that meets the path planning constraints and can reach the destination. The teaching mobile platform is tracked and controlled based on the target path to enable it to make autonomous decision-making and navigation.

2. The automatic navigation control method according to claim 1, characterized in that, The process of performing keyframe tracking, local map construction, and loop closure detection based on the image data to obtain perceived heterogeneous data, and then fusing the heterogeneous data to obtain an environmental perception map, includes: Feature extraction is performed on the image data to obtain feature point information; the map of the visual sensor is initialized based on the feature point information to obtain the corresponding three-dimensional spatial coordinates, and the pose estimation and tracking of the current keyframe is performed based on the key reference frame to obtain the re-estimated initial pose of the current keyframe; local map tracking is performed based on the re-estimated initial pose of the current keyframe to update the observation information of map points in the local map, and the tracking success is determined by counting the number of relevant feature points. If the tracking is successful, the corresponding keyframe is created. The keyframes in the keyframe queue are processed sequentially to obtain map points that correspond to each keyframe. Low-quality map points are filtered based on their recall and observation count to obtain filtered map points. New map points are generated based on these filtered map points and integrated into the corresponding keyframes. The coordinates of the map points and the pose of the visual sensor in the keyframes are locally optimized by minimizing reprojection error or photometric error to obtain an optimized second keyframe. The second keyframe is added to a loop closure detection queue for loop closure detection to remove redundant keyframes, resulting in a target keyframe after removing redundant keyframes. An environmental perception map is obtained based on the target keyframe.

3. The automatic navigation control method according to claim 2, characterized in that, The step of adding the second keyframe to the loop closure detection queue for loop closure detection to remove redundant keyframes, obtaining the target keyframe after removing redundant keyframes, and obtaining the environment-aware map based on the target keyframe specifically includes: The second keyframe is added to the loop closure detection queue for loop closure detection processing to obtain the loop closure detection result; based on the loop closure detection result, loop closure correction processing is performed to remove redundant keyframes to obtain the target keyframe after removing redundant keyframes; and an environmental perception map is obtained based on the target keyframe. The step of adding the second keyframe to the loop closure detection queue for loop closure detection processing to obtain the loop closure detection result includes: adding the second keyframe to the loop closure detection queue for database query and similarity calculation to obtain the loop closure detection result; the step of performing loop closure correction processing based on the loop closure detection result to remove redundant keyframes and obtain the target keyframe after removing the redundant keyframes, and obtaining the environment perception map based on the target keyframe includes: performing loop closure fusion and essence graph optimization processing based on the loop closure detection result to remove redundant keyframes and obtain the target keyframe after removing the redundant keyframes, and obtaining the environment perception map based on the target keyframe.

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

5. The automatic navigation control method according to claim 1, characterized in that, The process of using the location information and current pose state of the teaching mobile platform in the environmental perception map, and employing preset path planning constraints to perform path planning search on the movement path of the teaching mobile platform, to obtain a target path that satisfies the path planning constraints and can reach the destination, specifically includes: Based on the positioning information and current pose state of the teaching mobile platform in the environmental perception map, and using a preset spatiotemporal hybrid algorithm model, under the path planning constraints, the movement path of the teaching mobile platform is iteratively searched until a path that satisfies the path planning constraints and can reach the destination is found as the target path; wherein, the path planning constraints include: the teaching mobile platform moves within a preset movement area, and the teaching mobile platform avoids 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 keyframe tracking, local map construction and loop closure detection based on the image data, obtain perceived heterogeneous data, and perform fusion processing on the heterogeneous data to obtain an environment perception map. The autonomous positioning module is used to obtain the raw measurement data collected by the preset inertial measurement sensor on the teaching mobile platform, and to perform pose estimation processing on the visual sensor on the teaching mobile platform based on the raw measurement data to determine the pose state of the visual sensor in the teaching mobile platform. Obtain the location information of the teaching mobile platform collected by the preset positioning system on the teaching mobile platform; Based on the pose state and the location information, the teaching mobile platform is located in the environmental perception map to determine the location information and current pose state of the teaching mobile platform in the environmental perception map. The path planning module is used to perform path planning search on the mobile path of the teaching mobile platform based on the positioning information and current pose state of the teaching mobile platform in the environmental perception map, and using preset path planning constraints, to obtain a target path that meets the path planning constraints and can reach the destination. 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 and navigation.

7. A teaching experimental mobile platform system for implementing the automatic navigation control method as described in any one of claims 1-5, characterized in that, include: The system comprises a host computer, a remote control communication module, an unmanned vehicle, and an expansion module; the unmanned vehicle is equipped with a visual sensor and an inertial measurement sensor; the expansion module is equipped with a photoelectric detection module or a thermal imaging module for over-temperature detection; 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, which includes the following steps: Image data collected by a preset visual sensor on a teaching mobile platform is obtained. Based on the image data, keyframe tracking, local map construction, and loop closure detection are performed to obtain perceived heterogeneous data. The heterogeneous data is then fused to obtain an environmental perception map. The raw measurement data collected by the preset inertial measurement sensor on the teaching mobile platform is obtained, and the pose estimation processing of the visual sensor on the teaching mobile platform is performed based on the raw measurement data to determine the pose state of the visual sensor in the teaching mobile platform. Obtain the location information of the teaching mobile platform collected by the preset positioning system on the teaching mobile platform; Based on the pose state and the location information, the teaching mobile platform is located in the environmental perception map to determine the location information and current pose state of the teaching mobile platform in the environmental perception map. Based on the location information and current pose of the teaching mobile platform in the environmental perception map, and using preset path planning constraints, the movement path of the teaching mobile platform is searched to obtain a target path that meets the path planning constraints and can reach the destination. The teaching mobile platform is tracked and controlled based on the target path to enable it to make 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, it implements the automatic navigation control method as described in any one of claims 1 to 5.

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

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

Images