CN113386128B - Body potential interaction method for multi-degree-of-freedom robot - Google Patents

Abstract

The invention provides a body potential interaction method for a multi-degree-of-freedom robot, which comprises the following steps: obtaining the pixel coordinates of the human skeleton key points by adopting a human skeleton key point identification algorithm, and obtaining the three-dimensional space coordinates of each human skeleton key point according to the pixel coordinates of the human skeleton key points; detecting whether an abnormal error that the shoulder is shielded by the arm exists in the interaction process; correcting the human body space posture, and performing coordinate reconstruction on the space coordinates of the key points; normalizing the coordinate of the upper wrist part of the arm relative to the shoulder part to obtain the normalized coordinate, and meanwhile, establishing a local space coordinate system on the palm to obtain an attitude angle Eler (psi, theta, gamma) of the local coordinate system on the palm relative to the coordinate system taking the shoulder part as an origin; and combining the normalized coordinates, the length of the connecting rod of the robot and the palm posture to obtain joint angles of joints of the robot so as to drive the robot to move. The working space of the whole mechanical arm can be covered under the condition that people do not exceed the effective visual field of the sensor in the interaction process.

Description

A body posture interaction method for multi-degree-of-freedom robots

technical field

The invention belongs to the field of human-computer interaction, and in particular relates to a posture interaction method for a multi-degree-of-freedom robot.

Background technique

As many countries in the world continue to promote the development plan of Industry 4.0, industrial production has higher and higher requirements for robot intelligence, and the natural and efficient advanced human-robot interaction interface has been widely valued by the society.

Human-computer interaction is the process of using equipment to collect human information and transmit human intentions to machines; human-computer interaction interface is to convert human intentions into algorithms or programs that machines can execute instructions. According to different interaction methods, human-computer interaction includes voice interaction, wear sensor interaction, gamepad interaction, baton interaction, brain wave interaction, and visual interaction. Compared with the naturalness of the interaction process and the complexity of the design of the interaction system, the interaction process based on body posture can not only effectively avoid the interference of environmental noise, but also reduce the constraints brought by the sensors worn by people.

In the traditional human-computer interaction process, the number of interactive semantics defined based on features is always limited, and it is difficult to meet the diverse needs in complex interaction processes; when using dynamic body posture interaction, although the three-dimensional motion trajectory of key points of human skeleton is tracked. Complex interaction requirements can be achieved, but the limitation of the effective field of view of the sensor and the size difference of multi-degree-of-freedom robots with different structures limit the use of this interaction method, and the interaction process has to be interrupted and restored when the person exceeds the effective field of view of the sensor field of view. On the one hand, it restricts the scope of human activities, and on the other hand, it increases the probability of failure of the interaction process. While it is up to the researchers to use multiple sensor data to expand the field of view of a single sensor, this adds cost and complexity to the system.

SUMMARY OF THE INVENTION

In order to solve the problems existing in the prior art, the present invention provides a body posture interaction method based on a depth sensor and a human skeleton key point recognition algorithm.

In order to achieve the purpose of the present invention, the present invention provides a multi-degree-of-freedom robot-oriented body posture interaction method, comprising the following steps:

The human skeleton key point recognition algorithm is used to obtain the pixel coordinates of the human skeleton key points, and the three-dimensional space coordinates of each human skeleton key point are obtained according to the pixel coordinates of the human skeleton key points;

Detect whether there is an abnormal error that the shoulder is occluded by the arm during the interaction process, and if so, restore it or mark the key points of the shoulder as invalid points;

Correct the human body space posture, and reconstruct the coordinates of the key point space coordinates. The coordinate reconstruction is to establish a space rectangular coordinate system O x'y'z' with the left shoulder key point as the coordinate origin, and other skeleton key points p _i all use the coordinate system as the reference system to reconstruct the coordinates to obtain _pi ';

Normalize the coordinates of the wrist on the arm relative to the shoulder to obtain the normalized coordinates ^N p ₇ , and establish a local space coordinate system on the palm to obtain the coordinates of the local coordinate system on the palm relative to the shoulder as the origin The attitude angle of the system Eler(ψ,θ,γ), Eler(ψ,θ,γ) represents the spatial attitude of the palm;

Combined with the normalized coordinates, the length of the robot link and the palm space posture, the joint angles of each joint of the robot are obtained to drive the robot to move.

Further, the human skeleton key point recognition algorithm is Open Pose.

Further, obtaining the three-dimensional space coordinates of each human skeleton key point according to the pixel coordinates of the human skeleton key point, including:

Use the preset window size to filter to obtain the effective value of the pixel coordinates of the key point of the bone;

The depth map and RGB video frame collected at the same moment are aligned at the pixel level, and the three-dimensional coordinates corresponding to each pixel with the camera as the origin of the spatial coordinate system are obtained.

Further, whether there is an abnormal error that the left shoulder is occluded by the left arm in the described detection interaction process, if there is, then restore or mark the key point of the shoulder as an invalid point, including:

Calculate the direction vector of the left forearm p ₆ p ₇

_P7 points to the vector of _p5

_P6 points to the vector of _p5

Calculate the projected square value of p ₂ p ₄ on p ₂ p ₃

Among them, p ₃ is the skeleton key point at the junction of the right forearm and the upper arm, and p ₄ is the skeleton key point at the junction of the right forearm and the right wrist;

Detect whether p ₂ is occluded by calculating the spatial distance between p ₂ and the straight line p ₃ p ₄

的x，y，z分量。Among them, x _a , y _a , za _a , x _b , y _b , and z _b represent vectors respectively

The x, y, z components of .

并且分别经过p₆、p₇的平面之间的约束条件：Only the spatial distance between p ₅ and the straight line p ₆ p ₇ is not enough to judge whether occlusion really occurs, and p ₅ must be increased in the vertical direction.

And the constraints between the planes of p ₆ and p ₇ are respectively passed:

代入经过以

为法向量且经过p₆的空间平面方程：Will

Substitute through

is the normal vector and the space plane equation through p ₆ :

的x，y，z分量；x _n , y _n , and z _n represent vectors

the x, y, z components of ;

代入经过以

为法向量且经过p₇的空间平面方程：Will

Substitute through

is the normal vector and the space plane equation through p ₇ :

Take the signs of s ₁ and s _2. If the sign is a negative sign, it means that the left shoulder key point p ₅ is located between the two planes, then

s=s ₁ ·s ₂

成立，左肩关键点p₅发生遮挡。when

is established, the left shoulder key point p ₅ is occluded.

Further, in the correction of the human body space posture, the human body posture is corrected in a manner that the space vector between the two shoulders is parallel to the x-axis of the camera coordinate system O·xyz.

Further, the described establishment takes the left shoulder key point as the coordinate origin of the space Cartesian coordinate system O x'y'z', and other skeleton key points p _i all use this coordinate system as the reference system to carry out coordinate reconstruction to obtain p _i ', including :

为x'轴，在平行于传感坐标系的o·xz平面上垂直于

且指向传感器方向为y'轴，与传感器y轴相反方向作为z'轴的重建后的坐标系O·x'y'z'；Taking the left shoulder as the origin,

is the x' axis, which is perpendicular to the o xz plane parallel to the sensing coordinate system

And the direction pointing to the sensor is the y' axis, and the opposite direction to the sensor y axis is the reconstructed coordinate system O x'y'z' of the z'axis;

p ₂ points to the space vector v of p ₅

v=p ₂ -p ₅ =[xyz] ^T (7)

Angle between v and yoz plane

Rotation matrix corresponding to θ _x

v A space vector parallel to the yoz plane after transformation by R(θ _x ) rotation

v'=R(θ _x )×v (10)

The angle between the space vector v' and the xoy plane

Rotation matrix corresponding to θ _z

v' is a space vector parallel to the yoz plane after R(θ _x ) rotation transformation

v”=R(θ _z )×v’ (13)

After the rotation transformation, the new space coordinate of p ₂

p ₂ '=p ₅ +v” (14)

total rotation transformation

R=R(θ _z )×R(θ _x ) (15)

For the bone key point p _i , its reconstructed coordinate p _i '

p _i '=p ₅ +v _i '

Wherein, v _i ' ₌ R×vi

v _i = _pi -p ₅

where R is the rotation matrix from the camera coordinate system to the coordinate system with the shoulder as the origin.

Further, the coordinates of the upper wrist of the arm relative to the shoulder are normalized to obtain the coordinates ^N p ₇ , including:

Calculate the length dist ₁ of the big arm p ₅ 'p ₆ ', the length dist ₂ of the forearm p ₆ 'p ₇ ' and the distance dist ₃ from the palm to the shoulder p ₅ 'p ₇ ', the calculation formula is as follows:

^N p ₇ is the coordinate of the normalized hand in the coordinate system O x'y'z' space unit sphere with the left shoulder as the origin (the inner product of the coordinates on the sphere is 1), and sacle is the adaptive scaling factor.

Further, establishing a local space coordinate system on the palm, and obtaining the attitude represented by the attitude angle Eler (ψ, θ, γ) of the local coordinate system on the palm relative to the coordinate system whose origin is the shoulder, including:

作为局部坐标系的O·x轴，

与p₃₁'指向p₃₃'的向量

作为局部坐标系的O·xy平面，过p₃₁'的向量

且

且

以

作为O·z轴，则有：A vector pointing to p ₃₂ ' from the key point p ₃₀ ' on the palm

As the O x axis of the local coordinate system,

vector with _p31 ' pointing to _p33 '

As the O·xy plane of the local coordinate system, the vector through p ₃₁ '

and

and

by

As the O z axis, there are:

x, y, z are the coordinate components of each vector;

并且

将向量

归一化：Find the normal vector of the O.xz plane

and

the vector

Normalized:

归一化后的三个坐标分量，r₁₂、r₂₂、r₃₂为向量

归一化后的三个坐标分量，r₁₃、r₂₃、r₃₃为向量

归一化后的三个坐标分量；r ₁₁ , r ₂₁ , and r ₃₁ are vectors

The three coordinate components after normalization, r ₁₂ , r ₂₂ , and r ₃₂ are vectors

The three coordinate components after normalization, r ₁₃ , r ₂₃ , r ₃₃ are vectors

The three coordinate components after normalization;

R _h is the rotation matrix of O _h x'y'z' in O x'y'z', and his attitude angle Eler(ψ,θ,γ) is calculated by the following formula:

ψ represents the angle by which the coordinate system is rotated around the x-axis, θ is the angle by which the coordinate axis is rotated around the y-axis, and γ is the angle by which the coordinate system is rotated around the z-axis. atan2 is an inverse trigonometric function, and the tangent angle is calculated.

Further, before obtaining the joint angle by combining the normalized coordinates, the length of the robot link and the palm posture, a filtering operation is also performed on the normalized coordinates and the palm posture angle.

Further, the joint angle of each joint of the robot is obtained by combining the normalized coordinates, the length of the robot connecting rod and the palm posture, so as to drive the robot to move, including:

The ROS inverse kinematics solver obtains the joint angles of each joint of the robot according to the posture and attitude angle Eler(ψ, θ, γ) of the palm and the position of the end of the robot; among them, the calculation formula of the end position P _e of the robot is as follows:

P _e = ^N p ₇ ·L

In the formula, ^N p ₇ is the coordinate of the normalized wrist in the coordinate system with the shoulder as the origin, and L is the total length of the robot link. Eler(ψ, θ, γ) is used as the attitude of the end of the robot, and P _e is used as the end position. In the human-computer interaction system, the human-computer interaction system first calculates the joint angles of the robot for the target position and attitude through the built-in inverse kinematics solver of ROS. , and then control the robot movement through the network socket link.

Compared with the prior art, the beneficial effects that the present invention can achieve are at least as follows:

(1) The human-computer interaction system of the present invention utilizes the human arm and the palm to simultaneously control the position and posture of the multi-degree-of-freedom robot.

(2) Use the space triangle formed by the human arm to form the unique spatial position coordinates of the palm relative to the shoulder in the maximum working space. After normalizing the coordinates, map the coordinates to different sizes of robotic arms, which can make the interaction process in the The entire working space of the robotic arm is covered under the condition that the person does not exceed the effective field of view of the sensor.

(3) Compared with the problem that the scaling factor is difficult to determine in the traditional tracking dynamic gesture, the method of the present invention has strong stability, can adjust the scaling factor adaptively, and has wide practicability.

(4) Map the posture of the palm in space to the posture of the robotic arm TCP, which can realize the rapid and efficient transmission of human intentions to the robot.

(5) The present invention corrects the posture of the human body in advance, and uses the connection line between the two shoulders as a reference to reconstruct the coordinate system of the posture of the human body. The corrected posture of the human body faces the sensor no matter what, when the person does not leave the effective field of view of the sensor. , the relative position of each key point in the local coordinate system constructed by the human body will not change, which greatly improves the comfort of the human body.

(6) Since there is no need to calibrate the relative positional relationship between sensors, robots, and people in advance, the efficiency of human-computer interaction is improved.

(7) For the self-occlusion problem of the arm, the occlusion detection algorithm is used for detection and recovery, which ensures that it can work normally in complex environments, and the system has strong anti-interference.

Description of drawings

FIG. 1 is a schematic diagram of the system structure of the present invention.

FIG. 2 is a schematic flowchart of a body posture interaction method for a multi-degree-of-freedom robot according to an embodiment of the present invention.

FIG. 3 is a schematic diagram of the mapping relationship between the robot motion space and the human arm motion space.

FIG. 4 is a schematic diagram of human posture pre-correction.

FIG. 5 is a schematic diagram of calculating the effective value of the sliding window mean value filtering of the pixel coordinates of the key point of the bone.

FIG. 6 is a schematic diagram of occlusion detection and automatic recovery.

FIG. 7 is a schematic diagram of controlling the end of the robot using body posture.

FIG. 8 is a schematic diagram of key points of human skeleton and key points of knuckles.

Detailed ways

In order to make the purposes, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments These are some embodiments of the present invention, but not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts are within the protection scope of the present invention.

As shown in Figure 1, the interface of the human-computer interaction system includes posture recognition, virtual simulation, video surveillance and control panel. The system is developed based on the open source robot operating system ROS, and uses the simulation function of ROS and the motion planner Moveit to realize the functions of virtual simulation, collision detection, motion planning and inverse motion solution of the robot.

The present invention provides a multi-degree-of-freedom robot-oriented body posture interaction method using the human arm and the palm to control the position and posture of the multi-degree-of-freedom robot at the same time. The color image and its depth map of each frame of the video can be obtained at the same time; the color image is input into the human skeleton key point recognition algorithm to extract the skeleton key points, and then the depth information of each key point is obtained from the depth map for occlusion detection and recovery. Then, the spatial coordinates of the key points of the skeleton are obtained; the obtained key points of the human skeleton are reconstructed using the line connecting the two shoulders as a reference to reconstruct the coordinate system; finally, the spatial coordinates of the wrist relative to the shoulder are normalized and mapped as the robot end relative to the base The spatial coordinates of the palm are mapped to the posture Eler (ψ, θ, γ) of the palm as the robot end posture to realize human-computer interaction. In this method, the spatial triangle formed by the human arm is used to form the unique spatial position coordinates of the palm relative to the shoulder in the maximum working space, and the coordinates are normalized and mapped to different sizes of robotic arms, which can make the interaction process easier. Covering the entire working space of the robotic arm under the condition that the human does not exceed the effective field of view of the sensor, as shown in Figure 3, as the human wrist stretches, the robot also stretches, and at the same time, when the human hand stretches to the limit, the end of the robot also approaches its working space edge. At the same time, the posture of the palm in space is mapped to the posture of the end of the robotic arm, which can realize the rapid and efficient transmission of human intentions to the robot. In order to solve the problem that the coordinates of the palm relative to the shoulder are not fixed due to the change of the position of the person and the sensor, the present invention adopts a posture pre-correction method. The space vector between the two shoulders is parallel to the x axis of the camera coordinate system O xyz, as shown in the appendix Figure 4, no matter how the corrected human body poses towards the sensor, when the person does not leave the effective field of view of the sensor, the relative position in the local coordinate system constructed by the human body will not change, which greatly improves the comfort of the person. Taking into account the self-occlusion problem of the arm, the occlusion detection algorithm is used for detection, and the historical coordinates of the key point and other unoccluded position coordinates are used to automatically recover, and the system has strong anti-interference.

Specifically, the present invention provides a multi-degree-of-freedom robot-oriented body posture interaction method, comprising the following steps:

Step S1, using the human skeleton key point recognition algorithm to obtain the pixel coordinates of the human skeleton key points, and obtaining the three-dimensional space coordinates of each human skeleton key point according to the pixel coordinates of the human skeleton key points, and the human skeleton key points include on the human body. Skeletal keys on the palm.

Step S1 includes the following steps:

Step S11: using an image acquisition sensor to collect human body video to obtain a color image and a depth image of each frame of the video;

In one embodiment of the present invention, the image acquisition sensor is a depth camera.

Step 12: Input the color image into the human skeleton key point recognition algorithm to extract the pixel coordinates of the skeleton key points;

In one embodiment of the present invention, the adopted human skeleton key point identification algorithm is Open Pose, of course, in other embodiments, other algorithms for identifying bone key points may also be used.

In one embodiment of the present invention, the skeleton key point information collected by the human skeleton key point recognition algorithm includes three elements k=[px py score], where px and py are the pixels corresponding to the skeleton key point recognition in each video frame Coordinate, score is the reliability of the key point.

Due to the differences of different ambient lighting, different image acquisition sensors, and different human-computer interaction personnel, the confidence of the identified key points will be different, so whether the traditional segmentation based on fixed threshold is an effective method for identifying information It is no longer applicable; in addition, although the automatic threshold segmentation algorithm based on the frequency domain can better identify the validity of the joints, the processing process of this method is relatively complicated and the computational load is large. In the present invention, when the key points of human skeleton are identified, their confidence levels are relatively close, and the key points that are wrongly identified are generally very different from those that are correctly identified. Therefore, an adaptive threshold for the key points that must be identified is adopted. The segmentation method, which is based on the traditional threshold segmentation method based on fixed values, takes the key points that must be identified as the benchmark, and takes the upper and lower preset threshold spaces as the effective threshold interval (this embodiment is taken after data analysis. The upper and lower threshold spaces of 20% are used as the effective threshold interval). In one embodiment of the present invention, the bone key points p ₅ and p ₂ of the left shoulder and the right shoulder are used as the key points that must be identified (later steps will use the line connecting these two points to perform coordinate system reconstruction and posture correction). The calculation process is as follows:

Mark whether each key point is effectively recognized: ValidMatrix=[[false][false][false]...[false]]

其中，score_i表示关键点k_i的置信度，_i为关键点的索引号；Selecting the confidence average value of the key points of the bones that must be identified as the reference confidence degree, in one embodiment of the present invention, selecting the confidence average value of the key points p ₅ and p ₂ of the left shoulder and the right shoulder as the reference confidence degree s:

Among them, score _i represents the confidence of the key point k _i , and _i is the index number of the key point;

判断每个关键点是否被有效识别，若ValidMatrix[i]为false,去除该无效点。

Determine whether each key point is effectively recognized, if ValidMatrix[i] is false, remove the invalid point.

In one embodiment of the present invention, a color image of the human body is collected by a depth camera, and 25 key points of the human body and 20 key points of the knuckles of the right hand are identified through the key point recognition algorithm of human bones. As shown in FIG. 8 , each key point The serial number and specific location are shown in Table 1 and Table 2. In the human bone key points, select the bone key points p ₆ , p ₇ on the left arm among the 25 joints, the bone key point p ₅ on the left shoulder, the bone key point p ₁ on the neck, and the bone key point on the right shoulder. p ₂ , the key points p ₃ and p ₄ of the bones on the right arm, as the most important key points, these key points are the basis of the subsequent steps; wherein, in one of the embodiments of the present invention, the human skeleton recognition algorithm obtains the key points Both are located at the joints of the bones. If the width of the limb is considered, it is located at the center. These are determined by the human skeleton key point recognition algorithm itself.

Table 1 Number and location of key points of human skeleton

Table 2 Number and location of key points of palm bone

Step S13 : using a fixed size window to perform moving average filtering to obtain the effective values of the pixel coordinates x and y of the skeleton key point (the effective value refers to the value of the pixel coordinates after filtering the noise of the skeleton key point recognition algorithm).

In one embodiment of the present invention, when the human body is stationary, the noise of the human skeleton key point recognition algorithm is approximately 30 video frames as periodic fluctuations, so a sliding window with a size of 30 is used to perform sliding mean filtering, as shown in the figure 5 shown. The specific process of filtering is as follows:

Step S131 : configure a window with a preset size for each skeleton key point, and the preset size of the preset window in this embodiment is 30.

表示关键点k_i的第j个输入值，取

中的px，py计算像素点坐标；

represents the _jth input value of the key point ki, take

In px, py calculates pixel coordinates;

Step S132: configure a sliding filter window WINDOW for all bone key points;

WINDOW=[window ₀ ... window _i ...]

window _i is the sliding filter window of the key point _ki ;

sum_i为第i个滤波窗口内元素的和，i表示这是为第i个骨骼关键点配置的滤波窗口。如果采集的图像帧数大于30，将原来的和

加上新输入数据

再减去最先输入的数

即更新过程为

Step S133: If the number of captured image frames is less than 30, then

sum _i is the sum of the elements in the ith filtering window, i indicates that this is the filtering window configured for the ith bone key point. If the number of captured image frames is greater than 30, the original and

plus new input data

Subtract the number entered first

That is, the update process is

In the sliding mean filtering process, the method of adding the sum in the current window to the data to be inserted minus the earliest inserted element in the window can effectively reduce the drawbacks of repeated summation in the filtering process, and is implemented by a fixed-size circular queue.

根据分析实际实验的数据和周期函数的特点，周期性上下波动噪音在一个完整周期内函数值和为零，因为sum是累计了30个帧的和，通过求平均得到单帧的结果。本步骤是为了降低人体骨骼关键点识别算法本身的噪音。Step S134: Calculate the average value of all the numbers in the sliding window of the key point p _i :

According to the analysis of the actual experimental data and the characteristics of the periodic function, the function value of the periodic up and down fluctuation noise is zero in a complete cycle, because the sum is the sum of 30 frames, and the result of a single frame is obtained by averaging. This step is to reduce the noise of the human skeleton key point recognition algorithm itself.

Step S14: Convert the coordinates of key points of human bones from two-dimensional pixel coordinates to three-dimensional space coordinates.

In one embodiment of the present invention, the depth map and the RGB video frame at the same time collected by the image acquisition sensor are aligned at the pixel level, and the three-dimensional coordinate p(x, y, z). Convert the two-dimensional pixel coordinates into three-dimensional coordinates through the depth map of the depth camera: p _i =remap[px _i py _i ], remap is the development interface (API) provided by the depth camera development kit (SDK), the function is to convert the pixel coordinates Convert to spatial coordinates with the camera as the origin.

The spatial coordinates of each bone key point are: p=[x y z]

Step S2 , detecting an abnormal error that the shoulder is occluded by the arm during the interaction process, and automatically recovering or marking it as an invalid point, as shown in FIG. 6 .

作为当前时刻的坐标值进行恢复，如果穷尽上述两种方法还不能恢复右肩关键点p₅深度信息，则将左肩关键点p₅标记为无效点，放弃本帧图像采集的数据。所述遮挡探测算法的过程如下：In this step, the occlusion detection algorithm is first used to detect whether the left shoulder key point p ₅ is occluded, and after detection, the depth information of the right shoulder key point p ₂ is used to restore; if the right shoulder key point p ₂ is also occluded, Then select the historical coordinates of the left shoulder key point

As the coordinate value at the current moment to restore, if the depth information of the right shoulder key point p ₅ cannot be restored after exhausting the above two methods, the left shoulder key point p ₅ is marked as an invalid point, and the data collected in this frame of image is discarded. The process of the occlusion detection algorithm is as follows:

Calculate the direction vector of the left forearm p ₆ p ₇

_P7 points to the vector of _p5

_P6 points to the vector of _p5

Calculate the projected square value of p ₂ p ₄ (the line between the two keypoints) on p ₂ p ₃

Detect whether p ₂ is occluded by calculating the spatial distance between p ₂ and the straight line p ₃ p ₄

的x，y，z分量。Among them, x _a , y _a , za _a , x _b , y _b , and z _b represent vectors respectively

The x, y, z components of .

并且分别经过p₆、p₇的平面之间的约束条件。Only the spatial distance between p ₅ and the straight line p ₆ p ₇ is not enough to judge whether occlusion really occurs, and p ₅ must be increased in the vertical direction.

And the constraints between the planes of p ₆ and p ₇ are respectively passed.

代入经过以

为法向量且经过p₆的空间平面方程：Will

Substitute through

is the normal vector and the space plane equation through p ₆ :

的x，y，z分量；x _n , y _n , and z _n represent vectors

the x, y, z components of ;

代入经过以

为法向量且经过p₇的空间平面方程：Will

Substitute through

is the normal vector and the space plane equation through p ₇ :

Take the signs of s ₁ and s _2. If the sign is a negative sign, it means that the left shoulder key point p ₅ is located between the two planes, then

s=s ₁ ·s ₂

成立，左肩关键点p₅发生遮挡。threshhold可以调节，取决于传感器噪音水平，它表示左手腕与左小臂所在空间直线的空间距离，另外，由于图像与真实世界的镜像关系，图像与真实世界是左右颠倒的。在本发明其中一个实施例中，threshhold取值50mm，当然，在其他实施例中，也可根据实际采用其他数值。when

is established, the left shoulder key point p ₅ is occluded. Threshhold can be adjusted, depending on the noise level of the sensor, it represents the spatial distance between the left wrist and the space line where the left forearm is located. In addition, due to the mirror image relationship between the image and the real world, the image and the real world are reversed left and right. In one embodiment of the present invention, the threshold value is 50 mm. Of course, in other embodiments, other values can also be used according to actual conditions.

S3, correcting the human body space posture, and reconstructing the coordinates of the key point space coordinates.

The three-dimensional coordinate system of the key points of the human skeleton is initially obtained with the sensor as the origin. When the human and the sensor are in different orientations, the posture of the human body will be different in the camera coordinate system, as shown in the example before correction in Figure 4. In one of the embodiments of the present invention, a pre-correction method is used, that is, the line connecting the human body posture with the left shoulder p ₅ (x, y, z) to the right shoulder p ₂ (x, y, z) is transformed by the rotation R, and finally It is parallel to the x-coordinate axis ox of the camera coordinate system, that is, the human body posture is corrected in a way that the space vector between the two shoulders is parallel to the x-axis of the camera coordinate system O·xyz.

为x'轴，在平行于传感器坐标系的o·xz平面上垂直于

且指向传感器方向为y'轴，与传感器y轴相反方向作为z'轴的重建后的坐标系O·x'y'z'，如附图7。过程如下：When the human arm changes the posture, the relative positions of the left and right shoulders p ₂ and p ₅ will not change, and the coordinate system needs to be established on a stable reference. The same rotation transformation is performed on the three-dimensional coordinates pointing from the left shoulder to other key points of the other bones, and finally the left shoulder is used as the origin.

is the x' axis, perpendicular to the o xz plane parallel to the sensor coordinate system

And the direction pointing to the sensor is the y' axis, and the direction opposite to the y axis of the sensor is the reconstructed coordinate system O·x'y'z' of the z' axis, as shown in FIG. 7 . The process is as follows:

p ₂ points to the space vector v of p ₅

v=p ₂ -p ₅ =[xyz] ^T (7)

Angle between v and yoz plane

Rotation matrix corresponding to θ _x

v A space vector parallel to the yoz plane after transformation by R(θ _x ) rotation

v'=R(θ _x )×v (10)

The angle between the space vector v' and the xoy plane

Rotation matrix corresponding to θ _z

v' is a space vector parallel to the yoz plane after R(θ _x ) rotation transformation

v”=R(θ _z )×v’ (13)

After the rotation transformation, the new spatial position of p ₂

p ₂ '=p ₅ +v” (14)

total rotation transformation

R=R(θ _z )×R(θ _x ) (15)

R in the formula (15) is a rotation matrix from the camera coordinate system to the coordinate system with the shoulder as the origin.

For the bone key point p _i , its reconstructed coordinate p _i '

p _i '=p ₅ +v _i '

Wherein, v _i ' ₌ R×vi

v _i = _pi -p ₅

v _i is the vector between the bone key point p _i and the left shoulder p ₅ , and v _i ' is the space vector transformed by rotation.

Step S4, normalize the coordinates of the palm on the arm relative to the shoulder to obtain the normalized coordinates ^N p ₇ , and establish a local space coordinate system on the palm at the same time, and solve the coordinates of the local coordinate system established in S3 is the attitude Euler (ψ, θ, γ) represented by Euler angles.

In one embodiment of the present invention, step S4 includes the following steps:

Step S41: respectively obtain the length dist 1 of the big arm p ₅ 'p ₆ ', the length dist ₂ of the forearm p ₆ 'p ₇ ' and the distance dist ₃ from the palm to the shoulder p ₅ 'p ₇ ', the calculation _formula is as follows:

x ₆ ', y ₆ ', z ₆ ' are the coordinate components of p ₆ ' in the reconstructed spatial coordinate system O·x'y'z', x ₅ ', y ₅ ', z ₅ ' are the coordinates of p ₅ ' in the The coordinate components of the reconstructed spatial coordinate system O x'y'z';

^N p ₇ is the coordinate of the normalized hand in the coordinate system O x'y'z' space unit sphere with the left shoulder as the origin (the inner product of the coordinates on the sphere is 1), and sacle is the adaptive scaling factor. This factor can be easily converted to other coordinate systems.

作为局部坐标系的O·x轴，

与p₃₁'指向p₃₃'的向量

作为局部坐标系的O·xy平面，过p₃₁'的向量

且

且

以

作为O·z轴，则有：Step S42: In order to solve the posture of the palm in the O·x'y'z' coordinate system, a local space coordinate system O _h ·x'y'z' is established on the palm. A vector pointing to p ₃₂ ' from the key point p ₃₀ ' on the palm

As the O x axis of the local coordinate system,

vector with _p31 ' pointing to _p33 '

As the O·xy plane of the local coordinate system, the vector through p ₃₁ '

and

and

by

As the O z axis, there are:

的三个坐标分量，x_d、y_d、z_d是向量

的三个坐标分量；x _c , y _c , z _c are vectors

The three coordinate components of , x _d , y _d , z _d are vectors

The three coordinate components of ;

并且

将向量

归一化：Find the normal vector of the O.xz plane

and

the vector

Normalized:

归一化后的三个坐标分量，r₁₂、r₂₂、r₃₂为向量

归一化后的三个坐标分量，r₁₃、r₂₃、r₃₃为向量

归一化后的三个坐标分量；r ₁₁ , r ₂₁ , and r ₃₁ are vectors

The three coordinate components after normalization, r ₁₂ , r ₂₂ , and r ₃₂ are vectors

The three coordinate components after normalization, r ₁₃ , r ₂₃ , r ₃₃ are vectors

The three coordinate components after normalization;

R _h is the rotation matrix of O _h x'y'z' in O x'y'z', and the attitude angle Eler(ψ,θ,γ), that is, the spatial attitude of the palm, is calculated by the following formula:

In the formula, ψ represents the angle by which the coordinate system is rotated around the x-axis, θ is the angle by which the coordinate axis is rotated around the y-axis, and γ is the angle by which the coordinate system is rotated around the z-axis. atan2 is an inverse trigonometric function, and the tangent angle is calculated.

轨迹点进行滤波处理，降低噪音对手腕空间坐标的不利影响，减少传感器累计误差的影响；对S4得到的Eler(ψ,θ,γ)进行滤波处理，减少手部局部坐标系姿态的抖动。Step S5, to the obtained in step S4

The trajectory points are filtered to reduce the adverse effect of noise on the wrist space coordinate and the influence of the cumulative error of the sensor; the Eler (ψ, θ, γ) obtained by S4 is filtered to reduce the jitter of the local coordinate system of the hand.

乘以与机器人连杆长度之和L，与手掌姿态组成一个空间位姿p_s(x,y,z,ψ,θ,γ)。Step S6, the filter processed in step S5

Multiply it by the sum L of the length of the robot link, and form a spatial pose p _s (x, y, z, ψ, θ, γ) with the palm pose.

Using the above p _s (x, y, z, ψ, θ, γ), input it into the human-computer interaction system. The human-computer interaction system first calculates the joint angles of the robot for the target position and attitude through the built-in inverse kinematics solver of ROS. Robot motion is controlled via a web socket link.

Calculate the total length of the robot connecting rod through the dynamic data of the robot

l _idx is the length of the idx-th link of the robot, dof is the degree of freedom of the robot, and idx is the serial number of the robot link.

Robot end position

P _e = ^N p ₇ ·L (22)

^N p ₇ is the coordinate of the normalized wrist in the coordinate system with the shoulder as the origin, which represents a coordinate in the space unit sphere.

The body posture interaction method of the present invention has great advantages, each human limb can express rich semantics, and there are rich spatial relationships between human limbs, the movement process of the human arm and the movement process of the robot arm have the same very similar features. The human-computer interaction system of the present invention utilizes the human arm and the palm to simultaneously control the position and posture of the multi-degree-of-freedom robot. Using the spatial triangle formed by the human arm to form the unique spatial position coordinates of the palm relative to the shoulder in the maximum working space. After normalizing this coordinate, it is mapped to different sizes of robotic arms, so that the human does not exceed the limit during the interaction process. Covers the entire working space of the robotic arm under the condition of the effective field of view of the sensor. Compared with the problem that it is difficult to determine the scaling factor in the traditional tracking dynamic gesture, the method of the present invention has strong stability, can adjust the scaling factor adaptively, and has wide practicability. The gesture of the palm in space is mapped to the gesture of the robotic arm TCP, which can realize the rapid and efficient transmission of human intentions to the robot. The present invention adopts a human body posture pre-correction method, and takes the connection line between the two shoulders as a reference to reconstruct the coordinate system of the human body posture. The relative position of each key point in the local coordinate system constructed by the human body will not change, which greatly improves the comfort of human beings. Since there is no need to calibrate the relative positional relationship between sensors, robots, and people in advance, the efficiency of human-computer interaction is improved. For the self-occlusion problem of the arm, the occlusion detection algorithm is used for detection and recovery, which ensures that it can work normally in complex environments, and the system has strong anti-interference.

The above description of the disclosed embodiments enables any person skilled in the art to make or use the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Thus, the present invention is not intended to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims (10)

1. a kind of body posture interaction method for multi-degree-of-freedom robot, is characterized in that, comprises the following steps: The human skeleton key point recognition algorithm is used to obtain the pixel coordinates of the human skeleton key points, and the three-dimensional space coordinates of each human skeleton key point are obtained according to the pixel coordinates of the human skeleton key points; Detect whether there is an abnormal error that the shoulder is occluded by the arm during the interaction process, and if so, restore it or mark the key points of the shoulder as invalid points; Correct the human body space posture, and reconstruct the coordinates of the key point space coordinates. The coordinate reconstruction is to establish a space rectangular coordinate system O x'y'z' with the left shoulder key point as the coordinate origin, and other skeleton key points p _i all use the coordinate system as the reference system to reconstruct the coordinates to obtain _pi '; Normalize the coordinates of the wrist on the arm relative to the shoulder to obtain the normalized coordinate ^N p ₇ , and establish a local space coordinate system on the palm to obtain the coordinates of the local coordinate system on the palm relative to the shoulder as the origin The attitude angle Eler(ψ, θ, γ) of the system, and the attitude angle Eler(ψ, θ, γ) represents the spatial attitude of the palm; Combined with the normalized coordinates, the length of the robot link and the palm space posture, the joint angles of each joint of the robot are obtained to drive the robot to move. 2 . The method for body and posture interaction for a multi-degree-of-freedom robot according to claim 1 , wherein the algorithm for identifying key points of human skeleton is Open Pose. 3 . 3. a kind of posture interaction method oriented to multi-degree-of-freedom robot according to claim 1, is characterized in that, described according to the pixel coordinates to the human skeleton key point to obtain the three-dimensional space coordinate of each human skeleton key point, comprising: Use the preset window size to filter to obtain the effective value of the pixel coordinates of the key point of the bone; The depth map and RGB video frame collected at the same moment are aligned at the pixel level, and the three-dimensional coordinates corresponding to each pixel with the camera as the origin of the spatial coordinate system are obtained. 4. a kind of body posture interaction method for multi-degree-of-freedom robot according to claim 1, is characterized in that, whether there is an abnormal error that the left shoulder is blocked by the left arm in the described detection interaction process, if there is, then carry out. Recover or mark key points on the shoulder as invalid, including: Calculate the direction vector of the left forearm p ₆ p ₇

_P7 points to the vector of _p5

_P6 points to the vector of _p5

Calculate the projected square value of p ₂ p ₄ on p ₂ p ₃

Detect whether p ₂ is occluded by calculating the spatial distance between p ₂ and the straight line p ₃ p ₄

Among them, x _a , y _a , za _a , x _b , y _b , and z _b represent vectors respectively

the x, y, z components of ; p ₅ is perpendicular to

And the constraints between the planes of p ₆ and p ₇ are respectively passed: Will

Substitute through

is the normal vector and the space plane equation through p ₆ :

x _n , y _n , and z _n represent vectors

the x, y, z components of ; Will

Substitute through

is the normal vector and the space plane equation through p ₇ :

Take the signs of s ₁ and s _2. If the sign is a negative sign, it means that the left shoulder key point p ₅ is located between the two planes, then s=s ₁ ·s ₂ when

If established, the key point p ₅ of the left shoulder is occluded, and the threshold represents the spatial distance between the left wrist and the space straight line where the left forearm is located. 5. a kind of body posture interaction method for multi-degree-of-freedom robot according to claim 1 is characterized in that, in the described body space posture is corrected, with the space vector between two shoulders parallel to camera coordinate system 0 · The way of the x-axis of xyz to correct the human body posture. 6. a kind of body posture interaction method for multi-degree-of-freedom robot according to claim 1, is characterized in that, described establishing the space Cartesian coordinate system O x'y'z' with the left shoulder key point as coordinate origin, The other skeleton key points _pi are reconstructed with this coordinate system as the reference system to obtain _pi ', including: Taking the left shoulder as the origin,

is the x' axis, which is perpendicular to the o xz plane parallel to the sensing coordinate system

And the direction pointing to the sensor is the y' axis, and the opposite direction to the sensor y axis is the reconstructed coordinate system O x'y'z' of the z'axis; p ₂ points to the space vector v of p ₅ v=p ₂ -p ₅ =[xyz] ^T (7) Angle between v and yoz plane

Rotation matrix corresponding to θ _x

v A space vector parallel to the yoz plane after transformation by R(θ _x ) rotation v'=R(θ _x )×v (10) The angle between the space vector v' and the xoy plane

Rotation matrix corresponding to θ _z

v' is a space vector parallel to the yoz plane after R(θ _x ) rotation transformation v”=R(θ _z )×v’ (13) After the rotation transformation, the new space coordinate of p ₂ p ₂ '=p ₅ +v” (14) total rotation transformation R=R(θ _z )×R(θ _x ) (15) For the bone key point p _i , its reconstructed coordinate p _i ' p _i '=p ₅ +v _i ' Wherein, v _i ' ₌ R×vi v _i = _pi -p ₅ where R is the rotation matrix from the camera coordinate system to the coordinate system with the shoulder as the origin. 7 . The method for interaction of body and posture for a multi-degree-of-freedom robot according to claim 1 , wherein the coordinates of the upper wrist of the arm relative to the shoulder are normalized to obtain the coordinates ^N p ₇ , comprising: 8 . : Calculate the length dist ₁ of the big arm p ₅ 'p ₆ ', the length dist ₂ of the forearm p ₆ 'p ₇ ' and the distance dist ₃ from the palm to the shoulder p ₅ 'p ₇ ', the calculation formula is as follows:

^N p ₇ is the coordinate of the normalized hand in the coordinate system O x'y'z' space unit sphere with the left shoulder as the origin, x ₆ ', y ₆ ', z ₆ ' are p ₆ ' in the reconstruction The coordinate components of the reconstructed spatial coordinate system O·x'y'z', x ₅ ', y ₅ ', z ₅ ' are the coordinate components of p ₅ ' in the reconstructed spatial coordinate system O·x'y'z' , sacle is the adaptive scaling factor. 8. The method for interacting with a multi-degree-of-freedom robot according to claim 1, characterized in that, establishing a local space coordinate system on the palm to obtain the origin of the local coordinate system on the palm relative to the shoulder. The attitude represented by the attitude angle Eler (ψ, θ, γ) of the coordinate system, including: A vector pointing to p ₃₂ ' from the key point p ₃₀ ' on the palm

As the O x axis of the local coordinate system,

vector with _p31 ' pointing to _p33 '

As the O·xy plane of the local coordinate system, the vector through p ₃₁ '

and

and

by

As the O z axis, there are:

x, y, z are the coordinate components of each vector; Find the normal vector of the O.xz plane

and

the vector

Normalized:

r ₁₁ , r ₂₁ , and r ₃₁ are vectors

The three coordinate components after normalization, r ₁₂ , r ₂₂ , and r ₃₂ are vectors

The three coordinate components after normalization, r ₁₃ , r ₂₃ , r ₃₃ are vectors

The three coordinate components after normalization; R _h is the rotation matrix of O _h x'y'z' at O x'y'z', and its attitude angle Eler(ψ,θ,γ) is calculated by the following formula:

ψ represents the angle that the coordinate system rotates around the x-axis, θ represents the angle that the coordinate axis rotates around the y-axis, γ represents the angle that the coordinate axis rotates around the z-axis, atan2 is an inverse trigonometric function, and the tangent angle is calculated. 9 . The method for interacting with a multi-degree-of-freedom robot according to claim 1 , wherein the joint angles of each joint of the robot are obtained by combining the normalized coordinates, the length of the robot link and the palm posture. 10 . The former also includes filtering operations on the normalized coordinates and the palm posture angle. 10. The method for interaction of body postures for a multi-degree-of-freedom robot according to any one of claims 1 to 9, wherein the robot is obtained by combining the normalized coordinates, the length of the robot connecting rod and the palm posture The joint angle of each joint to drive the robot motion, including: The ROS inverse kinematics solver obtains the joint angles of each joint of the robot according to the posture and attitude angle Eler(ψ, θ, γ) of the palm and the position of the end of the robot; among them, the calculation formula of the end position P _e of the robot is as follows: P _e = ^N p ₇ ·L In the formula, ^N p ₇ is the coordinate of the normalized wrist in the coordinate system with the shoulder as the origin, and L is the total length of the robot link.

Images