Mechanical arm path planning method and device based on stereoscopic vision

Abstract

The invention relates to the technical field of intelligent information, in particular to a mechanical arm path planning method and device based on stereoscopic vision, and the method comprises the steps: obtaining an environment map of a mechanical arm operation scene; dynamic obstacle point clouds in the mechanical arm operation scene are extracted; the dynamic obstacle point cloud closest to the control point on the mechanical arm is set as the closest obstacle point, and the motion state of the closest obstacle point is tracked; based on the environment map of the mechanical arm operation scene, a global path of the mechanical arm is planned in an off-line mode; and based on the nearest obstacle point, an artificial potential field method is adopted to make the mechanical arm perform real-time obstacle avoidance on the dynamic obstacle, and the operation speed of the mechanical arm is output. The environment adaptability and operation safety of the mechanical arm are enhanced, the task completion efficiency of the mechanical arm is improved, and the operation cost is reduced.

Description

Technical Field

[0001] The present invention relates to the field of intelligent information technology, and in particular to a robot arm path planning method and device based on stereoscopic vision. Background Art

[0002] As an entity that interacts with the real physical world, every action performed by the robot needs to be based on a planning algorithm. The robot path planning problem is to find a path connecting the starting point to the target point in a known environment according to certain indicators. The path consists of a series of discrete robot posture path points. Collaborative robotic arms need to have the ability to autonomously plan paths in the environment. They can perform global path planning based on known environmental information, and can also perform local obstacle avoidance path planning in a timely manner when encountering unknown obstacles.

[0003] When environmental information is known, the existing path planning method of collaborative robotic arms can achieve a certain degree of autonomous path planning, which can be specifically classified into global planning and local planning according to different scenarios and purposes.

[0004] In the global path planning problem, it is necessary to know the obstacle location information of the entire environment in advance. Random sampling search path planning methods represented by RRT and PRM have good results. The random sampling search path planning method avoids the modeling of complex environments and only needs to perform collision detection to determine whether the generated path is feasible. However, the path directly obtained by this method is usually tortuous, and it can only obtain the task execution path of the collaborative robot arm in a static known environment. For dynamic obstacles that are unknown in advance, such as workers on the collaborative assembly line, it is impossible to avoid them in real time.

[0005] Among the local path planning methods, the potential field method has a good application in collaborative manipulators. The potential field method has a simple solution method, good real-time performance, and low dependence on obstacle models. However, the potential field method also has some inherent defects. It is easy to fall into local minima and cannot generate complex manipulator trajectories. It has some obvious limitations when facing complex dynamic environments. Summary of the invention

[0006] To this end, the technical problem to be solved by the present invention is to overcome the problems in the prior art that the path planning of the collaborative robot arm is low in efficiency and easy to collide with dynamic obstacles.

[0007] In order to solve the above technical problems, the present invention provides a robot arm path planning method based on stereo vision, comprising:

[0008] Obtain the environmental map of the robot arm operation scene;

[0009] Extract dynamic obstacle point clouds in robot arm operation scenes;

[0010] Set the dynamic obstacle point cloud closest to the control point on the robot arm as the nearest obstacle point, and track the nearest obstacle point;

[0011] Offline planning of the global path of the robot arm based on the environmental map of the robot arm's operation scene;

[0012] When the robot moves along the global path, the artificial potential field method is used to enable the robot to avoid dynamic obstacles in real time based on the nearest obstacle point, and the running speed of the robot is output.

[0013] Preferably, extracting the dynamic obstacle point cloud in the robot arm operation scene includes:

[0014] Set the robotic arm to different positions, take and store multiple original depth images without dynamic obstacles offline;

[0015] Filter out the robotic arm from the original depth map to obtain a background depth map after filtering out the robotic arm;

[0016] Compare multiple background depth images after filtering out the robotic arm, retain the maximum depth value, and obtain a static background depth image;

[0017] The depth map with dynamic obstacles is collected in real time using a depth camera. The robot arm in the depth map with dynamic obstacles is filtered out and compared with the static background depth map to obtain the dynamic obstacle depth map.

[0018] The dynamic obstacle depth map is transformed according to the internal parameters of the depth camera to obtain the dynamic obstacle point cloud in the camera coordinate system.

[0019] Preferably, Kalman filtering is used to track the nearest obstacle point.

[0020] Preferably, the offline planning of the global path of the robot arm based on the environment map of the robot arm operation scene includes:

[0021] Based on the RRT_Connect algorithm, the initial global path of the robot arm is planned offline in the joint space;

[0022] Use the greedy algorithm to eliminate redundant points in the initial global path;

[0023] The cubic spline curve is used to interpolate and smooth the global path optimized by the greedy algorithm to obtain the target global path.

[0024] Preferably, when the robot arm moves along the global path, based on the nearest obstacle point, an artificial potential field method is used to instruct the robot arm to avoid dynamic obstacles in real time, and the running speed of the robot arm is output, including:

[0025] An attraction velocity potential field is set in the joint space where each path point of the global path is located. When the robot moves to the next path point according to the global path, the attraction velocity is calculated according to the Euclidean distance between the current position of the robot and the next path point.

[0026] Set the repulsive velocity potential field at the nearest obstacle point and calculate the repulsive velocity;

[0027] The repulsion speed in Cartesian space is converted to joint space to obtain the joint obstacle avoidance speed of the robot arm; the joint attraction speed of the robot arm is set as the attraction speed; the joint attraction speed and the joint obstacle avoidance speed are superimposed to obtain the running speed of the robot arm.

[0028] Preferably, setting an attraction velocity potential field in the joint space where each path point of the global path is located and calculating the attraction velocity comprises:

[0029] Set the attractive velocity potential field function to:

[0030]

[0031] Among them, d goal is the Euclidean distance between the current position of the robot arm joint space and the next path point, and is the variable of the attractive velocity potential field function; K goal represents the maximum value of the attraction velocity, α represents the critical distance; f(d goal ) represents the magnitude of the attractive velocity potential field;

[0032] Calculate the attraction speed using the formula:

[0033]

[0034] in, To attract speed, The direction vector pointing from the current robot arm joint space position to the next path point.

[0035] Preferably, the step of setting a repulsion velocity potential field at the nearest obstacle point and calculating the repulsion velocity comprises:

[0036] Set the repulsive velocity potential field function to:

[0037]

[0038] Among them, K obs represents the maximum value of the repulsion velocity, ρ represents the radius of the spherical obstacle detection area, and β represents the drop constant; d obs Represents the distance between the robot control point and the nearest obstacle point, f obs (d obs ) represents the magnitude of the repulsive velocity potential field;

[0039] Calculate the repulsion velocity using the formula:

[0040]

[0041] in, is the repulsion velocity, The direction vector pointing from the nearest obstacle point to the robot control point.

[0042] Preferably, after obtaining the running speed of the robot arm, the joint obstacle avoidance speed of the robot arm is projected onto the global path, and the running speed of the robot arm is optimized, including:

[0043] For the path points in the global path, suppose that from the path point p l To the path point p l+1 The connection line is Will As a linear space A, the projection matrix P of the linear space A is A for:

[0044] P A =A(A T A) -1 A T

[0045] in,

[0046] Projecting the joint obstacle avoidance speed to the linear space A, the obstacle avoidance projection speed is:

[0047]

[0048] in, is the joint obstacle avoidance speed, Projection speed for obstacle avoidance;

[0049] The running speed of the optimized robotic arm is the combined speed of the obstacle avoidance projection speed and the joint attraction speed.

[0050] Preferably, an obstacle avoidance strategy is added during the operation of the robotic arm, and the obstacle avoidance strategy includes:

[0051] When the joint attraction speed and obstacle avoidance projection speed are in the same direction, the running speed of the robot arm is the joint attraction speed;

[0052] When the joint attraction speed and the obstacle avoidance projection speed are in different directions, if the modulus of the obstacle avoidance projection speed is greater than the modulus of the joint attraction speed, the running speed of the robot arm is 0; otherwise, the running speed of the robot arm is the combined speed of the joint attraction speed and the obstacle avoidance projection speed.

[0053] The present invention also provides a robot arm path planning device based on stereo vision, comprising:

[0054] A map acquisition module is used to obtain an environmental map of the robot arm operation scene;

[0055] Dynamic obstacle point cloud extraction module, used to extract dynamic obstacle point clouds in the robot arm operation scene;

[0056] The dynamic obstacle tracking module is used to set the dynamic obstacle point cloud closest to the control point on the robot arm as the nearest obstacle point and track the nearest obstacle point;

[0057] The global path planning module is used to plan the global path of the robot arm offline based on the static environment map of the robot arm operation scene;

[0058] The real-time obstacle avoidance module is used to enable the robot to avoid dynamic obstacles in real time based on the nearest obstacle point and output the running speed of the robot when the robot moves along the global path.

[0059] The above technical solution of the present invention has the following beneficial effects compared with the prior art:

[0060] The present invention discloses a method for robot path planning based on stereo vision. On the basis of acquiring static environment and dynamic obstacle information, the method first plans a global path based on the static environment, and then uses an artificial potential field method based on dynamic obstacle information, i.e., the nearest obstacle point, to achieve real-time obstacle avoidance of the robot for dynamic obstacles, thereby improving the efficiency of the robot path planning, enabling the robot to have the ability to avoid obstacles autonomously, actively perceive the surrounding dynamic environment and make autonomous plans, and at the same time improving the efficiency of the robot path planning. The present invention significantly enhances the environmental adaptability and operational safety of the robot, improves the task completion efficiency of the robot, and reduces the operating cost. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] In order to make the content of the present invention more clearly understood, the present invention is further described in detail below according to specific embodiments of the present invention in conjunction with the accompanying drawings, wherein:

[0062] Figure 1 It is a flow chart of a robot arm path planning method based on stereo vision of the present invention;

[0063] Figure 2 It is the flow chart of dynamic obstacle point cloud extraction;

[0064] Figure 3 It is a schematic diagram of using a greedy algorithm to eliminate redundant points in the initial global path;

[0065] Figure 4 It is a schematic diagram of a cubic spline curve;

[0066] Figure 5 is the attractive velocity potential field curve;

[0067] Figure 6 is the repulsive velocity potential field curve;

[0068] Figure 7 It is a curve diagram of the joint positions of the robot arm during operation in the second embodiment. DETAILED DESCRIPTION

[0069] The present invention is further described below in conjunction with the accompanying drawings and specific embodiments so that those skilled in the art can better understand the present invention and implement it, but the embodiments are not intended to limit the present invention.

[0070] Embodiment 1

[0071] Reference Figure 1 As shown, the present invention provides a robot arm path planning method based on stereo vision, the steps comprising:

[0072] S10, obtaining an environmental map of the robot arm operation scene;

[0073] S20, extracting the dynamic obstacle point cloud in the robot arm operation scene;

[0074] S30, setting the dynamic obstacle point cloud closest to the control point on the robot arm as the nearest obstacle point, and tracking the nearest obstacle point;

[0075] S40, offline planning of the global path of the robotic arm based on the static environment map of the robotic arm operation scene;

[0076] S50. When the robot arm moves along the global path, based on the nearest obstacle point, an artificial potential field method is used to instruct the robot arm to avoid dynamic obstacles in real time, and the running speed of the robot arm is output.

[0077] The following is a detailed introduction to each step.

[0078] Most of the existing technologies extract the point cloud of dynamic obstacles by collecting the depth map of the robot arm operation scene in real time. The depth map of the robot arm operation scene mainly includes three parts, namely the static scene, the robot arm, and the dynamic collaborator, among which the dynamic collaborator is the dynamic obstacle. In order to extract the point cloud of the dynamic obstacle, the static scene and the robot arm need to be filtered out. Since the method of directly extracting and filtering the point cloud is usually complicated and may sacrifice the real-time performance of the system, the present invention first processes the collected depth map of the operation scene, first extracts the information of the dynamic obstacle part in the depth map, and then converts it into a point cloud.

[0079] Specifically, refer to Figure 2 As shown, in step S20, the dynamic obstacle point cloud in the robot arm operation scene is extracted, including:

[0080] S201, setting the robotic arm to different postures, offline capturing and storing multiple original depth maps without dynamic obstacles;

[0081] S202, filtering out the mechanical arm from the original depth map to obtain a background depth map after filtering out the mechanical arm, the formula is:

[0082]

[0083] Among them, I bg (x, y) represents the depth of the pixel with coordinates (x, y) in the background depth map after filtering out the robotic arm, I raw (x, y) represents the depth of the pixel with coordinates (x, y) in the original depth map, I robot (x, y) represents the depth of the pixel with coordinates (x, y) in the robot arm depth map obtained in the simulation environment;

[0084] S203, compare the multiple background depth images after filtering out the robotic arm, retain the maximum depth, and obtain a static background depth image. The formula is:

[0085] I static_bg (x,y)=max i<times I i_bg (x,y)

[0086] Among them, I static_bg (x, y) is the depth of the pixel with coordinates (x, y) in the static background depth map, times represents the total number of original depth maps captured, I i_bg (x, y) represents the depth of the pixel with coordinates (x, y) in the i-th background depth map after the robotic arm is filtered out;

[0087] S204, using a depth camera to collect a depth map with dynamic obstacles in real time, filtering out the robotic arm in the depth map with dynamic obstacles, and comparing it with the static background depth map to obtain a dynamic obstacle depth map, the formula is:

[0088]

[0089] The formula means that the real-time collected background depth map after filtering out the robotic arm is compared with the static background depth map. If the depth value is less than the depth value of the corresponding pixel in the static background depth map, the depth value is the depth of the dynamic obstacle; where I obs (x,y) represents the depth of the pixel with coordinates (x,y) in the dynamic obstacle depth map;

[0090] S205 . Convert the dynamic obstacle depth map according to the internal parameters of the depth camera to obtain a dynamic obstacle point cloud in the camera coordinate system.

[0091] The dynamic obstacle point cloud is usually a collection of more than 300 points, and all of them cannot be directly used as obstacle information. Therefore, it is necessary to search on the point cloud set closest to the robotic arm and track the dynamic obstacle point cloud closest to the control point on the robotic arm as the nearest obstacle point.

[0092] Although the nearest obstacle point can be found in the dynamic obstacle point cloud through direct search, the nearest obstacle point calculated in this way only relies on the depth camera perception information, which can easily be deviated due to edge noise, image communication delays, or changes in the movement of dynamic obstacles.

[0093] In order to obtain more accurate information about the nearest obstacle point, this embodiment uses Kalman filtering to track the nearest obstacle point in S30. The specific steps include:

[0094] Assume that the motion model of the nearest obstacle point in three-dimensional space is a linear model, and the formula is:

[0095]

[0096] Among them, x t ,y t and z t is the position coordinate of the nearest obstacle point at time t, x t-1 ,y t-1 and z t-1 is the position coordinate of the nearest obstacle point at time t-1, and are the velocities of the nearest obstacle point on the x-axis, y-axis, and z-axis at time t-1, respectively, and Δt represents the time interval;

[0097] The motion state equation of the nearest obstacle point is constructed based on the motion model of the nearest obstacle point in three-dimensional space:

[0098]

[0099] The state equation of the nearest obstacle point is expressed in matrix form:

[0100]

[0101] in, represents the state of the nearest obstacle point at time t, represents the state of the nearest obstacle point at time t-1, and B represents the state transfer matrix;

[0102] The prediction equation for constructing the mean square error is:

[0103]

[0104] Among them, P t-1represents the posterior estimated covariance at time t-1, represents the prior estimated covariance at time t, Q represents the system noise covariance matrix;

[0105] The Kalman filter state update equation for constructing the nearest obstacle point is:

[0106]

[0107] Among them, K t represents the Kalman gain at time t, H is the transfer matrix from state quantity to observation quantity, which is the unit matrix here, R is the observation noise covariance matrix; I represents the unit matrix; Z t Represents the observation value of the nearest obstacle point, expressed as:

[0108]

[0109] in, and is the observed value of the position of the nearest obstacle point at time t, and are the observed speed values ​​of the nearest obstacle point on the x-axis, y-axis and z-axis at time t, respectively, t represents the measurement noise and obeys Gaussian distribution. In this embodiment, Δx / Δt is used as the velocity observation value.

[0110] In each path planning process, the robot arm first needs to plan an initial global path offline based on a static and fixed environment map. This initial global path not only requires the robot arm to reach the target position, but also ensures that the robot arm's links do not collide with the environment.

[0111] The core problem of path planning is to avoid collision, that is, to avoid overlap in the configuration space. Let C be the configuration space of the robot, then C∈R n , where R n represents an n-dimensional real number set. Since a common robotic arm is a six-joint robotic arm, the dimension n=6 in this embodiment, and the robotic arm is composed of the dimensions of six joints.

[0112] For a robot arm, a set of joint configurations corresponds to a robot arm posture state, denoted as q∈C, where each q=[θ 1 ,θ 2 ,…,θ 6 ] T ,θ 1 ~θ 6 Represents the position states of the six joints. The configuration space C of the manipulator can be divided into a free configuration space C that does not overlap with the manipulator posture state q free and the obstacle configuration space C that overlaps with q obs , the two complement each other.

[0113] Therefore, the global path planning problem can be described as: find a set of continuous robot configurations P in the robot configuration space such that P∈C free , satisfying the starting point is the initial configuration q init , the end point is the target configuration q goal The obtained P is the collision-free global path of the robot.

[0114] In order to reduce the amount of calculation and optimize the global path, step S40 includes:

[0115] S401, offline planning of an initial global path of the robot arm in the joint space based on the RRT_Connect algorithm;

[0116] S402, using a greedy algorithm to eliminate redundant points in the initial global path;

[0117] S403, using a cubic spline curve to perform interpolation and smoothing processing on the global path optimized by the greedy algorithm to obtain a target global path.

[0118] The following is a detailed introduction to each step.

[0119] Since the characteristic of robot arm path planning is that it needs to search for paths in high-dimensional joint space, which often leads to a large amount of calculation, S401 of this embodiment plans the initial global path of the robot arm offline in the joint space based on the RRT_Connect algorithm of sampling search.

[0120] RRT_Connect is an improved algorithm of RRT. The main idea of ​​RRT is to randomly sample points in the planning space, and generate a path tree by expanding from the initial point to the sampling point to generate a feasible path to the target point. Compared with RRT, RRT_Connect can accelerate the path search by alternately expanding and growing from the starting point to the end point, and the path search time is greatly improved compared to RRT. However, the path generation method based on random sampling may cause RRT_Connect to generate redundant path points, and random expansion may also generate some sharp corners, resulting in a very uneven path.

[0121] Reference Figure 3 As shown, based on the environment map of the static robot arm operation scene, the initial global path of the robot arm is planned offline in the joint space based on the RRT_Connect algorithm. The path from point A to point E in the initial global path can go directly from point A to point E, but due to random generation, this path detours through points B, C, and D. Based on the problem of generating redundant path points, this embodiment uses a greedy algorithm to optimize the initial global path, and the steps include:

[0122] Step a: Starting from the initial configuration q init The point q to be optimized temp ;

[0123] Step b: sequentially set the points to be optimized q temp Connect with the subsequent path points until the connection line is not in the free configuration space C free The path point q in j ; This shows that from the point to be optimized q temp To the waypoint q j-1 is redundant; use the point to be optimized q temp and path point q j-1 The connection between them replaces the initial global path from the point to be optimized q temp To the waypoint q j-1 The path is divided on the line according to the expansion step length ε to obtain new path points on the line;

[0124] Step c: Set the path point q j As the new point to be optimized q temp , return to step b until the target configuration q of the target point is reached goal .

[0125] The greedy algorithm eliminates redundant points in the initial global path and avoids the situation of "taking a long detour". However, in the global path optimized by the greedy algorithm, the path points are relatively large, and there are path turning points in the path. Sharp path turning points can cause a sudden change in the speed of the robot arm during operation, affecting smooth operation. Therefore, this embodiment uses a cubic spline curve to interpolate and smooth the global path optimized by the greedy algorithm.

[0126] Spline function is a method of defining a piecewise polynomial to make the total function satisfy discrete point constraints. Cubic spline means using a cubic polynomial to define the path function. For the global path optimized by the greedy algorithm, there are a total of N segments in the path. The role of cubic spline is to build a smooth cubic function path on this segment while satisfying the position constraints of the first and last path points of each segment. Adjacent segments also need to satisfy the continuity of zero-order, first-order, and second-order derivatives to connect smoothly. Then, points are evenly sampled on the cubic function path between the path points to form a more detailed and smooth path. See the schematic diagram of the cubic spline curve for reference. Figure 4 As shown, the specific calculation process is as follows:

[0127] For each path point in the global path optimized by the greedy algorithm, the robot arm posture state q is constructed k , where k = 0, 1, ..., N, It is necessary to construct a cubic spline function for each joint position state. Since the step size is fixed and the distance between adjacent points is close, the time Δt' required to execute each path is also close. The time to reach the kth path point is t k =k·Δt′.

[0128] Take the position state θ of the nth joint m Take t as an example to construct the segmented path function P(t):

[0129]

[0130] Among them, P N-1 (t) represents the segment path function of the Nth segment path, a N-1 、b N-1 、c N-1 and d N-1 are the coefficients of the segmented path function of the Nth segment of the path, and t represents time.

[0131] The segment path function P of the k+1th segment path k The formula for (t) is:

[0132] P k (t) = a k +b k (tt k )+c k (tt k ) 2 +d k (tt k ) 3

[0133] The coefficients of each piecewise function are unknown, so there are 4N unknown coefficients in total. According to the requirements of the spline function, the function needs to pass through all path points, so P k (t) The first and last two endpoints are on the path points, satisfying zero-order continuity, and a total of 2N equations can be constructed. The boundary conditions are:

[0134]

[0135] The path except the first and last points needs to satisfy the speed continuity, so P k (t) requires the first-order derivative to be continuous, and N-1 equations can be constructed:

[0136]

[0137] The path except the first and last points needs to satisfy acceleration continuity to make the curvature smooth at the path points, so P k (t) requires the second-order derivative to be continuous, and N-1 equations can be constructed:

[0138]

[0139] The constraint that the speed is 0 needs to be satisfied at the beginning and end of the path, and two equations can be constructed:

[0140]

[0141] Combining the above equations, we can get 4N equations in total, which can solve the 4N unknown parameters in P(t) and obtain the smoothed global path curve.

[0142] Press the smoothed global path curve The time is divided equally into four path points on each path segment, thereby obtaining the target global path.

[0143] In summary, this embodiment uses the RRT_Connect algorithm to reduce the time of initial global path planning, and uses the greedy algorithm to eliminate redundant points in the initial global path. Finally, the cubic spline curve is used to interpolate and smooth the global path optimized by the greedy algorithm, which improves the efficiency of path planning and ensures the smooth and safe operation of the robotic arm.

[0144] However, in human-machine collaborative operation scenarios, offline global path planning alone cannot meet the safety requirements of the system, and dynamic obstacles in the environment must be considered at the same time. Therefore, this embodiment also uses an artificial potential field method to perform local real-time obstacle avoidance in the joint space of the robot arm, allowing the robot arm to respond to dynamic obstacles in real time while completing the task.

[0145] Specifically, in S50, when the robot arm moves along the global path, based on the nearest obstacle point, the artificial potential field method is used to instruct the robot arm to avoid dynamic obstacles in real time, and the running speed of the robot arm is output, including:

[0146] S501. An attraction velocity potential field is set in the six-dimensional joint space where each path point of the global path is located. When the robot moves to the next path point along the global path, the attraction velocity is calculated according to the Euclidean distance between the current position of the robot and the next path point.

[0147] The robot arm needs to reach each path point one by one. According to the idea of ​​the potential field method, starting from the first path point of the global path point, the next path point of the current path point is set as the target point; at each moment, the next path point of the current position is used as the target point to generate an attractive velocity potential field to make the robot arm approach it; when the robot arm reaches the target point, the next path point is set as the target point, and the process is repeated in sequence.

[0148] The attraction velocity potential field is set in the six-dimensional joint space where the path point is located, and the design idea of ​​the attraction velocity potential field is: let the robot quickly approach the target point when it is far away from the target point; gradually slow down when the robot is about to reach the target point; stop when the distance between the robot and the target point is zero. Therefore, the attraction velocity potential field function is:

[0149]

[0150] Among them, d goal is the Euclidean distance between the current position of the robot arm joint space and the next path point, and is the variable of the attractive velocity potential field function; K goal represents the maximum value of the attraction velocity, and α represents the critical distance. The obtained f(d goal ) represents the size of the attractive velocity potential field and is also the attraction rate.

[0151] Under the attractive velocity potential field function, by adjusting K goal By adjusting K and α, the maximum value and range of the attraction velocity potential field can be controlled. When the distance between the manipulator and the target point is less than α, the attraction velocity potential field will decrease as the distance decreases; when the distance between the manipulator and the target point is greater than or equal to α, the attraction velocity potential field tends to remain unchanged as the distance increases. goal =1, α = 0.05 when the attractive velocity potential field curve is referenced Figure 5 shown.

[0152] The attraction velocity takes the attraction velocity potential field function as the coefficient, and the attraction velocity is expressed as:

[0153]

[0154] in, To attract speed, The direction vector pointing from the current robot arm joint space position to the next path point.

[0155] S502: Set a repulsion velocity potential field at the nearest obstacle point and calculate the repulsion velocity.

[0156] The presence of dynamic obstacles will not only interfere with the execution of the robot's tasks, but may also cause dangerous collision accidents. In order to avoid the occurrence of collisions, this embodiment effectively prevents the robot from moving toward the obstacle by setting a repulsive velocity potential field at the nearest obstacle point.

[0157] The design idea of ​​the repulsive velocity potential field is: the repulsive velocity increases as the distance between the robot control point and the nearest obstacle point decreases, forming a certain inverse relationship. In this way, when the dynamic obstacle is close, it can quickly avoid collision, and when the dynamic obstacle is far away, it will not cause significant interference to the normal operation of the robot. In addition, the repulsive velocity has a certain limit and cannot tend to infinity; when the nearest obstacle point exceeds a certain range, the repulsive velocity needs to approach zero, otherwise the dynamic obstacle at a long distance will also affect the movement of the robot. Therefore, the repulsive velocity potential field function is:

[0158]

[0159] Among them, K obs represents the maximum value of the repulsion velocity, ρ represents the radius of the spherical obstacle detection area, and β represents the drop constant; by adjusting K obs , ρ and β can be customized to adjust the potential field. obs Represents the distance between the robot control point and the nearest obstacle point, f obs (d obs ) represents the size of the repulsive velocity potential field, which is also the repulsion rate.

[0160] Under the action of the repulsive velocity potential field function, the maximum repulsive velocity is K obs , the minimum value is 0, and the repulsion speed increases with the distance d obs It decreases as it increases, decreases fastest at ρ / 2, and approaches 0 at ρ. obs =1, ρ = 0.5, β = 5 when the repulsive velocity potential field curve is referenced Figure 6 shown.

[0161] The repulsive velocity uses the repulsive velocity potential field function as a coefficient, and the repulsive velocity is expressed as:

[0162]

[0163] in, is the repulsion velocity, The direction vector pointing from the nearest obstacle point to the robot control point.

[0164] S503, converting the repulsion speed in Cartesian space to six-dimensional joint space to obtain the joint obstacle avoidance speed of the robot arm; the joint attraction speed of the robot arm is the attraction speed; the joint attraction speed and the joint obstacle avoidance speed are superimposed to obtain the running speed of the robot arm.

[0165] The formulas for the joint attraction speed and joint obstacle avoidance speed of the robot arm are:

[0166]

[0167] in, is the joint attraction speed, J is the joint obstacle avoidance speed; + It is the pseudo-inverse of the Jacobian matrix, which is an n×6 matrix. Since the robot arm in this embodiment is a six-joint robot arm, n is 6.

[0168] Under the superposition of the attraction velocity potential field and the repulsion velocity potential field, the operation mode of the robot arm is as follows: under the action of the attraction velocity potential field at the path point of the robot arm, the robot arm moves at the joint attraction velocity. When a dynamic obstacle approaches, under the action of the repulsive velocity potential field at the nearest obstacle point, the robot arm will obtain a joint obstacle avoidance velocity that prevents movement toward the dynamic obstacle. The joint attraction speed and the joint obstacle avoidance speed are superimposed to obtain the combined speed, that is, the running speed of the robot arm. The combined speed is sent to the robot arm for execution through the real-time speed control interface in each control cycle.

[0169] Under the action of the repulsive velocity potential field, the robot will generate a speed away from the obstacle. Since the repulsive velocity potential field is generated by the dynamic obstacle, the robot will deviate from the planned global path when moving in the opposite direction of the dynamic obstacle, which may cause a collision with the static obstacle. In order to avoid this situation and keep the robot on the global path, it is necessary to add a path constraint to the joint obstacle avoidance speed.

[0170] Preferably, after S503, the following steps may also be included:

[0171] S504: Project the joint obstacle avoidance speed of the robot arm onto the global path to optimize the running speed of the robot arm.

[0172] This embodiment uses the attraction velocity potential field to control the robot arm to pass through each path point of the global path in sequence, which is essentially equivalent to breaking the path into segments for execution. Therefore, in order to constrain the obstacle avoidance motion on the path, the joint obstacle avoidance speed can be projected onto the currently executed path segment.

[0173] For the path points in the global path, suppose that from the path point p l To the path point p l+1 The connection line is Will As a linear space A, the projection matrix P of this space is A for:

[0174] P A =A(A T A) -1 A T

[0175] in

[0176] Set the joint obstacle avoidance speed Project to linear space A to get obstacle avoidance projection speed for:

[0177]

[0178] Therefore, the operating speed of the constrained robotic arm is the obstacle avoidance projection speed and joint attraction speed The operation mode of the robot arm is: when there is no external dynamic obstacle, the joint obstacle avoidance speed is 0, and the robot arm can attract the target point at the joint attraction speed. Run along the global path; when a dynamic obstacle appears and is close, the joint obstacle avoidance speed increases, but under the path constraint, the direction of the obstacle avoidance speed is still on the originally planned path, so the robot will not avoid the obstacle in the opposite direction. If the dynamic obstacle is not blocking the planned path, then the obstacle avoidance projection speed after the path constraint will not affect the movement of the robot, and the robot will continue to move along the original path; if the dynamic obstacle is in front of the robot path, then the joint obstacle avoidance speed will offset the joint attraction speed generated by the attraction velocity potential field after the path constraint, thereby slowing down or stopping the robot.

[0179] Under the path constraint, the robot arm will not deviate from the planned path when avoiding obstacles, and will not collide with the static environment due to avoiding dynamic obstacles.

[0180] In addition to path constraints, it is also important to note that although the repulsive velocity potential field can prevent the robot from collision, under the pure repulsive force, the robot may move away from the obstacle quickly, similar to the quick reflex movement of animals when they encounter danger. For example, when a dynamic obstacle suddenly approaches in front of the robot, the robot will instantly obtain a large joint obstacle avoidance speed, which will cause the robot to quickly retreat along the original path under the path constraint. This kind of fast rebound movement is unsafe for the robot. To avoid this situation, it is also necessary to constrain the repulsive effect generated by dynamic obstacles.

[0181] Preferably, after S504, the following steps may also be included:

[0182] S505. Add an obstacle avoidance strategy during the operation of the robotic arm.

[0183] In the desired case, the magnitude of the joint's obstacle avoidance velocity is still determined by the repulsive velocity potential field, but it should only offset the velocity toward the dynamic obstacle.

[0184] When the robot moves toward a dynamic obstacle, the obstacle avoidance projection speed and the joint attraction speed are in different directions. At this time, if the obstacle avoidance projection speed is greater than the joint attraction speed, the superimposed combined speed will be toward the rear. To prevent the robot from moving backward along the global path, the combined speed is set to 0 at this time. When the robot moves away from the dynamic obstacle, the robot gradually moves away from the obstacle under the action of the joint attraction speed. To avoid the superposition speed being too fast, the obstacle avoidance projection speed is set not to be superimposed at this time. Therefore, the obstacle avoidance strategy satisfies the following formula:

[0185]

[0186]

[0187] in, Indicates the running speed of the robot. When λ>0, it means that the joint attraction speed and the obstacle avoidance projection speed are in the same direction, and the running speed of the robot is the joint attraction speed; when λ<0, it means that the joint attraction speed and the obstacle avoidance projection speed are in different directions. At this time, if the module value of the obstacle avoidance projection speed is greater than the module value of the joint attraction speed, it means that the obstacle is very close, and the running speed of the robot is 0. Otherwise, the running speed of the robot is the combined speed of the joint attraction speed and the obstacle avoidance projection speed.

[0188] Through the above obstacle avoidance strategy, the robot arm can avoid collisions with dynamic and static obstacles in the environment while maintaining operation, and will not cause safety hazards due to deviation from the global path.

[0189] In summary, the present invention realizes local real-time obstacle avoidance of the robotic arm, so that the robotic arm has autonomy, can actively perceive the surrounding environment and make plans independently, and does not need repeated manual teaching when the environment changes, thereby reducing time costs.

[0190] Embodiment 2

[0191] In order to verify the robot path planning method based on stereo vision provided in Example 1, this example obtains the depth map of the whole scene through the depth camera Kinect V2, and develops a software system for the collaborative robot safety operation platform based on ROS for simulation analysis. The results of the simulation experiment include: dynamic obstacle point cloud extraction, trajectory tracking, and terminal real-time obstacle avoidance. Through Euclidean clustering, the original dynamic obstacle point cloud can be filtered. Set the KD-Tree search radius to 0.6m, the minimum number of points required for clustering is 300, and the maximum limit is 10,000.

[0192] Reference of joint position curve during the operation of the robot arm Figure 7As shown in the figure. According to the experimental results, in the initial stage, when the robot has not yet entered the range of the repulsive velocity potential field of the dynamic obstacle, the robot moves along the global path according to the joint attraction speed. After the robot enters the range of the repulsive velocity potential field, the obstacle avoidance projection speed gradually increases, causing the combined speed to gradually decrease, and the robot's running speed slows down, but the robot does not deviate from the global path under the action of the path constraint. After that, the robot decelerates until the obstacle avoidance projection speed and the joint attraction speed reach a balance, and the robot is stationary at the equilibrium position.

[0193] When the dynamic obstacle leaves, the robot gradually leaves the range of the repulsive velocity potential field, the combined speed increases, and the robot starts to run along the global path again. According to the experimental results, under the potential field speed control with path constraints, the robot can run along the planned global path. At the same time, when a dynamic obstacle appears in front of the path, the robot will gradually slow down under the action of the speed generated by the repulsive velocity potential field until the repulsive speed and the attractive speed reach a balance. The effect is that the robot stops in time before colliding with the dynamic obstacle. When the obstacle is far away, the robot gradually resumes its operation under the action of the attractive velocity potential field and returns to the execution of the original task.

[0194] Therefore, the robot arm path planning method based on stereo vision described in the present invention has high path planning accuracy and good robustness, which helps to improve the operating efficiency of the robot arm.

[0195] Based on the robot arm path planning method based on stereo vision provided in the first embodiment, this embodiment further provides a robot arm path planning device based on stereo vision, including:

[0196] A map acquisition module is used to obtain an environmental map of the robot arm operation scene;

[0197] Dynamic obstacle point cloud extraction module, used to extract dynamic obstacle point clouds in the robot arm operation scene;

[0198] The dynamic obstacle tracking module is used to set the dynamic obstacle point cloud closest to the control point on the robot arm as the nearest obstacle point and track the nearest obstacle point;

[0199] The global path planning module is used to plan the global path of the robot arm offline based on the static environment map of the robot arm operation scene;

[0200] The real-time obstacle avoidance module is used to enable the robot to avoid dynamic obstacles in real time based on the nearest obstacle point and output the running speed of the robot when the robot moves along the global path.

[0201] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application may adopt the form of a computer program product implemented in one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that include computer-usable program code.

[0202] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0203] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0204] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0205] Obviously, the above embodiments are merely examples for the purpose of clear explanation and are not intended to limit the implementation methods. For those skilled in the art, other different forms of changes or modifications can be made based on the above description. It is not necessary and impossible to list all the implementation methods here. The obvious changes or modifications derived therefrom are still within the scope of protection of the present invention.

Claims

1. A robot arm path planning method based on stereo vision, characterized in that: include: Obtain the environmental map of the robot arm operation scene; Extract dynamic obstacle point clouds in robot arm operation scenes; Set the dynamic obstacle point cloud closest to the control point on the robot arm as the nearest obstacle point, and track the nearest obstacle point; Offline planning of the global path of the robot arm based on the environmental map of the robot arm's operation scene; When the robot moves along the global path, the artificial potential field method is used to enable the robot to avoid dynamic obstacles in real time based on the nearest obstacle point, and the running speed of the robot is output.

2. A robot arm path planning method based on stereo vision according to claim 1, characterized in that: The step of extracting a dynamic obstacle point cloud in a robot arm operation scene includes: Set the robotic arm to different positions, take and store multiple original depth images without dynamic obstacles offline; Filter out the robotic arm from the original depth map to obtain a background depth map after filtering out the robotic arm; Compare multiple background depth maps after filtering out the robotic arm, retain the maximum depth value, and obtain a static background depth map; The depth map with dynamic obstacles is collected in real time using a depth camera. The robot arm in the depth map with dynamic obstacles is filtered out and compared with the static background depth map to obtain the dynamic obstacle depth map. The dynamic obstacle depth map is transformed according to the internal parameters of the depth camera to obtain the dynamic obstacle point cloud in the camera coordinate system.

3. The method for robot arm path planning based on stereo vision according to claim 1, characterized in that: Kalman filtering is used to track the nearest obstacle point.

4. The method for robot arm path planning based on stereo vision according to claim 1, characterized in that: The offline planning of the global path of the robot arm based on the environment map of the robot arm operation scene includes: Based on the RRT_Connect algorithm, the initial global path of the robot arm is planned offline in the joint space; Use the greedy algorithm to eliminate redundant points in the initial global path; The cubic spline curve is used to interpolate and smooth the global path optimized by the greedy algorithm to obtain the target global path.

5. The method for robot arm path planning based on stereo vision according to claim 1, characterized in that: When the robot moves along the global path, based on the nearest obstacle point, the artificial potential field method is used to enable the robot to avoid dynamic obstacles in real time and output the running speed of the robot, including: An attraction velocity potential field is set in the joint space where each path point of the global path is located. When the robot moves to the next path point according to the global path, the attraction velocity is calculated according to the Euclidean distance between the current position of the robot and the next path point. Set the repulsive velocity potential field at the nearest obstacle point and calculate the repulsive velocity; The repulsion speed in Cartesian space is converted to joint space to obtain the joint obstacle avoidance speed of the robot arm; the joint attraction speed of the robot arm is set as the attraction speed; the joint attraction speed and the joint obstacle avoidance speed are superimposed to obtain the running speed of the robot arm.

6. A robot arm path planning method based on stereo vision according to claim 5, characterized in that: The attraction velocity potential field is set in the joint space where each path point of the global path is located. When the robot moves to the next path point according to the global path, the attraction velocity is calculated according to the Euclidean distance between the current position of the robot and the next path point, including: Set the attractive velocity potential field function to: Among them, d goal is the Euclidean distance between the current position of the robot arm joint space and the next path point, and is the variable of the attractive velocity potential field function; K goal represents the maximum value of the attraction velocity, α represents the critical distance; f(d goal ) represents the magnitude of the attractive velocity potential field; Calculate the attraction speed using the formula: in, To attract speed, The direction vector pointing from the current robot arm joint space position to the next path point.

7. The method for robot arm path planning based on stereo vision according to claim 5, characterized in that: The step of setting a repulsive velocity potential field at the nearest obstacle point and calculating the repulsive velocity includes: Set the repulsive velocity potential field function to: Among them, K obs represents the maximum value of the repulsion velocity, ρ represents the radius of the spherical obstacle detection area, and β represents the drop constant; d obs Represents the distance between the robot control point and the nearest obstacle point, f obs (d obs ) represents the magnitude of the repulsive velocity potential field; Calculate the repulsion velocity using the formula: in, is the repulsion velocity, The direction vector pointing from the nearest obstacle point to the robot control point.

8. The method for robot arm path planning based on stereo vision according to claim 5, characterized in that: After obtaining the running speed of the robot arm, the joint obstacle avoidance speed of the robot arm is projected onto the global path to optimize the running speed of the robot arm, including: For the path points in the global path, suppose that from the path point p l To the path point p l+1 The connection is Will As a linear space A, the projection matrix P of the linear space A is A for: P A =A(A T A) -1 A T in, Projecting the joint obstacle avoidance speed to the linear space A, the obstacle avoidance projection speed is: in, is the joint obstacle avoidance speed, Projection speed for obstacle avoidance; The running speed of the optimized robotic arm is the combined speed of the obstacle avoidance projection speed and the joint attraction speed.

9. A robot arm path planning method based on stereo vision according to claim 8, characterized in that: An obstacle avoidance strategy is added during the operation of the robot arm. The obstacle avoidance strategy includes: When the joint attraction speed and obstacle avoidance projection speed are in the same direction, the running speed of the robot arm is the joint attraction speed; When the joint attraction speed and the obstacle avoidance projection speed are in different directions, if the modulus of the obstacle avoidance projection speed is greater than the modulus of the joint attraction speed, the running speed of the robot arm is 0; otherwise, the running speed of the robot arm is the combined speed of the joint attraction speed and the obstacle avoidance projection speed.

10. A robot arm path planning device based on stereo vision, characterized in that: include: A map acquisition module is used to obtain an environmental map of the robot arm operation scene; Dynamic obstacle point cloud extraction module, used to extract dynamic obstacle point clouds in the robot arm operation scene; The dynamic obstacle tracking module is used to set the dynamic obstacle point cloud closest to the control point on the robot arm as the nearest obstacle point and track the nearest obstacle point; The global path planning module is used to plan the global path of the robot arm offline based on the static environment map of the robot arm operation scene; The real-time obstacle avoidance module is used to enable the robot to avoid dynamic obstacles in real time based on the nearest obstacle point and output the running speed of the robot when the robot moves along the global path.

Images