CN115533897A - Mechanical arm obstacle avoidance planning method based on dynamic system and Gaussian cluster ellipsoid - Google Patents
Mechanical arm obstacle avoidance planning method based on dynamic system and Gaussian cluster ellipsoid Download PDFInfo
- Publication number
- CN115533897A CN115533897A CN202211149409.0A CN202211149409A CN115533897A CN 115533897 A CN115533897 A CN 115533897A CN 202211149409 A CN202211149409 A CN 202211149409A CN 115533897 A CN115533897 A CN 115533897A
- Authority
- CN
- China
- Prior art keywords
- obstacle
- gaussian
- mechanical arm
- point cloud
- space
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 56
- 239000011159 matrix material Substances 0.000 claims abstract description 44
- 238000005315 distribution function Methods 0.000 claims abstract description 32
- 230000004888 barrier function Effects 0.000 claims abstract description 22
- 230000000149 penetrating effect Effects 0.000 claims abstract description 11
- 238000004458 analytical method Methods 0.000 claims abstract description 7
- 239000000203 mixture Substances 0.000 claims description 32
- 238000009826 distribution Methods 0.000 claims description 21
- 230000008569 process Effects 0.000 claims description 11
- 238000005259 measurement Methods 0.000 claims description 10
- 238000001914 filtration Methods 0.000 claims description 7
- 238000004422 calculation algorithm Methods 0.000 claims description 6
- 230000000694 effects Effects 0.000 claims description 6
- 238000012804 iterative process Methods 0.000 claims description 5
- 238000005457 optimization Methods 0.000 claims description 4
- 230000009467 reduction Effects 0.000 claims description 4
- 238000007621 cluster analysis Methods 0.000 claims description 3
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 2
- 102000002274 Matrix Metalloproteinases Human genes 0.000 claims description 2
- 108010000684 Matrix Metalloproteinases Proteins 0.000 claims description 2
- 150000001875 compounds Chemical class 0.000 claims description 2
- 238000000354 decomposition reaction Methods 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 12
- 238000005070 sampling Methods 0.000 description 4
- 238000004519 manufacturing process Methods 0.000 description 2
- 230000002776 aggregation Effects 0.000 description 1
- 238000004220 aggregation Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000010355 oscillation Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
- B25J9/1666—Avoiding collision or forbidden zones
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/1605—Simulation of manipulator lay-out, design, modelling of manipulator
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Image Processing (AREA)
Abstract
The invention discloses a mechanical arm obstacle avoidance planning method based on a dynamic system and Gaussian cluster ellipsoids, and belongs to the technical field of mechanical arm obstacle avoidance. The implementation method of the invention comprises the following steps: generating a three-dimensional point cloud model of a space environment according to the depth image and the color image, and obtaining a Gaussian probability density distribution function for judging whether the depth image is a space obstacle or not through clustering analysis; inputting a three-dimensional point coordinate of a path space to be determined into a point cloud Gaussian probability density distribution function, and comparing and determining whether the path space is an obstacle or not; enveloping the point cloud belonging to the space obstacle by using a Gaussian cluster ellipsoid model to obtain a Gaussian cluster ellipsoid outer envelope surface equation; and obtaining an extended tangent plane normal vector of the outer envelope of the barrier according to the analytical expression of the envelope surface equation, and constructing a dynamic modulation matrix for adjusting the motion direction of the mechanical arm close to the barrier, so that the tail end of the mechanical arm moves along the surface of the barrier without penetrating through the barrier, thereby improving the path planning efficiency on the basis of ensuring the path planning precision.
Description
Technical Field
The invention relates to a mechanical arm obstacle avoidance planning method based on a dynamic system and Gaussian cluster ellipsoids, and belongs to the technical field of mechanical arm obstacle avoidance.
Background
With the rapid development of the robot technology, in recent years, the mechanical arm plays an important role in the fields of industrial manufacturing, medical rehabilitation, express sorting and the like, and the production efficiency is greatly improved. Meanwhile, the wide application of the human-computer cooperation scene also puts higher requirements on the intelligence of the mechanical arm, and a collision-free planning path needs to be output in real time. In a traditional mechanical arm obstacle avoidance path planning method, such as an artificial potential field method, local optimization and motion oscillation phenomena are easy to occur, so that a mechanical arm cannot reach a desired point; although the RRT sampling method can avoid modeling a high-dimensional joint space, the sampling direction is randomly generated, which easily causes sudden changes in a planned path, and collision detection is required at any time, resulting in a large amount of calculation. Compared with an artificial potential field method and an RRT sampling method, the mechanical arm obstacle avoidance method based on a dynamic System can be automatically adapted to different mechanical arm starting positions and target positions, and can ensure that the mechanical arm can finally converge to a target point. However, this method requires modeling the environment in advance to obtain an analytical expression of the surface of the obstacle, and therefore has a limitation in application scenarios where dynamics are unknown. Therefore, it is necessary to research an algorithm capable of planning an obstacle avoidance path of a mechanical arm in a dynamically unknown environment to solve the above problems.
Disclosure of Invention
The invention mainly aims to provide a mechanical arm obstacle avoidance planning method based on a dynamic System and a Gaussian cluster ellipsoid, which realizes mechanical arm obstacle avoidance planning based on the dynamic System and the Gaussian cluster ellipsoid, can enable the tail end of a mechanical arm to move along the surface of an obstacle without penetrating the obstacle, can avoid the path planning from being trapped into local optimization, and improves obstacle avoidance path planning efficiency on the basis of ensuring path planning precision.
The technical scheme adopted by the invention for solving the technical problem is as follows:
the invention discloses a mechanical arm obstacle avoidance planning method based on a dynamic system and a Gaussian cluster ellipsoid, which comprises the steps of generating a three-dimensional point cloud model of a space environment by using a depth image and a color image of the space environment measured by a camera, and removing noise and simplifying the number of the three-dimensional point cloud model; performing clustering analysis on the point cloud three-dimensional coordinates of the space environment by using a Gaussian mixture model, and constructing a Gaussian probability density distribution function of the point cloud of the space environment for judging whether the point cloud of the space environment is a space obstacle; selecting a set space obstacle discrimination threshold value by using an iteration method according to the Gaussian probability density distribution function of the spatial environment point cloud; inputting coordinates of three-dimensional points of a path space to be judged in the motion process of the mechanical arm into a point cloud Gaussian probability density distribution function to obtain corresponding Gaussian probability density values, and comparing the Gaussian probability density values corresponding to the three-dimensional points of the path space to be judged with a space obstacle judgment threshold value to judge whether the path space is an obstacle or not; further enveloping the point cloud belonging to the space obstacle area by using a Gaussian cluster ellipsoid model to obtain a corresponding Gaussian cluster ellipsoid outer envelope surface equation for obstacle avoidance path planning, and obtaining an analytical expression for describing the space obstacle according to a camera vision measurement result; obtaining a normal vector of an extended tangent plane of the outer envelope of the obstacle according to the analytical expression of the outer envelope surface equation of the space obstacle Gaussian cluster ellipsoid; an extended tangent plane orthogonal coordinate system used for guiding the mechanical arm to avoid collision is constructed through the normal vector, a dynamic modulation matrix used for adjusting the motion direction of the mechanical arm close to the obstacle is constructed through the extended tangent plane orthogonal coordinate system, the speed of the tail end of the mechanical arm is corrected through the dynamic modulation matrix, the tail end of the mechanical arm moves along the surface of the obstacle without penetrating through the obstacle, the mechanical arm is planned for avoiding the obstacle in real time, and the obstacle avoidance path planning efficiency is improved on the basis of ensuring the path planning precision.
The invention discloses a mechanical arm obstacle avoidance planning method based on a dynamic system and Gaussian cluster ellipsoids, which comprises the following steps:
the method comprises the following steps: according to a depth image and a color image of a space environment obtained by camera measurement, generating a three-dimensional point cloud model of the space environment by using the depth image and the color image, and performing noise removal and quantity reduction; performing clustering analysis on the point cloud three-dimensional coordinates of the space environment by using a Gaussian mixture model, and constructing a Gaussian probability density distribution function of the point cloud of the space environment for judging whether the point cloud of the space environment is a space obstacle; selecting a set space obstacle discrimination threshold value by using an iteration method according to the space environment point cloud Gaussian probability density distribution function; inputting the coordinates of the three-dimensional point of the path space to be judged in the motion process of the mechanical arm into a point cloud Gaussian probability density distribution function to obtain a corresponding Gaussian probability density value, and comparing the Gaussian probability density value corresponding to the three-dimensional point of the path space to be judged with a space obstacle judgment threshold value to judge whether the path space is an obstacle.
Step 1.1: according to a depth image and a color image of a space environment obtained by camera measurement, generating a three-dimensional point cloud model of the space environment by using the depth image and the color image; preferably, noise removal is carried out on the constructed three-dimensional point cloud model of the space environment by adopting statistical filtering, and the number of the point cloud models after denoising is reduced by adopting voxel grid filtering, so that the efficiency of subsequently constructing a point cloud Gaussian probability density distribution function is improved.
Step 1.2: and (3) performing clustering analysis on the point cloud three-dimensional coordinates of the space environment subjected to denoising and simplification in the step 1.1 by using a Gaussian mixture model, and constructing a Gaussian probability density distribution function of the point cloud of the space environment for judging whether the point cloud is a space obstacle.
The gaussian mixture model is represented as,
wherein r is the three-dimensional coordinate of the point cloud of the space environment after denoising simplification, K is the number of Gaussian distribution in the Gaussian mixture model, and pi k The weight occupied by the k-th Gaussian distribution satisfies the following constraint
N(r；μ k ,Σ k ) Is the k-th Gaussian distribution probability density function, μ k Sum-sigma k Representing the mean vector and covariance matrix, respectively. Pi k ,μ k Sum-sigma k I.e. the parameters to be solved in the gaussian mixture model.
Preferably, the parameter pi to be solved is firstly treated by adopting a K-means algorithm k ,μ k Sum-sigma k Initializing, and after initialization, performing iterative solution by using an Expectation-Maximization (EM) algorithm, wherein the iterative solution is divided into two steps of an Expectation step (E-setp) and a Maximization step (M-step), and the iterative process is as follows:
e-step: using the current Gaussian mixture model parameter π k ,μ k Sum-sigma k Solving the expected value of the log likelihood.
Introducing an implicit variable z ik The value of which is
Expanding the point cloud three-dimensional coordinate data into complete data
(r i ,z i1 ,z i2 ,...,z iK ),i＝1,2,...,N (4)
Wherein N is the number of the point cloud three-dimensional coordinate data, and the log likelihood function of the complete data is
Note E (z) ik )＝γ ik Represents the ith point cloud three-dimensional coordinate r under the current Gaussian mixture model parameters { pi, mu, sigma } i The probability from the kth Gaussian distribution is calculated as
M-step: and optimizing the parameters of the Gaussian mixture model by maximizing the expectation value of the log-likelihood function in the E-step. Considering pi k If the constraint condition expression (2) is required to be satisfied, setting an optimization objective function J with a Lagrangian lambda as
Respectively solving the target function J to pi k ,μ k ,Σ k And the partial derivative of the Gaussian mixture model is set to be 0, so that a new estimation value of the Gaussian mixture model parameter can be obtained:
repeating the E-Step and the M-Step until the estimated value of the Gaussian mixture model parameter obtained by the two iterations is almost unchanged, terminating the iteration, and outputting the model parameter pi k ,μ k Sum sigma k 。
And (3) obtaining a Gaussian probability density distribution function of the spatial environment point cloud for judging whether the space obstacle exists as shown in the formula (1) based on the Gaussian mixture model with known parameters obtained by solving.
Step 1.3: and (3) selecting and setting a space obstacle discrimination threshold value by using an iteration method according to the space environment point cloud Gaussian probability density distribution function obtained in the step (1.2).
The process of iteratively selecting the spatial obstacle discrimination threshold epsilon is as follows: (1) selecting a median value of Gaussian probability density of the spatial environment point cloud as an initial spatial obstacle discrimination threshold epsilon 0 (ii) a (2) According to a threshold value epsilon 0 Dividing the spatial environment point cloud into two regions, and if the Gaussian probability density corresponding to the spatial environment point cloud is less than or equal to a discrimination threshold epsilon 0 Dividing the spatial environment point cloud into a non-obstacle area R1, otherwise, dividing into an obstacle area R2, and respectively calculating median values M1 and M2 of the spatial environment point cloud Gaussian probability densities of the non-obstacle area R1 and the obstacle area R2; (3)updating the spatial obstruction discrimination threshold ε i+1 = (M1 + M2)/2; (4) repeating the step (2) and the step (3) until the space obstacle distinguishing threshold epsilon i+1 And epsilon i If the difference is smaller than the preset value, the iterative process is stopped, and the final space obstacle judgment threshold value epsilon is obtained.
Step 1.4: inputting the coordinate x of the three-dimensional point of the path space to be judged in the motion process of the mechanical arm into the point cloud Gaussian probability density distribution function obtained in the step 1.2 to obtain a corresponding Gaussian probability density value p (x), judging a threshold value epsilon according to the Gaussian probability density value p (x) corresponding to the three-dimensional point of the path space to be judged and the space obstacle obtained in the step 1.3, if p (x) > epsilon, the three-dimensional point of the path space to be judged belongs to the space obstacle, and if p (x) is less than or equal to epsilon, the three-dimensional point of the path space to be judged does not belong to the space obstacle.
Step two: enveloping the point cloud belonging to the space obstacle area by further using a Gaussian cluster ellipsoid model to obtain a corresponding external envelope surface equation of the Gaussian cluster ellipsoid used for obstacle avoidance path planning for the point cloud three-dimensional coordinates judged as the obstacles in the step one, and obtaining an analytical expression used for describing the space obstacle according to a camera vision measurement result; and obtaining the normal vector of the extended tangent plane of the outer envelope of the obstacle according to the analytical expression of the outer envelope surface equation of the space obstacle Gaussian cluster ellipsoid.
Recording the three-dimensional point cloud set which belongs to the space barrier region and is obtained by the judgment in the step one as B, and establishing a Gaussian mixture model { pi ] of the point cloud set B m ,μ m ,Σ m } m＝1:M Wherein 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 point cloud set obeying the mth Gaussian distribution is enveloped by the Gaussian cluster ellipsoid model, and the outer enveloping surface equation of the Gaussian cluster ellipsoid for obtaining the mth obstacle is
Where xi is the three-dimensional point coordinate of the space environment and R is all points subject to the mth Gaussian distributionThe minimum radius of the cloud contained within the same ellipsoid; mu.s m Is the central point of the mth Gaussian cluster ellipsoid; sigma-shaped m Determines the size and the direction of the ellipsoid,
is a 3 x 3 dimensional positive definite matrix, the eigenvalue matrix Lambda and the eigenvector matrix V of which are shown in the formula (10)
In the formula r 1 ,r 2 ,r 3 Represents the semiaxial length of an ellipsoid, alpha 1 ,α 2 ,α 3 Is r of 1 ,r 2 ,r 3 The corresponding ellipsoid half-axis is oriented.
Will gamma m (ξ；μ m ,Σ m ) Abbreviated as gamma m (ξ). For points lying on the outer envelope of the Gaussian cluster ellipsoid of the mth obstacle
Using a normal vector n (xi) b ) Definition at point xi b Tangential plane of
By extending the obstacle, any three-dimensional point xi in the space environment can be located on the surface of the extended obstacle, and the normal vector of the tangent plane of the extended obstacle is
Step three: constructing an extended tangent plane orthogonal coordinate system for guiding the mechanical arm to avoid collision through the tangent plane normal vector of the extended barrier obtained in the second step, constructing a dynamic modulation matrix for adjusting the motion direction of the mechanical arm close to the barrier through the extended tangent plane orthogonal coordinate system, and correcting the speed of the tail end of the mechanical arm through the dynamic modulation matrix to enable the tail end of the mechanical arm to move along the surface of the barrier without penetrating through the barrier.
Establishing a first-order dynamic system as an expected motion trail equation of the tail end of the mechanical arm under the environment without obstacles, as shown in formula (13),
in the formula, xi is the spatial three-dimensional point coordinate of the tail end of the mechanical arm,
is the first derivative of ξ with respect to time, and f (-) is a continuously integrable function.
Constructing an extended tangent plane orthogonal coordinate system for guiding the mechanical arm to avoid collision according to the tangent plane normal vector n (xi) of the extended barrier obtained in the step two, wherein the orthogonal basis of the extended tangent plane orthogonal coordinate system is,
in the formula (I), the compound is shown in the specification,
representing the jth component of the ith basis vector.
Constructing a dynamic modulation matrix for adjusting the motion direction of the mechanical arm close to the obstacle through the extended tangent plane orthogonal coordinate system, and setting the dynamic modulation matrix
In order to realize the purpose,
in the formula, M m (xi) denotes a dynamic modulation matrix that only considers the effect of the mth obstacle on the end of the arm, E m (xi) and D m (xi) are each M m (xi) a feature vector matrix and a feature value matrix obtained by feature value decomposition,
E m (ξ)＝[n(ξ) e 1 (ξ) e 2 (ξ)] (16)
wherein, the first and the second end of the pipe are connected with each other,
solving by equation (18):
ω m (ξ) represents the weighting factor, and the specific expression is as formula (19):
formula (19) shows that m (xi) satisfies 0. Ltoreq. Omega m (xi) is less than or equal to 1; gaussian cluster ellipsoid outer envelope Γ at mth obstacle m (xi) =1, satisfy ω m (ξ) =1 and ω for any i ≠ m i (ξ) =0. When the tail end point xi of the mechanical arm is positioned in the Gaussian cluster ellipsoid outer envelope of the mth obstacle, the requirement is met
The normal speed of the barrier is 0, so that the barrier can not penetrate through the barrier boundary; and the characteristic value matrix D is gradually far away from the surface of the obstacle as xi m (xi) and the velocity modulation matrix M m (xi) gradually converges to the unit array I, hence M m (xi) the effect exerted on the surface of the obstacle is the greatest, and the effect of correcting the speed gradually disappears when the distance from the surface of the obstacle is long.
Introducing a safety threshold eta to the dynamic modulation matrix
Zooming is carried out, so that the tail end of the mechanical arm keeps a safe distance with the surface of the obstacle when moving along the surface of the obstacle,
using dynamic modulation matrices
Modulating the first-order dynamic system shown in the formula (13) to obtain the corrected motion speed of the tail end of the mechanical arm
Step four: and updating the three-dimensional position coordinates of the tail end of the mechanical arm according to the corrected motion speed of the tail end of the mechanical arm obtained in the step three, and using the three-dimensional position coordinates to control the motion of the mechanical arm at the moment.
Introducing a time step delta t, and setting the three-dimensional position coordinate of the tail end of the mechanical arm before the movement speed of the tail end of the mechanical arm to be xi t Updating the three-dimensional position coordinates of the tail end of the mechanical arm according to the corrected motion speed of the tail end of the mechanical arm,
according to the updated three-dimensional position coordinate xi of the tail end of the mechanical arm t+1 And controlling the motion of the mechanical arm.
Step five: and repeatedly executing the second step to the fourth step, and performing obstacle avoidance planning and control on the mechanical arm at each moment, namely enabling the tail end of the mechanical arm to move along the surface of the obstacle without penetrating the obstacle through the space obstacle Gaussian cluster ellipsoid outer envelope surface equation analytical expression constructed in the second step and the dynamic modulation matrix constructed in the third step, improving the obstacle avoidance path planning efficiency on the basis of ensuring the path planning precision, and realizing the planning of the real-time obstacle avoidance path of the mechanical arm until the tail end of the mechanical arm reaches the target position.
Has the advantages that:
1. the invention discloses a mechanical arm obstacle avoidance planning method based on a dynamic system and a Gaussian cluster ellipsoid, which comprises the steps of generating a three-dimensional point cloud model of a space environment according to a depth image and a color image which are obtained by camera measurement, and carrying out cluster analysis on a point cloud three-dimensional coordinate of the space environment by using a Gaussian mixture model to obtain a Gaussian probability density distribution function of the point cloud of the space environment for judging whether the point cloud is a space obstacle or not; inputting coordinates of three-dimensional points of a path space to be judged in the motion process of the mechanical arm into a point cloud Gaussian probability density distribution function to obtain corresponding Gaussian probability density values, and comparing the Gaussian probability density values corresponding to the three-dimensional points of the path space to be judged with a space obstacle judgment threshold value to judge whether the path space is an obstacle or not; further enveloping the point cloud belonging to the space obstacle by using a Gaussian cluster ellipsoid model to obtain a corresponding Gaussian cluster ellipsoid outer envelope surface equation for obstacle avoidance path planning on the point cloud three-dimensional coordinates judged as the obstacle; and obtaining a normal vector of an extended tangent plane of the outer envelope of the obstacle according to the analytical expression of the outer envelope surface equation of the Gaussian cluster ellipsoid of the space obstacle, and constructing a dynamic modulation matrix for adjusting the motion direction of the mechanical arm close to the obstacle according to the normal vector, so that the tail end of the mechanical arm moves along the surface of the obstacle without penetrating the obstacle, planning an obstacle avoiding path of the mechanical arm in real time, and improving the planning efficiency of the obstacle avoiding path on the basis of ensuring the path planning precision.
2. The invention discloses a mechanical arm obstacle avoidance planning method based on a dynamic system and a Gaussian cluster ellipsoid, wherein a normal vector of an extended tangent plane of an outer envelope of an obstacle is obtained according to an analytic expression of an outer envelope surface equation of the Gaussian cluster ellipsoid of a spatial obstacle; an extended tangent plane orthogonal coordinate system used for guiding the mechanical arm to avoid collision is constructed through the normal vector, a dynamic modulation matrix used for adjusting the motion direction of the mechanical arm close to the obstacle is constructed through the extended tangent plane orthogonal coordinate system, and the speed of the tail end of the mechanical arm is corrected through the dynamic modulation matrix, so that the tail end of the mechanical arm moves along the surface of the obstacle without penetrating through the obstacle.
3. The invention discloses a mechanical arm obstacle avoidance planning method based on a dynamic system and a Gaussian clustering ellipsoid.
4. The invention discloses a mechanical arm obstacle avoidance planning method based on a dynamic system and a Gaussian cluster ellipsoid.
5. On the basis of realizing the beneficial effects 1, 2, 3 and 4, the mechanical arm obstacle avoidance planning method based on the dynamic system and the Gaussian clustering ellipsoid can solve the problems that a traditional mechanical arm obstacle avoidance path planning method (such as an artificial potential field method and an RRT sampling method) is easy to fall into a local optimal value, algorithm time consumption is high, real-time planning effect is poor, a geometric analytic expression of an obstacle is difficult to obtain in a dynamic unknown environment, and the real-time obstacle avoidance capability of a mechanical arm is improved.
Drawings
Fig. 1 is a flow chart of a mechanical arm obstacle avoidance planning method based on a dynamic system and a gaussian cluster ellipsoid.
Fig. 2 is a three-dimensional point cloud model diagram of a spatial environment generated according to a depth image and a color image of the spatial environment measured by a camera in the embodiment.
FIG. 3 is a three-dimensional point cloud model diagram of the spatial environment after noise removal and number reduction in the embodiment.
Fig. 4 is a gaussian probability density distribution diagram corresponding to a point cloud three-dimensional coordinate of the spatial environment in the embodiment, where: FIG. 4 (a) shows a Gaussian probability density distribution in the x-y plane, FIG. 4 (b) shows a Gaussian probability density distribution in the x-z plane, and FIG. 4 (c) shows a Gaussian probability density distribution in the y-z plane.
FIG. 5 is a three-dimensional point cloud model diagram of the space obstacle determined according to the comparison of the Gaussian probability density value of the point cloud of the space environment and the discrimination threshold of the space obstacle in the embodiment.
FIG. 6 is a Gaussian cluster ellipsoid outer envelope diagram corresponding to the three-dimensional point cloud model of the spatial environment determined as the obstacle in the embodiment.
FIG. 7 is a schematic diagram of the tangent plane and the extended tangent plane of the outer envelope of the spatial obstacle Gaussian cluster ellipsoid in the embodiment.
Fig. 8 is a comparison between the obstacle-avoiding motion trajectory (right) of the end of the mechanical arm corrected by using the dynamic modulation matrix in the embodiment and the original motion trajectory (left) of the end of the mechanical arm in the case of no obstacle, where: fig. 8 (a) is a comparison of an obstacle avoidance trajectory of an x-y plane and an original trajectory, fig. 8 (b) is a comparison of an obstacle avoidance trajectory of an x-z plane and an original trajectory, and fig. 8 (c) is a comparison of an obstacle avoidance trajectory of a y-z plane and an original trajectory.
Detailed Description
For better illustrating the objects, technical solutions and advantages of the present invention, the following further description is provided in conjunction with the accompanying drawings and examples.
As shown in fig. 1, the mechanical arm obstacle avoidance planning method based on the dynamic system and the gaussian cluster ellipsoid disclosed in this embodiment includes the following specific implementation steps:
the method comprises the following steps: generating a three-dimensional point cloud model of a space environment according to a depth image and a color image which are obtained by measuring of a camera, and performing noise removal and quantity reduction; performing clustering analysis on the point cloud three-dimensional coordinates of the space environment by using a Gaussian mixture model, and constructing a Gaussian probability density distribution function of the point cloud of the space environment for judging whether the point cloud of the space environment is a space obstacle; selecting a set space obstacle discrimination threshold value by using an iteration method according to the space environment point cloud Gaussian probability density distribution function; inputting the coordinates of the three-dimensional point of the path space to be judged in the motion process of the mechanical arm into a point cloud Gaussian probability density distribution function to obtain a corresponding Gaussian probability density value, and comparing the Gaussian probability density value corresponding to the three-dimensional point of the path space to be judged with a space obstacle judgment threshold value to judge whether the path space is an obstacle.
Step 1.1: a three-dimensional point cloud model of a spatial environment is generated according to a depth image and a color image obtained by camera measurement, and fig. 2 is a three-dimensional point cloud model diagram of the spatial environment generated by the depth image and the color image in the embodiment. Preferably, noise removal is carried out on the constructed three-dimensional point cloud model of the space environment by adopting statistical filtering, and the quantity of the denoised point cloud model is reduced by adopting voxel grid filtering, so that the efficiency of subsequently constructing a point cloud Gaussian probability density distribution function is improved. FIG. 3 is a three-dimensional point cloud model diagram of the space environment after denoising and simplification in the embodiment. As can be seen from fig. 3, outlier noise points in the original point cloud of the spatial environment are removed after filtering, and the number of the point clouds of the spatial environment is reduced while maintaining shape features and spatial structure information included in the original point cloud of the spatial environment, so that the efficiency of subsequently constructing a gaussian probability density distribution function of the point clouds can be improved.
Step 1.2: and (3) performing clustering analysis on the point cloud three-dimensional coordinates of the space environment subjected to denoising and simplification in the step 1.1 by using a Gaussian mixture model, and constructing a Gaussian probability density distribution function of the point cloud of the space environment for judging whether the point cloud is a space obstacle.
In this embodiment, a gaussian mixture model containing 7 gaussian components is used to perform cluster analysis on the point cloud three-dimensional coordinates of the denoised and simplified space environment in the step two, and the point cloud three-dimensional coordinates r of the space environment obey a gaussian probability density distribution function as formula (1). Fig. 4 shows a gaussian probability density distribution diagram corresponding to the point cloud three-dimensional coordinates of the space environment. In fig. 4, it can be seen that the spatial environment point cloud has an obvious clustering feature, and the gaussian probability density corresponding to the clustering center is relatively large.
Step 1.3: and (3) selecting and setting a space obstacle discrimination threshold value by using an iteration method according to the space environment point cloud Gaussian probability density distribution function obtained in the step (1.2).
The process of iteratively selecting the spatial obstacle discrimination threshold epsilon is as follows: (1) selecting a median value of Gaussian probability density of spatial environment point cloud as an initial spatial obstacle discrimination threshold epsilon 0 (ii) a (2) According to a threshold value epsilon 0 Dividing the spatial environment point cloud into two regions, if the Gaussian probability density corresponding to the spatial environment point cloudDegree less than or equal to discrimination threshold epsilon 0 Dividing the spatial environment point cloud into a non-obstacle area R1, otherwise, dividing into an obstacle area R2, and respectively calculating median values M1 and M2 of the spatial environment point cloud Gaussian probability densities of the non-obstacle area R1 and the obstacle area R2; (3) updating the spatial obstacle discrimination threshold epsilon i+1 = (M1 + M2)/2; (4) repeating the step (2) and the step (3) until the space obstacle distinguishing threshold epsilon i+1 And ε i If the difference is smaller than the preset value, the iterative process is stopped, and the final space obstacle judgment threshold value epsilon is obtained. In the present embodiment, the iteratively determined spatial obstacle discrimination threshold ∈ =0.05.
Step 1.4: inputting the coordinate x of the three-dimensional point of the path space to be judged in the motion process of the mechanical arm into the point cloud Gaussian probability density distribution function obtained in the step 1.2 to obtain a corresponding Gaussian probability density value p (x), judging a threshold value epsilon according to the Gaussian probability density value p (x) corresponding to the three-dimensional point of the path space to be judged and the space obstacle obtained in the step 1.3, if p (x) > epsilon, the three-dimensional point of the path space to be judged belongs to the space obstacle, and if p (x) is less than or equal to epsilon, the three-dimensional point of the path space to be judged does not belong to the space obstacle. Fig. 5 is a three-dimensional point cloud model diagram belonging to the space obstacle after comparison determination according to the spatial environment point cloud gaussian probability density value and the space obstacle discrimination threshold in the embodiment.
Step two: enveloping the point cloud belonging to the space obstacle area by using a Gaussian cluster ellipsoid model to obtain a corresponding Gaussian cluster ellipsoid outer envelope surface equation for obstacle avoidance path planning, and obtaining an analytical expression for describing the space obstacle according to a camera vision measurement result; and obtaining the normal vector of the extended tangent plane of the outer envelope of the obstacle according to the analytical expression of the outer envelope surface equation of the space obstacle Gaussian cluster ellipsoid.
Recording the three-dimensional point cloud set which belongs to the space obstacle area and is obtained by judgment in the step one as B, and establishing a Gaussian mixture model { pi ] of the point cloud set B m ,μ m ,Σ m } m＝1:M Regarding the point cloud collection obeying the mth Gaussian distribution as an obstacle, obtaining the Gaussian aggregation of the mth obstacle as shown in the formula (9)An ellipsoid-like outer envelope surface equation. Fig. 6 is a gaussian cluster ellipsoid outer envelope diagram corresponding to the spatial environment three-dimensional point cloud model determined as an obstacle in the embodiment.
For point xi on the outer envelope of the gaussian cluster ellipsoid of the mth obstacle b Using a normal vector n ([ xi ]), as in equation (11) b ) Definition at point xi b And (4) obtaining a tangent plane normal vector n (xi) of the expanded obstacle as shown in the formula (12) by expanding the obstacle. FIG. 7 is a schematic diagram of the tangent plane and the extended tangent plane of the outer envelope of the spatial obstacle Gaussian cluster ellipsoid in the embodiment.
Step three: constructing an extended tangent plane orthogonal coordinate system for guiding the mechanical arm to avoid collision through the tangent plane normal vector of the extended obstacle obtained in the second step, constructing a dynamic modulation matrix for adjusting the movement direction of the mechanical arm close to the obstacle through the extended tangent plane orthogonal coordinate system, and correcting the speed of the tail end of the mechanical arm through the dynamic modulation matrix to enable the tail end of the mechanical arm to move along the surface of the obstacle without penetrating through the obstacle.
And (3) establishing a first-order dynamic system as a formula (13) as an expected motion trail equation of the tail end of the mechanical arm in an obstacle-free environment. And (4) constructing an extended tangent plane orthogonal coordinate system for guiding the mechanical arm to avoid collision according to the tangent plane normal vector n (xi) of the extended barrier obtained in the second step, wherein an orthogonal basis of the extended tangent plane orthogonal coordinate system is shown as a formula (14).
Constructing a dynamic modulation matrix as shown in a formula (15) through the extended tangent plane orthogonal coordinate system
The device is used for adjusting the motion direction of the mechanical arm close to the obstacle. Introducing a safety threshold eta to the dynamic modulation matrix
Zooming is carried out, so that the tail end of the mechanical arm keeps a safe distance with the surface of the obstacle when moving along the surface of the obstacle, and a zoomed dynamic modulation matrix shown as a formula (20) is obtained
By said scaled dynamic modulation matrix
Modulating the first-order dynamic system shown in the formula (13) to obtain the corrected motion speed of the tail end of the mechanical arm shown in the formula (21)
Step four: and updating the three-dimensional position coordinates of the tail end of the mechanical arm according to the corrected motion speed of the tail end of the mechanical arm obtained in the step three, and using the three-dimensional position coordinates to control the motion of the mechanical arm at the moment.
Introducing a time step delta t, and setting the three-dimensional position coordinate of the tail end of the mechanical arm before correcting the motion speed of the tail end of the mechanical arm as xi t Based on the corrected movement speed of the end of the robot arm, the updated three-dimensional position xi of the end of the robot arm based on the equation (22) t+1 And controlling the motion of the mechanical arm.
Step five: and repeatedly executing the second step to the fourth step, and performing obstacle avoidance planning and control on the mechanical arm at each moment, namely enabling the tail end of the mechanical arm to move along the surface of the obstacle without penetrating the obstacle through the space obstacle Gaussian cluster ellipsoid outer envelope surface equation analytical expression constructed in the second step and the dynamic modulation matrix constructed in the third step, improving the obstacle avoidance path planning efficiency on the basis of ensuring the path planning precision, and realizing the planning of the real-time obstacle avoidance path of the mechanical arm until the tail end of the mechanical arm reaches the target position.
In this embodiment, the set safety threshold η =1.2, the integration time step is Δ t =0.02s, and the target point is ξ * ＝[0m,0m,-0.28m]. Fig. 8 is a comparison between the obstacle-avoiding motion trajectory of the end of the mechanical arm corrected by using the dynamic modulation matrix in the embodiment and the original motion trajectory of the end of the mechanical arm in the case of no obstacle, and fig. 8 shows that after the dynamic modulation matrix is corrected, the end of the mechanical arm starts from different initial positions, can move along the surface of the obstacle to keep a safe distance with the surface of the obstacle, and does not collide with the obstacle, so that the purpose of avoiding the obstacle by using the mechanical arm is achieved.
The above detailed description is further intended to illustrate the objects, technical solutions and advantages of the present invention, and it should be understood that the above detailed description is only an example of the present invention and should not be used to limit the scope of the present invention, and any modifications, equivalents, improvements and the like made within the spirit and principle of the present invention should be included in the scope of the present invention.
Claims (6)
1. A mechanical arm obstacle avoidance planning method based on a dynamic system and a Gaussian cluster ellipsoid is characterized by comprising the following steps: comprises the following steps of (a) carrying out,
the method comprises the following steps: according to a depth image and a color image of a space environment obtained by camera measurement, generating a three-dimensional point cloud model of the space environment by using the depth image and the color image, and performing noise removal and quantity reduction; performing clustering analysis on the point cloud three-dimensional coordinates of the space environment by using a Gaussian mixture model, and constructing a Gaussian probability density distribution function of the point cloud of the space environment for judging whether the point cloud of the space environment is a space obstacle; selecting a set space obstacle discrimination threshold value by using an iteration method according to the Gaussian probability density distribution function of the spatial environment point cloud; inputting coordinates of three-dimensional points of a path space to be judged in the motion process of the mechanical arm into a point cloud Gaussian probability density distribution function to obtain corresponding Gaussian probability density values, and comparing the Gaussian probability density values corresponding to the three-dimensional points of the path space to be judged with a space obstacle judgment threshold value to judge whether the path space is an obstacle or not;
step two: enveloping the point cloud belonging to the space obstacle area by further using a Gaussian cluster ellipsoid model to obtain a corresponding external envelope surface equation of the Gaussian cluster ellipsoid used for obstacle avoidance path planning for the point cloud three-dimensional coordinates judged as the obstacles in the step one, and obtaining an analytical expression used for describing the space obstacle according to a camera vision measurement result; obtaining a normal vector of an expansion tangent plane of the outer envelope of the obstacle according to the equation analytic expression of the outer envelope surface of the space obstacle Gaussian cluster ellipsoid;
step three: constructing an extended tangent plane orthogonal coordinate system for guiding the mechanical arm to avoid collision through the tangent plane normal vector of the extended barrier obtained in the second step, constructing a dynamic modulation matrix for adjusting the motion direction of the mechanical arm close to the barrier through the extended tangent plane orthogonal coordinate system, and correcting the speed of the tail end of the mechanical arm through the dynamic modulation matrix to enable the tail end of the mechanical arm to move along the surface of the barrier without penetrating through the barrier;
step four: updating the three-dimensional position coordinate of the tail end of the mechanical arm according to the corrected motion speed of the tail end of the mechanical arm obtained in the step three, and using the three-dimensional position coordinate of the tail end of the mechanical arm for motion control of the mechanical arm at the moment;
step five: and repeatedly executing the second step to the fourth step, and performing obstacle avoidance planning and control on the mechanical arm at each moment, namely enabling the tail end of the mechanical arm to move along the surface of the obstacle without penetrating the obstacle through the space obstacle Gaussian cluster ellipsoid outer envelope surface equation analytical expression constructed in the second step and the dynamic modulation matrix constructed in the third step, improving the obstacle avoidance path planning efficiency on the basis of ensuring the path planning precision, and realizing the planning of the real-time obstacle avoidance path of the mechanical arm until the tail end of the mechanical arm reaches the target position.
2. The mechanical arm obstacle avoidance planning method based on the dynamic system and the Gaussian cluster ellipsoid as claimed in claim 1, characterized in that: the first implementation method comprises the following steps of,
step 1.1: according to a depth image and a color image of a space environment obtained by camera measurement, generating a three-dimensional point cloud model of the space environment by using the depth image and the color image; preferably, noise removal is carried out on the constructed three-dimensional point cloud model of the space environment by adopting statistical filtering, and the quantity of the denoised point cloud model is reduced by adopting voxel grid filtering, so that the efficiency of subsequently constructing a point cloud Gaussian probability density distribution function is improved;
step 1.2: performing cluster analysis on the point cloud three-dimensional coordinates of the space environment subjected to denoising and simplification in the step 1.1 by using a Gaussian mixture model, and constructing a Gaussian probability density distribution function of the point cloud of the space environment for judging whether the point cloud of the space environment is a space obstacle;
the gaussian mixture model is represented as,
in the formula, r is a point cloud three-dimensional coordinate of the denoised simplified space environment, K is the number of Gaussian distributions in the Gaussian mixture model, and pi k The weight occupied by the k-th Gaussian distribution satisfies the following constraint
N(r；μ k ,Σ k ) Is the k-th Gaussian distribution probability density function, μ k Sum sigma k Respectively representing a mean vector and a covariance matrix; pi k ,μ k Sum sigma k Namely the parameters to be solved in the Gaussian mixture model;
adopting K-means algorithm to treat parameter pi k ,μ k Sum-sigma k Initializing, and after initialization, performing iterative solution by using an Expectation-Maximization (EM) algorithm, wherein the iterative solution is divided into two steps of an Expectation step (E-setp) and a Maximization step (M-step), and the iterative process is as follows:
e-step: using the current Gaussian mixture model parameter π k ,μ k Sum-sigma k Solving a log-likelihood expected value;
introducing an implicit variable z ik The value of which is
Unfolding point cloud three-dimensional coordinate data into complete data
(r i ,z i1 ,z i2 ,...,z iK ),i＝1,2,...,N (4)
Wherein N is the number of point cloud three-dimensional coordinate data, and the log-likelihood function of the complete data is
Note E (z) ik )＝γ ik Represents the ith point cloud three-dimensional coordinate r under the current Gaussian mixture model parameters { pi, mu, sigma } i The probability from the kth Gaussian distribution is calculated as
M-step: optimizing Gaussian model parameters by maximizing the expectation value of a log-likelihood function in E-step; considering pi k The constraint condition formula (2) is satisfied, and an optimization objective function J with a Lagrangian lambda is set as
Respectively solving the target function J to pi k ,μ k ,Σ k And making it 0, a new estimation value of the gaussian mixture model parameter is obtained:
repeating the E-Step and the M-Step until the estimated values of the Gaussian mixture model parameters obtained by two iterations are almost unchanged, namely terminating the iteration and outputting the model parameters pi k ,μ k Sum-sigma k ；
Obtaining a Gaussian probability density distribution function of the spatial environment point cloud for judging whether the space obstacle exists as shown in the formula (1) based on the Gaussian mixture model with known parameters obtained by solving;
step 1.3: selecting a set space obstacle discrimination threshold value epsilon by using an iteration method according to the space environment point cloud Gaussian probability density distribution function obtained in the step 1.2;
step 1.4: inputting the coordinate x of the three-dimensional point of the path space to be judged in the motion process of the mechanical arm into the point cloud Gaussian probability density distribution function obtained in the step 1.2 to obtain a corresponding Gaussian probability density value p (x), judging a threshold value epsilon according to the Gaussian probability density value p (x) corresponding to the three-dimensional point of the path space to be judged and the space obstacle obtained in the step 1.3, if p (x) > epsilon, the three-dimensional point of the path space to be judged belongs to the space obstacle, and if p (x) is less than or equal to epsilon, the three-dimensional point of the path space to be judged does not belong to the space obstacle.
3. The mechanical arm obstacle avoidance planning method based on the dynamic system and the Gaussian cluster ellipsoid as claimed in claim 2, characterized in that: the process of iteratively selecting the spatial obstacle discrimination threshold epsilon is as follows: (1) selecting a median value of Gaussian probability density of spatial environment point cloud as an initial spatial obstacle discrimination threshold epsilon 0 (ii) a (2) According to a threshold value epsilon 0 Dividing the spatial environment point cloud into two regions, and if the Gaussian probability density corresponding to the spatial environment point cloud is less than or equal to a discrimination threshold epsilon 0 Dividing the spatial environment point cloud into a non-obstacle area R1, otherwise, dividing into an obstacle area R2, and respectively calculating median values M1 and M2 of the spatial environment point cloud Gaussian probability densities of the non-obstacle area R1 and the obstacle area R2; (3) updating the spatial obstacle discrimination threshold epsilon i+1 = (M1 + M2)/2; (4) repeating the step (2) and the step (3) until the space obstacle distinguishing threshold epsilon i+1 And ε i If the difference is smaller than the preset value, the iterative process is stopped, and the final space obstacle judgment threshold value epsilon is obtained.
4. The mechanical arm obstacle avoidance planning method based on the dynamic system and the Gaussian cluster ellipsoid as claimed in claim 2 or 3, characterized in that: the second step is realized by the method that,
recording the three-dimensional point cloud set which belongs to the space obstacle area and is obtained by judgment in the step one as B, and establishing a Gaussian mixture model { pi ] of the point cloud set B m ,μ m ,Σ m } m＝1:M Wherein M is the number of Gaussian distributions in the Gaussian mixture model, the point cloud set obeying the mth Gaussian distribution is taken as an obstacle, and the point cloud set obeying the mth Gaussian distribution is subjected to Gaussian clustering by using an ellipsoid modelEnveloping the point cloud set to obtain the outer enveloping surface equation of the Gaussian cluster ellipsoid of the mth obstacle as
Xi is the three-dimensional point coordinate of the space environment, R is the minimum radius of all point clouds obeying the mth Gaussian distribution and contained in the same ellipsoid; mu.s m Is the central point of the mth Gaussian cluster ellipsoid; sigma m Determines the size and the direction of the ellipsoid,
is a 3 x 3 dimensional positive definite matrix, the eigenvalue matrix Lambda and the eigenvector matrix V of which are shown in the formula (10)
In the formula r 1 ,r 2 ,r 3 Represents the semiaxial length of an ellipsoid, alpha 1 ,α 2 ,α 3 Is r of 1 ,r 2 ,r 3 The corresponding ellipsoid half-axis is oriented;
gamma-gamma is formed m (ξ；μ m ,Σ m ) Abbreviated as gamma m (ξ); for points lying on the outer envelope of the Gaussian cluster ellipsoid of the mth obstacle
Using a normal vector n (xi) b ) Definition at point xi b Tangential plane of
By extending the obstacle, any three-dimensional point xi in the space environment can be located on the surface of the extended obstacle, and the normal vector of the tangent plane of the extended obstacle is
5. The mechanical arm obstacle avoidance planning method based on the dynamic system and the Gaussian cluster ellipsoid as claimed in claim 4, characterized in that: the third step is to realize the method as follows,
establishing a first-order dynamic system as an expected motion trail equation of the tail end of the mechanical arm in an obstacle-free environment, as shown in formula (13),
in the formula, xi is the spatial three-dimensional point coordinate of the tail end of the mechanical arm,
is the first derivative of ξ with respect to time, f (-) being a continuously integrable function;
constructing an extended tangent plane orthogonal coordinate system for guiding the mechanical arm to avoid collision according to the tangent plane normal vector n (xi) of the extended barrier obtained in the step two, wherein the orthogonal basis of the extended tangent plane orthogonal coordinate system is,
in the formula (I), the compound is shown in the specification,
represents the jth component of the ith basis vector;
constructing a dynamic modulation matrix for adjusting the motion direction of the mechanical arm close to the obstacle through the extended tangent plane orthogonal coordinate system, and setting the dynamic modulation matrix
In order to realize the purpose,
in the formula, M m (xi) denotes a dynamic modulation matrix that only considers the effect of the mth obstacle on the end of the arm, E m (xi) and D m (xi) are each M m (xi) a feature vector matrix and a feature value matrix obtained by feature value decomposition,
E m (ξ)＝[n(ξ) e 1 (ξ) e 2 (ξ)] (16)
wherein, the first and the second end of the pipe are connected with each other,
the solution is given by equation (18):
ω m (ξ) represents the weighting factor, and the specific expression is as formula (19):
formula (19) shows that m (xi) satisfies 0. Ltoreq. Omega m (xi) is less than or equal to 1; gaussian cluster ellipsoid outer envelope Γ at mth obstacle m (xi) =1, satisfy ω m (xi) =1 and there is ω for any i ≠ m i (ξ) =0; when the tail end point xi of the mechanical arm is positioned in the Gaussian cluster ellipsoid outer envelope of the mth obstacle, the requirement is met
Show its relativeThe normal speed of the barrier is 0, so that the barrier can not penetrate through the boundary of the barrier; and the characteristic value matrix D is gradually far away from the surface of the obstacle as xi m (xi) and the velocity modulation matrix M m (xi) gradually converges to the unit array I, hence M m (ξ) exerts the greatest effect on the surface of the obstacle, and the action of correcting the speed gradually disappears as the distance from the surface of the obstacle is far;
introducing a safety threshold eta to the dynamic modulation matrix
Zooming is carried out, so that the tail end of the mechanical arm keeps a safe distance with the surface of the obstacle when moving along the surface of the obstacle,
using dynamic modulation matrices
Modulating the first-order dynamic system shown in the formula (13) to obtain the corrected tail end movement speed of the mechanical arm
6. The mechanical arm obstacle avoidance planning method based on the dynamic system and the Gaussian cluster ellipsoid as claimed in claim 1 or 2, characterized in that: the implementation method of the fourth step is that,
introducing a time step delta t, and setting the three-dimensional position coordinate of the tail end of the mechanical arm before the movement speed of the tail end of the mechanical arm to be xi t Updating the three-dimensional position coordinates of the tail end of the mechanical arm according to the corrected motion speed of the tail end of the mechanical arm,
according to the updated three-dimensional position coordinate xi of the tail end of the mechanical arm t+1 And controlling the motion of the mechanical arm.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211149409.0A CN115533897A (en) | 2022-09-21 | 2022-09-21 | Mechanical arm obstacle avoidance planning method based on dynamic system and Gaussian cluster ellipsoid |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211149409.0A CN115533897A (en) | 2022-09-21 | 2022-09-21 | Mechanical arm obstacle avoidance planning method based on dynamic system and Gaussian cluster ellipsoid |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115533897A true CN115533897A (en) | 2022-12-30 |
Family
ID=84727511
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211149409.0A Pending CN115533897A (en) | 2022-09-21 | 2022-09-21 | Mechanical arm obstacle avoidance planning method based on dynamic system and Gaussian cluster ellipsoid |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115533897A (en) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103020920A (en) * | 2013-01-10 | 2013-04-03 | 厦门大学 | Method for enhancing low-illumination images |
| CN103700089A (en) * | 2013-12-01 | 2014-04-02 | 北京航空航天大学 | Extracting and sorting method of multi-scale isomeric features of three-dimensional medical image |
| WO2018176593A1 (en) * | 2017-03-31 | 2018-10-04 | 深圳市靖洲科技有限公司 | Local obstacle avoidance path planning method for unmanned bicycle |
| CN110262478A (en) * | 2019-05-27 | 2019-09-20 | 浙江工业大学 | Man-machine safety obstacle-avoiding route planning method based on modified embedded-atom method |
| CN111168675A (en) * | 2020-01-08 | 2020-05-19 | 北京航空航天大学 | Dynamic obstacle avoidance motion planning method for mechanical arm of household service robot |
-
2022
- 2022-09-21 CN CN202211149409.0A patent/CN115533897A/en active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103020920A (en) * | 2013-01-10 | 2013-04-03 | 厦门大学 | Method for enhancing low-illumination images |
| CN103700089A (en) * | 2013-12-01 | 2014-04-02 | 北京航空航天大学 | Extracting and sorting method of multi-scale isomeric features of three-dimensional medical image |
| WO2018176593A1 (en) * | 2017-03-31 | 2018-10-04 | 深圳市靖洲科技有限公司 | Local obstacle avoidance path planning method for unmanned bicycle |
| CN110262478A (en) * | 2019-05-27 | 2019-09-20 | 浙江工业大学 | Man-machine safety obstacle-avoiding route planning method based on modified embedded-atom method |
| CN111168675A (en) * | 2020-01-08 | 2020-05-19 | 北京航空航天大学 | Dynamic obstacle avoidance motion planning method for mechanical arm of household service robot |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Jeong et al. | Data mining for aerodynamic design space | |
| CN108830931B (en) | Laser point cloud simplification method based on dynamic grid k neighborhood search | |
| Obayashi et al. | Multi-objective design exploration for aerodynamic configurations | |
| CN110543727B (en) | Improved particle swarm algorithm-based omnidirectional mobile intelligent wheelchair robot parameter identification method | |
| CN112308961B (en) | Robot rapid robust three-dimensional reconstruction method based on layered Gaussian mixture model | |
| CN110928189A (en) | Robust control method based on reinforcement learning and Lyapunov function | |
| CN107146237B (en) | Target tracking method based on online state learning and estimation | |
| CN108549866B (en) | Remote sensing airplane identification method based on dense convolutional neural network | |
| CN115222625A (en) | Laser radar point cloud denoising method based on multi-scale noise | |
| CN116486395A (en) | Space obstacle discrimination method based on Gaussian mixture model and depth-color image | |
| CN111028238A (en) | Robot vision-based three-dimensional segmentation method and system for complex special-shaped curved surface | |
| CN115291622A (en) | Obstacle avoidance unmanned aerial vehicle distributed formation fractional order sliding mode control method | |
| CN113627075B (en) | Projectile pneumatic coefficient identification method based on adaptive particle swarm optimization extreme learning | |
| CN114326810B (en) | Obstacle avoidance method of unmanned aerial vehicle in complex dynamic environment | |
| CN116306044A (en) | Uncertainty analysis method of full turbulence configuration and gradient optimization design method thereof | |
| CN115533897A (en) | Mechanical arm obstacle avoidance planning method based on dynamic system and Gaussian cluster ellipsoid | |
| Yuhang et al. | An Adaptive evolutionary multi-objective estimation of distribution algorithm and its application to multi-UAV path planning | |
| Liu et al. | A localizability estimation method for mobile robots based on 3d point cloud feature | |
| Kim et al. | Robust and fast 3-D scan registration using normal distributions transform with supervoxel segmentation | |
| CN115455670A (en) | non-Gaussian noise model establishment method based on Gaussian mixture model | |
| CN111986223B (en) | Method for extracting trees in outdoor point cloud scene based on energy function | |
| Zhang et al. | Object detection and grabbing based on machine vision for service robot | |
| Cui et al. | Weighted particle swarm clustering algorithm for self-organizing maps | |
| Ardilla et al. | Batch Learning Growing Neural Gas for Sequential Point Cloud Processing | |
| Lu et al. | A new unstructured hybrid mesh generation method based on BP-ANN |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |