CN116372938A - Surface sampling mechanical arm fine adjustment method and device based on binocular stereoscopic vision three-dimensional reconstruction - Google Patents
Surface sampling mechanical arm fine adjustment method and device based on binocular stereoscopic vision three-dimensional reconstruction Download PDFInfo
- Publication number
- CN116372938A CN116372938A CN202310523701.2A CN202310523701A CN116372938A CN 116372938 A CN116372938 A CN 116372938A CN 202310523701 A CN202310523701 A CN 202310523701A CN 116372938 A CN116372938 A CN 116372938A
- Authority
- CN
- China
- Prior art keywords
- sampling
- mechanical arm
- point
- point cloud
- sampling device
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000005070 sampling Methods 0.000 title claims abstract description 257
- 238000000034 method Methods 0.000 title claims abstract description 49
- 239000011159 matrix material Substances 0.000 claims description 28
- 230000009466 transformation Effects 0.000 claims description 8
- 230000000007 visual effect Effects 0.000 claims description 6
- 210000000707 wrist Anatomy 0.000 claims description 6
- 238000012800 visualization Methods 0.000 claims description 3
- 238000013507 mapping Methods 0.000 claims description 2
- 238000013178 mathematical model Methods 0.000 claims description 2
- 238000012876 topography Methods 0.000 claims 1
- 238000004364 calculation method Methods 0.000 description 5
- 230000000694 effects Effects 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 230000002159 abnormal effect Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 210000003857 wrist joint Anatomy 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 238000012790 confirmation Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
- B25J9/1697—Vision controlled systems
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/1607—Calculation of inertia, jacobian matrixes and inverses
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1679—Programme controls characterised by the tasks executed
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation
Abstract
The invention discloses a surface sampling mechanical arm fine adjustment method and device based on binocular stereoscopic vision three-dimensional reconstruction. The implementation system of the method of the invention comprises: the device comprises a binocular camera (1), a multi-joint traction mechanical arm (2), a sampling device (3) and a meter taking device (4), wherein a sampling area picture is shot through the binocular camera, a sampling area point cloud is obtained through binocular stereoscopic vision three-dimensional reconstruction, and a target pose of the sampling device at a sampling point is determined according to space information contained in the point cloud so as to be used for the mechanical arm to carry out movement traction. In order to solve the problem of large deviation in mechanical arm traction, the invention compares the difference between the actual pose and the target pose of the sampling device, and utilizes the three-dimensional point cloud to analyze the sampling state and give out a reference index for an operator to evaluate and adjust. The invention can assist operators to quickly and accurately realize the fine adjustment of the meter taking and sampling mechanical arm in the meter taking and sampling task, thereby improving the sampling efficiency and the sampling quality.
Description
Technical Field
The invention relates to the technical field of mechanical arm sampling, in particular to a pose fine adjustment method and device applied to a surface sampling mechanical arm.
Background
In the mechanical arm surface sampling task, a binocular camera is often adopted as one of information acquisition means to assist in the sampling process. After correcting the images shot by the binocular camera through camera calibration, the images can be accurately matched with the pixel corresponding relation to estimate parallax, depth information is calculated, and the two-dimensional images are projected into a three-dimensional space to obtain three-dimensional point cloud information.
The sampling device is fixedly connected to the tail end of the meter sampling mechanical arm, when the mechanical arm performs a meter sampling task, the mechanical arm starts moving from the initial position posture, stops moving when reaching the target sampling position posture, and performs meter sampling operation. In the whole sampling process, the position and the posture during the surface sampling can directly influence the sampling effect and the sampling quality.
When the traction sampling device reaches a preset target position, larger deviation often occurs due to flexible deformation, uncertain posture and other reasons of the traction mechanical arm, and further fine adjustment is needed by combining other measurement means through an autonomous adjustment or manual teleoperation mode. Aiming at the problem, most of the existing methods only input the approximate target pose to enable the mechanical arm to pull the sampling device to the adjacent area of the target point, and then the sampling device gradually approaches to a better sampling pose state by means of teleoperation adjustment manually according to telemetry data and two-dimensional image information. The process has less reference information, whether the adjustment is completed or not completely depends on the judgment of an operator, and the adjustment effect cannot be intuitively fed back to the operator, so that the problems of high adjustment difficulty, complicated adjustment process and high technical experience requirement on the operator are solved. The fine adjustment process is difficult to realize fast and accuracy, the overall adjustment efficiency is low, and the adjustment quality is difficult to ensure, so that the sampling efficiency and quality are greatly affected.
In the sampling tasks such as the surface sampling of an extraterrestrial object, the time window for sampling is very limited, and higher sampling efficiency and quality are required to ensure that the device collects a sufficient amount of the sample of interest. Therefore, how to improve the overall efficiency and quality of sampling is a problem that needs to be solved by those skilled in the art.
Disclosure of Invention
Therefore, the invention designs a fine adjustment strategy determination method applied to the surface sampling mechanical arm, and can realize rapid and accurate determination and fine adjustment of the sampling target pose. According to the method, through binocular stereoscopic vision three-dimensional reconstruction, sampling state and pose deviation are analyzed by utilizing three-dimensional point cloud information of a sampling area, so that an operator can intuitively perform reference and decision, or an auxiliary mechanical arm can realize autonomous adjustment, the sampling efficiency is improved, the sampling quality is ensured, and the whole sampling process is facilitated.
In order to achieve the above purpose, the present invention adopts the following technical scheme:
a surface sampling mechanical arm fine adjustment method based on binocular stereoscopic vision three-dimensional reconstruction comprises the following steps:
s1, a binocular camera shoots a complete sampling area picture, and the shot picture is corrected according to camera internal parameters and external parameters determined by calibration of the binocular camera, so that polar lines of the shot picture are aligned;
s2, performing stereo matching on the two corrected pictures, and searching for the corresponding relation of the homonymous point estimation pixels, so that parallax information is obtained. Calculating the depth of each pixel according to the binocular camera external parameters determined by camera calibration to obtain corresponding three-dimensional coordinates, and mapping the two-dimensional images to a global coordinate system three-dimensional space to obtain sampling area point cloud information;
s3, taking coordinates of the target sampling points as sphere centers, obtaining spherical surrounding areas according to set searching radiuses, and obtaining gradient angles of the target sampling points adjacent to local areas by utilizing point fitting planes in the surrounding areas in the point cloud of the sampling areas;
s4, determining wrist and heart coordinates of the mechanical arm and pitch angles of the sampling device according to the sampling point coordinates and the slope angle information of the local area and the set sampling depth, so as to determine an accurate target pose matrix of the sampling device;
s5, the mechanical arm pulls the sampling device to reach a target area according to the target pose matrix, calculates the deviation amount of the current pose and the target pose by acquiring the actual pose of the current sampling device, and controls the mechanical arm to adjust;
s6, according to the current pose of the adjusted sampler, the accessible terrain area of the meter is analyzed and the information such as depth, angle and the like is adopted by utilizing the point cloud of the sampling area and the point cloud of the standard sampling device, the distance between each part of the sampling device and the terrain is calculated and visualized, visual information feedback is given to operators, and the operators are confirmed manually and decide whether to continue adjusting or not.
After the steps are finished, the sampling device is positioned in an ideal sampling pose, and the fine adjustment effect of surface sampling is achieved.
The specific content of S3 comprises:
(1) And establishing a KD search tree for the point cloud of the sampling area, traversing the distance from each point to the target sampling point, and reserving the points with the distance smaller than the set search radius R, namely, the point set in the spherical surrounding area adjacent to the target sampling point. Specifically, searching the nearest neighbor point of the target sampling point in the sampling area point cloud by using the KD tree, if the distance is smaller than R, counting the point set, and searching the next nearest neighbor point. Repeating the steps until the distance between the searched nearest point and the target sampling point is larger than R, and counting the point cloud points in the adjacent spherical surrounding area of all the target sampling points into a point set;
(2) And under the global coordinate system, establishing a plane mathematical model of ax+by+Cz+D=0, finding out a plane parameter which maximizes the number of points in the point set By using a RANSAC method, and determining a fitting plane. Specifically, a plurality of points are randomly selected from the point set, a plane is determined by calculating plane equation parameters, the distances from all the points in the point set to the plane are traversed, and the points with the distances smaller than a certain threshold value are regarded as inner points. After a certain number of repetitions, the plane with the largest number of interior points is the best fit plane for the point set.
(3) Obtaining a fitting plane normal vector (A, B, C) through a plane model, so as to determine an included angle between the fitting plane normal vector and a Z axis of a global coordinate system, namely a gradient angle beta:
the normal vector and the gradient angle directly reflect the space inclination condition of the adjacent area of the target sampling point and can be used as the reference quantity of the pose matrix of the subsequent calculation target.
The specific content of S4 comprises:
(1) Setting a table to obtain depth h according to actual sampling requirements;
(2) In the global coordinate system, according to the target sampling point coordinates (x 0 ,y 0 ,z 0 ) Calculating to obtain the included angle between the extending direction of the mechanical arm and the X axis of the global coordinate system, namelyAzimuth angle:
and F is the transverse total offset of the joints of the mechanical arm, namely, the transverse distance between the wrist center of the mechanical arm and the origin of the base of the mechanical arm is fixed and is determined by the assembly relation of the mechanical arm. The arm extension direction of the mechanical arm, namely the direction of the sampling device connected with the tail end, and the azimuth angle is the azimuth direction of the sampling device;
(3) In the global coordinate system, calculating the components of the gradient angle along the azimuth direction of the sampling device according to the gradient angle beta determined in the azimuth angles psi and S3 and the normal vector (A, B, C) of the fitting plane:
the meaning of this angle is that the sampling device is approximately parallel to the ground in the direction in which the sampling device is oriented after the sampling device is rotated in the pitch direction by the angle. In this way, the sampling device is ensured not to contact with the terrain when sampling is performed as much as possible. The alpha calculated in the step is the pitch angle of the sampling device under the target pose;
(4) Determining wrist and center coordinates (x, y, z) of the mechanical arm under the global coordinate system through azimuth angle, pitch angle and target sampling point coordinates:
H＝R 0 -h
x＝Lcosψ+Fsinψ
y＝Lsinψ-Fcosψ
wherein K is the rotation shaft of the wrist core-to-meter taking device of the mechanical armDistance of line, R 0 The radius of rotation of the device is determined by the mechanical arm assembly relationship. The wrist center coordinates of the mechanical arm obtained through calculation in the step can enable the connecting line of the central point of the rotation axis of the meter and the target sampling point to be perpendicular to the ground, so that the maximum sampling depth is ensured to be positioned at the target sampling point as far as possible, and the maximum sampling depth accords with the set meter sampling depth h.
(5) The wrist center of the mechanical arm is used as an origin of a coordinate system, the direction of the wrist center vertical to the rotation axis of the meter-taking device is used as an X axis, the direction vertical to the X axis in the rotation plane of the meter-taking device is used as a Y axis, a Z axis is established by a right hand rule, and a coordinate system of the sampling device is established. And determining a coordinate transformation matrix between the two coordinate systems, namely a target pose matrix, by using the azimuth angle, the pitch angle and the wrist center coordinates according to the rotation relation between the coordinate system of the sampling device and the global coordinate system.
The specific content of S5 comprises:
(1) According to the target pose matrix obtained in the step S4, sampling the structure and constraint property of the mechanical arm according to a table, and obtaining accurate joint angles of the mechanical arm by inverse solution of the pose matrix;
(2) Transmitting the joint rotation angle obtained by the solution into a mechanical arm control motor to enable the mechanical arm to move and pull a sampling device connected with the tail end to reach the position and the position close to the target position and the gesture;
(3) The binocular vision or other measuring means is used for acquiring the current pose information of the sampling device, comparing the current pose information with the target pose, and calculating to obtain the pose deviation. And converting the deviation amount into an adjustment amount of the joint angle of the mechanical arm, and transmitting the adjustment amount to a mechanical arm control motor to adjust.
The specific content of S6 comprises:
(1) According to the coordinate transformation matrix of the current pose of the sampling device, the sampling area point cloud is transferred from the global coordinate system to the coordinate system of the sampling device, namely:
wherein P is i s ampler For each point P of the point cloud under the coordinate system of the sampling device i global Is all thatThe point cloud points under the local coordinate system,
the coordinate transformation matrix from the global coordinate system to the coordinate system of the sampling device is the inverse matrix of the current pose matrix;
(2) Under the coordinate system of the sampling device, the point cloud of the sampling area is projected to an XOZ plane, the point closest to the plane origin (0, 0) is searched to obtain the point cloud projection point of the wrist center of the mechanical arm, and the point closest to the distance (K, 0) is searched to obtain the point cloud projection point of the rotation axis center of the meter acquisition device, namely the projection point of the actual sampling point. Determining an actual sampling point in the three-dimensional point cloud by the projection point on the XOZ plane, and determining a three-dimensional coordinate (x 0 ,y 0 ,z 0 ) Calculating the actual depth of the sampling point:
D＝R 0 +y 0
(3) Under the coordinate system of the sampling device, according to the distance K from the wrist center to the rotation axis of the meter acquisition device, a linear equation of the rotation axis of the meter acquisition device under the coordinate system of the sampling device is obtained:
x＝K
according to the effective acquisition width of the meter acquisition device, an X coordinate interval is set, and the distance from the meter acquisition device to the rotation axis of the meter acquisition device is calculated for all points meeting the interval requirement of X coordinates in the local area point cloud:
marking the points with the distance smaller than the radius of the tabulated device as the available points, and increasing the RGB values of the corresponding points to enable the points to have a prominent visual effect in the point cloud visualization. Finding the maximum x of the x-coordinates among all the possible points max And a minimum value x min Thereby calculate the entry angle and the exit angle of the sampling process of the surface taking device:
(4) Under a coordinate system of a sampling device, taking a three-dimensional point cloud point corresponding to an XOZ plane projection point of a wrist center of the mechanical arm as a sphere center, and searching to obtain a local area point cloud according to the method of the step S3;
(5) And establishing a KD tree for the local area point cloud under the coordinate system of the sampling device. And turning the point cloud of the standard sampling device to the coordinate system of the sampling device according to the establishment mode of the coordinate system of the sampling device. Then searching the nearest point in the local area point cloud for each standard sampling device point cloud point by utilizing the KD tree so as to obtain the minimum distance d between the point and the terrain i . After all the points have been traversed, according to d i Changing RGB values of each point of the standard scanning point cloud, so that the color of the point cloud smoothly changes among red, green and blue according to the distance from the terrain from small to large;
(6) And visualizing the three-dimensional point cloud of the sampling area and the processed point cloud of the standard sampling device together, and displaying the three-dimensional point cloud and the processed point cloud of the standard sampling device on a computer software window. The operator evaluates the reference information provided by the method and decides whether further adjustment is needed. After confirming that the error is not found, the input sampling instruction starts sampling.
In the above procedure, the parameters in the binocular camera are respectively an internal parameter matrix and a distortion coefficient of the left camera and the right camera, wherein the internal parameter matrix comprises parameters such as principal point coordinates of the camera, focal length of the lens, pixel size of the detector, and the like. The binocular camera external parameters are the coordinate rotation matrix and translation matrix between the left camera and the right camera. The transformation relation between the internal reference matrix corresponding to the camera coordinate system and the image coordinate system can be uniquely determined through camera calibration. The distortion coefficient represents the degree of distortion of the camera lens, which can be determined via camera calibration and used for de-distortion of the image. The camera extrinsic coordinate transformation matrix represents the transformation relation of the space point coordinates under the left camera coordinate system and the right camera coordinate system, and can be uniquely determined through camera calibration.
In the above flow, in order to ensure the speed and quality of the point cloud analysis, the three-dimensional point cloud in the down-sampling area of the global coordinate system reconstructed in the step S2 may be subjected to down-sampling and outlier removal processing. The specific step of downsampling is to divide the voxel grid according to the set size so as to completely cover the whole point cloud. For each voxel grid, the gravity centers of all points cloud points in the voxel grid are taken as representative points, and the points are used for replacing points in the voxels. After representative point replacement is carried out on all voxels, a new point cloud after downsampling is obtained. The processing mode is at the cost of slight precision loss, greatly reduces the calculation amount of the process, and is suitable for application occasions with higher real-time requirements. The specific step of outlier removal is to establish a KD tree for each point in the point cloud, search the nearest ten points by using the KD tree, calculate the average value of the distances from the point to the adjacent ten points, and if the average value is larger than a set threshold value, consider the outlier and remove the outlier. The processing mode can effectively remove abnormal values caused by pixel matching failure in the reconstruction process, and prevent interference caused by the abnormal values.
In the above flow, the target sampling point may be determined by an operator evaluating the region of interest and the obstacle distribution according to the three-dimensional point cloud information of the sampling region under the global coordinate system reconstructed in step S2. The coordinates of the target sampling points in the global coordinate system can be specified by other means, and can be determined by searching the nearest neighbor point and projecting the nearest neighbor point onto the reconstructed three-dimensional point cloud.
Further, in the above procedure, the mechanical arm may pull the sampling device to reach, and all terrain areas where sampling is planned to be performed should be completely photographed by the binocular camera.
Further, the search radius of the adjacent spherical surrounding area should be set according to the actual size of the sampling device. Specifically, the wrist center of the mechanical arm is taken as a sphere center, a sphere with a determined searching radius is used, and the whole sampling device is completely enclosed and a certain margin is reserved. Therefore, the projection of the sampling device on the terrain point cloud during calculation can be guaranteed to be positioned in the local area point cloud determined by the searching radius. On the basis, the sphere should be reduced as much as possible to reduce the calculation amount.
Further, the meter sampling device in the step is a sampling component which can rotate around a fixed axis, and meter sampling is realized on the sampling device through rotary motion.
Further, the standard sampling device point cloud in the step is the complete scanning point cloud of the sampling device or the standard point cloud obtained through the three-dimensional engineering model of the sampling device, and the complete appearance structure of the sampling device can be accurately reflected.
The invention provides a device for a fine adjustment method of a table sampling mechanical arm based on binocular stereoscopic vision three-dimensional reconstruction, which is used for executing the fine adjustment method of the table sampling mechanical arm based on binocular stereoscopic vision three-dimensional reconstruction.
According to the technical scheme, the invention discloses a table sampling mechanical arm fine adjustment method based on binocular stereoscopic vision three-dimensional reconstruction, and the detailed and accurate analysis of the sampling condition of a sampling device is performed by utilizing three-dimensional space information contained in binocular images through binocular stereoscopic vision three-dimensional reconstruction. On one hand, the method can calculate the pose of the target sampling device according to the expected sampling target point, so that the mechanical arm can automatically and quickly pull the sampling device to reach the expected position pose; on the other hand, the method can give an adjustment reference strategy and intuitively display the sampling key reference information to operators, thereby assisting in realizing rapid and accurate pose adjustment operation and improving the overall efficiency and sampling quality of the sampling process. Compared with the prior art, the invention solves the problems of low fine adjustment efficiency of the sampling device in the meter taking and sampling task and higher fine adjustment difficulty caused by less reference information, and can be used for a technician to simply and conveniently carry out the fine adjustment work of the meter taking and sampling mechanical arm, thereby ensuring the smooth development and completion of the sampling process to a great extent.
Drawings
Fig. 1 is a schematic diagram showing an example of the system configuration according to the embodiment of the present invention.
Wherein: 1. the binocular camera is fixedly connected with the mechanical arm base, and the field of view of the camera can shoot a complete sampling area; 2. a multi-joint traction mechanical arm; 3. the sampling device is connected with the tail end joint of the mechanical arm; 4. the meter sampling device is positioned at the front end of the sampling device and can perform rotary motion around the axis to sample.
Fig. 2 is a schematic diagram of an example of a coordinate system of a sampling device according to an embodiment of the present invention.
FIG. 3 is a flowchart of determining a gradient angle of a target sampling point adjacent area in an embodiment of the present invention.
Fig. 4 is a flowchart of determining a target pose matrix in an embodiment of the invention.
Detailed Description
Embodiments of the present invention will be described below with reference to the accompanying drawings.
The invention discloses a fine adjustment method of a surface sampling mechanical arm based on binocular stereoscopic vision three-dimensional reconstruction, in the embodiment, the composition of an implementation system is shown as shown in figure 1, the method comprises a multi-joint surface sampling mechanical arm (2) and a binocular camera (1) fixedly connected with a mechanical arm base, the mechanical arm is provided with four joint angles of shoulder yaw, shoulder pitch, elbow pitch and wrist pitch, a sampling device (3) is connected at the wrist joint at the tail end of the mechanical arm, and a digging type surface sampling device (4) capable of rotating around a fixed axis is arranged at the front end of the sampling device. The specific implementation steps are as follows:
s1, controlling a mechanical arm to pull a sampling device so that the mechanical arm moves out of the visual field range of a binocular camera, and transmitting a camera shooting instruction to enable the binocular camera to shoot a complete sampling area without shielding;
s2, correcting Ji Jixian the shot binocular image of the sampling area by using the binocular camera calibration parameters, three-dimensionally reconstructing the binocular image of the sampling area after three-dimensional matching to obtain a point cloud of the sampling area, and transferring the point cloud of the sampling area to a coordinate system of the mechanical arm through a hand-eye calibration matrix;
s3, according to the flow shown in FIG. 3, firstly establishing a KD tree by utilizing the three-dimensional point cloud of the sampling area, then searching the nearest point of the target sampling point in the point cloud of the sampling area, counting a point set if the distance is smaller than R, and searching the next nearest point. And repeating the steps until the distance between the searched nearest neighbor point and the target sampling point is greater than R, and determining a neighbor region surrounding point set of the target sampling point. And then randomly selecting a plurality of points from the point set, determining a plane ax+by+cz+d=0, traversing the distances from all the points in the point set to the plane, and taking the points with the distances smaller than a certain threshold value as inner points. The process of determining the plane by random point selection is repeated continuously, and the result is updated when the plane is determined to have more interior points. After a certain number of iterations, a fitting plane with a better surrounding point set can be obtained, and a fitting plane normal vector (A, B, C) and a gradient angle beta are obtained:
s4, establishing a coordinate system of the sampling device by referring to FIG. 2, taking the wrist center of the mechanical arm as an origin of the coordinate system, taking the direction of the rotation axis of the device as an X axis according to the vertical direction of the wrist center, taking the vertical X axis in the rotation plane of the device as a Y axis, and establishing a Z axis by a right hand rule. According to the flow shown in fig. 4, the table depth h and the target sampling point coordinates (x 0 ,y 0 ,z 0 ) According to the target sampling point coordinates (x 0 ,y 0 ,z 0 ) Calculating azimuth angle psi:
and F is the transverse total offset of the joints of the mechanical arm, namely, the transverse distance between the wrist center of the mechanical arm and the origin of the base of the mechanical arm is fixed and is determined by the assembly relation of the mechanical arm.
Then, the pitch angle α is calculated from the slope angle β determined in the azimuth angle ψ and S3 steps and the fitting plane normal vector (a, B, C):
and determining wrist center coordinates (x, y, z) under a mechanical arm coordinate system according to the table depth h, the azimuth angle psi and the pitch angle alpha:
H＝R 0 -h
x＝L cosψ+F sinψ
y＝L sinψ-F cosψ
wherein K is the distance from the wrist center of the mechanical arm to the rotation axis of the meter acquisition device, R 0 The radius of rotation of the device is determined by the mechanical arm assembly relationship.
Finally, according to the coordinate system rotation relation between the traction mechanical arm and the wrist joint connecting sampling device, determining a target pose matrix T of the sampling device by the azimuth angle psi, the pitch angle alpha and the wrist center coordinates (x, y, z) of the mechanical arm:
s5, inversely solving the target pose matrix into four joint angles of the mechanical arm according to the size information of the mechanical arm, and transmitting the four joint angles into a motor of the mechanical arm to enable the mechanical arm to drag the sampling device to reach the approximate target position pose;
s6, a camera shooting instruction is transmitted to enable the binocular camera to shoot images of which the view field contains the sampling device, visual feature points and geometric outlines of the sampling device in the binocular images are matched by utilizing image features, and current pose information T' of the sampling device is calculated according to binocular stereoscopic vision original understanding;
s7, calculating corresponding XYZ coordinates and pitch angle information (x ', y ', z ', alpha ') under the current pose T ' according to constraint properties of the four-joint mechanical arm by using the current pose information:
x′＝T′ 14 y′＝T′ 24 z′＝T′ 34
R′＝R z (-ψ′) -1 T′
α′＝-arccos(R′ 11 )
meanwhile, according to the target pose XYZ coordinates and the pitch angle information (x, y, z, alpha) determined in the step S4, the adjustment amounts (Deltax, deltay, deltaz, deltaalpha) are obtained by subtraction:
(Δx,Δy,Δz,Δa)＝(x,y,z,α)-(x′,y′,z′,α′)
s8, combining the adjustment amounts (delta x, delta y, delta z and delta alpha) with the joint angles of the current mechanical arm, reversely solving to obtain the adjustment amounts of four joint angles, and transmitting the adjustment amounts into a mechanical arm motor to carry out small adjustment on the position and the posture of the sampling device;
s9, repeatedly updating current pose information, scanning the point cloud by using a three-dimensional point cloud and a known standard sampling device, analyzing the accessible terrain area of a current pose downsampling device, taking depth, angle and other information, calculating the distance from each part of the sampling device to the terrain, performing color visualization, evaluating the reference information by an operator, and determining whether to continue to adjust;
s10, after confirmation of personnel without further adjustment, a sampling instruction is transmitted, and the surface acquisition device performs power-on movement to collect the sample.
In this embodiment, the robot arm coordinate system is taken as the global coordinate system.
In this embodiment, it is known that the scanning point cloud of the standard sampling device can be obtained by scanning the sampling device by the laser scanning system alone.
In this embodiment, the binocular camera, the mechanical arm base and the ground are fixedly connected, that is, the mechanical arm base and the binocular camera do not move relative to the ground in the whole process.
Claims (10)
1. A surface sampling mechanical arm fine adjustment method based on binocular stereoscopic vision three-dimensional reconstruction is characterized by comprising the following steps:
s1, a binocular camera shoots a complete sampling area picture, and the shot picture is corrected according to camera internal parameters and external parameters determined by calibration of the binocular camera;
s2, performing stereo matching on the two corrected pictures, estimating a pixel corresponding relation to obtain parallax information, and then calculating depth according to camera external parameters, and mapping a two-dimensional image to a three-dimensional space of a global coordinate system to obtain sampling area point cloud information;
s3, taking coordinates of the target sampling points as sphere centers, obtaining spherical surrounding areas according to set searching radiuses, and obtaining gradient angles of the target sampling points adjacent to local areas by utilizing point fitting planes in the surrounding areas in the point cloud of the sampling areas;
s4, determining wrist and heart coordinates of the mechanical arm and pitch angles of the sampling device according to the sampling point coordinates and the slope angle information of the local area and the set sampling depth, so as to determine an accurate target pose matrix of the sampling device;
s5, the mechanical arm pulls the sampling device to reach a target area according to the target pose matrix, calculates the deviation amount of the current pose and the target pose by acquiring the actual pose of the current sampling device, and controls the mechanical arm to adjust;
s6, according to the current pose of the adjusted sampler, the accessible terrain area of the meter is analyzed and the information such as depth, angle and the like is adopted by utilizing the point cloud of the sampling area and the point cloud of the standard sampling device, the distance between each part of the sampling device and the terrain is calculated and visualized, visual information feedback is given to operators, and the operators are confirmed manually and decide whether to continue adjusting or not.
2. The method for fine tuning a surface sampling mechanical arm based on binocular stereoscopic three-dimensional reconstruction according to claim 1, wherein the specific content of S3 comprises:
(1) Establishing a KD search tree for the point cloud of the sampling area, traversing the distance from each point to the target sampling point, and reserving the points with the distance smaller than the set search radius R, namely, the point set in the spherical surrounding area adjacent to the target sampling point;
(2) Establishing a plane mathematical model of ax+by+Cz+D=0, finding out a plane parameter which maximizes the number of points in a point set By using a RANSAC method, and determining a fitting plane;
(3) Obtaining a fitting plane normal vector (A, B, C) through a plane model, thereby determining an included angle between the fitting plane normal vector and a Z axis of a global coordinate system, namely a gradient angle:
3. the method for fine tuning a table sampling mechanical arm based on binocular stereoscopic three-dimensional reconstruction according to claim 1, wherein the specific content of S4 comprises:
(1) Setting a table to obtain depth h according to actual sampling requirements;
(2) In the global coordinate system, according to the target sampling point coordinates (x 0 ,y 0 ,z 0 ) Calculating to obtain an included angle between the arm expanding direction of the mechanical arm and the X axis of the global coordinate system, namely an azimuth angle:
wherein F is the transverse total offset of the joints of the mechanical arm, namely, the transverse distance between the wrist center of the mechanical arm and the origin of the base of the mechanical arm is fixed and is determined by the assembly relation;
(3) Under the global coordinate system, calculating the component of the gradient angle along the azimuth direction of the sampling device according to the azimuth angle, namely the pitch angle of the sampling device:
(4) Determining wrist and center coordinates (x, y, z) of the mechanical arm under the global coordinate system through azimuth angle, pitch angle and target sampling point coordinates:
H＝r 0 -h
x＝Lcosψ+Fsinψ
y＝Lsinψ-Fcosψ
wherein K is the distance from the wrist center of the mechanical arm to the rotation axis of the meter acquisition device, R 0 The radius of rotation of the device is measured and determined by the assembly relation;
(5) The wrist center of the mechanical arm is taken as an origin of a coordinate system, the direction of the wrist center vertical to the rotation axis of the meter device is taken as an X axis, the direction of the vertical X axis in the rotation plane of the meter device is taken as a Y axis, a Z axis is established by a right hand rule, a coordinate system of the sampling device is established, and a coordinate transformation matrix between the two coordinate systems, namely a target pose matrix, is determined by using an azimuth angle, a pitch angle and wrist center coordinates according to the rotation relation between the coordinate system of the sampling device and a global coordinate system.
4. The method for fine tuning a surface sampling mechanical arm based on binocular stereoscopic three-dimensional reconstruction according to claim 1, wherein the specific content of S5 comprises:
(1) According to the target pose matrix obtained in the step S4, sampling the structure and constraint property of the mechanical arm according to a table, and obtaining accurate joint angles of the mechanical arm by inverse solution of the pose matrix;
(2) Transmitting the joint rotation angle obtained by the solution into a mechanical arm control motor to enable the mechanical arm to move and pull a sampling device connected with the tail end to reach the position and the position close to the target position and the gesture;
(3) The binocular vision or other measuring means is used for acquiring current pose information of the sampling device, comparing the current pose information with a target pose, calculating to obtain a pose deviation amount, converting the deviation amount into an adjustment amount of a joint angle of the mechanical arm, and transmitting the adjustment amount into a mechanical arm control motor to implement adjustment.
5. The method for fine tuning a surface sampling mechanical arm based on binocular stereoscopic three-dimensional reconstruction according to claim 1, wherein the specific content of S6 comprises:
(1) According to the coordinate transformation matrix of the current pose of the sampling device, the sampling area point cloud is transferred from the global coordinate system to the coordinate system of the sampling device, namely:
(2) Under the coordinate system of the sampling device, projecting the point cloud of the sampling area to an XOZ plane, searching the point closest to the plane origin (0, 0) to obtain the point cloud projection point of the wrist center of the mechanical arm, searching the point closest to the distance (K, 0) to obtain the point cloud projection point of the rotation axis center of the meter acquisition device, namely the projection point of the actual sampling point, determining the actual sampling point in the three-dimensional point cloud through the projection point on the XOZ plane, and determining the three-dimensional coordinate (x 0 ,y 0 ,z 0 ) Calculating the actual depth of the sampling point:
D＝R 0 +y 0
(3) Under the coordinate system of the sampling device, according to the distance K from the wrist center to the rotation axis of the meter acquisition device, a linear equation of the rotation axis of the meter acquisition device under the coordinate system of the sampling device is obtained:
x＝K
according to the effective acquisition width of the meter acquisition device, an X coordinate interval is set, and the distance from the meter acquisition device to the rotation axis of the meter acquisition device is calculated for all points meeting the interval requirement of X coordinates in the local area point cloud:
marking points with the distance smaller than the radius of the table taking device as the available points, and increasing RGB values of the corresponding points to enable the points to have a prominent visual effect in point cloud visualization, wherein the maximum value x of x coordinates is found in all available points max And a minimum value x min Thereby calculating the soil-entry angle lambda of the sampling process of the surface taking device in And go outSoil angle lambda out ：
(4) Under a coordinate system of a sampling device, taking a three-dimensional point cloud point corresponding to an XOZ plane projection point of a wrist center of the mechanical arm as a sphere center, and searching to obtain a local area point cloud according to the method of the step S3;
(5) Setting up KD tree for local area point cloud under sampling device coordinate system, transferring standard sampling device point cloud to sampling device coordinate system according to setting up mode of sampling device coordinate system, then searching nearest neighbor point in local area point cloud for each standard sampling device point cloud point by KD tree so as to obtain minimum distance d of point from topography i After all the points are traversed, according to d i Changing RGB values of each point of the standard scanning point cloud, so that the color of the point cloud smoothly changes among red, green and blue according to the distance from the terrain from small to large;
(6) And visualizing the three-dimensional point cloud of the sampling area and the processed point cloud of the standard sampling device together, displaying the three-dimensional point cloud and the processed point cloud of the standard sampling device on a computer software window, evaluating by an operator according to the reference information provided by the method, and inputting a sampling instruction to start sampling after confirming no errors.
6. The method for fine tuning a surface sampling manipulator based on binocular stereoscopic three-dimensional reconstruction according to claim 1, wherein the manipulator can pull the sampling device to reach and plan all terrain areas to be sampled to be completely shot by a binocular camera.
7. The fine adjustment method of the surface sampling mechanical arm based on binocular stereoscopic vision three-dimensional reconstruction of claim 1, wherein the search radius of the adjacent spherical surrounding area is set according to the actual size of the sampling device, and when the wrist center of the mechanical arm is taken as the center of sphere, the whole sampling device can be completely surrounded according to the set radius, and a certain margin is reserved.
8. The fine adjustment method of a surface sampling mechanical arm based on binocular stereoscopic vision three-dimensional reconstruction according to claim 1, wherein the surface sampling device is a sampling component capable of rotating around a fixed axis, and surface sampling is realized on a sampling device through rotary motion.
9. The fine adjustment method of the surface sampling mechanical arm based on binocular stereoscopic vision three-dimensional reconstruction of claim 1 is characterized in that the point cloud of the standard sampling device is the complete scanning point cloud of the sampling device or the standard point cloud obtained through the three-dimensional engineering model of the sampling device can accurately reflect the complete appearance structure of the sampling device.
10. A device for performing the fine adjustment method of the table sampling mechanical arm based on binocular stereoscopic three-dimensional reconstruction, which is characterized in that the fine adjustment method of the table sampling mechanical arm based on binocular stereoscopic three-dimensional reconstruction is performed according to any one of claims 1 to 9.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310523701.2A CN116372938A (en) | 2023-05-10 | 2023-05-10 | Surface sampling mechanical arm fine adjustment method and device based on binocular stereoscopic vision three-dimensional reconstruction |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310523701.2A CN116372938A (en) | 2023-05-10 | 2023-05-10 | Surface sampling mechanical arm fine adjustment method and device based on binocular stereoscopic vision three-dimensional reconstruction |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116372938A true CN116372938A (en) | 2023-07-04 |
Family
ID=86961839
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310523701.2A Pending CN116372938A (en) | 2023-05-10 | 2023-05-10 | Surface sampling mechanical arm fine adjustment method and device based on binocular stereoscopic vision three-dimensional reconstruction |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116372938A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116977328A (en) * | 2023-09-19 | 2023-10-31 | 中科海拓(无锡)科技有限公司 | Image quality evaluation method in active vision of vehicle bottom robot |
-
2023
- 2023-05-10 CN CN202310523701.2A patent/CN116372938A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116977328A (en) * | 2023-09-19 | 2023-10-31 | 中科海拓(无锡)科技有限公司 | Image quality evaluation method in active vision of vehicle bottom robot |
| CN116977328B (en) * | 2023-09-19 | 2023-12-19 | 中科海拓(无锡)科技有限公司 | Image quality evaluation method in active vision of vehicle bottom robot |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111775146B (en) | Visual alignment method under industrial mechanical arm multi-station operation | |
| CN112505065B (en) | Method for detecting surface defects of large part by indoor unmanned aerial vehicle | |
| CN108177143A (en) | A kind of robot localization grasping means and system based on laser vision guiding | |
| CN112949478A (en) | Target detection method based on holder camera | |
| CN114066983A (en) | Intelligent supplementary scanning method based on two-axis rotary table and computer readable storage medium | |
| CN116372938A (en) | Surface sampling mechanical arm fine adjustment method and device based on binocular stereoscopic vision three-dimensional reconstruction | |
| US20020150288A1 (en) | Method for processing image data and modeling device | |
| CN111060006A (en) | Viewpoint planning method based on three-dimensional model | |
| CN112658643B (en) | Connector assembly method | |
| CN112862878A (en) | Mechanical arm trimming method based on 3D vision | |
| CN113724337B (en) | Camera dynamic external parameter calibration method and device without depending on tripod head angle | |
| Olson et al. | Wide-baseline stereo vision for Mars rovers | |
| CN111047631A (en) | Multi-view three-dimensional point cloud registration method based on single Kinect and round box | |
| CN108694713B (en) | Stereo vision based satellite-rocket docking ring local ring segment identification and measurement method | |
| WO2023207257A1 (en) | Tailing dam surface deformation patrol method based on rail-mounted rail robot | |
| CN116766194A (en) | Binocular vision-based disc workpiece positioning and grabbing system and method | |
| JP3668769B2 (en) | Method for calculating position / orientation of target object and method for calculating position / orientation of observation camera | |
| CN111833392A (en) | Multi-angle scanning method, system and device for mark points | |
| CN116872216B (en) | Robot vision servo operation method based on finite time control | |
| Pachidis et al. | Vision-based path generation method for a robot-based arc welding system | |
| CN112508933A (en) | Flexible mechanical arm movement obstacle avoidance method based on complex space obstacle positioning | |
| Ilg et al. | Reconstruction of rigid body models from motion distorted laser range data using optical flow | |
| CN115164752B (en) | Self-adaptive measurement equipment and method for gap and step difference of butt joint of large part | |
| CN111477318A (en) | Virtual ultrasonic probe tracking method for remote control | |
| CN115971635A (en) | Friction stir welding control method and system based on visual sensing and machine tool |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |