Bed surface normal vector estimation and mechanical arm attitude parallel correction method

Abstract

The invention relates to a bed surface normal vector estimation and mechanical arm attitude parallel correction method. The bed surface normal vector estimation method comprises the following steps: acquiring a bed surface RGB image and a depth image; extracting a bed surface area from the RGB image of the bed surface; dividing the extracted bed surface area into a plurality of sub-blocks, and calculating point cloud features of each sub-block based on the bed surface depth map; according to the calculated point cloud features, sub-blocks in a flat area are screened out to serve as target sub-blocks; the obtained normal vector of the bed surface is the weighted sum of the normal vector of each target sub-block, and the flatter the target sub-block is, the larger the weight corresponding to the flatter target sub-block is. The method can effectively solve the problems of inaccurate bed surface posture estimation and difficulty in mechanical arm posture adjustment in a complex environment, and is widely applicable to scenes such as physical therapy nursing, rehabilitation assistance, bed surface laying and robot service.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of intelligent medical systems and medical device research, and particularly relates to a bed surface normal vector estimation and mechanical arm posture parallel correction method. BACKGROUND

[0002] In physiotherapy or rehabilitation, robot service and the like, the posture, key points or body features of a patient on a bed surface often need to be detected and measured. An RGBD camera can be arranged in a mechanical arm installation or a fixed support installation to simultaneously obtain color information and depth information, thereby guiding the adjustment of the posture of the mechanical arm. However, in actual applications, the accuracy of the obtained depth data is reduced due to the following problems.

[0003] 1. There is an angle deviation between the camera and the bed surface.

[0004] 2. The bed surface is not an ideal rigid plane, and has a certain curvature and local unevenness.

[0005] 3. If the camera is tilted during shooting, the posture of the human body in the image will also be distorted.

[0006] At the same time, two-dimensional image detection will also have errors due to the tilting of the human body posture, thereby affecting the accuracy of subsequent measurement and intelligent analysis. Therefore, how to obtain a more accurate bed surface normal vector, and then automatically correct the posture of the camera (mechanical arm) based on the normal vector, so that it remains parallel to the bed surface, becomes a key problem to improve the measurement accuracy. SUMMARY

[0007] The technical problem to be solved by the present application is to provide a bed surface normal vector estimation and mechanical arm posture parallel correction method, which can effectively solve the problems of inaccurate bed surface posture estimation and difficult mechanical arm posture adjustment in a complex environment.

[0008] The technical scheme adopted by the present application to solve the technical problem is to provide a bed surface normal vector estimation method, comprising:

[0009] obtaining a bed surface RGB image and a depth image;

[0010] extracting a bed surface region from the RGB image of the bed surface;

[0011] dividing the extracted bed surface region into a plurality of sub-blocks, and calculating the point cloud features of each sub-block based on the bed surface depth image;

[0012] According to the calculated point cloud features, the sub-blocks of the flat region are selected as target sub-blocks;

[0013] The bed surface normal vector is obtained as the weighted sum of the normal vectors of each target sub-block, and the weight corresponding to the flatter target sub-block is greater.

[0014] Further, the point cloud feature is a normal vector of the corresponding sub-block.

[0015] Further, the point cloud feature of each sub-block is calculated based on the bed surface depth map, including:

[0016] The depth map region corresponding to each sub-block is converted into a point cloud, and then a covariance matrix of each sub-block is calculated;

[0017] The covariance matrix is subjected to eigenvalue decomposition, and a feature vector corresponding to the minimum eigenvalue is extracted as the normal vector of the current sub-block.

[0018] Further, the sub-block of the flat region is screened out as the target sub-block, including:

[0019] The curvature of each sub-block is calculated according to the point cloud feature;

[0020] The target sub-block with a curvature less than a first set threshold is screened out.

[0021] Further, the weight of the target sub-block is greater, which is realized by setting the curvature of the target sub-block as the weight.

[0022] Further, before the step of calculating the point cloud feature of each sub-block based on the bed surface depth map, the step of counting the effective pixel ratio of each sub-block and eliminating the sub-block with an effective pixel ratio less than a second set threshold is further included.

[0023] Further, before the step of dividing the extracted bed surface region into a plurality of sub-blocks, the step of performing a morphological erosion operation on the extracted bed surface region is further included.

[0024] Further, between the step of performing a morphological erosion operation on the extracted bed surface region and the step of dividing the extracted bed surface region into a plurality of sub-blocks, the step of calculating a minimum circumscribed rectangle as a new bed surface region according to the contour of the bed surface region after erosion is further included.

[0025] Further, the bed surface region is a binary mask.

[0026] The application also provides a mechanical arm posture parallel correction method based on a bed surface normal vector, a camera is arranged on the mechanical arm, and the camera is used to acquire a bed surface RGB image and a depth map, including:

[0027] The bed surface normal vector is estimated by using the method as described above;

[0028] A rotation matrix is calculated by using the bed surface normal vector;

[0029] A translation vector is calculated according to the center point coordinates of the flat region and the target height of the bed surface distance camera;

[0030] Correct the pose of the robot arm based on the rotation matrix and the translation vector, so that the camera is parallel to the bed surface.

[0031] Advantages

[0032] Compared with the prior art, the present application has the following advantages and positive effects: By dividing the bed surface area and performing block analysis, the present application extracts the sub-block of the flat area and calculates the bed surface normal vector using the weighted fusion mechanism, thereby enhancing the robustness in complex environments. In the presence of wrinkles, bedding undulations, and partial occlusions, the globally stable normal vector can still be extracted. Meanwhile, when the camera has a limited viewing angle, the block and weighted fusion mechanism can make full use of the local visible information, reducing the dependence on the complete bed surface point cloud. The present application calculates the rotation matrix through the Rodrigues formula, realizes the automatic alignment of the bed surface normal vector and the Z-axis of the camera coordinate system, and thereby makes the end tool coordinate system of the robot arm parallel to the bed surface. The introduction of the translation matrix ensures that the robot arm can correct the height with the bed surface center as the reference, thereby improving the stability of positioning and operation, and the accuracy of the alignment of the robot arm and the bed surface. The present application corrects the camera pose, so that the bed surface and the camera form a standard parallel relationship, reduces the depth error caused by tilting, improves the accuracy of subsequent 2D detection and depth estimation, and provides high-quality input data for applications such as physiotherapy. The algorithm framework provided by the present application is independent of specific hardware and can be applied to different models of RGBD cameras and robot arm systems. This method is not only suitable for bed surface correction, but can also be extended to other plane detection and robot arm correction scenarios, such as desktop operation and ground detection. BRIEF DESCRIPTION OF DRAWINGS

[0033] Figure 1 is a flowchart of the first embodiment of the present application;

[0034] Figure 2 is a flowchart of the second embodiment of the present application. DETAILED DESCRIPTION

[0035] The present application will be further described in conjunction with specific embodiments. It should be understood that these embodiments are only used to illustrate the present application and not to limit the scope of the present application. Furthermore, it should be understood that those skilled in the art can make various modifications or changes to the present application after reading the content taught by the present application, and these equivalent forms also fall within the scope defined by the appended claims.

[0036] The first embodiment of the present application relates to a bed surface normal vector estimation method, as shown in Figure 1 , comprising:

[0037] obtaining a bed surface RGB image and a depth image;

[0038] extracting a bed surface region from the RGB image of the bed surface;

[0039] dividing the extracted bed surface region into a plurality of sub-blocks, and calculating point cloud features of each sub-block based on the bed surface depth map;

[0040] According to the calculated point cloud features, the sub-blocks of the flat area are selected as target sub-blocks;

[0041] The bed surface normal vector is obtained as the weighted sum of the normal vectors of each target sub-block, and the flatter the target sub-block corresponds to the greater the weight.

[0042] The bed surface region can be segmented by existing methods, such as a deep learning semantic segmentation method based on an RGB image. Specifically, a lightweight semantic segmentation network (such as DeepLabV3+, SegFormer-B0) can be used to segment the bed surface from the RGB image, excluding the interference of other objects on the bed surface, and improving the robustness of the scheme. The input is an RGB image, and the output is a bed surface region binary image mask.

[0043] In addition, the following methods can also be used:

[0044] A clustering segmentation method based on a depth map, such as using Euclidean clustering, region growing, RANSAC plane fitting, etc. to extract the bed surface region from the point cloud;

[0045] A segmentation method based on traditional image processing, such as using color thresholding, edge detection, morphological operations, etc. to realize bed surface region extraction;

[0046] A multi-modal segmentation method combining RGB and depth features to enhance the ability to distinguish non-bed surface objects such as pillows and quilts.

[0047] In some preferred embodiments, to exclude the interference of the segmentation accuracy of the edge region and the problem that the bed edge is often slightly lower than the center of the bed surface, a morphological erosion operation can be used to remove the edge pixels of the mask and only keep the core bed surface region. At this time, mask2 is obtained for participating in the subsequent normal vector calculation.

[0048] In addition, before the block analysis of the bed surface region, the contour of mask2 can be extracted, the minimum bounding rectangle rect can be calculated, and the bounding rectangle rect can be obtained as the input of the subsequent block analysis.

[0049] Next, the bounding rectangle rect is divided into blocks and the normal vector is selected. The rect is divided into a plurality of sub-blocks mask_p according to M*N blocks at equal intervals, and the effective pixel ratio k of each block region is counted. If k>50%, the normal vector of the region mask_p is calculated; otherwise, the region (near the edge) is discarded.

[0050] A method based on block fitting and curvature weighting can be used to estimate the bed surface normal vector. Specifically, this includes:

[0051] (1) Calculate the normal vector and curvature:

[0052] Convert the selected depth map region corresponding to mask_p into a camera coordinate system point cloud PC_p:

[0053]

[0054] in, 3D point cloud coordinates obtained through depth map transformation For pixel coordinates, This is the depth value of that pixel (i.e., the distance from the camera to the bed surface). This is the camera intrinsic parameter matrix.

[0055] To calculate the normal vector of PC_p, first calculate the covariance matrix:

[0056]

[0057] in, Let be the 3D coordinates of the i-th point in the point cloud PC_p. The mean point of PC_p, This represents the total number of points in PC_p.

[0058] Eigenvalues ​​are obtained by decomposing the covariance matrix. < < ; normal vector Corresponding minimum eigenvalue eigenvectors;

[0059] curvature The smaller the better, the bed surface is usually Very small:

[0060]

[0061] (2) Filter and weighted merge the normal vectors:

[0062] Select target sub-block:

[0063]

[0064] in, It is a set of target sub-blocks. This is the screening threshold. If the area is less than the threshold, it is considered to be flat enough and is a bed surface; otherwise, it is discarded and not included in subsequent calculations.

[0065] curvature As a confidence weight, the merged normal vector is:

[0066] Set merge weights :

[0067]

[0068] in, The scaling parameter controls the rate of weight decay and determines the boundary between low and high curvature. The larger the value, the slower the weight decreases, and more points are retained. The smaller the value, the more rigorous the screening, and the fewer points are retained. Specifically, a suitable threshold can be selected from the curvature distribution as κ0, and then fine-tuned to optimize the task performance.

[0069] Global inter-block weighted fusion:

[0070]

[0071] That is the final normal vector of the bed surface.

[0072] In addition to methods based on block fitting and curvature weighting, the following methods can also be used to estimate the bed surface normal vector:

[0073] Principal component analysis (PCA) is used to fit the point cloud of the bed surface as a whole and extract the principal direction vector as the normal vector of the bed surface.

[0074] The plane fitting method based on Random Sample Consensus (RANSAC) directly estimates the equation of the bed surface plane and obtains the normal vector from it;

[0075] By using a deep learning regression model, the normal vector of the bed surface can be predicted directly from the point cloud or depth map.

[0076] A preferred embodiment 1 of this implementation: A method for estimating the bed surface normal vector and automatically correcting the robotic arm based on an RGBD camera, the main steps of which include:

[0077] 1. Obtain RGB and depth maps using an RGBD camera, and segment the image to obtain the point cloud of the bed surface;

[0078] 2. Perform block fitting on the plane and filter the normal vector based on local curvature;

[0079] 3. The global plane normal vector is obtained by fusion;

[0080] 4. Calculate the robot arm's posture transformation matrix to achieve parallelism between the camera and the bed surface, thereby improving the accuracy of depth map and 2D image detection.

[0081] This embodiment can adapt to complex situations such as local undulations in the bed surface, limited camera field of view, and partial obstruction.

[0082] The specific implementation steps include:

[0083] a. Bed surface area division:

[0084] 1. Use a lightweight semantic segmentation network (such as DeepLabV3+, SegFormer-B0) to segment the bed surface of the RGB image, eliminate the interference of other objects on the bed surface, and improve the robustness of the solution; the input is an RGB image, and the output is a binary image mask of the bed surface region.

[0085] 2. To eliminate the interference of segmentation accuracy in the edge region and the problem that the edge of the bed is often slightly lower than the center of the bed surface, a morphological erosion operation is used to remove the edge pixels of the mask, leaving only the core bed surface region. This yields mask2, which is used to participate in the subsequent normal vector calculation.

[0086] b. Block fitting, filtering normal vectors

[0087] 1. Calculate the bounding rectangle of mask2: Extract the outline of mask2 and calculate the minimum bounding rectangle rect.

[0088] 2. Divide the rect into blocks: Divide the rect into M*N equally spaced blocks called mask_p, and calculate the effective pixel percentage k of each block. If k > 50%, calculate the normal vector of mask_p for that region; otherwise, discard that region (regions near the edge).

[0089] 3. Calculate the normal vector and curvature:

[0090] - Convert the depth map region corresponding to the selected mask_p in step 2 into a point cloud PC_p:

[0091]

[0092] -To calculate the normal vector of PC_p, first calculate the covariance matrix:

[0093]

[0094] -Eigenvalues ​​are obtained by decomposing the covariance matrix. < < ; normal vector Corresponding minimum eigenvalue eigenvectors;

[0095] -curvature The smaller the better, the bed surface is usually Very small:

[0096]

[0097] 4. Filter and weighted merge normal vectors:

[0098] - Select bed surface:

[0099]

[0100] Less than the threshold If the area is flat enough, it is considered a bed surface; otherwise, it is discarded and not included in subsequent calculations.

[0101] - Curvature As a confidence weight, the merged normal vector is:

[0102] Set merge weights :

[0103]

[0104] Global inter-block weighted fusion:

[0105]

[0106] That is the final normal vector of the bed surface.

[0107] c. Calculate the robot arm's posture transformation matrix

[0108] 1. Calculate the rotation matrix, with the goal of making the bed surface parallel to the XY plane, i.e., the normal vector. Parallel to the Z-axis. Assuming the standard Z-axis is [0,0,1], the rotation matrix R can be obtained using the following method:

[0109] - Calculate the axis of rotation :

[0110]

[0111] - Rotation angle :

[0112]

[0113] The rotation matrix is ​​obtained using Rodrigues' formula. :

[0114]

[0115] 2. Calculate the translation matrix:

[0116] - Calculate the center point c=(xc,yc,zc) of the bed surface. This point is obtained by using the point cloud of the mask_p region finally selected in step b.4 as the bed surface and calculating the mean of the point cloud.

[0117] If the distance from the bed surface to the expected height z0 of the camera is [0, 0, z0 - zc], then the translation vector is t = [0, 0, z0 - zc].

[0118] 3. Determine the reasonableness of the current correction calculation based on the rotation angle and translation vector: for practical application scenarios. A value of <5° and ||t|| < 100mm is considered reasonable. Values ​​greater than this threshold require manual intervention to confirm whether recalibration is necessary.

[0119] The second embodiment of the present invention relates to a method for parallel correction of the posture of a robotic arm based on the normal vector of the bed surface, such as... Figure 2 As shown, the robotic arm is equipped with cameras for acquiring RGB and depth maps of the bed surface, including:

[0120] The bed surface normal vector is estimated using the RGB image and depth image of the bed surface;

[0121] The rotation matrix is ​​calculated using the bed surface normal vector;

[0122] The translation vector is calculated based on the coordinates of the center point of the flat area and the target height of the bed surface from the camera.

[0123] The robot arm's posture is corrected using the rotation matrix and translation vector to make the camera parallel to the bed surface.

[0124] The bed surface normal vector can be estimated using the bed surface RGB image and depth map, including:

[0125] Extract the bed surface area from the RGB image of the bed surface;

[0126] The extracted bed surface area is divided into several sub-blocks, and the point cloud features of each sub-block are calculated based on the bed surface depth map.

[0127] Based on the calculated point cloud features, sub-blocks in flat areas are selected as target sub-blocks;

[0128] The normal vector of the bed surface is obtained as a weighted sum of the normal vectors of each target sub-block, and the flatter the target sub-block, the greater the weight.

[0129] The bed surface region can be segmented using existing methods, such as deep learning semantic segmentation methods based on RGB images. Specifically, a lightweight semantic segmentation network (such as DeepLabV3+, SegFormer-B0) can be used to segment the RGB image into the bed surface, eliminating interference from other objects on the bed surface and improving the robustness of the solution; the input is an RGB image, and the output is a binary image mask of the bed surface region.

[0130] In addition to this, the following can also be used:

[0131] Clustering and segmentation methods based on depth maps, such as Euclidean clustering, region growing, and RANSAC plane fitting, are used to extract the bed surface region from the point cloud.

[0132] Traditional image processing segmentation methods, such as color thresholding, edge detection, and morphological operations, are used to extract the bed surface region.

[0133] A multimodal segmentation method combining RGB and depth features is proposed to enhance the ability to distinguish non-bed surface objects such as pillows and blankets.

[0134] In some preferred embodiments, to eliminate the interference of segmentation accuracy in the edge region and the problem that the edge of the bed is often slightly lower than the center region of the bed surface, a morphological erosion operation can be used to remove the edge pixels of the mask and retain only the core bed surface region, thus obtaining mask2 for subsequent normal vector calculation.

[0135] In addition, before performing block analysis on the bed surface area, the minimum bounding rectangle rect can be calculated by extracting the mask2 contour, and the bounding rectangle rect can be used as input for subsequent block analysis.

[0136] Next, the outer rectangle rect is segmented and fitted, and the normal vectors are selected. The rect is divided into M*N equally spaced blocks to obtain several sub-blocks mask_p, and the effective pixel ratio k of each region is calculated. If k>50%, the normal vector of the mask_p of that region is calculated; otherwise, the region (the region near the edge) is discarded.

[0137] A method based on block fitting and curvature weighting can be used to estimate the bed surface normal vector. Specifically, this includes:

[0138] (3) Calculate the normal vector and curvature:

[0139] Convert the selected depth map region corresponding to mask_p into a camera coordinate system point cloud PC_p:

[0140]

[0141] in, 3D point cloud coordinates obtained through depth map transformation For pixel coordinates, This is the depth value of that pixel (i.e., the distance from the camera to the bed surface). This is the camera intrinsic parameter matrix.

[0142] To calculate the normal vector of PC_p, first calculate the covariance matrix:

[0143]

[0144] in, Let be the 3D coordinates of the i-th point in the point cloud PC_p. The mean point of PC_p, This represents the total number of points in PC_p.

[0145] Eigenvalues ​​are obtained by decomposing the covariance matrix. < < ; normal vector Corresponding minimum eigenvalue eigenvectors;

[0146] curvature The smaller the better, the bed surface is usually Very small:

[0147]

[0148] (4) Filter and weighted merge the normal vectors:

[0149] Select target sub-block:

[0150]

[0151] in, It is a set of target sub-blocks. This is the screening threshold. If the area is less than the threshold, it is considered to be flat enough and is a bed surface; otherwise, it is discarded and not included in subsequent calculations.

[0152] curvature As a confidence weight, the merged normal vector is:

[0153] Set merge weights :

[0154]

[0155] in, For scale parameters;

[0156] Global inter-block weighted fusion:

[0157]

[0158] That is the final normal vector of the bed surface.

[0159] In addition to methods based on block fitting and curvature weighting, the following methods can also be used to estimate the bed surface normal vector:

[0160] Principal component analysis (PCA) is used to fit the point cloud of the bed surface as a whole and extract the principal direction vector as the normal vector of the bed surface.

[0161] The plane fitting method based on Random Sample Consensus (RANSAC) directly estimates the equation of the bed surface plane and obtains the normal vector from it;

[0162] By using a deep learning regression model, the normal vector of the bed surface can be predicted directly from the point cloud or depth map.

[0163] The goal of calculating the rotation matrix is ​​to make the bed surface parallel to the XY plane, i.e., the normal vector. Parallel to the Z-axis. Assuming the standard Z-axis is [0,0,1], the rotation matrix R can be obtained using the following method:

[0164] Calculate the axis of rotation :

[0165]

[0166] Rotation angle :

[0167]

[0168] Rodrigues' formula yields the rotation matrix. :

[0169]

[0170] The center point c=(xc,yc,zc) of the bed surface can be obtained by using the point cloud of the mask_p region finally selected in the steps as the bed surface and calculating the mean of the point cloud.

[0171] If the distance from the bed surface to the expected height of the camera is z0, then the translation vector is t = [0, 0, z0 - zc].

[0172] The validity of this correction calculation is determined based on the rotation angle and translation vector: for practical application scenarios. A value of <5° and ||t|| < 100mm is considered reasonable. Values ​​greater than this threshold require manual intervention to confirm whether recalibration is necessary.

[0173] When performing parallel correction, in addition to aligning the bed surface normal vector with the camera coordinate system Z-axis by generating a rotation matrix using the Rodrigues formula, the following methods can also be used:

[0174] The rotation calculation method based on quaternion interpolation performs quaternion interpolation between the current orientation of the robotic arm end and the target orientation to achieve smooth alignment;

[0175] Based on the homogeneous transformation matrix optimization method, the bed surface normal vector constraint is used as the optimization objective to directly optimize the pose of the robotic arm end effector.

[0176] The constraint solving method based on inverse kinematics (IK) incorporates the bed alignment constraint into the objective function of the robotic arm, and the IK solver directly outputs the end pose that satisfies the bed alignment.

[0177] Furthermore, the following alternative solutions can also be adopted:

[0178] RGBD cameras can be replaced with depth sensors such as stereo vision cameras, structured light cameras, and LiDAR.

[0179] The robotic arm can be replaced with other adjustable actuators, such as translation platforms, parallel mechanisms, or six-degree-of-freedom gimbals;

[0180] System deployment can be implemented on edge devices, PCs, or embedded hardware to meet the needs of different application scenarios.

Claims

1. A bed surface normal vector estimation method, characterized by, The method comprises the following steps: obtaining a bed surface RGB image and a depth image; extracting a bed surface region from the bed surface RGB image; dividing the extracted bed surface region into a plurality of sub-blocks, and calculating point cloud features of each sub-block based on the bed surface depth image; selecting sub-blocks of flat regions as target sub-blocks according to the calculated point cloud features; obtaining a bed surface normal vector as a weighted sum of normal vectors of each target sub-block, and the weight of a target sub-block corresponding to a flatter target sub-block is larger.

2. The method of claim 1, wherein, The point cloud features are normal vectors of the corresponding sub-blocks.

3. The method of claim 2, wherein, The step of calculating the point cloud features of each sub-block based on the bed surface depth image comprises the following steps: converting a depth image region corresponding to each sub-block into a point cloud, and then calculating a covariance matrix of each sub-block; performing eigenvalue decomposition on the covariance matrix, and extracting a feature vector corresponding to the smallest eigenvalue as a normal vector of the current sub-block.

4. The method of claim 2, wherein, The step of selecting sub-blocks of flat regions as target sub-blocks comprises the following steps: calculating curvatures of each sub-block according to the point cloud features; selecting target sub-blocks with curvatures less than a first set threshold.

5. The method of claim 4, wherein, The weight of a target sub-block corresponding to a flatter target sub-block is larger by setting the curvature of the target sub-block as the weight.

6. The method of claim 1, wherein, Before the step of calculating the point cloud features of each sub-block based on the bed surface depth image, the method further comprises the following steps:

7. The method of claim 1, wherein, statistically determining a proportion of valid pixels of each sub-block, and eliminating sub-blocks with a proportion of valid pixels less than a second set threshold.

8. The method of claim 7, wherein, Before the step of dividing the extracted bed surface region into a plurality of sub-blocks, the method further comprises the following step:

9. The method of claim 1, wherein, performing a morphological erosion operation on the extracted bed surface region. 10.A method for parallel correction of a robot pose based on a bed normal vector, a camera being provided on the robot, the camera being configured to acquire a bed RGB image and a depth image, the method comprising: determining a bed normal vector based on the bed RGB image and the depth image; determining a robot pose based on the bed normal vector; and correcting the robot pose to be parallel to the bed normal vector. Between the step of performing a morphological erosion operation on the extracted bed surface region and the step of dividing the extracted bed surface region into a plurality of sub-blocks, the method further comprises the following step: calculating a minimum circumscribed rectangle as a new bed surface region according to an outline of the bed surface region after erosion. The bed surface region is a binary mask. The method comprises the following steps: estimating a bed surface normal vector by using the method according to any one of claims 1-9; calculating a rotation matrix by using the bed surface normal vector; calculating a translation vector according to a center point coordinate of a flat region and a target height of a bed surface from a camera; correcting a mechanical arm posture based on the rotation matrix and the translation vector, so that the camera is parallel to the bed surface.

Images