WO2021133186A1

WO2021133186A1 - Method for controlling robotic manipulator

Info

Publication number: WO2021133186A1
Application number: PCT/RU2019/001001
Authority: WO
Inventors: Андрей Владимирович НОВИКОВ; Владимир Николаевич ГЕРАСИМОВ; Роман Александрович ГОРБАЧЕВ; Никита Евгеньевич ШВИНДТ; Владимир Иванович НОВИКОВ; Андрей Евгеньевич ЕФРЕМЕНКО; Дмитрий Леонидович ШИШКОВ; Михаил Нилович ЗАРИПОВ; Филипп Александрович КОЗИН; Алексей Михайлович СТАРОСТЕНКО
Original assignee: Общество С Ограниченной Ответственностью "Нейроассистивные Технологии"
Priority date: 2019-12-23
Filing date: 2019-12-23
Publication date: 2021-07-01

Abstract

The invention relates to robotic systems for handling objects by means of a manipulator with a gripping device. The claimed method for controlling a robotic manipulator comprises selecting an area for projection of a beam from a laser emitter by determining a gaze direction; receiving an image of the scene around the projection area; determining the possibility of projecting a beam onto the projection area; selecting the optimal point for the laser emitter position; determining the current position of the laser emitter in a system of coordinates of a stereoscopic camera of the scene; bringing the laser emitter to the optimal point; and projecting a beam from the laser emitter beam onto the projection area. The method can be realised by means of a robotic system comprising a manipulator with a laser emitter mounted thereon, a stereoscopic camera of the scene and a stereoscopic camera of the eyes. The technical result of the invention consists in increasing the degree of accuracy of controlling a light pointer, more particularly a laser pointer, by means only of the user's gaze.

Description

METHOD FOR CONTROL OF A ROBOTIC MANIPULATOR

The invention relates to robotics, in particular, to robotic systems for easy control of pointing devices such as a laser pointer.

For patients with limited hand mobility, one of the urgent tasks is direct interaction with environmental objects and other people. That said, it is sometimes important to be able to point to the object of interest directly, rather than through the application interface displayed on the screen. This can be done, for example, with a laser pointer.

From US patent US 8442661 known telepresence system, which includes a mobile robot equipped with a laser and a user interface device. The laser can be targeted by the robot operator through a user interface device. The user interface device contains a head tracking module and an input module including, for example, a joystick, mouse, keyboard, trackball, touchpad. The disadvantage of this system is the inability to use it by users with limited hand mobility.

In the application for US patent US20150331484 it is proposed to enter an animated laser pointer into the image perceived by the camera of the user device and control its movement by the user's gaze. The user's gaze is tracked by an eye-tracking device. The disadvantage of this system is the mandatory use of an indicator (spots of an animated laser pointer) not on a real object, but on the image displayed on the screen, which, among other things, reduces the positioning accuracy of the laser pointer on the object.

A. Petrushin, B. Giacinto, M. Leonardo Gaze-controlled Laser Pointer Platform for People with Severe Motor Impairments: Preliminary Test in Telepresence (published in Conference proceedings: Annual International Conference of the IEEE Engineering in Medicine and Biology Society. IEEE Engineering in Medicine and Biology Society. Conference. 2018.1813-1816, DOI: 10.1109 / EMBC.2018.8512722), the task is to ensure the possibility of direct interaction of the patient with the environment its environment and other people by pointing at objects with a laser beam. To solve this problem, a mobile platform equipped with a laser pointer and a video camera has been proposed. The mobile platform is able to move in space at the direction of the patient. The scene recorded by the video camera of the mobile platform is streamed to the patient's device. The patient controls the mobile platform by moving or keeping the gaze on the patient's device. The disadvantages of this solution include the following: the patient is forced to point only to objects in the field of view of the video camera of the mobile platform; the patient is forced to control the mobile platform through the patient's device, that is, the patient himself perceives the environment not directly, but through the screen; the mode of providing streaming video requires a sufficiently reliable communication channel.

Thus, there is a problem of developing such a method for controlling a robotic arm, which provides the user with the ability to directly point to the object of interest, while ensuring high positioning accuracy of the pointing device using only the user's gaze, i.e. without using additional tools such as joystick, keyboard, etc.

The technical result of the invention is to improve the control accuracy of the light pointer, in particular the laser pointer, when using only the user's gaze.

The problem is solved, and the claimed technical result is achieved in the proposed method for controlling a robotic manipulator containing a laser emitter, a stereo scene camera for obtaining images of the scene and a stereo eye camera for obtaining images of the left eye and right eye. The method includes selecting a projection area of a beam of a laser emitter by determining a direction of sight; obtaining an image of the scene around the projection area; determination of the possibility of beam projection onto the projection area; selection of the optimal point for the position of the laser emitter; determination of the current position of the laser emitter in the coordinate system of the scene stereo camera; approach of the laser emitter to the optimal point; and projection of the laser emitter beam onto the projection area. The problem is solved, and the claimed technical result is also achieved in particular embodiments of the method for controlling a robotic arm, which, however, the present invention is not limited to.

If it is determined that it is impossible to project the beam onto the projection area, it is preferable to determine the points of the position of the laser emitter, from which the projection area is reachable, taking into account the scene image around the projection area and the dimensions of the obstacles.

In addition, to determine the coordinates of the current position of the laser emitter in the coordinate system of the stereo camera, it is preferable if: select the first target point, the second target point, the third target point and the fourth target point, not lying on the same plane; sequentially direct the beam to the specified target points and determine the coordinates of the specified target points in the coordinate system of the scene stereo camera; determining the distance from the first target point to the laser emitter, the distance from the second target point to the laser emitter and the distance from the third target point to the laser emitter in the coordinate system of the scene stereo camera, and the position of the fourth target point is used to unambiguously determine the indicated distances; build an orthonormal coordinate system Oxyz with unit unit vectors i, j, k, while: select the first point by the origin, select the unit unit vector passing from the first target point to the second target point, with the direction of the unit unit unit i in the direction Ox, perform Gram orthogonalization -Schmidt of the vector passing from the first target point to the third target point, relative to the unit reference unit / to obtain the unit unit unit unit j in the Oy direction, perform the vector product of the unit unit unit vectors i and j to obtain the unit unit unit unit k in the Oz direction, and then perform the decomposition of the position vector of the laser emitter in the Oxyz coordinate system on the basis of the scene stereo camera coordinate system.

Further, the claimed invention and some variants of its implementation are explained in more detail with reference to the figure, which schematically shows a block diagram of an algorithm for implementing the projection of a laser emitter beam onto a user-selected projection area.

The figure uses the following abbreviations: LI - laser emitter; SC - coordinate system.

As a robotic manipulator, any known robotic system can be used, which is a manipulator with a laser emitter installed on it, as well as including a stereo camera of the scene and a stereo camera of the eyes.

Instead of a laser emitter, any known directional light source can be used, for example a semiconductor LED with a focusing lens system, therefore the term "laser emitter" should not be understood as limiting this invention to the use of only a laser emitter therein.

A stereo scene camera is designed to capture an image of the surrounding area, also called a scene. The stereo eye camera is used to acquire images of the left eye and right eye through its constituent left eye cameras and right eye cameras.

According to the operation algorithm of the claimed system, shown in the figure, in order to project the laser emitter beam onto the projection area selected by the user, it is necessary to obtain images of the left eye and the right eye using a stereo camera of the eyes for subsequent determination of the desired projection area of the laser emitter beam.

In this step, an image of the left eye and an image of the right eye are obtained containing glare from the light sources of the left eye and the right eye, which may be located near the stereo camera of the eyes. Next, the position of the pupil of the eye is determined, the position and numbering of the highlights on the cornea of the eye are determined, the optical axis of the direction of the gaze of each eye is determined, and the direction of gaze is already determined from it.

When determining the position of the pupil of each eye on the image of each eye, a preliminary search for the pupil is performed, a preliminary ellipse of the pupil is constructed, and an ellipse of the pupil is constructed from its nodal points. When determining the position and numbering of flares on the cornea of each eye in the eye image, searching for flares, calculating the size of the iris, eliminating flares outside the iris, and numbering the flares to determine the gaze direction vector. When determining the optical axis of the gaze direction of each eye, the nodal point of the eye, the refractive point for the center of the pupil, and the position of the center of the pupil in the coordinate system of the scene camera are determined. The determination of the gaze direction is performed based on the determined optical gaze direction and the gaze direction calibration.

For preliminary search of the pupil, a preliminary position of the center of the pupil can be determined, as well as the number of pixels in the area of the pupil, which preliminarily characterizes its size. When constructing the preliminary pupil ellipse, it is preferable if the binarization threshold is found in the preliminary pupil area and binarization is performed to determine the pupil boundary to construct the preliminary pupil ellipse. To construct an ellipse of the pupil from the anchor points of the boundary of the preliminary ellipse of the pupil, you can use the method of least squares. In this case, it is preferable to filter the nodal points so that they form a convex figure.

The search for highlights on the cornea of the eye can be performed, for example, by thresholding the image of the pelvis with the selection of clusters and filtering the clusters by brightness, size and the parameter of deviation from roundness.

When calculating the size of the iris, it is preferable to use information about the average size of the human iris and information about the distance from the corresponding camera of the left eye or the camera of the right eye to the pupil, which improves the accuracy of determining the direction of gaze.

Flare numbering can be performed from one flare from the upper pair closest to the bridge of the nose, in a circle, away from the bridge of the nose, i.e. clockwise for the right eye and counterclockwise for the left eye.

When determining the optical axis of the gaze direction of each eye, the nodal point of the eye, the refractive point for the center of the pupil, and the position of the center of the pupil in the coordinate system of the scene camera are determined. In addition, the step of determining the gaze direction may include calibration of the gaze direction, which is performed either at one of the indicated steps or in advance. In this case, it is enough to calibrate once for a specific user, and re-calibration is no longer required. In particular, when calibrating the direction of gaze, the individual characteristics of the user and the relative position of the cameras of the left and right eyes and the scene camera are taken into account.

After the selection of the projection area of the laser emitter corresponding to the viewing direction, the step of obtaining information about the scene around the projection area by means of the scene stereo camera is carried out.

As one of the options for obtaining information about the scene, a method for obtaining a set of objects of a three-dimensional scene can be used, in which images of frames from the left camera and the right camera are simultaneously obtained as part of the scene stereo camera, for each image point with pixel coordinates a disparity map is formed by the method of semi-global establishment of stereo correspondences , according to the disparity map, the true coordinates of the specified point are determined (i.e., the coordinates of the point in the coordinate system of the scene stereo camera), the map of the depths of the points in true coordinates is formed, a two-dimensional image in the gray scale is formed, in which the brightness of the point depends on the true distance to the point, and on the obtained two-dimensional image in gray scale, detection and identification of objects is performed by one of the methods selected from the Viola-Jones method (R. Viola, MJ Jones. Robust Real-Time Face Detection International Journal of Computer Vision 57 (2), 137-154, 2004), SSD-mobilenet neural network method (see, n for example, Chinese patent application CN109398688) and the Mask R-CNN neural network method (Kaiming He, Georgia Gkioxari, Piotr Dollar Ross Girshick. Mask R-CNN; published on 01.24.2018, available at the link on the Internet https://arxiv.org/pdf/1703.06870.pdf). with obtaining a set of objects of a three-dimensional scene. In this case, the disparity map is formed by the method of semi-global establishment of stereo correspondences (Semi-Global Matching, or SGM; the method is described, for example, in Heiko Hirschmuller. Accurate and Efficient Stereo Processing by Semi-Global Matching and Mutual Information. IEEE Conference on Computer Vision and Pattern Recognition ( CVPR), San Diego, CA, USA, June 20-26, 2005). The true coordinates of a point are determined taking into account focal lengths of cameras in the scene stereo camera and the distance between them. The brightness of a point is assumed to be zero if the true distance to it is outside the specified range.

The disparity map is a visual display of shifts between equally spaced fragments of images of the left and right cameras of the scene stereo camera (the closer the scene point is, the greater these shifts). As you know, this "discrepancy" can be represented as a numeric array, the elements of which show the difference in pixels of the points of the right and left images, tied to one of them. Rectification of images from different angles (alignment of the right and left images horizontally) allows you to reduce the dimension of the array - to reduce it to two-dimensional. For ease of perception, this matrix is presented in a graphical form: the greater the discrepancy between the images, the brighter the corresponding pixels in the image.

To construct disparity maps, a number of algorithms are used, generally subdivided into three classes: local, global, and semi-global (partially global).

Local algorithms calculate disparity separately for each pixel, while taking into account information only from its narrow neighborhood. The algorithms mainly use square or rectangular windows of a fixed size and, according to some metric, compare the sums of the absolute values of the brightness within these windows. Such algorithms are characterized by high speed and computational efficiency. However, acceptable performance is only ensured if the pixel intensity function is smooth. At the boundaries of objects, where the intensity function breaks, the algorithms make a significant number of errors. Further development of the methods led to the emergence of multi-window algorithms and windows with an adaptive structure, which improved the quality of disparity calculation. But the "payment" for this was a significant increase in operating time, which often leads to the impossibility of analyzing images in real time.

Global algorithms are based on calculating the disparity simultaneously for the entire image, while each pixel of the image affects the solution in all other pixels. Global algorithms differ both in the form of unary and pair potentials, and in the minimization algorithms and the structure of the graph. Despite the fact that, as a rule, in terms of efficiency, global algorithms are superior to local ones, the resulting disparity maps are not free from errors caused by the simplifications that were originally incorporated into the formula for the energy functional. Moreover, global algorithms are slower.

Semi-global, or partially global, methods are a reasonable compromise between fast but imprecise local methods and more accurate but slow global methods, allowing rational use of their strengths. The idea of the methods consists in the independence of the solution for each pixel, taking into account the influence of all (or a part, not limited by the local neighborhood) of the remaining pixels of the image.

In particular, to obtain information about the scene around the projection area, the left frame from the left camera of the scene stereo camera and the right frame from the right camera of the scene stereo camera are obtained substantially simultaneously when the scene is shot. A disparity map is formed by the method of semi-global establishment of stereo correspondences with obtaining disparity d (x, y) for each image point with pixel coordinates (x, y). Determine the true coordinates (C, U, Z) of a point with pixel coordinates (x, y) by the formulas:

X = (x Q00 + Q03) / W,

Y ⁼ (y Q 11 + Q 13) / w,

Z-Q23 / W, where W = d Q32 + Q33, a Q00, Q03, Q11, Q13, Q23 are constants determined by the focal lengths of the left camera and right camera and the distance between the left camera and the right camera. Next, a depth map D (x, y) is formed, where D is the true distance from the left camera or right camera to the point with pixel coordinates (x, y). A two-dimensional gray-scale image is formed, on which the brightness Ф (x, y) of a point with pixel coordinates (x, y) is set by the formulas:

Ф (x, y) = 0 if D (x, y) <Dmin,

Ф (x, y) = 255, if D (x, y)> Dmax,

Ф (x, y) = 255 (D (x, y) - Dmin) / (Dmax - Dmin) - in other cases, where Dmin and Dmax are the specified minimum and maximum depth values, respectively, determined from the context of the application of the claimed method.

On the obtained two-dimensional image in gray scale, objects are detected and identified by one of the methods selected from the Viola-Jones method, the SSD-mobilenet neural network method and the Mask R-CNN neural network method, to obtain a set of objects in a three-dimensional scene.

After obtaining an image of the scene around the projection area, it is determined whether it is possible to project the laser emitter beam onto the projection area, taking into account the obtained scene. If there are objects on the path of the beam to the projection area, the position of the manipulator and / or the direction of the laser emitter must be adjusted so that the beam can easily reach the projection area, taking into account the resulting scene, including obstacle objects and their sizes. In this case, there may be several suitable positions for trouble-free projection of the beam onto the projection area, and from such points of the position of the laser emitter the most optimal point is selected, for example, the one closest to the current position, or one that is distant from the boundaries of the area of possible movement of the manipulator, or according to some other criterion ...

Next, you should determine the current position of the laser emitter, for which first determine the current position of the laser emitter in the coordinate system of the manipulator, and then - in the coordinate system of the scene stereo camera. It should be borne in mind that the current position of the laser emitter in the manipulator coordinate system is available at any time, since the laser emitter and the manipulator are mechanically connected to each other, therefore, their relative position is known.

To determine the position of the laser emitter in the coordinate system of the scene camera, various methods can be used, and the most preferable of them, characterized by simplicity, accuracy and high speed, is described below.

First, in space, the first target point, the second target point, the third target point and the fourth target point, which do not lie on the same plane, are arbitrarily selected in space, as well as with the condition that they must be visible to the scene stereo camera and available to the laser beam. Here, the numbering of target points is indicated solely for the convenience of understanding the further description.

Next, the laser emitter beam is sequentially directed at the indicated target points and the coordinates of the indicated target points are determined in the coordinate system of the scene stereo camera.

After that, the distance from the first target point to the laser emitter, the distance from the second target point to the laser emitter and the distance from the third target point to the laser emitter are determined in the coordinate system of the scene stereo camera, and the position of the fourth target point is used to unambiguously determine the indicated distances , which will be discussed in more detail below.

For the convenience of understanding the proposed method of converting the coordinates of the position of the laser emitter from the manipulator coordinate system to the stereo camera coordinate system, we introduce the following designations:

Ai - first target point;

Ar is the second target point;

Az is the third target point;

A - fourth target point;

О - point of position of the laser emitter.

Initially, the coordinates of the points Ai, Ar, Az, Ad are known in the coordinate system of the scene stereo camera, and the coordinates of the point O are known in the coordinate system of the manipulator.

Points O, Ai, Ar, Az should not lie in the same plane, which is almost always done automatically.

To find the coordinates of point O in the scene stereo camera coordinate system, solve the system of equations:

AiA _j ² = OAi ² + OA _j ² - 2OAiOA _{j CO} s (AiOA _j ), where i, j = 1, 2, 3; AiA _j - distance between points Ai and A _j ; OAi is the distance between points O and Ai, i.e. segment length OAi; AiOA _j - angle between segments OAi and OAj.

Having solved the equation, for example, by the Ferrari method, several variants of the values of the segments OAi, OA ₂ , OA ₃ are obtained, from which the desired one is selected using the coordinates of the Ad point as follows. For each option, calculate the coordinates of the point O. Choose the only option in which the angles A ₁ OA4, A ₂ OA ₄ and A ₃ OA ₄ coincide with the known angles between the directions of the beam of the laser emitter. Thus, the unique coordinates of the point O are obtained in the coordinate system of the scene stereo camera.

Next, an orthonormal coordinate system Oxyz is constructed with unit unit vectors i, j, k. The origin of coordinates, for example, the first point Ab, is chosen. The unit vectors of the vector passing from Ai to the second point Ai are selected by the direction of the unit unit unit r in the direction Ox. The Gram-Schmidt orthogonalization of the vector passing from Ai to the third point Az is performed relative to the unit reference unit / to obtain a unit unit reference unit j in the direction Оу. The vector product of unit unit vectors and j is performed to obtain unit unit unit unit vectors k in the direction Oz. And finally, the vector of the position of the laser emitter is decomposed in the Oxyz coordinate system in terms of the coordinate system of the scene stereo camera.

The constructed system Oxyz allows for any point or vector to recalculate its coordinates from the coordinate system of the scene stereo camera to the coordinate system of the manipulator (or laser emitter), or in the opposite direction. Indeed, since the construction is performed simultaneously in the manipulator coordinate system and the scene stereo camera coordinate system, for any vector in the manipulator coordinate system and the scene stereo camera coordinate system, it is possible to determine its coordinates in the Oxyz coordinate system by scalar multiplying this vector and the unit vectors i, j, k. Accordingly , for any vector in the Oxyz coordinate system, substituting instead of the unit vectors /, j, to their explicit expansions in the basis of the scene stereo camera coordinate system or the manipulator coordinate system, we can obtain the vector coordinates, respectively, in the scene stereo camera coordinate system or the manipulator coordinate system.

Similarly, for an arbitrary point, subtracting from its coordinates the coordinates of the first point Ai (since it was chosen as the origin of the orthonormal coordinate system Oxyz) in the original coordinate system, you can go to a vector with which you can do any of the above actions. After that, adding the coordinates of the first point in the final coordinate system, they pass from the vector to the point. The claimed method of controlling a robotic manipulator was tested under the following conditions: linear displacement between the attachment point (origin of the coordinate system) of the laser emitter and the attachment point (origin of the coordinate system) of the stage stereo camera from 1 to 5 meters; angular displacement within 1.5 radians along each axis; for the scene stereo camera, the modes of the left and right cameras are 1280x720, the distance between the left and right cameras is 0.1 m; distances between target points from 1 to 3 meters. According to the test results, the positioning error did not exceed 1 cm.

The method is quick and easy to use, for distributed robotics with difficultly predictable movement and rotation of the scene stereo camera relative to the laser emitter, it allows you to quickly determine or constantly monitor the relationship between the coordinate systems of the scene stereo camera and the laser emitter.

Claims

CLAIM

1. A method of controlling a robotic arm containing a laser emitter, a stereo scene camera for obtaining images of the scene and a stereo camera of the eyes for obtaining images of the left eye and right eye, including: a) selecting the projection area of the laser emitter beam by determining the direction of gaze, in which: al) obtaining images of the left eye and right eye, a2) determine the position of the pupil of each eye, a3) determine the direction of the gaze direction vector of each eye, and a4) determine the gaze direction in the scene stereo camera coordinate system;

B) obtaining an image of the scene around the projection area; c) determining whether the beam can be projected onto the projection area; d) selection of the optimum point for the position of the laser emitter; e) determining the current position of the laser emitter, at which: el) determining the current position of the laser emitter, including the coordinates and the direction vector of the laser emitter, in the manipulator coordinate system, and e2) determining the current position of the laser emitter, including the current position point and the direction vector of the laser emitter , in the coordinate system of the scene stereo camera; f) bringing the laser emitter to the optimum point, and g) projection of the laser emitter beam onto the projection area.

2. The method according to claim 1, in which, if at step c) the impossibility of projection of the beam onto the projection area is determined, the points of the position of the laser emitter are determined, from which the projection area is reachable, taking into account the scene image around the projection area and the size of the obstacles.

3. The method according to claim 1, in which, to perform step e2), the coordinates of the current position of the laser emitter are converted from the manipulator coordinate system to the stereo camera coordinate system, in which: the first target point, the second target point, the third target point and the fourth target point, not lying on the same plane; sequentially direct the beam to the specified target points and determine the coordinates of the specified target points in the coordinate system of the scene stereo camera; determining the distance from the first target point to the laser emitter, the distance from the second target point to the laser emitter and the distance from the third target point to the laser emitter in the coordinate system of the scene stereo camera, and the position of the fourth target point is used to unambiguously determine the indicated distances; an orthonormal coordinate system Oxyz is constructed with unit vectors i, j, k, at which: select the first point by the origin, select the unit vector of the vector passing from the first target point to the second target point, with the direction of the unit unit i in the direction Ox, perform Gram orthogonalization -Schmidt of the vector passing from the first target point to the third target point, relative to the unit vector i to obtain the unit vector j in the direction Oy, perform the vector product of the unit vectors i and j to obtain the unit vector k in the direction Oz, and decompose the vector the position of the laser emitter in the Oxyz coordinate system on the basis of the scene stereo camera coordinate system.