Spatial obstacle recognition method based on Gaussian mixture model and depth-color image

Abstract

The spatial obstacle identification method based on Gaussian mixture model and depth-color image disclosed by the present invention belongs to the field of aerospace technology. The implementation method of the present invention is as follows: based on the depth image and color image of the spatial environment, a three-dimensional point cloud model of the spatial environment is generated; the noise of the point cloud model is removed by statistical filtering; the three-dimensional coordinates of the point cloud of the spatial environment are clustered and analyzed using the Gaussian mixture model to obtain the Gaussian probability density distribution function of the spatial environment point cloud for determining whether it is a spatial obstacle; an iterative method is used to select and set the spatial obstacle identification threshold; the coordinates of the three-dimensional point in the path space to be determined during the movement of the robotic arm are input into the point cloud Gaussian probability density distribution function to obtain the corresponding Gaussian probability density value; according to the comparison between the Gaussian probability density value corresponding to the three-dimensional point in the path space to be determined and the spatial obstacle identification threshold, the path space point to be determined is identified, and the estimation and division of the spatial obstacle area and the manipulator's operable area are realized.

Description

Technical Field

[0001] The present invention relates to a spatial obstacle discrimination method based on a Gaussian mixture model and a depth-color image, and belongs to the technical field of aerospace. Background Art

[0002] With the rapid development of space technology, robotic arms will play a vital role in complex space missions such as space debris removal, on-orbit spacecraft maintenance, and on-orbit assembly. They can assist or replace astronauts in time-consuming and dangerous space operations, significantly reducing the cost and risk of on-orbit operations and improving the efficiency of space exploration and development.

[0003] Currently, the diversification and complexity of on-orbit servicing missions place higher technical demands on the operational capabilities of robotic arms. When performing high-precision on-orbit operations such as maintenance and assembly, robotic arms must locate, grasp, and store tools. This requires obtaining information about the distribution of obstacles in the space environment, avoiding collisions with obstacles, and finding a suitable operating area for the robotic arm. Currently, most on-orbit servicing robotic arms use traditional frame-based image cameras for measurement. This process requires processing a large amount of data, hindering the efficient and timely identification of spatial obstacles. Therefore, it is necessary to develop a fast and accurate spatial obstacle identification method to address this challenge. Summary of the Invention

[0004] The technical problem to be solved by the spatial obstacle discrimination method based on Gaussian mixture model and depth-color image disclosed in the present invention is as follows: based on the depth image and color image of the spatial environment measured by the camera, a three-dimensional point cloud model of the spatial environment is generated using the depth image and color image, the point cloud model is de-noised using statistical filtering, and the number of the denoised point cloud model is reduced using voxel grid filtering to improve the efficiency of subsequent construction of the point cloud Gaussian probability density distribution function; the three-dimensional coordinates of the point cloud of the spatial environment are clustered and analyzed using the Gaussian mixture model to obtain a spatial environment point cloud Gaussian probability density distribution function for determining whether it is a spatial obstacle; an iterative method is used to select and set a spatial obstacle discrimination threshold based on the spatial environment point cloud Gaussian probability density distribution function; the coordinates of the three-dimensional point in the path space to be determined during the movement of the robotic arm are input into the point cloud Gaussian probability density distribution function to obtain a corresponding Gaussian probability density value; the Gaussian probability density value corresponding to the three-dimensional point in the path space to be determined is compared with the spatial obstacle discrimination threshold, and after the path space point to be determined is determined, the spatial obstacle area and the robotic arm operable area are estimated and divided.

[0005] The purpose of the present invention is achieved through the following technical solutions.

[0006] The spatial obstacle discrimination method based on Gaussian mixture model and depth-color image disclosed in the present invention includes the following steps:

[0007] Step 1: Generate a three-dimensional point cloud model of the spatial environment using the depth image and color image of the spatial environment measured by the camera.

[0008] Step 2: Use statistical filtering to remove noise from the three-dimensional point cloud model of the spatial environment constructed in step 1, and use voxel grid filtering to reduce the number of point cloud models after noise removal, so as to improve the efficiency of the subsequent construction of the point cloud Gaussian probability density distribution function.

[0009] Statistical filtering: Let the nth data point in the three-dimensional point cloud model of the spatial environment constructed in step 1 be q n , n=1,2,3,...,S, let q n The distance to any point is d i , define q n The average distance D to all its k neighboring points avr-n It follows a Gaussian distribution with mean μ and standard deviation σ,

[0010]

[0011] Set the standard range S pa-n , used to determine whether the data points in the model are noise points,

[0012] S pa-n =μ±g·σ(g=0,1,2,...) (2)

[0013] According to the average distance D avr-n and standard range S pa-n Select the corresponding point. That is, when point q n The corresponding average distance D avr-n Greater than the standard range S pa-n , delete the point; when point q n The corresponding average distance D avr-n Less than or equal to the standard range S pa-n , then retain the point.

[0014] Voxel grid filtering: Get the minimum and maximum coordinate values ​​of the denoised 3D point cloud model in the x-axis, y-axis and z-axis directions, recorded as X min 、X max 、Y min 、Y max 、Z min 、Z max The lengths of the cuboid bounding boxes of the denoised 3D point cloud model are:

[0015]

[0016] Among them, L x 、L y 、L z is the side length of the cuboid bounding box of the denoised 3D point cloud model in the x-axis, y-axis, and z-axis directions.

[0017] The cuboid bounding box of the denoised 3D point cloud model is divided into several voxel grids with a side length of L. As a preferred method,

[0018] L = (2% to 10%) L min (4)

[0019] Among them, L min For L x 、L y 、L z Traverse the denoised 3D point cloud model, record the number of point cloud data in each voxel grid as m, and calculate the center of gravity of each voxel grid.

[0020]

[0021] The center of mass of the voxel grid (X c ,Y c ,Z c ) represents all data points in the voxel grid, thereby reducing the number of point clouds.

[0022] Step 3: Use the Gaussian mixture model to perform cluster analysis on the three-dimensional coordinates of the point cloud of the spatial environment after denoising and simplification in step 2 to obtain the Gaussian probability density distribution function of the spatial environment point cloud used to determine whether it is a spatial obstacle.

[0023] The Gaussian mixture model is used to perform cluster analysis on the 3D coordinates of the point cloud of the denoised and streamlined spatial environment. The Gaussian mixture model is expressed as:

[0024]

[0025] Where r is the three-dimensional coordinate of the point cloud of the spatial environment after denoising and simplification, K is the number of Gaussian distributions in the Gaussian mixture model, and π k is the weight of the kth Gaussian distribution, satisfying the following constraints

[0026]

[0027] N(r;μ k ,Σ k ) is the kth Gaussian distribution probability density function, μ k and Σ k Represent the mean vector and covariance matrix respectively. k ,μ k and Σ kThis is the parameter to be determined in the Gaussian mixture model.

[0028] As a preference, we first use the K-means algorithm to find the parameter π k ,μ k and Σ k After initialization, the expectation-maximization algorithm (EM) is used for iterative solution, which is divided into two steps: expectation step (E-setp) and maximization step (M-step). The iterative process is as follows:

[0029] E-step: Using the current Gaussian mixture model parameter π k ,μ k and Σ k Solve for the expected value of the log-likelihood.

[0030] Introducing hidden variables z ik , its value is

[0031]

[0032] Expand point cloud 3D coordinate data into complete data

[0033] (r i ,z i1 ,z i2 ,...,z iK ),i=1,2,...,N (9)

[0034] Where N is the number of point cloud three-dimensional coordinate data, and the log-likelihood function of the complete data is

[0035]

[0036] Compute the expectation of the log-likelihood function

[0037]

[0038] Note E(z ik )=γ ik , represents the three-dimensional coordinate r of the i-th point cloud under the current Gaussian mixture model parameters {π, μ, Σ} i The probability from the kth Gaussian distribution is calculated as

[0039]

[0040] M-step: Optimize the Gaussian mixture model parameters by maximizing the expected value of the log-likelihood function in E-step. Considering π k The constraint condition (7) must be satisfied, and the optimization objective function J with the Lagrangian operator λ is set as

[0041]

[0042] Find the objective function J for π respectively k ,μ k ,Σ k The partial derivative of and set it to 0 can get the new estimated value of the Gaussian mixture model parameters:

[0043]

[0044] Repeat E-step and M-step until the estimated values ​​of the Gaussian mixture model parameters obtained from two iterations are almost unchanged, then terminate the iteration and output the model parameter π k ,μ k and Σ k .

[0045] Based on the solved Gaussian mixture model with known parameters, the Gaussian probability density distribution function of the spatial environment point cloud is obtained as shown in formula (6) for determining whether it is a spatial obstacle.

[0046] Step 4: Based on the Gaussian probability density distribution function of the spatial environment point cloud obtained in step 3, an iterative method is used to select and set the spatial obstacle discrimination threshold.

[0047] The process of iteratively determining the spatial obstacle discrimination threshold ε is as follows:

[0048] Step 4.1: Select the median of the Gaussian probability density of the spatial environment point cloud as the initial spatial obstacle discrimination threshold ε0,

[0049] Step 4.2: Divide the spatial environment point cloud into two regions according to the threshold ε0. If the Gaussian probability density corresponding to the spatial environment point cloud is less than or equal to the discrimination threshold ε0, the spatial environment point cloud is divided into the non-obstruction region R1, otherwise it is divided into the obstacle region R2. Calculate the median M1 and M2 of the Gaussian probability density of the spatial environment point cloud in the non-obstruction region R1 and the obstacle region R2 respectively.

[0050] Step 4.3: Update the spatial obstacle discrimination threshold ε i+1 ,

[0051]

[0052] Step 4.4: Repeat steps 4.2 and 4.3 until the spatial obstacle discrimination threshold ε i+1 and ε i If the difference is less than the preset value, the iterative process is stopped and the final spatial obstacle discrimination threshold ε is obtained.

[0053] Step 5: Input the coordinates of the three-dimensional point in the path space to be determined during the movement of the robotic arm into the Gaussian probability density distribution function of the point cloud obtained in step 3 to obtain the corresponding Gaussian probability density value. According to the Gaussian probability density value corresponding to the three-dimensional point in the path space to be determined and the spatial obstacle discrimination threshold obtained in step 4, the spatial obstacle area and the manipulator's operable area are estimated and divided after the path space point to be determined is determined.

[0054] Input the three-dimensional point coordinate x of the path space to be determined during the movement of the robotic arm into the point cloud Gaussian probability density distribution function obtained in step 3 to obtain the corresponding Gaussian probability density value

[0055]

[0056] Compare the Gaussian probability density value p(x) corresponding to the three-dimensional point in the path space to be determined with the spatial obstacle discrimination threshold ε obtained in step 4. If p(x)>ε, the three-dimensional point in the path space to be determined is a spatial obstacle. If p(x)≤ε, the three-dimensional point in the path space to be determined does not belong to a spatial obstacle. After completing the judgment of whether the path space point to be determined is a spatial obstacle, the estimation and division of the spatial obstacle area and the manipulator's operable area are realized.

[0057] Step 6: Based on the three-dimensional coordinates of the point cloud belonging to the spatial obstacle area determined in step 5, the Gaussian clustering ellipsoid model is further used to envelop the point cloud belonging to the spatial obstacle area to obtain the corresponding Gaussian clustering ellipsoid outer envelope surface equation for obstacle avoidance path planning; based on the analytical expression of the spatial obstacle Gaussian clustering ellipsoid outer envelope surface equation, the normal vector of the extended tangent plane of the obstacle outer envelope is obtained, and the real-time obstacle avoidance path of the robotic arm is planned based on the normal vector, thereby improving the efficiency of obstacle avoidance path planning while ensuring the accuracy of path planning.

[0058] As a preferred method, step six is ​​implemented as follows:

[0059] The three-dimensional point cloud set belonging to the spatial obstacle area obtained in step 5 is recorded as B, and the Gaussian mixture model of the point cloud set B is established {π m ,μ m ,Σ m} m＝1:M , where M is the number of Gaussian distributions in the Gaussian mixture model. The point cloud set obeying the mth Gaussian distribution is regarded as an obstacle. The Gaussian clustering ellipsoid model is used to envelop the point cloud set obeying the mth Gaussian distribution. The outer envelope surface equation of the Gaussian clustering ellipsoid of the mth obstacle is obtained as follows:

[0060]

[0061] Where ξ is the coordinate of the three-dimensional point in the spatial environment, R is the minimum radius that makes all point clouds that obey the mth Gaussian distribution included in the same ellipsoid; μ m is the center point of the mth Gaussian cluster ellipsoid; Σ m Determines the size and direction of the ellipsoid, is a 3×3 dimensional positive definite matrix, and its eigenvalue matrix Λ and eigenvector matrix V are shown in formula (18):

[0062]

[0063] Where r1, r2, r3 represent the lengths of the semi-axis of the ellipsoid, and α1, α2, α3 are the orientations of the semi-axis of the ellipsoid corresponding to r1, r2, r3.

[0064] Γ m (ξ;μ m ,Σ m ) is abbreviated as Γ m (ξ). For the points on the outer envelope of the Gaussian clustering ellipsoid of the mth obstacle Use the normal vector n(ξ b ) is defined at point ξ b The tangent plane at:

[0065]

[0066] By extending the obstacle, any three-dimensional point ξ in the spatial environment can be located on the surface of the extended obstacle, and the normal vector of the tangent plane of the extended obstacle is

[0067]

[0068] According to the normal vector of the extended tangent plane of the obstacle envelope as shown in formula (20), the real-time obstacle avoidance path of the robotic arm is planned, and the efficiency of obstacle avoidance path planning is improved while ensuring the accuracy of path planning.

[0069] Beneficial effects:

[0070] 1. The present invention discloses a spatial obstacle discrimination method based on a Gaussian mixture model and a depth-color image. According to the depth image and color image of the spatial environment measured by a camera, the depth image and the color image are used to generate a three-dimensional point cloud model of the spatial environment. The Gaussian mixture model is used to perform cluster analysis on the three-dimensional coordinates of the point cloud of the spatial environment to obtain a Gaussian probability density distribution function of the point cloud of the spatial environment for judging whether it is a spatial obstacle. The coordinates of the three-dimensional point in the path space to be judged during the movement of the robotic arm are input into the Gaussian probability density distribution function of the point cloud to obtain the corresponding Gaussian probability density value. According to the comparison between the Gaussian probability density value corresponding to the three-dimensional point in the path space to be judged and the spatial obstacle discrimination threshold, the spatial obstacle area and the robotic arm operable area are estimated and divided after the path space point to be judged is judged, which can improve the discrimination efficiency while ensuring the accuracy of spatial obstacle discrimination.

[0071] 2. The spatial obstacle discrimination method based on Gaussian mixture model and depth-color image disclosed in the present invention uses an iterative method to select and set the spatial obstacle discrimination threshold according to the Gaussian probability density distribution function of the spatial environment point cloud, thereby further improving the accuracy and efficiency of spatial obstacle discrimination.

[0072] 3. The spatial obstacle discrimination method based on Gaussian mixture model and depth-color image disclosed in the present invention uses statistical filtering to remove noise from the point cloud model, and uses voxel grid filtering to simplify the number of the denoised point cloud model, so as to improve the efficiency of constructing the point cloud Gaussian probability density distribution function, thereby improving the efficiency of spatial obstacle discrimination.

[0073] 4. The spatial obstacle discrimination method based on Gaussian mixture model and depth-color image disclosed in the present invention, on the basis of achieving the above-mentioned beneficial effects 1, 2, and 3, further uses the Gaussian clustering ellipsoid model to envelop the point cloud belonging to the spatial obstacle area according to the three-dimensional coordinates of the point cloud identified by the present invention, and obtains the corresponding Gaussian clustering ellipsoid outer envelope surface equation for obstacle avoidance path planning; according to the analytical expression of the spatial obstacle Gaussian clustering ellipsoid outer envelope surface equation, the normal vector of the extended tangent plane of the obstacle outer envelope is obtained, and the real-time obstacle avoidance path of the robotic arm is planned according to the normal vector, thereby improving the efficiency of obstacle avoidance path planning while ensuring the accuracy of path planning.

[0074] 5. The spatial obstacle discrimination method based on Gaussian mixture model and depth-color image disclosed in the present invention, according to the analysis results of beneficial effect 4, can solve relevant technical problems in the aerospace field, including identifying space environment obstacles and supporting on-orbit operation of robotic arms. BRIEF DESCRIPTION OF THE DRAWINGS

[0075] Figure 1This is a flow chart of the spatial obstacle discrimination method based on Gaussian mixture model and depth-color image disclosed in the present invention.

[0076] Figure 2 It is a three-dimensional point cloud model diagram of the spatial environment generated according to the depth image and color image of the spatial environment measured by the camera in the embodiment.

[0077] Figure 3 This is a three-dimensional point cloud model diagram of the spatial environment after noise removal and quantity reduction in the embodiment.

[0078] Figure 4 is a Gaussian probability density distribution diagram corresponding to the three-dimensional coordinates of the point cloud of the spatial environment in the embodiment, where: Figure 4 (a) is the Gaussian probability density distribution in the xy plane, Figure 4 (b) is the Gaussian probability density distribution in the xz plane, Figure 4 (c) is the Gaussian probability density distribution in the yz plane.

[0079] Figure 5 The obstacle area distribution diagram of the space environment in the embodiment, where: Figure 5 (a) is the distribution of obstacle areas in the spatial environment of the xy plane. Figure 5 (b) is the distribution of obstacle areas in the spatial environment of the xz plane. Figure 5 (c) is the distribution of obstacle areas in the spatial environment on the yz plane. DETAILED DESCRIPTION

[0080] In order to better illustrate the purpose, technical solutions and advantages of the present invention, the invention is further described below with reference to the accompanying drawings and embodiments.

[0081] like Figure 1 As shown, the spatial obstacle discrimination method based on Gaussian mixture model and depth-color image disclosed in this embodiment is specifically implemented as follows:

[0082] Step 1: Generate a three-dimensional point cloud model of the spatial environment using the depth image and color image of the spatial environment measured by the camera. Figure 2 3D point cloud model of the spatial environment generated by the depth image and the color image in the embodiment.

[0083] Step 2: Use statistical filtering to remove noise from the three-dimensional point cloud model of the spatial environment constructed in step 1, and use voxel grid filtering to reduce the number of point cloud models after noise removal. Figure 3 This is a 3D point cloud model of the spatial environment after denoising and simplification in the embodiment. Figure 3It can be seen that the outlier noise points in the original point cloud of the spatial environment have been removed after filtering, and while maintaining the shape features and spatial structure information contained in the original point cloud of the spatial environment, the number of spatial environment point clouds has been streamlined, which can improve the efficiency of the subsequent construction of the point cloud Gaussian probability density distribution function.

[0084] Step 3: Use the Gaussian mixture model to perform cluster analysis on the three-dimensional coordinates of the point cloud of the spatial environment after denoising and simplification in step 2 to obtain the Gaussian probability density distribution function of the spatial environment point cloud used to determine whether it is a spatial obstacle.

[0085] In this embodiment, a Gaussian mixture model containing 7 Gaussian components is used to perform cluster analysis on the three-dimensional coordinates of the point cloud of the spatial environment after denoising and simplification in step 2. The three-dimensional coordinates r of the point cloud of the spatial environment obey the Gaussian probability density distribution function as shown in formula (6). Figure 4 A Gaussian probability density distribution map corresponding to the three-dimensional coordinates of the point cloud representing the spatial environment. Figure 4 It can be seen that the spatial environment point cloud has obvious clustering characteristics, and the Gaussian probability density corresponding to the cluster center is large.

[0086] Step 4: Based on the Gaussian probability density distribution function of the spatial environment point cloud obtained in step 3, an iterative method is used to select and set the spatial obstacle discrimination threshold.

[0087] The process of iteratively determining the spatial obstacle discrimination threshold ε is as follows:

[0088] Step 4.1: Select the median of the Gaussian probability density of the spatial environment point cloud as the initial spatial obstacle discrimination threshold ε0,

[0089] Step 4.2: Divide the spatial environment point cloud into two regions according to the threshold ε0. If the Gaussian probability density corresponding to the spatial environment point cloud is less than or equal to the discrimination threshold ε0, the spatial environment point cloud is divided into the non-obstruction region R1, otherwise it is divided into the obstacle region R2. Calculate the median M1 and M2 of the Gaussian probability density of the spatial environment point cloud in the non-obstruction region R1 and the obstacle region R2 respectively.

[0090] Step 4.3: Update the spatial obstacle discrimination threshold ε according to formula (15) i+1 ,

[0091] Step 4.4: Repeat steps 4.2 and 4.3 until the spatial obstacle discrimination threshold ε i+1 and ε i If the difference is less than the preset value, the iterative process is stopped and the final spatial obstacle discrimination threshold ε is obtained.

[0092] In this embodiment, the spatial obstacle discrimination threshold ε=0.05 is determined through iteration.

[0093] Step 5: Input the coordinate x of the three-dimensional point in the path space to be determined during the movement of the robotic arm into the Gaussian probability density distribution function of the point cloud obtained in step 3 to obtain the corresponding Gaussian probability density value p(x). Compare the Gaussian probability density value p(x) corresponding to the three-dimensional point in the path space to be determined with the spatial obstacle discrimination threshold ε obtained in step 4. If p(x)>ε, the three-dimensional point in the path space to be determined is a spatial obstacle. If p(x)≤ε, the three-dimensional point in the path space to be determined does not belong to a spatial obstacle. After completing the judgment of whether the path space point to be determined is a spatial obstacle, the estimation and division of the spatial obstacle area and the manipulator's operable area are realized. Figure 5 This is a distribution map of the obstacle area of ​​the spatial environment after comparison and determination based on the Gaussian probability density value of the spatial environment point cloud and the spatial obstacle discrimination threshold in the embodiment.

[0094] Step 6: Based on the three-dimensional coordinates of the point cloud belonging to the spatial obstacle area determined in step 5, the Gaussian clustering ellipsoid model is further used to envelop the point cloud belonging to the spatial obstacle area to obtain the corresponding Gaussian clustering ellipsoid outer envelope surface equation for obstacle avoidance path planning; based on the analytical expression of the spatial obstacle Gaussian clustering ellipsoid outer envelope surface equation, the normal vector of the extended tangent plane of the obstacle outer envelope is obtained, and the real-time obstacle avoidance path of the robotic arm is planned based on the normal vector, thereby improving the efficiency of obstacle avoidance path planning while ensuring the accuracy of path planning.

[0095] The three-dimensional point cloud set belonging to the spatial obstacle area obtained in step 5 is recorded as B, and the Gaussian mixture model of the point cloud set B is established {π m ,μ m ,Σ m} m＝1:M , where M is the number of Gaussian distributions in the Gaussian mixture model. The point cloud set obeying the mth Gaussian distribution is regarded as an obstacle. The Gaussian clustering ellipsoid model is used to envelop the point cloud set obeying the mth Gaussian distribution, and the outer envelope surface equation of the Gaussian clustering ellipsoid of the mth obstacle is obtained as shown in formula (17). For the outer envelope of the Gaussian clustering ellipsoid of the mth obstacle, μ m is the center point of the ellipsoid, Σ m Determines the size and direction of the ellipsoid.

[0096] For the points on the outer envelope of the Gaussian clustering ellipsoid of the mth obstacle Using the normal vector n(ξ) as in Equation (19) b ) is defined at point ξ b The tangent plane at .

[0097] By extending the obstacle, any three-dimensional point ξ in the spatial environment can be located on the surface of the extended obstacle. The normal vector n(ξ) of the tangent plane of the extended obstacle is shown in Equation (20). Based on the normal vector of the extended tangent plane of the obstacle envelope, the robot arm's real-time obstacle avoidance path is planned, improving the efficiency of obstacle avoidance path planning while ensuring path planning accuracy.

[0098] The above specific description further illustrates the purpose, technical solutions and beneficial effects of the invention in detail. It should be understood that the above description is only a specific embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A spatial obstacle detection method based on a Gaussian mixture model and depth-color images, characterized by: The following steps are included: Step 1: Generate a 3D point cloud model of the spatial environment using the depth image and color image of the spatial environment measured by the camera; Step 2: Use statistical filtering to remove noise from the three-dimensional point cloud model of the spatial environment constructed in step 1, and use voxel grid filtering to reduce the number of point cloud models after noise removal to improve the efficiency of the subsequent construction of the point cloud Gaussian probability density distribution function; Step 3: Use the Gaussian mixture model to perform cluster analysis on the three-dimensional coordinates of the spatial environment point cloud after denoising and streamlining in step 2, and obtain the Gaussian probability density distribution function of the spatial environment point cloud for determining whether it is a spatial obstacle; Step 4: Based on the Gaussian probability density distribution function of the spatial environment point cloud obtained in step 3, an iterative method is used to select and set the spatial obstacle discrimination threshold; Step 5: Input the coordinates of the 3D point in the path space to be determined during the movement of the manipulator into the Gaussian probability density distribution function of the point cloud obtained in step 3 to obtain the corresponding Gaussian probability density value. Compare the Gaussian probability density value corresponding to the 3D point in the path space to be determined with the spatial obstacle discrimination threshold obtained in step 4. After the point in the path space to be determined is determined, the spatial obstacle area and the manipulator's operable area are estimated and divided. Step 6: Based on the three-dimensional coordinates of the point cloud belonging to the spatial obstacle area determined in step 5, the Gaussian clustering ellipsoid model is further used to envelop the point cloud belonging to the spatial obstacle area to obtain the corresponding Gaussian clustering ellipsoid outer envelope surface equation for obstacle avoidance path planning; based on the analytical expression of the spatial obstacle Gaussian clustering ellipsoid outer envelope surface equation, the normal vector of the extended tangent plane of the obstacle outer envelope is obtained, and the real-time obstacle avoidance path of the robotic arm is planned based on the normal vector, thereby improving the efficiency of obstacle avoidance path planning while ensuring the accuracy of path planning.

2. The spatial obstacle identification method based on a Gaussian mixture model and depth-color images according to claim 1, wherein: The implementation method of step 2 is: Statistical filtering: Let the nth data point in the three-dimensional point cloud model of the spatial environment constructed in step 1 be q n , n=1,2,3,...,S, let q n The distance to any point is d i , define q n The average distance D to all its k neighboring points avr-n It follows a Gaussian distribution with mean μ and standard deviation σ, Set the standard range S pa-n , used to determine whether the data points in the model are noise points, S pa-n =μ±g·σ(g=0,1,2,...) (2) According to the average distance D avr-n and standard range S pa-n Select the corresponding point, that is, when point q n The corresponding average distance D avr-n Greater than the standard range S pa-n , delete the point; when point q n The corresponding average distance D avr-n Less than or equal to the standard range S pa-n , then keep this point. Voxel grid filtering: Get the minimum and maximum coordinate values ​​of the denoised 3D point cloud model in the x-axis, y-axis and z-axis directions, recorded as X min 、X max 、Y min 、Y max 、Z min 、Z max， The side lengths of the cuboid bounding box of the 3D point cloud model after denoising are: Among them, L x 、L y 、L z is the side length of the cuboid bounding box of the denoised 3D point cloud model in the x-axis, y-axis, and z-axis directions, The cuboid bounding box of the denoised 3D point cloud model is divided into several voxel grids with a side length of L; the denoised 3D point cloud model is traversed, the number of point cloud data in each voxel grid is recorded as m, and the centroid of each voxel grid is calculated. The center of mass of the voxel grid (X c ,Y c ,Z c ) represents all data points in the voxel grid, thereby reducing the number of point clouds.

3. The spatial obstacle identification method based on a Gaussian mixture model and depth-color images according to claim 2, wherein: The implementation method of step three is: The Gaussian mixture model is used to perform cluster analysis on the three-dimensional coordinates of the point cloud of the denoised and streamlined spatial environment; the Gaussian mixture model is expressed as, Where r is the three-dimensional coordinate of the point cloud of the spatial environment after denoising and simplification, K is the number of Gaussian distributions in the Gaussian mixture model, and π k is the weight of the kth Gaussian distribution, satisfying the following constraints N(r;μ k ,Σ k ) is the kth Gaussian distribution probability density function, μ k and Σ k Represent the mean vector and covariance matrix respectively; π k ,μ k and Σ k This is the parameter to be determined in the Gaussian mixture model; First, the K-means algorithm is used to find the parameter π k ,μ k and Σ k After initialization, the expectation-maximization algorithm (EM) is used for iterative solution, which is divided into two steps: expectation step (E-setp) and maximization step (M-step). The iterative process is as follows: E-step: Using the current Gaussian mixture model parameter π k ,μ k and Σ k Solve for the expected value of the log-likelihood; Introducing hidden variables z ik , its value is Expand point cloud 3D coordinate data into complete data (r i ,z i1 ,z i2 ,...,z iK ),i=1,2,...,N (8) Where N is the number of point cloud three-dimensional coordinate data, and the log-likelihood function of the complete data is Compute the expectation of the log-likelihood function Note E(z ik )=γ ik , represents the three-dimensional coordinate r of the i-th point cloud under the current Gaussian mixture model parameters {π, μ, Σ} i The probability from the kth Gaussian distribution is calculated as M-step: Optimize the Gaussian mixture model parameters by maximizing the expected value of the log-likelihood function in E-step; considering π k The constraint condition (6) must be satisfied, and the optimization objective function J with the Lagrangian operator λ is set as Find the objective function J for π respectively k ,μ k ,Σ k The partial derivative of and set it to 0 can get the new estimated value of the Gaussian mixture model parameters: Repeat E-step and M-step until the estimated values ​​of the Gaussian mixture model parameters obtained from two iterations are almost unchanged, then terminate the iteration and output the model parameter π k ,μ k and Σ k ; Based on the solved Gaussian mixture model with known parameters, the Gaussian probability density distribution function of the spatial environment point cloud is obtained as shown in formula (5) for determining whether it is a spatial obstacle.

4. The spatial obstacle identification method based on a Gaussian mixture model and depth-color images according to claim 3, wherein: The process of iteratively determining the spatial obstacle discrimination threshold ε is as follows: Step 4.1: Select the median of the Gaussian probability density of the spatial environment point cloud as the initial spatial obstacle discrimination threshold ε0, Step 4.2: Divide the spatial environment point cloud into two regions according to the threshold ε0. If the Gaussian probability density corresponding to the spatial environment point cloud is less than or equal to the discrimination threshold ε0, the spatial environment point cloud is divided into the non-obstruction region R1, otherwise it is divided into the obstacle region R2. Calculate the median M1 and M2 of the Gaussian probability density of the spatial environment point cloud in the non-obstruction region R1 and the obstacle region R2 respectively. Step 4.3: Update the spatial obstacle discrimination threshold ε i+1 , Step 4.4: Repeat steps 4.2 and 4.3 until the spatial obstacle discrimination threshold ε i+1 and ε i If the difference is less than the preset value, the iterative process is stopped and the final spatial obstacle discrimination threshold ε is obtained.

5. The spatial obstacle identification method based on a Gaussian mixture model and depth-color images according to claim 4, characterized in that: Input the three-dimensional point coordinate x of the path space to be determined during the movement of the robotic arm into the point cloud Gaussian probability density distribution function obtained in step 3 to obtain the corresponding Gaussian probability density value Compare the Gaussian probability density value p(x) corresponding to the 3D point in the path space to be determined with the spatial obstacle discrimination threshold ε obtained in step 4. If p(x)>ε, the 3D point in the path space to be determined is a spatial obstacle. If p(x)≤ε, the 3D point in the path space to be determined is not a spatial obstacle. After completing the judgment on whether the path space point to be determined is a spatial obstacle, the estimation and division of the spatial obstacle area and the manipulator's operable area are realized.

6. The spatial obstacle identification method based on a Gaussian mixture model and depth-color images according to claim 5, characterized in that: Step 6 is implemented as follows: The three-dimensional point cloud set belonging to the spatial obstacle area obtained in step 5 is recorded as B, and the Gaussian mixture model of the point cloud set B is established {π m ,μ m ,Σ m } m＝1:M , where M is the number of Gaussian distributions in the Gaussian mixture model. The point cloud set obeying the mth Gaussian distribution is regarded as an obstacle. The Gaussian clustering ellipsoid model is used to envelop the point cloud set obeying the mth Gaussian distribution. The outer envelope surface equation of the Gaussian clustering ellipsoid of the mth obstacle is obtained as follows: Where ξ is the coordinate of the three-dimensional point in the spatial environment, R is the minimum radius that makes all point clouds that obey the mth Gaussian distribution included in the same ellipsoid; μ m is the center point of the mth Gaussian cluster ellipsoid; Σ m Determines the size and direction of the ellipsoid, is a 3×3 dimensional positive definite matrix, and its eigenvalue matrix Λ and eigenvector matrix V are shown in formula (17): Where r1, r2, r3 represent the length of the ellipsoid semi-axis, α1, α2, α3 are the orientations of the ellipsoid semi-axis corresponding to r1, r2, r3; Γ m (ξ;μ m ,Σ m ) is abbreviated as Γ m (ξ); for the points on the outer envelope of the Gaussian clustering ellipsoid of the mth obstacle Use the normal vector n(ξ b ) is defined at point ξ b The tangent plane at: By extending the obstacle, any three-dimensional point ξ in the spatial environment can be located on the surface of the extended obstacle, and the normal vector of the tangent plane of the extended obstacle is According to the normal vector of the extended tangent plane of the obstacle envelope as shown in formula (19), the real-time obstacle avoidance path of the robotic arm is planned, and the efficiency of obstacle avoidance path planning is improved while ensuring the accuracy of path planning.

7. The method for spatial obstacle identification based on a Gaussian mixture model and depth-color images according to claim 3, 4, 5 or 6, wherein: L = (2% to 10%) L min , where L min For L x 、L y 、L z The minimum value in .

Images