Human safety boundary extraction method and robot path planning method

Abstract

The application discloses a human body safety boundary extraction method and a robot path planning method, which comprises the following steps: (1) acquiring human body images, calculating the gradient of each pixel point in each sequence image, and selecting the pixel points with the gradient greater than a set threshold as the edge points of each sequence image; (2) fitting a quadratic surface according to the edge points of each sequence image obtained in step (1), and obtaining a human body contour surface; and (3) translating the human body contour surface in the front-back direction of the human body by a set distance in the direction away from the human body, and obtaining a human body safety boundary. The selected human body contour points are very fast and accurate, the obtained human body safety boundary is always a certain distance away from the human body contour, the safety of a patient is further protected, and the collision between a robot arm and the human body is effectively avoided.

Description

Technical Field

[0001] This invention relates to the field of image processing technology, and in particular to a method for extracting human safety boundaries and a method for robot path planning. Background Technology

[0002] With advancements in computer technology and robotics, robots are increasingly being used in medical surgeries. The main principle of a surgical navigation system is to register the preoperative image space coordinate system, navigation coordinate system, and machine coordinate system. The surgeon then plans the puncture channel in the preoperative image and translates its pose into the robotic arm coordinate system based on the registration relationship. Guided by the navigation device, the robotic arm moves to the predetermined position to perform the puncture, thereby improving accuracy. However, collisions between the robotic arm and the patient frequently occur during surgery due to improperly planned paths. To effectively avoid this, the robotic arm system needs to be aware of the patient's safety boundaries. Summary of the Invention

[0003] Purpose of the invention: To address the above-mentioned shortcomings, this invention proposes a method for extracting human safety boundaries and a robot path planning method, which can effectively prevent collisions between robotic arms and human bodies.

[0004] Technical solution:

[0005] Methods for extracting human body safety boundaries include:

[0006] (1) Acquire human images, calculate the gradient of each pixel in each sequence of images, and select the pixels whose gradient is greater than a set threshold as the edge points of each sequence of images.

[0007] (2) Fit a quadratic surface to the edge points of each sequence image obtained in step (1) to obtain the human body contour surface;

[0008] (3) The human body contour surface is translated a certain distance away from the human body in the front-back direction to obtain the human body safety boundary.

[0009] Specifically, the set threshold is as follows: the gradient distribution of each pixel in each sequence image satisfies a Gaussian distribution, and the pixel whose gradient is greater than the upper limit of the confidence interval corresponding to the set confidence level of the Gaussian distribution is selected as the edge point of each sequence image.

[0010] More specifically, the set confidence level is 95% confidence level.

[0011] Specifically, after obtaining the edge points of each sequence image in step (1), the method further includes removing false points from the edge points of each sequence image to obtain the human body contour points corresponding to each sequence image. Then, in step (2), a quadratic surface is fitted with the human body contour points corresponding to each sequence image to obtain the human body contour surface.

[0012] More specifically, the removal of false points from the edge points of each image sequence is performed using at least one of the following methods:

[0013] 1) Iterate through the neighborhood points of the edge point obtained by S1 to determine whether there are at least a first fixed number of points whose gradients are greater than the set threshold. If so, the point is considered an edge point; otherwise, the point is considered a pseudo point and is removed.

[0014] 2) Fit a quadratic curve to the edge points of each image sequence, and take the second derivative of the quadratic curve to obtain the constants for each edge point. Select points whose constants deviate from the average value of all constants as pseudo points and remove them.

[0015] Furthermore, the two methods described above are used to remove false points from the edge points of each sequence of images, and the results are verified against each other.

[0016] Specifically, in step (1), the gradient of each pixel in each sequence of images is calculated as follows:

[0017] The gradient G of each pixel along the x and y directions is obtained by performing Sobel filtering on each pixel in each sequence of human body images. x G y Therefore, the gradient G = G for each pixel can be calculated. x +G y .

[0018] Specifically, in S2, the least squares method is used to fit the quadratic surface to the edge points of each sequence image obtained in step (1).

[0019] More specifically, the fitted quadratic surface equation is:

[0020] y(x,z)=ax 2 +bz 2 +cxz+dx+ez+f

[0021] Where (x,y,z) are the coordinates of the points at the edge of the human body;

[0022] Construct a column vector Y of the y-coordinate values ​​of all human body edge points, and set the parameter matrix Θ = (a, b, c, d, e). The x and z coordinate values ​​of all contour points constitute the sample matrix X = {P}. i =(x i2 ,z i 2 ,x i z i ,x i ,z i If i∈[0,n], then Y=Θ T ×X;

[0023] Calculate the y-axis of a point on the edge of a human body. i The coordinate values ​​and their corresponding values ​​y(x) obtained from the aforementioned quadratic surface equation i ,z i The squared differences between the points are then summed over the squared differences at all human body edge points, and this sum is used as the objective function value, as follows:

[0024]

[0025] Where n is the total number of all human body edge points;

[0026] Based on the above, convert it into a matrix representation:

[0027]

[0028] Find:

[0029] Θ=(a,b,c,d,e,f)=(X T X) -1 X T Y

[0030] That is, the parameters in the fitted quadratic surface are obtained, and then the quadratic surface is obtained.

[0031] The present invention also provides a robot path planning method, comprising:

[0032] S1. The human safety boundary is obtained using the aforementioned human safety boundary extraction method;

[0033] S2. Based on the human safety boundary obtained in S1, calculate the interpolation points for the robot's motion path;

[0034] S3. Control the robot's movement based on the interpolation points of the robot's motion path determined by S2.

[0035] Specifically, in S2, the interpolation points for planning the robot's motion path based on the human safety boundary are as follows:

[0036] The current pose and target pose of the robot's end effector are obtained respectively. Based on the transformation relationship between the robot and the image, the current pose and target pose of the robot's end effector in the image are transformed, and then the interpolation points of the robot's motion path are calculated.

[0037] More specifically, the interpolation points for calculating the robot's motion path are:

[0038] The first and second interpolation points of the robot's motion path are obtained by projecting the current position and target position of the robot's end effector onto the human safety boundary, respectively. The posture of the robot's end effector at the first and second interpolation points is consistent with the current posture and target posture of the robot's end effector, respectively.

[0039] The intermediate interpolation point of the robot's motion path is obtained by projecting the midpoint of the current position and the target position of the robot's end effector onto the human safety boundary.

[0040] The corresponding posture of the intermediate interpolation point is calculated based on the current posture and target posture of the robot's end effector.

[0041] Furthermore, the calculation of the corresponding pose of the intermediate interpolation point specifically involves:

[0042] Convert the current pose and target pose of the robot's end effector into quaternion representations q1 and q2, respectively. Then, by normalizing thread interpolation, the quaternion representation q0 of the pose corresponding to the intermediate interpolation point t0 can be obtained:

[0043]

[0044] in, t0, t1, and t2 are the position coordinates of the intermediate interpolation point, the first interpolation point, and the second interpolation point of the robot path, respectively.

[0045] Furthermore, it also includes S4, which calculates the shortest distance between the robot's end effector and the human body contour surface in real time during the robot's movement. If the distance is less than a set threshold, the robot is controlled to stop moving.

[0046] Beneficial effects: The human body contour points selected by this invention are very fast and accurate, and the obtained human body safety boundary is always a certain distance away from the human body contour, which further protects the patient's safety and effectively avoids the situation where the robotic arm collides with the human body. Attached Figure Description

[0047] Figure 1 This is a flowchart of the human safety boundary fitting method of the present invention;

[0048] Figure 2 A schematic diagram of a Sobel filter in the x and y directions;

[0049] Figure 3 Example plot of Gaussian function fitting;

[0050] Figure 4 This is a distribution map of contour points in a single image;

[0051] Figure 5 The distribution of the second derivative of the contour point sequence;

[0052] Figure 6 A schematic diagram of the fitted surface and contour points;

[0053] Figure 7 This is a schematic diagram of the interpolation point position of the robot end effector projected onto the sectional plane. Detailed Implementation

[0054] The present invention will be further explained below with reference to the accompanying drawings and specific embodiments.

[0055] This invention provides a method for fitting human safety boundaries, such as... Figure 1 As shown, the steps include:

[0056] (1) Acquire human images and obtain the edge points of each sequence of images;

[0057] Specifically, by calculating the gradient of each pixel in each sequence of human images, the gradient distribution of each pixel in each sequence of images is obtained, which satisfies a Gaussian distribution with a gradient mean of μ and a variance of σ. Based on this, pixels with gradients greater than μ+2σ are selected as edge points of each sequence of images.

[0058] More specifically, the gradient G of each pixel along the x and y directions is obtained by performing Sobel filtering on each pixel in each sequence of human body images. x G y ,like Figure 2 As shown, the gradient G = G for each pixel is calculated from this. x +G y .

[0059] In the various sequences of human images, the gradients of pixels in the edge regions of the human body are relatively large, while the gradients of pixels in the air and tissue areas are almost zero. Therefore, the gradient histogram of all pixels generally conforms to a Gaussian distribution with a mean gradient of μ and a variance of σ. Figure 3As shown, the gradients in most areas of each image sequence are very low, meaning the mean gradient of the Gaussian distribution is close to zero. Therefore, it is necessary to filter out the human body tissue and background information. That is, the pixels representing the human body tissue and background information must be within the confidence interval of the Gaussian distribution at a set confidence level. In this embodiment, a confidence interval with a 95% confidence level is generally selected. The pixels at the human body edge have large gradients but a small proportion, and they are located to the right of the selected Gaussian distribution confidence interval. Therefore, pixels with gradients greater than the upper limit of the selected Gaussian distribution confidence interval can be selected as edge points of each image sequence. In this invention, the selected Gaussian distribution confidence interval is a 95% confidence interval. Therefore, pixels with gradients greater than the boundary threshold μ+2σ are selected as edge points of each image sequence. Figure 4 To determine the distribution of contour points in a single image, simply extract the white contour points.

[0060] (2) Remove false points from the edge points of each sequence of human images obtained in step (1) to obtain the human contour points corresponding to each sequence of images.

[0061] Reference Figure 4 There are some defect points outside the human body, etc. These defect points have large gradients, but there are no continuous pixels around them. These defect points are pseudo points and will also be considered as edge points in step (1). Therefore, it is necessary to filter out isolated strong gradient points. The present invention can use any of the following methods to remove pseudo points in the edge points, and can also use the following methods to verify each other. Specifically:

[0062] 1) Traverse and judge whether there are at least a first fixed number of points whose gradient is greater than the boundary threshold among the neighboring points of the edge point obtained in step (1). If so, the point is considered to be an edge point; otherwise, the point is considered to be a pseudo point and is removed. According to the distribution of human body boundary contour, each point has 8 neighboring points in the two-dimensional image and 26 neighboring points in the three-dimensional image. Therefore, in this invention, it is sufficient to judge whether there are at least 4 neighboring points whose gradient is greater than the boundary threshold among the 26 neighboring points of a certain edge point. Therefore, the first fixed value is 4.

[0063] 2) Fit a quadratic curve to the edge points of each image sequence, and then take the second derivative of the quadratic curve to obtain the constants for each edge point. Finally, obtain the average value of all constants, and filter out points whose constants deviate from the set average value as pseudo-points. Figure 5 As shown.

[0064] (3) Fit a quadratic surface to the human contour points corresponding to each sequence image obtained in step (2) to obtain the human contour surface of the human image.

[0065] Based on the human contour points corresponding to each sequence of images obtained in step (2), a quadratic surface is fitted using the least squares method, such as... Figure 6 As shown, the specific equation of the quadratic surface is:

[0066] y(x,z)=ax 2 +bz 2 +cxz+dx+ez+f

[0067] Where (x,y,z) are the coordinates of the human body contour points in the image coordinate system;

[0068] Construct a column vector Y of the y-coordinate values ​​of all human contour points, and set the parameter matrix Θ = (a, b, c, d, e). The x and z coordinate values ​​of all contour points constitute the sample matrix X = {P}. i =(x i 2 ,z i 2 ,x i z i ,x i ,z i If i∈[0,n], then Y=Θ T ×X;

[0069] Calculate the y-axis of a certain human body contour point i The coordinate values ​​and their corresponding values ​​y(x) obtained from the aforementioned quadratic surface equation i ,z i The squared differences between the points are then summed over the squared differences for all human contour points, and this sum is used as the objective function value, as follows:

[0070]

[0071] Where n is the total number of human body contour points;

[0072] Based on the above, convert it into a matrix representation:

[0073]

[0074] Taking the derivative with respect to the parameter matrix Θ, we get:

[0075]

[0076] Setting the derivative to zero, we get:

[0077] Θ=(a,b,c,d,e,f)=(X T X) -1 X T Y

[0078] That is, the parameters in the fitted quadratic surface are obtained, and then the quadratic surface is obtained.

[0079] In this invention, the human body edge surface of the human body image can also be obtained by fitting the edge points of each sequence image obtained in step (1). The aforementioned elimination of false points makes the obtained human body edge surface of the human body image more accurate.

[0080] (4) The human body contour surface is translated a distance away from the human body in the front-back direction to obtain the human body safety boundary.

[0081] To ensure that all human body contour points are within the enclosure of the aforementioned human body contour surface, this invention translates the quadratic surface a set distance away from the human body in the front-back direction, so that the robotic arm will absolutely not touch the human body contour surface.

[0082] In this invention, the distance is set to K, the value of which depends on the physical size of a single pixel in the human body image. The human body contour surface is then defined as follows:

[0083] y(x,z)=ax 2 +bz 2 +cxz+dx+ez+f+K.

[0084] That is, to obtain the safety boundaries of the human body.

[0085] This invention calculates the gradient of each pixel in each sequence of human body images. This gradient distribution follows a Gaussian distribution. Pixels with gradients greater than the upper limit of the confidence interval of the selected Gaussian distribution are selected as human body contour points for each sequence of images. The selection of human body contour points is very rapid and accurate. Furthermore, this invention provides rejection criteria for potential false points, further improving the accuracy of human body contour point selection. After obtaining the human body contour surface, it is translated a predetermined distance away from the human body in the forward-backward direction as a human body safety boundary. This ensures that the human body safety boundary obtained by this invention is always at a certain distance from the human body contour, further protecting patient safety and effectively avoiding collisions between the robotic arm and the human body.

[0086] This invention also provides a robot path planning method based on the aforementioned human safety boundary fitting method, such as... Figure 7 As shown, it includes:

[0087] S1. Obtain the human safety boundary according to the aforementioned method;

[0088] S2. Based on the human safety boundary obtained in S1, calculate the interpolation points for the robot's motion path;

[0089] S21. Obtain the current pose of the robot's end effector. Transform the robot's end effector in the image according to the transformation relationship between the robot and the image. Specifically, transform the current pose of the robot's end effector from the robot coordinate system to the image coordinate system, and construct the corresponding end effector coordinate system, denoted as the current end effector coordinate system. Figure 7 Current;

[0090] S22. Obtain the target pose of the robot's end effector. Transform the image according to the transformation relationship between the robot and the image to obtain the target pose of the robot's end effector. Specifically, transform the target pose of the robot's end effector from the robot coordinate system to the image coordinate system to obtain the target pose of the robot's end effector in the image, and construct the corresponding end effector coordinate system, denoted as the target end effector coordinate system, such as... Figure 7 middleDst;

[0091] S23. Calculate the interpolation points of the robot motion path and the corresponding posture of the robot end effector based on the current pose of the robot end effector in the image obtained in S21 and the target pose of the robot end effector in the image obtained in S22.

[0092] Specifically, the projections of the current position and target position of the robot end effector onto the aforementioned human safety boundary are used to obtain the corresponding interpolation points of the robot motion path on the human safety boundary. The posture of the robot end effector at the corresponding interpolation point is consistent with the current posture and target posture of the robot end effector. Then, based on the projection of the midpoint of the current position and target position of the robot end effector onto the aforementioned human safety boundary, the intermediate interpolation points of the robot motion path on the human safety boundary are obtained. Finally, the corresponding posture of the intermediate interpolation points is calculated based on the current posture and target posture of the robot end effector.

[0093] In this invention, the current position and target position of the robot end effector can be the position of any point above it.

[0094] The interpolation points for calculating the robot's motion path specifically include:

[0095] S231. Convert the x and z coordinates of the origin of the current end-point tool coordinate system obtained in S21. u z u Substituting into the previously obtained human safety boundary, we obtain the corresponding y-coordinate value y. u This allows us to obtain the projection of the origin of the current end-effector coordinate system onto the human safety boundary. At this point, the robot end-effector's pose remains unchanged, thus yielding its position. (x...) u ,y u ′,z u Let t1 be the first interpolation point of the robot's motion path;

[0096] S232, the x and z coordinate values ​​of the origin of the target end-effector coordinate system obtained in S22 are... v z v Substituting these values ​​into the previously obtained human safety boundary, we obtain the corresponding coordinate value y in the y-direction. v This allows us to obtain the projection of the origin of the target end-effector coordinate system onto the human safety boundary. Since this process involves approaching the human body, excessive adjustment of the robot's end-effector poses a risk; therefore, the robot's end-effector pose remains unchanged at this point, thus obtaining the robot's end-effector pose. (x...) v ,y v ′,z v Let t2 be the second interpolation point on the robot's motion path;

[0097] S233. The midpoint of the x and z coordinates of the first interpolation point t1 obtained in step S231 and the second interpolation point t2 obtained in step S232 is used to obtain the coordinates x of the intermediate interpolation point in the x and z directions. w z w x w =(x u +x v ) / 2, z w =(z u +z v ) / 2, x w z w Substituting these values ​​into the previously obtained human safety boundary, we obtain the corresponding coordinate value y in the y-direction. w , will (x w ,y w ,z w Let t0 be the intermediate interpolation point of the robot's motion path. Since the robot's end-effector pose is different between the first and second interpolation points, the robot's end-effector pose corresponding to this intermediate interpolation point t0 must be adjusted to ensure a smooth transition of the robot's end-effector pose. The specific pose adjustment is as follows:

[0098] Converting the robot end-effector poses corresponding to the first interpolation point t1 and the second interpolation point t2 into quaternion representations q1 and q2 respectively, the quaternion representation q0 of the pose corresponding to the intermediate interpolation point t0 can be obtained through normalized thread interpolation:

[0099]

[0100] in,

[0101] S3. Control the robot's movement based on the interpolation points of the robot's motion path determined by S2.

[0102] Depend on Figure 7 As can be seen, the entire movement of the robot in this invention occurs on a safe boundary above the human body, which greatly reduces the possibility of collision between the robot's end effector and the human body.

[0103] This invention also includes step S4, which calculates in real time the shortest distance between the robot's end effector and the previously obtained human body contour surface during robot movement. If this distance is less than a set threshold, the robot is controlled to stop moving. The set threshold can be set to 3cm.

[0104] This invention aligns the human body boundary extracted from the image with the robotic arm based on the changing relationship between the robot and the image. It then plans the robot's movement path by interpolating along the human body boundary. The planned path remains entirely within a safe boundary above the human body, effectively preventing collisions between the robotic arm and the patient. Furthermore, this invention monitors the shortest distance between the robot's end effector and the actual human body contour in real time during robot movement, further ensuring patient safety and eliminating the possibility of collisions between the robotic arm and the patient.

[0105] The preferred embodiments of the present invention have been described in detail above. However, the present invention is not limited to the specific details of the above embodiments. Within the scope of the technical concept of the present invention, various equivalent transformations (such as quantity, shape, position, etc.) can be made to the technical solution of the present invention, and these equivalent transformations all fall within the protection scope of the present invention.

Claims

1. A method for extracting human body safety boundaries, characterized in that, include: (1) Acquire human images, calculate the gradient of each pixel in each sequence of images, select the pixels with gradients greater than a set threshold as edge points of each sequence of images, and remove the false points to obtain the human contour points corresponding to each sequence of images. The removal of false points from the edge points of each image sequence is performed using at least one of the following methods: 1) Iterate through the neighborhood points of the edge point obtained by S1 to determine whether there are at least a first fixed number of points whose gradients are greater than the set threshold. If so, the point is considered an edge point; otherwise, the point is considered a pseudo point and is removed. 2) Fit a quadratic curve to the edge points of each image sequence, and take the second derivative of the quadratic curve to obtain the constants for each edge point. Select points whose constants deviate from the set average value of all constants as pseudo points and remove them. (2) Fit a quadratic surface to the human body contour points of each sequence image obtained in step (1) to obtain the human body contour surface; (3) The human body contour surface is translated a certain distance away from the human body in the front-back direction to obtain the human body safety boundary.

2. The method for extracting human safety boundaries according to claim 1, characterized in that, The set threshold is specifically defined as follows: the gradient distribution of each pixel in each image sequence satisfies a Gaussian distribution, and the pixel whose gradient is greater than the upper limit of the confidence interval corresponding to the set confidence level of the Gaussian distribution is selected as the edge point of each image sequence.

3. The method for extracting human safety boundaries according to claim 2, characterized in that, The set confidence level is 95%.

4. The method for extracting human safety boundaries according to claim 1, characterized in that, The two methods described above are used to remove false points from the edge points of each image sequence, and the results are verified against each other.

5. The method for extracting human safety boundaries according to claim 1, characterized in that, In step (1), the gradient of each pixel in each sequence of images is calculated, specifically as follows: The gradient G of each pixel along the x and y directions is obtained by performing Sobel filtering on each pixel in each sequence of human body images. x G y Therefore, the gradient G=G of each pixel can be calculated. x +G y .

6. The method for extracting human safety boundaries according to claim 1, characterized in that, In S2, the least squares method is used to fit quadratic surfaces to the edge points of each sequence image obtained in step (1).

7. The method for extracting human safety boundaries according to claim 6, characterized in that, The fitted quadratic surface equation is: y(x,z)=ax 2 +bz 2 +cxz+dx+ez+f: Where (x,y,z) are the coordinates of the points at the edge of the human body; Construct a column vector Y of the y-coordinate values ​​of all human body edge points, and set the parameter matrix Θ=(a,b,c,d,e). The x and z coordinate values ​​of all contour points constitute the sample matrix X={P}. i =(x i 2 ,z i 2 ,x i z i ,x i ,z i If ,1)}, i∈[0,n], then Y=Θ T ×X; Calculate the y-axis of a point on the edge of a human body. i The coordinate values ​​and their corresponding values ​​y(x) obtained from the aforementioned quadratic surface equation i ,z i The squared differences between the points are then summed over the squared differences at all human body edge points, and this sum is used as the objective function value, as follows: ； Where n is the total number of all human body edge points; Based on the above, convert it into a matrix representation: = )； Find: ； That is, the parameters in the fitted quadratic surface are obtained, and then the quadratic surface is obtained.

8. A robot path planning method, characterized in that, include: S1. Obtain the human safety boundary using the human safety boundary extraction method described in any one of claims 1 to 7; S2. Based on the human safety boundary obtained in S1, calculate the interpolation points for the robot's motion path; S3. Control the robot's movement based on the interpolation points of the robot's motion path determined by S2.

9. The robot path planning method according to claim 8, characterized in that, In step S2, the interpolation points for planning the robot's motion path based on the human safety boundary are specifically as follows: The current pose and target pose of the robot's end effector are obtained respectively. Based on the transformation relationship between the robot and the image, the current pose and target pose of the robot's end effector in the image are transformed, and then the interpolation points of the robot's motion path are calculated.

10. The robot path planning method according to claim 9, characterized in that, The interpolation points for calculating the robot's motion path are specifically: The first and second interpolation points of the robot's motion path are obtained by projecting the current position and target position of the robot's end effector onto the human safety boundary, respectively. The postures of the robot's end effector at the first and second interpolation points are consistent with the current posture and target posture of the robot's end effector, respectively. The intermediate interpolation point of the robot's motion path is obtained by projecting the midpoint of the current position and the target position of the robot's end effector onto the human safety boundary. The corresponding posture of the intermediate interpolation point is calculated based on the current posture and target posture of the robot's end effector.

11. The robot path planning method according to claim 10, characterized in that, The specific steps for calculating the corresponding pose of the intermediate interpolation point are as follows: Convert the current pose and target pose of the robot's end effector into quaternion representations q1 and q2, respectively. Then, by normalizing thread interpolation, the quaternion representation q0 of the pose corresponding to the intermediate interpolation point t0 can be obtained: ； in, t0, t1, and t2 are the position coordinates of the intermediate interpolation point, the first interpolation point, and the second interpolation point of the robot path, respectively.

12. The robot path planning method according to claim 8, characterized in that, It also includes S4, which calculates the shortest distance between the robot's end effector and the human body contour surface in real time during the robot's movement. If the distance is less than a set threshold, the robot is controlled to stop moving.

Images