CN116012442A - Visual servo method for automatically correcting pose of tail end of mechanical arm - Google Patents

Abstract

According to the visual servo method for automatically correcting the pose of the tail end of the mechanical arm, 3D point cloud data and 2D color images of a target position are collected by using a depth camera, SIFT matching is carried out on the 2D color images of the current position and the 2D color images of the target, and pixel information of n matching points is obtained; according to the pixel coordinates of the matching points, finding out corresponding 3D point cloud coordinates to serve as ICP, and obtaining a point cloud conversion matrix; and converting to a mechanical arm tail end transformation matrix through calculation. The planar image feature point is extracted by SIFT by combining SIFT and ICP algorithms in a planar-before-three-dimensional mode. And (3) performing ICP matching by using point cloud coordinates corresponding to the extracted pixel characteristics, calculating a pose transformation matrix of the target point cloud and the current point cloud, combining with an optimization SIFT algorithm, finding out the mean value and standard deviation of the Euclidean distance difference of the corresponding characteristic points on the original matching points, reducing the extraction interval of the Euclidean distance difference of the matching points, and performing secondary screening.

Description

Visual servo method for automatically correcting pose of tail end of mechanical arm

Technical Field

The invention belongs to the field of computer vision and industrial robot automatic operation, and relates to a vision servo method for automatically correcting the pose of the tail end of a mechanical arm.

Background

With the intensive research of intelligent robots, automatic operation robots for inspection of subway train bottom tracks are gradually rising. In the operation process of the robot, the motion paths of the chassis and the mechanical arm are planned, and the current work can be completed only by shooting the images of the parts at the bottom of the vehicle when the robot runs to a specified place. However, it is impossible to completely stop the train at the same position every time it stops at the station. Acceleration during the stopping of the train also affects the distance between the cars. Therefore, the working environment of the inspection robot cannot be unchanged, and the high-precision requirement of image acquisition cannot be met by simply relying on the fixed movement of the chassis and the mechanical arm.

Disclosure of Invention

Aiming at the problems in the prior art, the invention aims to provide a visual servo method for automatically correcting the pose of the tail end of a mechanical arm based on a monocular depth camera.

The invention is realized by the following technical scheme:

a visual servo method for automatically correcting the pose of the tail end of a mechanical arm comprises the following steps:

1) The inspection robot moves to a preset position, the mechanical arm moves to a preset pose, a monocular depth camera is started, and RGB images and depth images of the bottom of the train under the current pose are collected;

2) Performing SIFT feature detection and matching on the current RGB image and the actual target RGB image, filtering a matching result, and removing matching features with larger errors; the data of the actual target RGB image is transported in a patrol robot picture library;

3) Converting the matched plane feature points to pixel coordinates on a corresponding target depth map, and converting the matched points on the target depth map to three-dimensional point cloud coordinates under a camera coordinate system according to the internal parameters of the monocular depth camera; the target depth map data are stored in the inspection robot memory;

4) Calculating a pose conversion matrix between the point clouds by adopting a nearest neighbor point iterative algorithm ICP according to the coordinates of the current point cloud and the target point cloud;

5) And converting the point cloud pose conversion matrix into a conversion matrix of the current pose of the camera and the target pose of the camera through the mapping relation of the camera, and calculating a rotation matrix and a translation vector of the mechanical arm moving from the current pose to the target pose through the pose relation of the mechanical arm and the camera.

Preferably, the monocular depth camera employs an intel realsense D455 monocular depth camera and the robotic arm employs an auboi5 six degree of freedom robotic arm.

Preferably, the specific steps of performing SIFT feature detection and matching on the current RGB image and the actual target RGB image are as follows:

let n pairs of matched feature points in total, and the pixel coordinates of the matched points on the current RGB image be (x _2i ，y _2i ) The pixel coordinates of the matching points on the actual target RGB image are (x _1i ，y _1i )，

Δx _i ＝x _1i -x _2i ………………………………………………………………(1)

Δy _i ＝y _1i -y _2i ……………………………………………………………………(2)

Then:

when Deltax _i E (μx-0.5 x σx, μx+0.5 x σx) and Δy _i When E (μy-0.5 x σy, μy+0.5 x σy), the required feature point condition is satisfied.

The invention adopts a mode of firstly plane and then stereo, combines a SIFT algorithm and an ICP algorithm, and firstly uses SIFT to extract the characteristic points of the plane image before the three-dimensional point cloud space position matching is carried out. Performing ICP matching by using point cloud coordinates corresponding to the extracted pixel characteristics, calculating a pose transformation matrix of the target point cloud and the current point cloud, and combining with an optimization SIFT algorithm: in feature point filtering, the SIFT algorithm itself adopts the ratio of the Euclidean distance of adjacent feature points to the Euclidean distance of next adjacent feature points to be in a certain threshold interval so as to improve the matching accuracy. The ICP algorithm is greatly affected by the initial value, and 2D images taken at different angles often have similar vector variations for the feature descriptors of the pixels. In order to collect more effective and accurate characteristic points, on the basis of a SIFT algorithm, a statistical method is adopted to find out the mean value and standard deviation of the difference of Euclidean distances of the corresponding characteristic points on the original matching points, the extraction interval of the difference of Euclidean distances of the matching points is reduced, and secondary screening is carried out.

Drawings

Fig. 1 is a simplified monocular visual servoing schematic.

Detailed Description

The present invention is described in further detail below in conjunction with specific embodiments to provide a better understanding of the present technical solution.

The invention is implemented by using an Intel Realsense D455 monocular depth camera and an auboi5 six-degree-of-freedom mechanical arm. The method mainly comprises three steps: and (3) data acquisition, target determination, feature matching and tail end pose adjustment of the mechanical arm.

Step one, data acquisition and target determination: since the target position needs to be known, an RGB image (color 1) and a depth image (depth 1) of the target position need to be acquired before a servo experiment is performed. After the target position is determined and data are acquired, the mechanical arm enters a force control mode, and the mechanical arm is manually moved for a certain distance (including translation and rotation, and the displacement error of the detection target under the robot stop position during simulated inspection) at will.

Step two, feature matching: the pose of the tail end of the mechanical arm after the artificial movement is identified as the current pose. The camera captures an RGB image (color 2) and a depth image (depth 2) in the current pose. SIFT feature detection and matching are carried out on color1 and color2, n pairs of matched feature points are shared, and pixel coordinates of the matched points on color1 are (x _1i ，y _1i ) The matching point pixel coordinates on color2 are (x _2i ，y _2i )。

Δx _i ＝x _1i -x _2i ……………………………………………………………(1)

Δy _i ＝y _1i -y _2i ………………………………………………(2)

Then:

when Deltax _i E (μx-0.5 x σx, μx+0.5 x σx) and Δy _i When the E (mu y-0.5 x sigma y, mu y+0.5 x sigma y), the required characteristic point condition is satisfied.

Matching and filtering RGB images to the characteristic points one by oneIn the depth map, m pairs of matched feature points are obtained after filtering, and the corresponding depth map pixel coordinates are depth1 (u _1i ，v _1i )，depth2(u _2i ，v _2i )。

According to the conversion relation between pixel coordinates and camera coordinates:

converting the matched depth map pixel coordinates to camera coordinates depth1: (u) _1i ，v _1i )→P ₁ (x _1i ，y _1i ，z _1i )depth2：(u _1i ，v _1i )→P ₂ (x _2i ，y _2i ，z _2i )

Calculating to obtain P through an ICP point cloud matching algorithm ₁ And P ₂ Pose conversion matrix T of (2) _x (R _x ，t _x )。

As shown in fig. 1, a simple monocular visual servo is shown. If the mechanical arm is to be driven from T ₁ (R ₁ ，t ₁ ) (the former in brackets represents the rotation matrix and the latter represents the translation vector) motion to T ₂ (R ₂ ，t ₂ ),. The two poses are respectively seen by the monocular stereo camera: t (T) _c1 (R _c1 ，t _c1 ) T is as follows _c2 (R _c2 ，t _c2 ). The pose of the tail end of the mechanical arm to be moved is as follows: t (T) _x (R _x ，t _x ). The pose is transformed as: t ' (R ', T ').

From the geometric relationships, a system of equations can be established:

T′·T _C1 ＝T _x ·T′·T _C2 ..............(7)

T ₁ ＝T _x ·T ₂ .........................................(8)

the method can obtain the following steps:

it is known that:

/>

the method can obtain the following steps:

the T is calculated ₂ (R ₂ ，t ₂ ) The pose matrix of the pose of the tail end of the mechanical arm reaching the target is obtained.

And thirdly, adjusting the pose of the tail end of the mechanical arm. To move the mechanical arm to T ₂ Pose state, need to rotate matrix R ₂ Converted into quaternion q.

Is provided with

q＝(w，x，y，z)。

Then

Thus, the target pose (t) of the mechanical arm can be obtained ₂ Q), i.e. (X, Y, Z, w, X, Y,z)。

the results of the multiple experiments are as follows:

/>

Claims (3)

1. the visual servo method for automatically correcting the pose of the tail end of the mechanical arm is characterized by comprising the following steps of:

1) The inspection robot moves to a preset position, the mechanical arm moves to a preset pose, a monocular depth camera is started, and RGB images and depth images of the bottom of the train under the current pose are collected;

2) Performing SIFT feature detection and matching on the current RGB image and the actual target RGB image, filtering a matching result, and removing matching features with larger errors; the data of the actual target RGB image is transported in a patrol robot picture library;

3) Converting the matched plane feature points to pixel coordinates on a corresponding target depth map, and converting the matched points on the target depth map to three-dimensional point cloud coordinates under a camera coordinate system according to the internal parameters of the monocular depth camera; the target depth map data are stored in the inspection robot memory;

4) Calculating a pose conversion matrix between the point clouds by adopting a nearest neighbor point iterative algorithm ICP according to the coordinates of the current point cloud and the target point cloud;

5) And converting the point cloud pose conversion matrix into a conversion matrix of the current pose of the camera and the target pose of the camera through the mapping relation of the camera, and calculating a rotation matrix and a translation vector of the mechanical arm moving from the current pose to the target pose through the pose relation of the mechanical arm and the camera.

2. The visual servo method for automatically correcting the pose of the tail end of the mechanical arm according to claim 1, wherein the monocular depth camera is an intel realsense D455 monocular depth camera, and the mechanical arm is an auboi5 six-degree-of-freedom mechanical arm.

3. The visual servo method for automatically correcting the pose of the tail end of the mechanical arm according to claim 1, wherein the specific steps of performing SIFT feature detection and matching on a current RGB image and an actual target RGB image are as follows:

let n pairs of matched feature points in total, and the pixel coordinates of the matched points on the current RGB image be (x _2i ,y _2i ) The pixel coordinates of the matching points on the actual target RGB image are (x _1i ,y _1i )，

Δx _i ＝x _1i -x _2i ...........................(1)

Δy _i ＝y _1i -y _2i ..............................................(2)

Then:

when Deltax _i E (μx-0.5 x σx, μx+0.5 x σx) and Δy _i When E (μy-0.5 x σy, μy+0.5 x σy), the required feature point condition is satisfied.

Images