CN116909274A - Robot autonomous man-machine collision prevention method and system - Google Patents
Robot autonomous man-machine collision prevention method and system Download PDFInfo
- Publication number
- CN116909274A CN116909274A CN202310854597.5A CN202310854597A CN116909274A CN 116909274 A CN116909274 A CN 116909274A CN 202310854597 A CN202310854597 A CN 202310854597A CN 116909274 A CN116909274 A CN 116909274A
- Authority
- CN
- China
- Prior art keywords
- robot
- model
- track
- constraint
- collision
- 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 84
- 230000002265 prevention Effects 0.000 title abstract description 8
- 239000011159 matrix material Substances 0.000 claims abstract description 51
- 238000005457 optimization Methods 0.000 claims abstract description 25
- 238000007781 pre-processing Methods 0.000 claims abstract description 25
- 238000001514 detection method Methods 0.000 claims abstract description 17
- 230000010354 integration Effects 0.000 claims abstract description 17
- 230000007613 environmental effect Effects 0.000 claims description 10
- 238000005070 sampling Methods 0.000 claims description 10
- 238000004364 calculation method Methods 0.000 claims description 8
- 230000008569 process Effects 0.000 claims description 8
- 238000013507 mapping Methods 0.000 claims description 7
- 238000006243 chemical reaction Methods 0.000 abstract description 3
- 230000009471 action Effects 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 7
- 238000001914 filtration Methods 0.000 description 4
- 230000004888 barrier function Effects 0.000 description 3
- 210000002310 elbow joint Anatomy 0.000 description 3
- 210000000323 shoulder joint Anatomy 0.000 description 3
- 210000003857 wrist joint Anatomy 0.000 description 3
- 239000004973 liquid crystal related substance Substances 0.000 description 2
- 238000013179 statistical model Methods 0.000 description 2
- 235000002566 Capsicum Nutrition 0.000 description 1
- 239000006002 Pepper Substances 0.000 description 1
- 235000016761 Piper aduncum Nutrition 0.000 description 1
- 235000017804 Piper guineense Nutrition 0.000 description 1
- 244000203593 Piper nigrum Species 0.000 description 1
- 235000008184 Piper nigrum Nutrition 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 210000001503 joint Anatomy 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000008288 physiological mechanism Effects 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 150000003839 salts Chemical class 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention relates to a robot autonomous man-machine collision prevention method and a system, wherein the method comprises the following steps: acquiring a depth image and the current state of the robot; performing point cloud conversion on the depth image and completing point cloud model preprocessing; according to the preprocessing point cloud model and the current action of the robot, estimating the distance between the obstacle and the robot, and detecting collision risk; according to the collision detection result, predicting a collision-free track of the robot at the next moment by adopting an environment dynamic model, and marking the collision-free track as an initial track; the environment dynamic model is determined by model prediction track integration which adopts a penalty function method to carry out soft constraint on collision; optimizing the initial track by adopting an iterative optimization method based on an equation constraint model of a projection matrix to obtain an optimized track; and determining and controlling the motion instruction of the robot according to the optimized track. The invention can effectively improve the safety and the working efficiency of the robot for carrying out man-machine cooperation tasks in an unknown dynamic multi-obstacle environment.
Description
Technical Field
The invention relates to the technical field of robots, in particular to an autonomous man-machine collision avoidance method and system for robots.
Background
Along with the gradual expansion of robots from the traditional industrial field to wider application scenes, the robots autonomously complete the man-machine collision avoidance tasks from the task level under the unknown dynamic environment and complex constraint, and become the necessary requirement for improving the man-machine cooperation co-fusion level. Inspired by a human hand-eye coordination motion mechanism, related technicians propose to adopt a depth vision and global motion planning method to enable the robot to have autonomous collision prevention capability. However, the current related method faces the key problems that the unknown environment is poor in perceived real-time performance, low in detection precision, difficult to consider real-time performance and optimality under complex constraint by a collision prevention strategy, and the like, and is difficult to be widely applied to actual operation scenes.
Disclosure of Invention
The invention aims to provide an autonomous man-machine collision prevention method and system for a robot, which improve the safety and the working efficiency of the robot in the man-machine cooperation process.
In order to achieve the above object, the present invention provides the following solutions:
the first aspect of the invention provides a robot autonomous man-machine collision avoidance method, which comprises the following steps:
step 101, obtaining obstacle information of the current surrounding environment of a robot and the current state of the robot;
102, converting a point cloud model and preprocessing the model according to the current environmental obstacle information;
step 103, according to the current state of the robot and the preprocessed point cloud model, adopting a collision distance estimation algorithm based on a robot skeleton model to obtain the collision distance between the robot and the obstacle point cloud model; the skeleton model is a plurality of joint connecting lines determined by adopting the positive kinematics of the robot;
104, obtaining a motion track of the collision-free robot by adopting a model prediction track integration method combined with a penalty function soft constraint based on the collision distance estimation result;
step 105, optimizing the motion trail of the collision-free robot by adopting a projection matrix hard constraint method to obtain an optimized trail meeting the constraint of a complex equation;
and 106, determining and controlling a motion instruction of the robot according to the optimized track.
Preferably, the step 103 further includes:
based on the motion mapping relation between the human arms and the skeletons thereof, skeletonizing the robot model to realize real-time full-arm collision distance estimation;
and obtaining the distance from the obstacle point to the robot by adopting a point-line distance calculation method.
Preferably, the step 104 further includes:
the dynamic penalty function method is adopted, the penalty coefficient value is adaptively adjusted through the evolutionary algorithm, and the optimization efficiency and the convergence rate of the algorithm are ensured;
constructing inequality constraint into an objective function of a model prediction model through the dynamic penalty function method;
randomly sampling the predicted track through a noise model, and obtaining the collision-free robot motion track by performing error analysis on the predicted track sample and the expected track.
Preferably, the step 105 further includes:
extracting a constraint matrix in a Cartesian space of the tail end of the robot according to the equality constraint relation of the tail end of the robot;
projecting the constraint matrix to a robot joint space and applying the constraint matrix to a noise model of the predicted trajectory sample, thereby realizing hard constraint of the projection matrix;
and updating the control quantity of the robot control system through the result of the soft constraint of the penalty function by adopting an iterative optimization method, then transmitting the control quantity to the projection matrix hard constraint model to process noise items, and transmitting the updated control quantity to the soft constraint.
Preferably, the environmental obstacle information is a depth image obtained by a depth camera, and the current state of the robot includes a joint angle value, a joint angular velocity value of the robot, and a preset cartesian space position of the robot tip as a desired task.
The second aspect of the invention provides an autonomous man-machine collision avoidance system of a robot, comprising:
the current robot data acquisition module is used for acquiring current environment obstacle information of the robot and the current state of the robot;
the point cloud preprocessing module is used for carrying out downsampling and denoising preprocessing on the obstacle point cloud model according to the current environment obstacle information;
the collision detection module is used for realizing collision detection of the robot on the obstacle by adopting a collision distance estimation algorithm based on a robot framework according to the current state of the robot and the preprocessing point cloud model;
the track prediction module is used for predicting a collision-free track of the robot at the next moment according to the environment dynamic model; the environment dynamic model is determined by model prediction track integration of soft constraint on collision by a penalty function method, the input of the penalty function method is inequality constraint based on collision distance, and the inequality constraint is acted in an objective function of a model prediction track integration algorithm to be used as one of optimization targets of a prediction track sample;
the track optimization module is used for optimizing the initial track by adopting an iterative optimization method based on an equality constraint model of the projection matrix to obtain an optimized track;
and the motion instruction determining module is used for determining and controlling the motion instruction of the robot according to the optimized track.
Preferably, the track prediction module obtains the collision-free motion track of the robot in a random sampling mode.
Preferably, the track optimization module optimizes the motion track of the collision-free robot by adopting a projection matrix hard constraint method to obtain an optimized track meeting the constraint of a complex equation.
Preferably, the method further comprises: the track optimization module also updates the control quantity of the robot by combining the result of the model prediction model of the penalty function soft constraint, then the control quantity is transmitted to a projection matrix hard constraint model for processing noise items of a predicted track sample, and the optimized control quantity is transmitted to the model prediction model to obtain the optimized robot motion track meeting the soft and hard constraints.
Preferably, the environmental obstacle information is a depth image obtained by a depth camera, and the current state of the robot includes a joint angle value, a joint angular velocity value of the robot and a preset cartesian space position of the robot tip as a desired task.
According to the specific embodiment provided by the invention, the invention discloses the following technical effects:
the invention discloses a robot autonomous man-machine collision prevention method and a system, wherein a man-machine distance estimation model based on a mechanical arm framework is built under the inspired of a human arm framework physiological mechanism, so that real-time collision detection of a full arm of a mechanical arm on a multi-obstacle point cloud model can be realized.
The invention also provides a random optimal collision avoidance strategy based on model prediction track integration, a penalty function and a projection matrix are introduced to respectively carry out soft and hard constraint on an objective function and a control quantity, a parallel computing framework is constructed to improve the real-time performance of the optimal strategy solving of the robot under complex constraint, and a new theory and method are provided for the refined operation and autonomous efficient collision avoidance of the robot under complex unknown environment.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings that are needed in the embodiments will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a schematic flow chart of a robot autonomous man-machine collision avoidance method according to the invention;
FIG. 2 is a flow chart of an embodiment of a robot autonomous man-machine collision avoidance method of FIG. 1;
FIG. 3 is a block flow diagram of the real-time collision detection section of FIG. 2 based on depth vision and a robotic skeleton;
FIG. 4 is a block flow diagram of a portion of FIG. 2 other than real-time collision detection;
fig. 5 is a schematic structural diagram of a robot autonomous man-machine collision avoidance system according to the present invention.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
The invention aims to provide an autonomous man-machine collision prevention method and system for a robot, which improve the safety and the working efficiency of robot-machine cooperation.
In order that the above-recited objects, features and advantages of the present invention will become more readily apparent, a more particular description of the invention will be rendered by reference to the appended drawings and appended detailed description.
FIG. 1 is a schematic flow chart of a robot autonomous man-machine collision avoidance method according to the invention; FIG. 2 is a flow chart of an embodiment of a robot autonomous man-machine collision avoidance method of FIG. 1; FIG. 3 is a block flow diagram of the real-time collision detection section of FIG. 2 based on depth vision and a robotic skeleton; FIG. 4 is a block flow diagram of a portion of FIG. 2 other than real-time collision detection; fig. 5 is a schematic structural diagram of a robot autonomous man-machine collision avoidance system according to the present invention. As shown in fig. 1 to 4, the autonomous man-machine collision avoidance method of the robot according to the present invention includes the steps of:
step 101: and acquiring barrier information of the current surrounding environment of the robot and the current state of the robot.
The obstacle information of the current surrounding environment of the robot is a depth image acquired by a depth camera.
The current state of the robot includes the joint angle value, the joint angular velocity value of the robot, and the cartesian space position of the target (joint end of preset joint).
In a specific embodiment, the robot is a robotic arm, and the current state parameter includes a joint angle value, a joint angular velocity value, and a cartesian space position of a distal end of the robotic arm.
Step 102: and according to the current environmental obstacle information, performing point cloud model conversion and model preprocessing.
The step 102 specifically includes: and converting the depth image obtained by the depth camera into an original point cloud model according to the coordinate system mapping relation. And reducing the data volume of the original point cloud model by adopting a voxelized downsampling method. And according to the noise characteristics of the point cloud model, eliminating sparse noise and outliers in the point cloud model by adopting a statistical filtering algorithm to obtain a preprocessed point cloud model.
According to an internal reference matrix kappa of the depth camera, a mapping relation from an image coordinate system to a camera coordinate system is established:
z c ＝d p
wherein f x 、f y 、c x 、c y Is an intrinsic coefficient of the depth camera, (x) c ,y c ,z c ) Is the three-dimensional coordinates of an actual point in the camera coordinate system, (p) x ,p y ) D is the corresponding pixel coordinate on the depth image p Is the depth value for this pixel coordinate. Then, by establishing the above-described mapping relationship, the depth image is used to generate a three-dimensional point cloud.
And in the preprocessing stage of the point cloud model, adopting a voxelized downsampling method to reduce the data volume of the point cloud model. Aiming at noise interference mainly comprising salt and pepper noise in the point cloud model, eliminating sparse noise and outliers in the global point cloud by adopting a statistical filtering algorithm to obtain a preprocessing point cloud model shown in fig. 3. A; the statistical model of the statistical filtering algorithm is as follows:
the statistical model is obeyed to be the average value mu d Variance is sigma d Gaussian distribution G (d).
Step 103: according to the current state of the robot and the preprocessing point cloud model, a collision distance estimation algorithm based on a robot framework model is adopted to obtain the distance between the robot and the obstacle point cloud model; the skeleton model is a plurality of joint connecting lines determined by adopting the positive kinematics of the robot. The method specifically comprises the following steps: and selecting the seven-degree-of-freedom redundant mechanical arm as the robot model, and skeletonizing the robot model under the inspired of the mapping relation between the human arm and the skeleton motion of the human arm. The robot joint has coaxiality in structure, and a two-link skeleton model is formed based on a shoulder joint, an elbow joint and a wrist joint. In order to remove the robot model from the preprocessing point cloud model and generate an obstacle point cloud model, a sphere envelope is adopted at the joint of the framework model in consideration of the slender structure of the robot model, and other parts adopt a cylinder envelope. The envelope is to include the robotic point cloud model of the preprocessed point cloud models within a specific sphere or cylinder shape, further separated from the obstacle point cloud model. And obtaining the distance from the obstacle point to the robot framework by adopting a point-line distance calculation method according to the obstacle point cloud model and the robot framework model.
Based on the motion mapping relation between the human arms and the skeletons thereof, the robot model is skeletonized so as to realize real-time full-arm collision distance estimation. In order to avoid the robot being used as an obstacle, the robot needs to be removed from the preprocessing point cloud model, and only the pose information of the obstacle is reserved, namely an obstacle space is formed. To achieve this object, the invention forms an envelope around the robot based on the current state of the robot and rejects it. The current state of the robot is obtained in real time through the robot controller and the kinematic calculation. Since the robot joints shoulder joint, elbow joint and wrist joint have structural coaxiality, i.e., J in fig. 3 (b) 2-4 And J 4-7 Thus based on joint J 2 、J 4 、J 7 Two connecting rod skeletons can be formed. Considering the slender structure of the robot, a sphere envelope is adopted at the joint of the framework, and other parts are enveloped by cylinders.
Considering an obstacle point O in the obstacle space, its Cartesian coordinates are O C ＝(x O ,y O ,z O ). Due to the similarity of the robot skeleton structures, only the first link of the robot skeleton in fig. 3 (b), i.e. the robot structure from joint 2 to joint 4, is considered here. Cartesian coordinates of joint 2 and joint 4 are J respectively 2C ＝(x J2 ,y J2 ,z J2 ) And J 4C ＝(x J4 ,y J4 ,z J4 ). The coordinates of the robot and the obstacle are both located in the same Cartesian coordinate system, i.e. the robot base coordinate system.
The distance OP of the obstacle point to the robot is obtained through a point-line distance calculation method, wherein the P point is the position closest to the obstacle on the robot:
wherein θ is a vector J 2 O and J 2 J 4 Is shown as J 2 O is in vector J 2 J 4 Projection in the direction and J 2 J 4 Proportion of I.
Step 104: and based on the collision distance estimation result, obtaining the robot motion trail without collision by adopting a model prediction trail integration method combined with a penalty function soft constraint.
Step 104 specifically includes: the dynamic penalty function method is adopted, the penalty coefficient value is adaptively adjusted through the evolutionary algorithm, and the optimization efficiency and the convergence rate of the algorithm are ensured; and then constructing the inequality collision avoidance constraint into an objective function of the model prediction model through the dynamic penalty function method. The model prediction model adopts a model prediction track integration algorithm based on an information theory, randomly samples a prediction track through a Gaussian noise model, and performs error analysis on a prediction track sample and an expected track by utilizing a KL divergence theory in the information theory. The objective function result of the predicted track sample needs to be calculated by utilizing the result of the collision distance estimation algorithm, wherein the result comprises the nearest barrier distance, the nearest barrier point and the most dangerous position on the robot structure, then the weight calculation is carried out on the predicted track sample by adopting an importance sampling method, and the optimized track meeting the inequality soft constraint is obtained by adopting a sample summation mode.
And constructing the inequality constraint into an objective function of a model prediction model through a dynamic penalty function method, and obtaining the collision-free robot motion trail through a random sampling mode.
The inequality constraint is expressed as:
wherein, the liquid crystal display device comprises a liquid crystal display device,for an initial position x 0 U and x are control quantity and state quantity respectively, and the predicted track sample is generated by a robot control model; t is t 0 ...t f From t 0 To t f Is a time range of (2); r is a hypothesized positive symmetric cost matrix; phi is the final time t f Cost of the robot state. Inequality constraint is collision avoidance constraint, expressed as robot position r robot To the obstacle position r obstacle Is less than the risk threshold p.
The dynamic penalty function method is to adaptively adjust penalty coefficients through an intelligent algorithm, so as to ensure the optimization efficiency and convergence rate of the penalty function method, and the dynamic penalty function method can be expressed as:
F(x,ξ)＝f(x)+∑ξ j g j (x),
where f (x) is an objective function that does not take into account constraints, ζ j For dynamic penalty coefficient g j (x) For the inequality constraint, j represents the j-th constraint. The dynamic penalty coefficient is adaptively adjusted through an intelligent evolutionary algorithm, and zeta is calculated by the algorithm 1 And xi 2 Is in the range of [0,1]An internal constant; m is m j For the total number of j-th constraint violations at this time, m A For all constraintsTotal number of violations; n is the number of feasible solutions in the population at this time, and N is the size of the population.
The model prediction model is a model prediction track integration algorithm based on an information theory, a prediction track is randomly sampled through a Gaussian noise model, and an error analysis is carried out on a prediction track sample and an expected track by utilizing the KL divergence theory of the information theory. The objective function of the sample includes a desired function and a constraint function that go to a desired location. And then, carrying out weight calculation on the predicted track sample by adopting an importance sampling method, and obtaining the collision-free motion track meeting inequality constraint by adopting a sample summation mode. The core model comprises the following contents:
wherein S is k K represents a kth track prediction sample for a track error obtained by the KL divergence theory; omega k A weight of a sample is predicted for each trajectory obtained by the importance sampling method.
Step 105: and optimizing the motion track of the collision-free robot by adopting a projection matrix hard constraint method to obtain an optimized track meeting the constraint of a complex equation.
Step 105 specifically includes: and extracting a constraint matrix in the Cartesian space of the tail end of the robot according to the equation constraint relation of the tail end of the robot, and then projecting the constraint matrix into a robot joint space and applying the constraint matrix to a noise model of the predicted track sample, so that the hard constraint of the projection matrix is realized. In order to process soft constraint and hard constraint simultaneously, an iterative optimization method is adopted, the control quantity is updated through the result of the soft constraint of the penalty function, then the control quantity is transmitted to the projection matrix hard constraint model to process noise items, and the updated control quantity is transmitted to the soft constraint again. And the control quantity of the predicted track sample simultaneously meets the equality constraint of the tail end of the robot and the inequality constraint of collision avoidance in the iterative optimization process, so that the processing capacity of the collision avoidance strategy on the complex constraint is ensured.
And updating the control quantity according to the result of the step 104, transmitting the control quantity to a noise item in a projection matrix hard constraint model for processing a predicted track sample, and transmitting the optimized control quantity to the model prediction model of the step 104 to obtain the optimized robot motion track meeting the soft and hard constraints.
The projection matrix hard constraint model is as follows: and calculating to obtain a constraint matrix of the Cartesian space of the tail end of the robot according to the equality constraint relation of the tail end of the robot, and finally obtaining a projection matrix hard constraint model projected to the joint space of the robot. Wherein the mathematical expression of the model is:
v t ＝u t +ε t ,c(t,x)+D(t,x)u＝0
wherein v is t 、u t 、ε t The input quantity, the control quantity and the noise item of the robot control system at the time t are provided; c (t, x) +d (t, x) u=0 is the equality constraint relation of the robot end, and matrix D is a constraint matrix;for the projection constraint matrix of the robot joint space, gamma J The matrix R is a control cost matrix, which is a constant coefficient.
The control amount optimizing process is that the control amount is updated by importance sampling in step 104, and then the control amount is optimized by noise item constraint in step 105 and transferred to the model predictive model in step 104. The concrete steps are as follows:
and continuously optimizing the control quantity to ensure that the control quantity simultaneously meets soft constraint from a penalty function and hard constraint from a projection constraint matrix, and outputting the optimized robot motion trail.
Step 106: and determining and controlling a motion instruction of the robot according to the optimized track.
The invention acquires the environment obstacle information and the current state of the robot; according to the environmental obstacle information, performing point cloud model conversion and preprocessing the model; according to the current state of the robot and the preprocessing point cloud model, adopting a collision distance estimation algorithm based on the robot framework model to realize collision detection of the robot; and solving the optimal control action of the robot by adopting a model prediction track integration method according to the collision detection result, the dynamic penalty function method, the projection constraint matrix method and the preset reward function, and outputting an instruction to control the robot to avoid the obstacle and complete the expected task. The autonomous man-machine collision avoidance method of the robot is an autonomous real-time collision avoidance method of the robot, and effectively improves the safety and the working efficiency of a physical robot for carrying out man-machine cooperation tasks in an unknown dynamic multi-obstacle environment.
Fig. 5 is a schematic structural diagram of an autonomous man-machine collision avoidance system of a robot according to the present invention, as shown in fig. 5, the autonomous man-machine collision avoidance system of a robot includes:
the current robot data acquisition module 201 is configured to acquire current environmental obstacle information of a robot and a current state of the robot.
The point cloud preprocessing module 202 is configured to perform downsampling and denoising preprocessing on the obstacle point cloud model according to the current environmental obstacle information.
And the collision detection module 203 is configured to implement collision detection of the robot on the obstacle by adopting a collision distance estimation algorithm based on a robot skeleton according to the current state of the robot and the preprocessing point cloud model.
The track prediction module 204 is used for predicting a collision-free track of the robot at the next moment according to the environment dynamic model; the environment dynamic model is determined by model prediction track integration of soft constraint on collision by a penalty function method, and the input of the penalty function method is inequality constraint based on collision distance and acts in an objective function of a model prediction track integration algorithm to be used as one of optimization targets of the prediction track sample.
The track optimization module 205 is configured to optimize the initial track by using an iterative optimization method based on an equality constraint model of the projection matrix, and obtain an optimized track.
And the motion instruction determining module 206 is used for determining and controlling the motion instruction of the robot according to the optimized track.
The point cloud preprocessing module 202 specifically includes:
and the point cloud preprocessing unit is used for generating an original point cloud model according to the depth image of the depth camera, and carrying out downsampling and denoising on the original point cloud model by adopting a voxelized downsampling and statistical filtering algorithm to obtain a preprocessed point cloud model.
The collision detection module 203 specifically includes:
and the collision detection unit is used for removing the robot model in the preprocessing point cloud model through an enveloping method according to the robot skeletonizing method, generating an obstacle point cloud model and estimating the distance between the robot skeletonizing model and the obstacle point cloud model.
The robot skeletonization method is characterized in that a robot point cloud model is converted into a two-connecting-rod skeleton model, and the end points of connecting rods are Cartesian space positions of a shoulder joint, an elbow joint and a wrist joint of the robot under a robot base coordinate system;
according to the structural characteristics of the robot, in the preprocessing point cloud model, the joint structure and the connecting rod structure of the robot are respectively packaged by adopting spheres and cylinders, and an obstacle point cloud model only comprising obstacle information is extracted.
The distance estimation process adopts a point-line distance calculation method to obtain the distance (OP) of an obstacle point to a robot, wherein the O point is an arbitrary obstacle point in the obstacle space, and the P point is the nearest position to the obstacle on the robot:
wherein θ is a vector J 2 O and J 2 J 4 Is shown as J 2 O is in vector J 2 J 4 Projection in the direction and J 2 J 4 Proportion of I.
The track prediction module 204 specifically includes:
and the track prediction unit is used for predicting the collision-free track of the robot at the next moment according to the environment dynamic model.
The environment dynamic model is determined by model prediction track integration of soft constraint on collision by a penalty function method, and the input of the penalty function method is inequality constraint based on collision distance and acts in an objective function of a model prediction track integration algorithm to be used as one of optimization targets of the prediction track sample.
The penalty function is a dynamic penalty function that takes into account adaptive penalty coefficients, and may be expressed as:
F(x,ξ)＝f(x)+∑ξ j g j (x),
where f (x) is an objective function that does not take into account constraints, ζ j For dynamic penalty coefficient g j (x) For the inequality constraint, j represents the j-th constraint. The dynamic penalty coefficient is adaptively adjusted through an intelligent evolutionary algorithm, and zeta is calculated by the algorithm 1 And xi 2 Is in the range of [0,1]An internal constant; m is m j For the total number of j-th constraint violations at this time, m A Total number of violations for all constraints; n is the number of feasible solutions in the population at this time, and N is the size of the population.
The model prediction model is a model prediction track integration algorithm based on an information theory, and the core model comprises the following contents:
wherein S is k K represents a kth track prediction sample for a track error obtained by the KL divergence theory; omega k A weight of a sample is predicted for each trajectory obtained by the importance sampling method.
The track optimization module 205 specifically includes:
and the track optimization unit is used for obtaining a constraint matrix according to the equation constraint relation of the robot and a projection matrix method and applying the constraint matrix to the predicted track sample so as to obtain an optimized track.
The projection matrix hard constraint model is as follows: and calculating to obtain a constraint matrix of the Cartesian space of the tail end of the robot according to the equality constraint relation of the tail end of the robot, and finally obtaining a projection matrix hard constraint model projected to the joint space of the robot. Wherein the mathematical expression of the model is:
v t ＝u t +ε t ,c(t,x)+D(t,x)u＝0
wherein v is t 、u t 、ε t The input quantity, the control quantity and the noise item of the robot control system at the time t are provided; c (t, x) +d (t, x) u=0 is the equality constraint relation of the robot end, and matrix D is a constraint matrix;for the projection constraint matrix of the robot joint space, gamma J The matrix R is a control cost matrix, which is a constant coefficient.
The environment obstacle information is a depth image obtained by a depth camera, and the current state of the robot comprises a joint angle value, a joint angular velocity value and a preset Cartesian space position of the tail end of the robot serving as a desired task.
In the present specification, each embodiment is described in a progressive manner, and each embodiment is mainly described in a different point from other embodiments, and identical and similar parts between the embodiments are all enough to refer to each other. For the system disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant points refer to the description of the method section.
The principles and embodiments of the present invention have been described herein with reference to specific examples, the description of which is intended only to assist in understanding the methods of the present invention and the core ideas thereof; also, it is within the scope of the present invention to be modified by those of ordinary skill in the art in light of the present teachings. In view of the foregoing, this description should not be construed as limiting the invention.
Claims (10)
1. An autonomous man-machine collision avoidance method for a robot, comprising the steps of:
step 101, obtaining obstacle information of the current surrounding environment of a robot and the current state of the robot;
102, converting a point cloud model and preprocessing the model according to the current environmental obstacle information;
step 103, according to the current state of the robot and the preprocessed point cloud model, adopting a collision distance estimation algorithm based on a robot skeleton model to obtain the collision distance between the robot and the obstacle point cloud model; the skeleton model is a plurality of joint connecting lines determined by adopting the positive kinematics of the robot;
104, obtaining a motion track of the collision-free robot by adopting a model prediction track integration method combined with a penalty function soft constraint based on the collision distance estimation result;
step 105, optimizing the motion trail of the collision-free robot by adopting a projection matrix hard constraint method to obtain an optimized trail meeting the constraint of a complex equation;
and 106, determining and controlling a motion instruction of the robot according to the optimized track.
2. The autonomous human-machine collision avoidance method of claim 1, wherein the step 103 further comprises:
based on the motion mapping relation between the human arms and the skeletons thereof, skeletonizing the robot model to realize real-time full-arm collision distance estimation;
and obtaining the distance from the obstacle point to the robot by adopting a point-line distance calculation method.
3. The robotic autonomous human-machine collision avoidance method of claim 1, wherein the step 104 further comprises:
the dynamic penalty function method is adopted, the penalty coefficient value is adaptively adjusted through the evolutionary algorithm, and the optimization efficiency and the convergence rate of the algorithm are ensured;
constructing inequality constraint into an objective function of a model prediction model through the dynamic penalty function method;
randomly sampling the predicted track through a noise model, and obtaining the collision-free robot motion track by performing error analysis on the predicted track sample and the expected track.
4. The robotic autonomous human-machine collision avoidance method of claim 1, wherein the step 105 further comprises:
extracting a constraint matrix in a Cartesian space of the tail end of the robot according to the equality constraint relation of the tail end of the robot;
projecting the constraint matrix to a robot joint space and applying the constraint matrix to a noise model of the predicted trajectory sample, thereby realizing hard constraint of the projection matrix;
and updating the control quantity of the robot control system through the result of the soft constraint of the penalty function by adopting an iterative optimization method, then transmitting the control quantity to the projection matrix hard constraint model to process noise items, and transmitting the updated control quantity to the soft constraint.
5. The autonomous robot man-machine collision avoidance method according to any of claims 1-4, wherein the environmental obstacle information is a depth image obtained by a depth camera, and the current state of the robot includes a joint angle value, a joint angular velocity value of the robot, and a cartesian space position of a robot tip preset as a desired task.
6. An autonomous robot man-machine collision avoidance system, comprising:
the current robot data acquisition module is used for acquiring current environment obstacle information of the robot and the current state of the robot;
the point cloud preprocessing module is used for carrying out downsampling and denoising preprocessing on the obstacle point cloud model according to the current environment obstacle information;
the collision detection module is used for realizing collision detection of the robot on the obstacle by adopting a collision distance estimation algorithm based on a robot framework according to the current state of the robot and the preprocessing point cloud model;
the track prediction module is used for predicting a collision-free track of the robot at the next moment according to the environment dynamic model; the environment dynamic model is determined by model prediction track integration of soft constraint on collision by a penalty function method, the input of the penalty function method is inequality constraint based on collision distance, and the inequality constraint is acted in an objective function of a model prediction track integration algorithm to be used as one of optimization targets of a prediction track sample;
the track optimization module is used for optimizing the initial track by adopting an iterative optimization method based on an equality constraint model of the projection matrix to obtain an optimized track;
and the motion instruction determining module is used for determining and controlling the motion instruction of the robot according to the optimized track.
7. The autonomous robot man-machine collision avoidance system of claim 6 wherein the trajectory prediction module obtains the collision-free motion trajectory of the robot by way of random sampling.
8. The autonomous man-machine collision avoidance system of claim 6 wherein the trajectory optimization module optimizes the collision-free robot motion trajectory using a projection matrix hard constraint method to obtain an optimized trajectory satisfying complex equality constraints.
9. The robotic autonomous human-machine collision avoidance system of claim 8, further comprising: the track optimization module also updates the control quantity of the robot by combining the result of the model prediction model of the penalty function soft constraint, then the control quantity is transmitted to a projection matrix hard constraint model for processing noise items of a predicted track sample, and the optimized control quantity is transmitted to the model prediction model to obtain the optimized robot motion track meeting the soft and hard constraints.
10. The robotic autonomous human-machine collision avoidance system of claim 6, wherein the environmental obstacle information is a depth image obtained by a depth camera; the current state of the robot comprises a joint angle value and a joint angular velocity value of the robot and a preset Cartesian space position of the tail end of the robot serving as a desired task.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310854597.5A CN116909274A (en) | 2023-07-12 | 2023-07-12 | Robot autonomous man-machine collision prevention method and system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310854597.5A CN116909274A (en) | 2023-07-12 | 2023-07-12 | Robot autonomous man-machine collision prevention method and system |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116909274A true CN116909274A (en) | 2023-10-20 |
Family
ID=88361105
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310854597.5A Pending CN116909274A (en) | 2023-07-12 | 2023-07-12 | Robot autonomous man-machine collision prevention method and system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116909274A (en) |
-
2023
- 2023-07-12 CN CN202310854597.5A patent/CN116909274A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Chen et al. | A deep reinforcement learning based method for real-time path planning and dynamic obstacle avoidance | |
| US9981389B2 (en) | Robotics platforms incorporating manipulators having common joint designs | |
| WO2018107851A1 (en) | Method and device for controlling redundant robot arm | |
| CN104057450B (en) | A kind of higher-dimension motion arm teleoperation method for service robot | |
| US20210299860A1 (en) | Method and system for robot action imitation learning in three-dimensional space | |
| CN110682286B (en) | Real-time obstacle avoidance method for cooperative robot | |
| Zhang et al. | Sim2real learning of obstacle avoidance for robotic manipulators in uncertain environments | |
| CN113442140B (en) | Cartesian space obstacle avoidance planning method based on Bezier optimization | |
| CN111152227A (en) | Mechanical arm control method based on guided DQN control | |
| Shi et al. | A distributed reward algorithm for inverse kinematics of arm robot | |
| CN116572258B (en) | Dynamic obstacle avoidance control method for welding robot and computer readable storage medium | |
| CN116909274A (en) | Robot autonomous man-machine collision prevention method and system | |
| Wang et al. | An end-to-end trajectory planning strategy for free-floating space robots | |
| CN116604557A (en) | Double-arm robot cooperative collision avoidance planning optimization method in unstructured environment | |
| CN1672881A (en) | On-line robot hand and eye calibrating method based on motion selection | |
| CN113359744B (en) | Robot obstacle avoidance system based on safety reinforcement learning and visual sensor | |
| Leng et al. | Flexible online planning based residual space object de-spinning for dual-arm space-borne maintenance | |
| CN113510699A (en) | Mechanical arm motion trajectory planning method based on improved ant colony optimization algorithm | |
| CN113146637A (en) | Robot Cartesian space motion planning method | |
| Jiang et al. | Globally perceived obstacle avoidance for robots based on virtual twin and deep reinforcement learning | |
| Hong et al. | Research of robotic arm control system based on deep learning and 3D point cloud target detection algorithm | |
| Xiang et al. | Research on Key Technology of Logistics Sorting Robot | |
| Sithamparanathan et al. | Pose Estimation of a Robot Arm from a Single Camera | |
| CN114083545B (en) | Moving object robot grabbing method and device based on visual perception | |
| Hirana et al. | Quantitative evaluation for skill controller based on comparison with human demonstration |
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 |