A robot path planning method and system based on optical vision

Abstract

This invention relates to a robot path planning method and system based on optical vision, comprising the following steps: acquiring environmental images of the robot's environment and robot motion information; obtaining a dense 3D point cloud map based on the environmental image through contrast enhancement, signal extraction, decoding, and interpolation optimization; generating a semantically labeled 3D point cloud map based on the dense 3D point cloud map and a color RGB visible light environmental image through background culling, image tracking, semantic recognition, and clustering; estimating the robot's motion state based on the semantically labeled 3D point cloud map and robot motion information, performing global and local path planning, and obtaining a local path after trajectory smoothing; and converting the local path into the robot's actual actions by combining the robot's motion state. This invention effectively achieves a balance between cost control and performance improvement through an active optical vision perception architecture and multi-algorithm collaborative design.

Description

Technical Field

[0001] This invention relates to the field of robot navigation and automatic control technology, and in particular to a method and system for three-dimensional environmental perception and real-time path planning using an active optical vision sensor. Background Technology

[0002] Mobile robots are increasingly used in warehousing and logistics, intelligent inspection, and autonomous driving. One of their core capabilities is autonomous navigation and path planning in complex environments. Traditional path planning methods rely on LiDAR to build environmental maps, which, while highly accurate, have limitations such as high cost and inability to acquire rich information like object surface texture and color.

[0003] Optical vision-based path planning schemes use cameras as the primary sensor, offering low cost and rich information dimensions. However, existing vision solutions largely rely on passive vision, making them highly sensitive to changes in ambient lighting. Their performance deteriorates sharply in low light, texture loss, or direct sunlight. Furthermore, reconstructing stable and rapid 3D environmental information for navigation from 2D images remains a technical challenge, making it difficult for existing visual navigation solutions to simultaneously achieve real-time performance, robustness, and accuracy.

[0004] Meanwhile, existing structured light-based vision solutions mainly include striped structured light, fixed-code structured light, and traditional speckle structured light. Striped structured light projects continuous or discrete stripe patterns and calculates depth based on triangulation principles. It relies on stitching together multiple frames of stripe-shifted images, which easily introduces motion blur for dynamic objects, resulting in poor real-time performance. Furthermore, the stripes are susceptible to interference from strong ambient light, exhibiting severe distortion outdoors or under complex lighting conditions, leading to a significant decrease in point cloud reconstruction accuracy. Fixed-code structured light projects a pre-set fixed-code pattern and obtains depth through pattern matching. It cannot adapt to scene changes with varying distances and textures. At close range, code overlap is common, while at long range, code recognition is low. Mismatches easily occur when the background texture is similar to the coded pattern, making it difficult to effectively remove background interference. Traditional speckle structured light projects randomly distributed speckle patterns and uses the uniqueness of the specks for matching. However, its speckle distribution is uneven, resulting in overly dense point clouds at close range leading to computational redundancy, and sparse point clouds at long range with gaps. Moreover, speckle matching is easily affected by motion speed during dynamic target tracking, resulting in insufficient tracking robustness and an inability to quickly respond to changes in target position.

[0005] At the path planning level, traditional global planning algorithms can generate theoretically optimal paths, but lack real-time response capabilities to dynamic environmental changes. When the environment changes abruptly, a new global search is required, which is time-consuming. Local planning algorithms respond quickly, but are prone to getting trapped in local optima and do not fully integrate environmental semantic information, making it difficult to distinguish obstacle types and the stability of traversable areas, resulting in insufficient safety and adaptability of the planned paths. Furthermore, most path planning methods have low integration with perception data, relying solely on two-dimensional geometric information for decision-making, failing to utilize the depth and semantic features of the three-dimensional environment, further limiting navigation robustness. Therefore, there is an urgent need in this field for a robot path planning method based on optical vision that can overcome the above shortcomings and achieve high robustness, high real-time performance, and low cost. Summary of the Invention

[0006] The purpose of this invention is to provide a robot path planning method and system based on optical vision, to solve the problems of high cost, sensitivity to ambient lighting, and insufficient real-time performance and robustness of existing technologies in 3D environment reconstruction. The technical solution is as follows:

[0007] The robot path planning method based on optical vision according to an embodiment of the present invention includes the following steps:

[0008] S1. Acquire environmental images of the robot's environment and robot motion information. The environmental images include deformable infrared structured light images modulated by environmental objects and color RGB visible light environmental images.

[0009] S2. Based on environmental images, a dense 3D point cloud map is obtained through contrast enhancement, signal extraction, decoding, and interpolation optimization.

[0010] S3. Based on dense 3D point cloud maps and color RGB visible light environment images, a 3D point cloud map with semantic labels is generated through background removal, image tracking, semantic recognition, and clustering.

[0011] S4. Based on the 3D point cloud map with semantic labels and robot motion information, estimate the robot motion state, perform global and local path planning, and obtain the local path after trajectory smoothing. The robot motion state includes the robot's global pose, linear velocity and angular velocity.

[0012] S5. Combine the robot's motion state to convert the local path into the robot's actual actions.

[0013] Furthermore, the specific steps of S1 include:

[0014] S101. A single-point infrared laser is used as the projection light source. The single-point infrared laser is scattered into multiple random scattering points by an optical diffuser. The laser is then optically convolved by a grating to form a random fingerprint-coded structured light, which is then projected onto the robot's environment.

[0015] S102. Use an infrared CCD sensor and a visible light CCD sensor to acquire deformed infrared structured light images and color RGB visible light environmental images modulated by environmental objects.

[0016] S103. A wheeled odometer and an inertial measurement unit are used to acquire robot motion information.

[0017] Furthermore, the specific steps of S2 include:

[0018] S201. Perform contrast enhancement processing on the modulated deformable infrared structured light image of the environmental object to obtain an enhanced two-dimensional infrared image matrix.

[0019] S202. The Laplace-Gauss operator is used to perform convolution calculation on the enhanced two-dimensional infrared image matrix, extract the coordinates of abrupt change points and mark them to obtain the initial point cloud;

[0020] S203. The initial point cloud is decoded using a two-dimensional cross-correlation algorithm. The initial point cloud matrix is ​​matched with a preset matching template of different depths to obtain the depth information of each point and obtain a point cloud containing depth information.

[0021] S204. A bicubic interpolation algorithm is used to optimize the interpolation of point clouds containing depth information to obtain a dense three-dimensional point cloud map.

[0022] Furthermore, the specific steps of S3 include:

[0023] S301. Based on a dense 3D point cloud map and a color RGB visible light environment image, a multimodal mixture Gaussian background modeling algorithm is used to remove the background and obtain the foreground image, including foreground depth point cloud data and foreground color RGB visible light image.

[0024] S302. Based on the foreground color RGB visible light image, an optical flow tracing algorithm is used to track the image and obtain the tracker result window and its confidence level.

[0025] S303. Based on the foreground color RGB visible light image, a cascaded classifier is used to perform semantic recognition on the foreground target, and the classifier result window and its confidence are obtained.

[0026] S304. Merge the tracker result window and the classifier result window to output the foreground target tracking window;

[0027] S305. The PN learning algorithm is used to perform semi-supervised learning on the random forest classifier and the nearest neighbor classifier.

[0028] S306. Based on the foreground image and the foreground target tracking window, calculate its geometric features and color texture information, distinguish between passable areas, static obstacles and dynamic obstacles, and assign corresponding semantic labels to different attribute areas to obtain a 3D point cloud map with semantic labels.

[0029] Furthermore, the specific steps of S303 include:

[0030] S3031. Concatenate the variance classifier, random forest classifier, and nearest neighbor classifier in sequence to form a cascaded classifier and construct a positive sample set.

[0031] S3032, In the variance classifier, the sum of gray values ​​of the image window is calculated using the integral image method. The variance classifier identifies the first potential foreground target and inputs the resulting classification window containing the first potential foreground target into the random forest classifier.

[0032] S3033. Identify the second potential foreground target using a random forest classifier, and input the resulting classification window containing the second potential foreground target into the nearest neighbor classifier.

[0033] S3034. In the nearest neighbor classifier, the classification window containing the second potential foreground target is compared with the positive sample set to calculate the correlation similarity and conservative similarity. If both similarities are greater than their respective preset thresholds, it is considered to contain the third potential foreground target, and the classification window containing the third potential foreground target is regarded as the classifier result window.

[0034] Furthermore, the specific steps of S304 include:

[0035] Based on the tracking trajectory obtained by the optical flow tracking algorithm, the erroneously labeled positive sample data is identified and corrected as negative samples by a negative constraint and added to the negative sample set. The random forest classifier and the nearest neighbor classifier are then trained with penalties. The overlap of the classification windows containing potential foreground targets in the random forest classifier and the nearest neighbor classifier with the foreground target tracking window of the previous frame is calculated. The window with the largest overlap is marked as a positive sample and added to the positive sample set. The random forest classifier and the nearest neighbor classifier are then trained with positive samples.

[0036] Furthermore, the specific steps of S4 include:

[0037] S401. Project the 3D point cloud map with semantic labels onto a 2D map, and perform cost dilation around static and dynamic obstacles to obtain a 2D cost map.

[0038] S402. The extended Kalman filter algorithm is used to estimate the robot's motion state based on the robot's motion information, including the robot's global pose, linear velocity and angular velocity.

[0039] S403. Using the A* algorithm with heuristic functions, the global path from the robot's current pose to the target is calculated based on the robot's motion state, a two-dimensional cost map, and a three-dimensional point cloud map with semantic labels.

[0040] S404. When the environment changes significantly or the target shifts, the D*Lite algorithm is used to incrementally update the global path based on the robot's motion state, the two-dimensional cost map, the three-dimensional point cloud map with semantic labels, and the global path. Significant changes in the environment are determined by cost mutation, node consistency, and global path feasibility. Target shift is determined by target pose shift and the rate of change of the heuristic function under the new and old targets.

[0041] S405. Using the dynamic window method, based on the global path, two-dimensional cost map and robot motion state, a coarse local path is obtained through local sampling and multi-objective optimization in the velocity space.

[0042] S406. Obtain the local path by smoothing the coarse local path trajectory using cubic B-splines.

[0043] Furthermore, in S403, the heuristic function is defined as:

[0044]

[0045] In the formula, For Euclidean distance For the distance from the starting point to the current node obtained based on the two-dimensional cost map The actual cost of movement, The semantic risk cost defined for the obtained semantic information. The cost of environmental adaptability is determined by the quality of sensor perception. , and These are the weighting coefficients.

[0046] Furthermore, in S405, the dynamic window method uses the following multi-objective weighted evaluation function to quantitatively evaluate each predicted trajectory and select the optimal speed:

[0047]

[0048] In the formula, The target orientation term represents the azimuth deviation between the predicted trajectory endpoint and the target point, and is calculated by the cosine of the angle between the line connecting the predicted trajectory endpoint and the target point and the robot's current orientation. The safety term represents the safety margin between the predicted trajectory and obstacles, and is calculated by the minimum distance from all points on the predicted trajectory to the nearest obstacle. For speed smoothing; , , These are weighting coefficients and need to be adjusted adaptively according to the task scenario.

[0049] This invention also provides a robot path planning system based on optical vision, comprising:

[0050] The sensing unit is used to collect environmental images of the robot's surroundings and robot motion information.

[0051] The processing unit is used to run algorithms to achieve 3D reconstruction, semantic recognition, and path planning;

[0052] The control unit is used to generate and execute robot control commands, drive the robot chassis motors, and communicate with peripheral devices.

[0053] The beneficial effects achieved by this invention are as follows:

[0054] This invention effectively balances cost control and performance improvement through an active optical vision perception architecture and multi-algorithm collaborative design. Employing active infrared structured light projection combined with visible light image fusion avoids the high cost of traditional LiDAR and overcomes the core deficiency of passive vision solutions being sensitive to ambient light. By using random fingerprint encoding, mutation extraction, and two-bit cross-correlation decoding techniques, the efficiency and accuracy of 3D environment reconstruction are significantly improved. Simultaneously, efficient dynamic tracking, cascaded classification, and semantic recognition algorithms can quickly distinguish traversable areas from various obstacles, ensuring the accuracy of environmental perception. Furthermore, the combination of global path planning and local real-time obstacle avoidance enhances the reliability and timeliness of path planning. The overall system possesses strong environmental adaptability and can be widely adapted to application needs in various fields such as warehousing and logistics, intelligent inspection, and autonomous driving, improving the robustness, real-time performance, and practical value of robot autonomous navigation. Attached Figure Description

[0055] Figure 1 This is a flowchart of the method provided by the present invention;

[0056] Figure 2 This is a schematic diagram of laser speckle convolution in an embodiment of the present invention;

[0057] Figure 3 This is a schematic diagram of a dense three-dimensional point cloud map in an embodiment of the present invention;

[0058] Figure 4 This is a flowchart of the algorithm for generating a semantically labeled 3D point cloud map based on a dense 3D point cloud map and a color RGB visible light environment image in an embodiment of the present invention.

[0059] Figure 5This is a flowchart of the multimodal Gaussian mixture background modeling algorithm in an embodiment of the present invention;

[0060] Figure 6 This is a flowchart of the algorithm for semantic recognition of foreground targets using a cascaded classifier in an embodiment of the present invention;

[0061] Figure 7 This is a flowchart of the algorithm for integrating the tracker result window and the classifier result window in an embodiment of the present invention.

[0062] Figure 8 This is a flowchart of the PN learning algorithm in an embodiment of the present invention;

[0063] Figure 9 This is an algorithm flowchart for estimating the robot's motion state and performing global and local path planning based on a 3D point cloud map with semantic labels and robot motion information in an embodiment of the present invention.

[0064] Figure 10 This is a flowchart of the global path planning algorithm using the A* algorithm in an embodiment of the present invention;

[0065] Figure 11 This is a flowchart of the algorithm for local path planning using the dynamic window method in this embodiment of the invention;

[0066] Figure 12 This is a system unit diagram provided by the present invention. Detailed Implementation

[0067] To make the core objectives, technical details, and application advantages of this technical solution embodiment clearer, the specific technical logic of this technical solution embodiment will be explained in detail and comprehensively below with the accompanying schematic diagrams. The embodiments presented here are only some typical embodiments of this technical solution, not all embodiments. It should be noted that this technical solution is not limited to the implementation forms detailed herein, and can also be implemented through other reasonable technical paths without departing from its core technical principles. Those skilled in the art can make adaptive adjustments based on the core connotation of this technical solution, therefore this technical solution is not bound by the specific embodiments disclosed below. Secondly, the "one embodiment" or "preferred embodiment" mentioned herein refers to the key technical features, implementation structures, or functional characteristics included in at least one practical application scenario of this technical solution; the phrase "in a feasible implementation" appearing in different paragraphs of this description does not all refer to the same embodiment, nor is it a specific embodiment that is independent of or exclusive to other embodiments.

[0068] Please refer to Figure 1 This specification provides an embodiment of a robot path planning method based on optical vision, the method comprising:

[0069] S1. Acquire environmental images of the robot's environment and robot motion information. The environmental images include deformable infrared structured light images modulated by environmental objects and color RGB visible light environmental images.

[0070] S2. Based on environmental images, a dense 3D point cloud map is obtained through contrast enhancement, signal extraction, decoding, and interpolation optimization.

[0071] S3. Based on dense 3D point cloud maps and color RGB visible light environment images, a 3D point cloud map with semantic labels is generated through background removal, image tracking, semantic recognition, and clustering.

[0072] S4. Based on the 3D point cloud map with semantic labels and robot motion information, estimate the robot motion state, perform global and local path planning, and obtain the local path after trajectory smoothing. The robot motion state includes the robot's global pose, linear velocity and angular velocity.

[0073] S5. Combine the robot's motion state to convert the local path into the robot's actual actions.

[0074] Mobile robots are widely used in warehousing and logistics, intelligent inspection, and other fields, with autonomous navigation being their core capability. Traditional path planning relies on LiDAR, which, while highly accurate, is expensive and cannot acquire surface texture and color information. Passive vision-based solutions are low-cost and information-rich, but are sensitive to lighting conditions, with performance plummeting under complex lighting. 3D reconstruction struggles to balance real-time performance, robustness, and accuracy, while existing structured light solutions suffer from poor real-time performance, susceptibility to interference, and uneven point cloud distribution. Furthermore, existing path planning algorithms struggle to balance global optimization with local obstacle avoidance and fail to fully integrate environmental semantics and 3D features. Therefore, the field urgently needs a low-cost, highly robust, and real-time optical vision path planning method that can overcome these shortcomings.

[0075] As described in steps S1-S5 above, in this embodiment, specific structured light is first projected onto the robot's environment, and environmental images are acquired. The environmental images include deformable infrared structured light images modulated by environmental objects and color RGB visible light environmental images, while robot motion information is also acquired. Based on the acquired environmental images, a 3D environment model is completed to obtain a dense 3D point cloud map. Then, combining this point cloud map with the color RGB visible light environmental images for image tracking, semantic recognition, and clustering, a 3D point cloud map with semantic labels is obtained. Subsequently, global path planning is performed by combining environmental semantic information and robot motion information, and local path planning is performed during the robot's movement. Finally, the planning results are converted into the robot's actual actions. This invention effectively balances cost and navigation performance, solving problems such as high cost, sensitivity to illumination, and insufficient real-time performance and robustness of traditional solutions, significantly improving the reliability, real-time performance, and environmental adaptability of robot autonomous navigation.

[0076] The following description Figure 1 The execution method of each step is shown.

[0077] Regarding S1, in a preferred embodiment, the step of acquiring environmental images of the robot's environment and robot motion information includes:

[0078] S101. A single-point infrared laser is used as the projection light source. The single-point infrared laser is scattered into multiple random scattering points by an optical diffuser. The scattering points are then optically convolved by a grating to form a random fingerprint-coded structured light, which is then projected onto the robot's environment.

[0079] In one embodiment of this specification, a single-point infrared laser with a wavelength of 850nm and a power of 40mW is used as the projection light source. Please refer to [reference needed]. Figure 2 When random fingerprint-coded structured light is projected onto the robot's environment, the average radius of the speckle changes with the propagation distance of the light path. Therefore, the speckle pattern at any point in space is unique, which means that random fingerprint encoding is performed on each coordinate point in the spatial coordinate system, forming a kind of "volume encoding" in the projection field. That is, the random fingerprint-coded structured light is modulated at different depths on the surface of the object in the measured environment.

[0080] S102. Use an infrared CCD sensor and a visible light CCD sensor to acquire deformed infrared structured light images and color RGB visible light environmental images modulated by environmental objects.

[0081] In one embodiment of this specification, an infrared CCD sensor is used to acquire deformed infrared structured light images modulated by environmental objects, while a visible light CCD sensor simultaneously acquires color RGB visible light environmental images at a frame rate of 30 frames per second. This effectively solves the shortcomings of traditional structured light encoding, which is fixed and susceptible to interference, providing high-quality, highly stable raw data for subsequent 3D reconstruction.

[0082] S103. Obtain robot motion information using a wheeled odometer and an inertial measurement unit, the robot motion information including robot linear velocity, angular velocity, linear acceleration, angular acceleration, etc.

[0083] In one embodiment of this specification, the rotation of the robot's wheels is obtained using a wheel odometer at a frequency of 100Hz and converted into its linear velocity and angular velocity. The linear acceleration and angular acceleration of the robot are obtained using an inertial measurement unit, which constitutes the robot's motion information.

[0084] Regarding S2, in a preferred embodiment, the step of obtaining a dense 3D point cloud map based on an environmental image through contrast enhancement, signal extraction, decoding, and interpolation optimization includes:

[0085] S201. Perform contrast enhancement processing on the modulated deformable infrared structured light image of the environmental object to obtain an enhanced two-dimensional infrared image matrix.

[0086] In one embodiment of this specification, an adaptive histogram equalization method with limited contrast is used to enhance the contrast of a deformed infrared structured light image of an environment object modulated by an infrared CCD sensor, resulting in an enhanced two-dimensional infrared image matrix. This method can improve the pattern contrast of objects in dark and bright areas through local block operations, while avoiding noise spikes, thereby highlighting speckle light spot features.

[0087] S202. The Laplace-Gauss operator is used to perform convolution calculation on the enhanced two-dimensional infrared image matrix, extract the coordinates of abrupt change points and mark them to obtain the initial point cloud.

[0088] In one embodiment of this specification, since the infrared filter coating on the surface of the infrared CCD sensor cannot completely shield visible light signals, some visible light ambient signals are superimposed with the infrared signals in the output. Therefore, the Laplace-Gauss operator is used to extract the point cloud signal. The Laplace operator is a second-order difference operator in Euclidean space, defined as the divergence of the gradient of a two-dimensional image. The grayscale of speckle points in the enhanced two-dimensional infrared image matrix will change, so the brightness can be enhanced by using the Laplace operator for convolution calculation, thereby extracting the speckle points. Since the Laplace operator is a second-order difference operator, it is sensitive to small noise. Therefore, the Gauss operator needs to be introduced to smooth the original image. For the characteristics of the encoded point cloud, the following 5th-order Laplace-Gauss operator model is used to convolve the enhanced two-dimensional infrared image matrix to obtain the encoded point cloud:

[0089]

[0090] After the acquired convolutional image is normalized by interval, the coordinates of the mutation points are extracted and marked using a preset fixed threshold. The current light spot matrix information is obtained through the coordinates of the mutation points as the initial point cloud.

[0091] S203. The initial point cloud is decoded using a two-dimensional cross-correlation algorithm. The initial point cloud matrix is ​​matched with a preset matching template of different depths to obtain the depth information of each point and obtain a point cloud containing depth information.

[0092] Specifically, matching templates of different depths are created, and the initial point cloud and matching templates of different depths are cropped and allocated into multiple small blocks. For each small block, the cross-correlation coefficient between the two is calculated. When the coefficient approaches 1, the matching template is considered to match the measured surface, thus creating a point cloud containing depth information.

[0093] S204. A bicubic interpolation algorithm is used to optimize the interpolation of point clouds containing depth information to obtain a dense three-dimensional point cloud map.

[0094] In one embodiment of this specification, considering the non-uniformity of speckle images, which leads to non-uniformity of the point cloud, a bicubic interpolation method is used to optimize the interpolation of the point cloud containing depth information, resulting in a dense 3D point cloud map. Please refer to [reference needed]. Figure 3 This provides environmental data support for subsequent semantic recognition, obstacle differentiation, and path planning.

[0095] Please refer to Figure 4 Regarding S3, in a preferred embodiment, the step of generating a 3D point cloud map with semantic labels based on a dense 3D point cloud map and a color RGB visible light environment image includes:

[0096] S301. Based on a dense 3D point cloud map and a color RGB visible light environment image, a multimodal mixture Gaussian background modeling algorithm is used to remove the background and obtain the foreground image, including foreground depth point cloud data and foreground color RGB visible light image.

[0097] In one embodiment of this specification, the external scene information is constantly moving during the robot's movement; therefore, a multimodal Gaussian mixture background modeling algorithm is used to remove the background. The algorithm flow is as follows: Figure 5 As shown. This algorithm is based on a large amount of pixel sample information and combines statistical data to perform background modeling. In this embodiment, for each pixel... It contains Three-color visible light channels and Depth channel, in time, The multimodal mixture Gaussian distribution model for this pixel is as follows:

[0098]

[0099]

[0100]

[0101] in, The probability of the current pixel value. They are respectively Color visible light channel and Gaussian weights for depth channels for Gaussian distribution formula at time ， for Mean of Gaussian distribution at time ， for Time-varying covariance matrix for Time variance It is a multi-order identity matrix. During the background model extraction process, the new input pixel values... The algorithm matches the optimal probability distribution among the existing Gaussian distributions in each channel and verifies whether it satisfies the Laida criterion. If it does, the pixel is identified as background, and its current weight, mean, and standard deviation are updated; otherwise, it is identified as foreground, and its weight is decayed. After each frame update, the background model is updated by sorting the weights and standard deviations, and the process proceeds to the next frame. This process is repeated until the background model is obtained. It should be noted that this algorithm first needs to train the background signal to obtain the distribution model, mean, and variance of each pixel in the current background. During the modeling process, the background is modeled through continuous training to remove the background and obtain the foreground image, including foreground depth point cloud data and foreground color RGB visible light image.

[0102] S302. Based on the foreground color RGB visible light image, an optical flow tracing algorithm is used to track the image and obtain the tracker result window and its confidence level.

[0103] In one embodiment of this specification, foreground segmentation may suffer from inter-frame breaks and target loss due to occlusion. The tracking algorithm, through inter-frame correlation and multi-feature fusion algorithms, can achieve real-time continuous localization of dynamic targets. An optical flow tracking algorithm is used to track the foreground color RGB visible light image, and the effectiveness is judged by combining median optical flow tracking and tracking error detection algorithms. Foreground image point A at time T is tracked via optical flow to obtain point B at time T+1, and traces back to point C at time T to form a trajectory. The distance between A and C, i.e., the backtracking error, is calculated; if it is less than a preset threshold, the tracking is effective. After determining the feature tracking points, a fixed-size matrix is ​​extracted for each point with sub-pixel precision, and matched with the corresponding matrix of the previous frame to generate a mapping image. The matching similarity is calculated. For the feature tracking points of the current frame, the backtracking error and matching error are calculated, invalid points where both are greater than the corresponding median value are removed, 50% of the feature points are retained, and gaps are filled. This algorithm estimates the current position of the target, outputs candidate bounding boxes (i.e., the tracker result window), and outputs the normalized value of the backtracking error as the confidence level.

[0104] S303. Based on the foreground color RGB visible light image, a cascaded classifier is used to perform semantic recognition on the foreground target, and the classifier result window and its confidence level are obtained.

[0105] In one embodiment of this specification, a variance classifier, a random forest classifier, and a nearest neighbor classifier are cascaded sequentially to form a cascaded classifier. This cascaded classifier is then used to perform semantic recognition on the foreground target. Figure 6 As shown. In the preprocessing, the foreground color RGB visible light image of the current frame is divided into multiple scanning windows at different scales for feature value lookup. A sliding window mechanism is used, scanning the entire image independently at each scale. The initial scale change is set to 20, increasing in increments of 1.2. Each scanning window is identified as having a feature target after passing through three classifiers.

[0106] In variance classifiers, calculating the image variance requires summing the gray values ​​of a large number of rectangular regions in an image, resulting in a huge computational load and reducing the algorithm's real-time performance. Therefore, this embodiment uses an integral image method to calculate the sum of gray values ​​of the image window, thereby obtaining the gray-level variance of the image window. When the variance is greater than 50% of the target patch variance, it is considered to contain a first potential foreground target, and the resulting classification window containing the first potential foreground target is input into the next module. In the random forest classifier, a total of 10 decision trees are used. Each decision tree uses a set of 16 random pixel comparisons as its splitting attribute to generate a 16-bit binary code. Each code corresponds to a posterior probability value. The posterior probabilities obtained from all decision trees are averaged to obtain the overall average value. When this average value is greater than a threshold, it is considered to contain a second potential foreground target, and the resulting classification window containing the second potential foreground target is input into the next module. In the nearest neighbor classifier, the classification window containing the second potential foreground target is compared with the positive sample set to calculate the correlation similarity and conservative similarity. If both similarities are greater than their respective preset thresholds, it is considered to contain the third potential foreground target. Based on the classification window containing the third potential foreground target obtained by the classifier, the candidate target bounding box is output, i.e., the classifier result window, and its corresponding correlation similarity is output as the confidence score.

[0107] S304. Merge the tracker result window and the classifier result window to output the foreground target tracking window.

[0108] In one embodiment of this specification, for the current frame, the tracker result window is a single result window, denoted as T, and the classifier result window is multiple result windows, denoted as... To integrate the two, such as Figure 7 As shown, it includes the following steps:

[0109] (i) Check whether the tracker and classifier output valid result windows, which can be divided into the following four cases:

[0110] Case 1: If neither outputs a valid result window, the marked target will be invisible;

[0111] Case 2: If only the tracker outputs a valid result window, then the tracker result window T is directly output as the foreground target tracking window;

[0112] Case 3: If only the classifier outputs a valid result window, proceed to the next step;

[0113] Case 4: If both output valid result windows, proceed to the next step;

[0114] (ii) Cluster all classifier result windows D using overlap detection, setting the overlap threshold to 0.5 so that the intra-class overlap of each class is greater than the overlap threshold, thus obtaining K classifier window classes. Next, we will handle cases 3 and 4 separately:

[0115] (iii) Case 3: For all classifier window classes Calculate the similarity for each class and select the class with the highest similarity. ,Pick The classifier result window with the highest confidence level As the output of the foreground target tracking window, and used Reinitialize the optical flow tracing algorithm;

[0116] (iv) Case 4: For each classifier window class Perform a search and count the number N of categories that simultaneously meet the following two conditions: Overlap with tracker result window T < 0.5, class The average confidence level is higher than the confidence level of the tracker result window T;

[0117] (v) Based on the statistical results, the following two situations apply:

[0118] Case 4.1: N=1, meaning there is only one classifier window. If both of the above conditions are met, then The classifier result window with the highest confidence level As the output of the foreground target tracking window, and used Reinitialize the optical flow tracing algorithm;

[0119] Case 4.2: If N=0 or N≥2, then select classifier result windows from all classifier result windows D that have an overlap of >0.7 with the tracker result window T. ,right The weighted average of T is accumulated and then used as the foreground target tracking window.

[0120] Specifically, re-initializing the optical flow tracking algorithm using the foreground target tracking window means clearing the original feature tracking points in the optical flow tracking algorithm, re-extracting feature tracking points in the selected foreground target tracking window, and then constructing a fixed-size matrix for each point with sub-pixel precision for matching with the corresponding matrix in the next frame.

[0121] S305. The PN learning algorithm is used to perform semi-supervised learning on the random forest classifier and the nearest neighbor classifier.

[0122] In one embodiment of this specification, the existing PN learning algorithm (Kalal Z, Matas J, Mikolajczyk K, et al. PN Learning: Bootstrapping Binary Classifiers by Structural Constraints; proceedings of the 23rd IEEE Conference on ComputerVision and Pattern Recognition (CVPR), San Francisco, USA, June 13-18, 2010[C]. IEEE: New York, USA.) is used to implement semi-supervised learning of random forest classifiers and nearest neighbor classifiers. This overcomes the limitation of traditional semi-supervised algorithms ignoring negative samples and combines optical flow tracing results for supervision and constraints, such as... Figure 8 As shown, the PN learning algorithm includes a classifier, a sample set, supervised learning, and positive and negative constraints. It iteratively utilizes the structural characteristics of the target to correct erroneous samples. In this embodiment, the classifiers are the aforementioned random forest classifier and nearest neighbor classifier. The sample set is initially preset and subsequently labeled by the tracker. Based on the tracking trajectory obtained by the optical flow tracking algorithm, data that is far from the trajectory and marked as positive samples are considered to be incorrectly labeled. These are corrected as negative samples by the negative constraint and added to the negative sample set. The random forest classifier and nearest neighbor classifier are then penalized and trained to improve their recognition accuracy. Simultaneously, the overlap calculation is performed between the classification windows containing potential foreground targets corresponding to the random forest classifier and nearest neighbor classifier and the foreground target tracking window of the previous frame. The window with the largest overlap is marked as a positive sample and added to the positive sample set. The random forest classifier and nearest neighbor classifier are then trained with positive samples to enhance their robustness.

[0123] S306. Based on the foreground image and the foreground target tracking window, calculate its geometric features and color texture information, distinguish between passable areas, static obstacles and dynamic obstacles, and assign corresponding semantic labels to different attribute areas to obtain a 3D point cloud map with semantic labels.

[0124] In one embodiment of this specification, 3D point cloud data with different foreground object segmentation masks can be obtained based on the foreground image and the foreground object tracking window. The RANSAC algorithm is used to perform planar segmentation on the downsampled point cloud, and the segmented inliers are marked as ground. Each foreground object is clustered, and geometric and color texture features are extracted. Geometric features include height, area, size, sphericity, verticality, and flatness; color texture features include saturation mean, saturation standard deviation, hue standard deviation, hue entropy, color variance, and simple texture score. Based on the obtained geometric and color texture features, an unsupervised K-means clustering algorithm is used in the feature space to assign labels, distinguishing traversable areas, static obstacles, and dynamic obstacles, resulting in a 3D point cloud map with semantic labels. This addresses the problems of insufficient robustness in dynamic object tracking and low semantic recognition accuracy, providing accurate and comprehensive environmental semantic information for path planning and ensuring the rationality of subsequent navigation decisions.

[0125] Please refer to Figure 9 Regarding S4, in a preferred embodiment, the steps of performing global and local path planning based on a semantically labeled 3D point cloud map and robot motion information include:

[0126] S401. Project the 3D point cloud map with semantic labels onto a 2D map, and perform cost dilation around static and dynamic obstacles to obtain a 2D cost map.

[0127] In one embodiment of this specification, to facilitate subsequent path planning, a three-dimensional point cloud map with semantic labels is projected into a two-dimensional grid centered on the robot, with a resolution of 0.05m and a range of 20m×20m. For each grid cell, the points within its boundary are counted, and a safety buffer is generated based on density and semantics to obtain a two-dimensional cost map.

[0128] S402. The extended Kalman filter algorithm is used to estimate the robot's motion state based on the robot's motion information, including the robot's global pose, linear velocity, and angular velocity.

[0129] In one embodiment of this specification, in step S103, robot motion information is acquired using a wheeled odometer and an inertial measurement unit (IMU). However, since the wheeled odometer is prone to drift due to wheel slippage, and the IMU is prone to errors due to bias and noise accumulation, an extended Kalman filter (EPF) algorithm is used to fuse the motion information provided by both to estimate the robot's global pose, linear velocity, and angular velocity. The EPF algorithm mainly consists of an update step and a prediction step, which are executed cyclically to correct the data. In this embodiment, the update step uses the velocity and angular velocity provided by the wheeled odometer as input to predict the robot's motion state and covariance at the next moment, i.e., global pose, linear velocity, and angular velocity. The prediction step uses the directly observed linear acceleration and angular acceleration provided by the IMU to correct the motion state and update the covariance, preparing for the next round of prediction. During robot operation, data from the wheeled odometer and the IMU are continuously received, and prediction and update steps are continuously performed to acquire the robot's real-time global pose, linear velocity, and angular velocity.

[0130] S403. Using the A* algorithm with heuristic functions, the global path from the robot's current pose to the target is calculated based on the robot's motion state, a two-dimensional cost map, and a three-dimensional point cloud map with semantic labels.

[0131] In one embodiment of this specification, based on the robot's motion state and a two-dimensional cost map, the A* algorithm is used to calculate the global path from the robot's current pose to the target, such as... Figure 10 As shown. The core value of the A* algorithm lies in achieving a balance between completeness and optimality in state-space search by integrating a two-dimensional evaluation mechanism that combines "costs already incurred" and "future cost estimates." Its principle is based on the following evaluation function... For each state Quantify the "search value":

[0132]

[0133] in Let be the cost function, representing the time from the initial state to the current state. The actual cost, i.e., the actual path length. For heuristic functions, it represents the function that starts from the current state. In this embodiment, the heuristic function for estimating the cost to the target state is defined as:

[0134]

[0135] in, For Euclidean distance For the distance from the starting point to the current node obtained based on the two-dimensional cost map The actual cost of movement, The semantic risk cost defined for the obtained semantic information. The cost of environmental adaptability is determined by the quality of sensor perception. , and These are the weighting coefficients. In each iteration, the evaluation function value is selected first. The minimum state is expanded until the target state is found. After obtaining the global coarse path, waypoints are interpolated to ensure kinematic feasibility. Based on a semantically labeled 3D point cloud map, for each waypoint of the interpolated global coarse path, a Kd-tree radius search is used to query nearby point clouds for 3D collision detection, rejecting paths with a collision probability higher than a preset threshold. Based on this, the path planning accuracy and computation time can be balanced to obtain the robot's global path from the current pose to the target.

[0136] S404. When the environment changes significantly or the target shifts, the D*Lite algorithm is used to incrementally update the global path based on the robot's motion state, the two-dimensional cost map, the three-dimensional point cloud map with semantic labels, and the global path.

[0137] In one embodiment of this specification, when the environment changes significantly or the target shifts, the global path is updated. This requires fully utilizing historical planning information and avoiding the computational overhead of frequent replanning. Therefore, this invention adopts the existing D*Lite algorithm (Koenig S, Likhachev M. D*Lite; proceedings of the 18th AAAI Conference on Artificial Intelligence, Edmonton, Canada, July 28 - August 1, 2002 [C]. AAAI: Washington DC, USA.), employing a heuristic function design consistent with the A* algorithm to incrementally update the global path. Significant environmental changes are determined by cost mutation, node consistency, and global path feasibility; target shifts are determined by target pose shift and the rate of change of the heuristic function under the old and new targets. The D*Lite algorithm calculates cost mutations by the change in movement cost between adjacent node pairs, node consistency by the proportion of inconsistent nodes, and global path feasibility by the proportion of impassable nodes on the current global path. Any one of these factors exceeding a preset threshold indicates a significant environmental change. Target pose offset is calculated using the Euclidean distance between the old and new targets, and the heuristic function change rate is calculated using the change rate of the heuristic function value under the old and new targets in the D*Lite algorithm. Any one of these factors exceeding a preset threshold indicates a target offset. When significant environmental changes or target offsets occur, the D*Lite algorithm maintains consistency relationships between nodes, efficiently handling dynamic environmental changes and target point shifts, significantly reducing time costs compared to complete replanning. Subsequently, based on a semantically labeled 3D point cloud map, for each waypoint on the updated global path, a Kd-tree radius search is used to query nearby point clouds for 3D collision detection, rejecting paths with collision probabilities higher than a preset threshold. This enables local updates of nodes and their neighborhoods affected by environmental changes in the global path, saving planning time.

[0138] S405. Using the dynamic window method, based on the global path, two-dimensional cost map and robot motion state, a coarse local path is obtained through local sampling and multi-objective optimization in the velocity space.

[0139] In one embodiment of this specification, during robot movement, the robot focuses on the next 1-3 waypoints, sets the intersection of the global path and a 5m range as the local target, and uses a dynamic window method for local path planning, such as... Figure 11 As shown. In the robot's velocity space In this context, a dynamic window is defined as the intersection of multiple constraints:

[0140]

[0141] in, As a dynamic constraint window, considering the robot's physical limits, the achievable speed range in a short time is derived from the current speed and time step. To establish an environmental safety window, speed combinations that could lead to collisions between the robot and obstacles are excluded by predicting the minimum distance between the candidate speed's trajectory and the obstacle. This is the maximum velocity constraint window, representing the upper limit of velocity determined by the robot's hardware performance. For each candidate velocity sampled within the dynamic window... Predicting future motion trajectories in the short time domain based on robot kinematics models In this embodiment, the optimal speed is selected by quantitatively evaluating each predicted trajectory using the following multi-objective weighted evaluation function:

[0142]

[0143] in, The target orientation term represents the azimuth deviation between the predicted trajectory endpoint and the target point, and is calculated by the cosine of the angle between the line connecting the predicted trajectory endpoint and the target point and the robot's current orientation. The safety term represents the safety margin between the predicted trajectory and obstacles, and is calculated by the minimum distance from all points on the predicted trajectory to the nearest obstacle. For the velocity smoothing term, to encourage the robot to maintain high motion efficiency and avoid frequent starts and stops, it is calculated using normalized values ​​of linear velocity and angular velocity; , , These are weighting coefficients and need to be adaptively adjusted according to the task scenario. By iterating through all candidate velocities within the dynamic window, the corresponding evaluation function value is calculated, and the one that best suits the task is selected. The maximized speed is taken as the optimal speed at the current time step, and the corresponding predicted trajectory is the planned coarse local path.

[0144] S406. Obtain the local path by smoothing the coarse local path trajectory using cubic B-splines.

[0145] In one embodiment of this specification, the obtained coarse local path is smoothed by cubic B-spline to obtain the local path, thereby realizing the robot's autonomous real-time path planning and ensuring its flexibility, safety and efficiency in navigation in complex dynamic environments.

[0146] Regarding S5, in a preferred embodiment, the step of converting the local path into the robot's actual action, in conjunction with the robot's motion state, includes:

[0147] First, the core steps include:

[0148] Based on the robot's motion model, the local path is converted into motion commands;

[0149] Motion commands are sent to the underlying motors to drive the robot actuators to track a smooth trajectory;

[0150] Based on the real-time acquired local path and robot motion state, the tracking deviation is calculated by comparing it with the planned path.

[0151] If the deviation exceeds the threshold, local replanning is triggered to correct the direction of travel.

[0152] In addition, during execution, key performance indicators are captured and logged, and alarms are issued for abnormal situations so that operators can intervene in a timely manner. To improve robustness, a watchdog timer is set up so that if any cycle exceeds a preset threshold, a safety shutdown is forcibly initiated. In terms of real-time priority design, local adjustments take precedence over global recalculation to minimize latency jitter and ensure that the robot has a rapid response capability in dynamic environments.

[0153] Please refer to Figure 12 The present invention also provides a robot path planning system based on optical vision, applied to the above-mentioned robot path planning method based on optical vision, comprising:

[0154] The sensing unit is used to collect environmental images and robot motion information of the robot's environment. It includes the active optical vision sensor, wheel odometry and inertial measurement unit. The active optical vision sensor integrates an infrared structured light projector, an infrared CCD sensor and a visible light CCD sensor. The structured light projector uses a single-point infrared laser light source and is equipped with an optical diffuser and a convolution grating. The infrared CCD sensor lens is coated with an infrared filter film.

[0155] The processing unit is used to run the algorithm, including core models such as 3D reconstruction, semantic recognition, and path planning;

[0156] The control unit is used to generate and execute robot control commands, drive the robot chassis motors, and communicate with peripheral devices.

[0157] It should be noted that, in the context of this article, the terms "comprising," "including," and any other variations thereof are intended to convey a non-exclusive inclusion relationship. This means that a process, apparatus, article, or method that comprises a series of elements may include, in addition to those explicitly covered, other elements not explicitly listed, or elements inherent to the process, apparatus, article, or method itself. Without additional restrictions, an element defined as "comprising one..." does not exclude the presence of other identical elements in the process, apparatus, article, or method that includes that element.

[0158] The above description is merely a preferred embodiment of the present invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the core principles of the present invention, and these improvements and modifications should also fall within the scope of protection of the present invention. Furthermore, structures, devices, and operating methods not specifically described or explained in this invention, unless otherwise specified or limited, shall be implemented using conventional methods in the art.

Claims

1. A robot path planning method based on optical vision, characterized in that, Includes the following steps: S1. Acquire environmental images and robot motion information of the robot's environment. The environmental images include deformable infrared structured light images modulated by environmental objects and color RGB visible light environmental images. The deformable infrared structured light images modulated by environmental objects are formed by projecting random fingerprint-coded structured light onto the environment and then modulating it by the environmental objects. The random fingerprint-coded structured light is generated by scattering a single-point infrared laser into multiple random scattering points through an optical diffuser and then performing optical convolution through a grating. S2. Based on environmental images, a dense 3D point cloud map is obtained through contrast enhancement, signal extraction, decoding, and interpolation optimization. S3. Based on dense 3D point cloud map and color RGB visible light environment image, background is removed by multimodal mixture Gaussian background modeling algorithm to obtain foreground image, including foreground depth point cloud data and foreground color RGB visible light image; An optical flow tracing algorithm is used to estimate the current position of the target and track the foreground target. Semantic recognition is achieved by combining a cascaded classifier; a foreground target tracking window is output through window fusion; the cascaded classifier is trained using the PN learning algorithm; geometric and color texture features of the foreground target are extracted, and passable areas, static and dynamic obstacles are distinguished by clustering and assigned semantic labels to obtain a 3D point cloud map with semantic labels. S4. Based on a semantically labeled 3D point cloud map and robot motion information, estimate the robot's motion state using a heuristic function that integrates actual movement costs, semantic risks, and environmental adaptability factors. The algorithm generates a global path; when the environment changes or the target shifts, it uses... The algorithm incrementally updates the global path; combining the two-dimensional cost map and the robot's motion state, it uses a dynamic window method for local path planning, and obtains the local path after trajectory smoothing. S5. Combine the robot's motion state to convert the local path into the robot's actual actions.

2. The robot path planning method based on optical vision according to claim 1, characterized in that, The specific steps of S2 include: S201. Perform contrast enhancement processing on the modulated deformable infrared structured light image of the environmental object to obtain an enhanced two-dimensional infrared image matrix. S202. The Laplace-Gauss operator is used to perform convolution calculation on the enhanced two-dimensional infrared image matrix, extract the coordinates of abrupt change points and mark them to obtain the initial point cloud; S203. The initial point cloud is decoded using a two-dimensional cross-correlation algorithm. The initial point cloud matrix is ​​matched with a preset matching template of different depths to obtain the depth information of each point and obtain a point cloud containing depth information. S204. A bicubic interpolation algorithm is used to optimize the interpolation of point clouds containing depth information to obtain a dense three-dimensional point cloud map.

3. The robot path planning method based on optical vision according to claim 1, characterized in that, The specific steps of S3 include: S301. The background is modeled using a multimodal Gaussian mixture background modeling algorithm. Based on the multimodal Gaussian mixture distribution model, the multi-channel data of each pixel in the current frame is matched and verified with the established background model. The background is modeled through continuous training and learning. After removing the background, the foreground image is obtained, including the foreground depth point cloud data and the foreground color RGB visible light image. S302. The optical flow tracking algorithm is used to track the foreground color RGB visible light image. The median optical flow tracking and tracking error detection algorithms are combined to judge the effect, estimate the current position of the target, output the candidate bounding box, i.e. the tracker result window, and output the normalized value of the backtracking error as the confidence level. S303. The variance classifier, random forest classifier, and nearest neighbor classifier are cascaded in sequence to form a cascaded classifier. The cascaded classifier is used to perform semantic recognition of the foreground target in the foreground color RGB visible light image, and the candidate target bounding box is output, i.e. the classifier result window. The corresponding correlation similarity is output as the confidence score. S304. Combine the tracker result window and the classifier result window with their confidence scores to output the foreground target tracking window. S305. The PN learning algorithm is used to perform semi-supervised learning on the cascaded classifier; S306. Based on the foreground image and the foreground target tracking window, calculate its geometric features and color texture information, and use the unsupervised K-means clustering algorithm to assign labels in the feature space to distinguish passable areas, static obstacles and dynamic obstacles, and obtain a 3D point cloud map with semantic labels.

4. The robot path planning method based on optical vision according to claim 3, characterized in that, The multimodal mixture Gaussian distribution model described in S301 includes: For each pixel It contains Three-color visible light channels and Depth channel, in time, The multimodal mixture Gaussian distribution model of this pixel is in, The probability of the current pixel value. They are respectively Color visible light channel and Gaussian weights for depth channels for The Gaussian distribution formula at time intervals, for The mean of the Gaussian distribution at time t. They are respectively Color visible light channel and Depth Channel The mean of the Gaussian distribution at time t. for Time-varying covariance matrix They are respectively Color visible light channel and Depth Channel Time-varying covariance matrix , , and They are respectively Color visible light channel and Gaussian distribution of depth channel for Time variance It is a multi-order identity matrix.

5. The robot path planning method based on optical vision according to claim 3, characterized in that, The specific steps described in S303 include: S3031. Concatenate the variance classifier, random forest classifier, and nearest neighbor classifier in sequence to form a cascaded classifier and construct a positive sample set. S3032, In the variance classifier, the sum of gray values ​​of the image window is calculated using the integral image method. The variance classifier identifies the first potential foreground target and inputs the resulting classification window containing the first potential foreground target into the random forest classifier. S3033. Identify the second potential foreground target using a random forest classifier, and input the resulting classification window containing the second potential foreground target into the nearest neighbor classifier. S3034. In the nearest neighbor classifier, the classification window containing the second potential foreground target is compared with the positive sample set to calculate the correlation similarity and conservative similarity. If both similarities are greater than their respective preset thresholds, it is considered to contain the third potential foreground target, and the classification window containing the third potential foreground target is regarded as the classifier result window.

6. The method according to claim 3, characterized in that, The specific steps of step S305 include: the PN learning algorithm includes a sample set and a negative constraint. Based on the tracking trajectory obtained by tracking the image using the optical flow tracking algorithm, the incorrectly labeled positive sample data is determined and corrected to negative samples by the negative constraint and added to the negative sample set. The random forest classifier and the nearest neighbor classifier are then trained with penalties. The overlap of the classification windows containing potential foreground targets corresponding to the random forest classifier and the nearest neighbor classifier with the foreground target tracking window of the previous frame is calculated. The window with the largest overlap is marked as a positive sample and added to the positive sample set. The random forest classifier and the nearest neighbor classifier are then trained with positive samples.

7. The robot path planning method based on optical vision according to claim 1, characterized in that, The specific steps of S4 include: S401. Project the 3D point cloud map with semantic labels onto a 2D map, and perform cost dilation around static and dynamic obstacles to obtain a 2D cost map. S402. The extended Kalman filter algorithm is used to estimate the robot's motion state based on the robot's motion information, including the robot's global pose, linear velocity and angular velocity. S403, using heuristic functions The algorithm, based on the robot's motion state, a 2D cost map, and a 3D point cloud map with semantic labels, calculates the global path from the robot's current pose to the target. The heuristic function is defined as follows: In the formula, For Euclidean distance For the distance from the starting point to the current node obtained based on the two-dimensional cost map The actual cost of movement, The semantic risk cost defined for the obtained semantic information. The cost of environmental adaptability is determined by the quality of sensor perception. , and These are the weighting coefficients; S404. When the environment changes significantly or the target shifts, based on the robot's motion state, two-dimensional cost map, three-dimensional point cloud map with semantic labels, and global path, the following steps are taken: The algorithm incrementally updates the global path; when the environment changes significantly, it is determined by cost mutation, node consistency and global path feasibility; the target offset is determined by the target pose offset and the rate of change of the heuristic function under the new and old targets. S405. Using the dynamic window method, based on the global path, two-dimensional cost map and robot motion state, a coarse local path is obtained through local sampling and multi-objective optimization in the velocity space. S406. Obtain the local path by smoothing the coarse local path trajectory using cubic B-splines.

8. The robot path planning method based on optical vision according to claim 7, characterized in that, In S405, the dynamic window method uses the following multi-objective weighted evaluation function to quantitatively evaluate each predicted trajectory and select the optimal speed: In the formula, The target orientation term represents the azimuth deviation between the predicted trajectory endpoint and the target point, and is calculated by the cosine of the angle between the line connecting the predicted trajectory endpoint and the target point and the robot's current orientation. The safety term represents the safety margin between the predicted trajectory and obstacles, and is calculated by the minimum distance from all points on the predicted trajectory to the nearest obstacle. For speed smoothing; , , These are weighting coefficients and need to be adjusted adaptively according to the task scenario.

9. A robot path planning system based on optical vision, used to implement the method according to any one of claims 1-8, characterized in that, include: The sensing unit is used to collect environmental images of the robot's surroundings and robot motion information. The processing unit is used to run algorithms to achieve 3D reconstruction, semantic recognition, and path planning; The control unit is used to generate and execute robot control commands, drive the robot chassis motors, and communicate with peripheral devices.

Images