Vision-based Dual-arm Follow-up Control Method, System and Robot for Humanoid Robot

Abstract

The present invention discloses a vision-based follow-up control method, system and robot for the double arms of a humanoid robot. The method includes the steps of: obtaining the spatial position information of human body joint points based on the collected real-time human body image data; respectively calculating the rotational degrees of freedom at the shoulder joints of the left and right upper arms of the human body, the included angles at the elbow joints of the left and right arms, and the axial rotation angles of the left and right upper arms according to the position information of the joint points; performing linear interpolation on the obtained parameter values and sending them to the robot control module at a set frequency to achieve the follow-up control of the double arms of the humanoid robot. The present invention realizes the follow-up control of the double arms of the humanoid robot through vision. Compared with the traditional direct inverse kinematics solution, there is no problem of multiple solutions; it has good real-time performance and can reach 25fps, and has a wide application prospect in the vision-based follow-up control of teleoperated robots; it has good smoothness, and the data synchronization frequency is greater than the vision detection frequency, making the humanoid robot have better smoothness during the vision-based follow-up process.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot control, and particularly relates to a vision-based double-arm follow-up control method, system and robot for humanoid robots. Background Art

[0002] The follow-up control of humanoid robots is an important part of the teleoperation technology of humanoid robots, which refers to collecting the motion data of human joints through various sensors, converting such data into the rotation data of each joint of the robot, so as to control the motion of the humanoid robot. In the existing technology, the collection of human joint data is mainly achieved by wearing a skeleton, and the operator needs to have a certain professional training to operate proficiently. It is not convenient to wear, has a high usage threshold and high cost, and is not conducive to the use in some non-high-precision occasions. Secondly, in the existing process of visually identifying the human skeleton, it is easily affected by environmental factors such as light, resulting in the jumping of the joint point positions during the recognition process, thus leading to a large deviation in the conversion of the joints into the angle data of the robot. In addition, the solution to inversely deduce the joint rotation through the three-dimensional space coordinate points is not unique, and it is difficult to choose which solution to control the motion of the humanoid robot joint rotation. Summary of the Invention

[0003] To solve the above technical problems, the present invention provides a vision-based double-arm follow-up control method, system and robot for humanoid robots.

[0004] The technical solution provided by the present invention is specifically as follows:

[0005] A vision-based double-arm follow-up control method for humanoid robots includes the steps of:

[0006] S1. Real-time collect human image data, including depth images and RGB images;

[0007] S2. Based on the collected image data, obtain the spatial position information of human joint points, and the joint points include head J1, shoulder center J2, right shoulder J3, right elbow J4, right wrist J5, right hand J6, left shoulder J7, left elbow J8, left wrist J9, left hand J10, spine J11, hip center J12, right hip J13, right knee J14, right ankle J15, right foot J16, left hip J17, left knee J18, left ankle J19 and left foot J20;

[0008] S3. According to the position information of the left shoulder J7 and the left elbow J8, calculate the rotational degree of freedom at the shoulder joint of the left upper arm of the human body;

[0009] S4. According to the position information of the right shoulder J3 and the right elbow J4, calculate the rotational degree of freedom at the shoulder joint of the right upper arm of the human body;

[0010] S5. Calculate the included angle at the elbow joint of the left arm according to the position information of the left shoulder J7, left elbow J8, and left wrist J9;

[0011] S6. Calculate the included angle at the elbow joint of the right arm according to the position information of the right shoulder J3, right elbow J4, and right wrist J5;

[0012] S7. Calculate the axial rotation angle of the left upper arm according to the position information of the left shoulder J7, left elbow J8, and left wrist J9;

[0013] S8. Calculate the axial rotation angle of the right upper arm according to the position information of the right shoulder J3, right elbow J4, and right wrist J5;

[0014] S9. Linearly interpolate the rotational degrees of freedom, included angles, and axial rotation angles calculated in S3 - S8, and send them to the corresponding joint control modules of the humanoid robot at a set frequency to achieve the follow - up control of the two arms of the humanoid robot.

[0015] Preferably, in step S1, the image data of the human body is obtained in real time through a Kinect vision sensor, and in step S2, based on the Kinect human skeleton recognition technology, the human skeleton within the image field of view is recognized, and then the position information of each joint point of the skeleton in three - dimensional space is obtained.

[0016] Further, in step S3, assuming that the spatial coordinates of the left shoulder J7 and left elbow J8 are ([[]] x 7, y 7, z 7) and ( x 8, y 8, z 8) respectively, then:

[0017] S301. Calculate the length of the left upper arm ;

[0018] S302. Set the initial state of the left upper arm to be vertically downward along the y - axis. Taking the left shoulder joint as the origin, the initial position of the left elbow joint , and the actual position is ;

[0019] S303. Set the angle range for the shoulder joint to lift as RXstart = 0, RXend = 90. If y8 - y7 ≥ 0, then set RXstart = 90, RXend = 180; and set Rz = 0, Rx = RXstart, minErr = 100;

[0020] S304. Calculate the predicted position of the left elbow joint , where the rotation matrices MR L1 and MR L2 are respectively: , ;

[0021] S305. Calculate the error between the predicted position and the actual position of the elbow joint. , if E L < minErr, then set minErr = E L , the angle L1 of the left shoulder joint lifted forward and backward is L1 = Rx, the angle L2 of the left shoulder joint lifted left and right is L2 = Rz. At this time, if E L < 0.01, then jump to S308; if E L ≥ minErr, then jump to S306;

[0022] S306. Set Rx = Rx + 1. If Rx ≥ RXend, jump to S307; otherwise, jump to S304.

[0023] S307. Set Rz = Rz + 1. If Rz ≥ 90, then jump to S308; otherwise, set Rx = RXstart and jump to S304.

[0024] S308. Obtain the angle L1 of the left shoulder joint lifted forward and backward and the angle L2 of the left shoulder joint lifted left and right.

[0025] Furthermore, in step S4, assume that the spatial coordinates of the right shoulder J3 and the right elbow J4 are respectively ( x 3, y 3, z 3) and ( x 4, y 4, z 4), then:

[0026] S401. Calculate the length of the right upper arm ;

[0027] S402. Set the initial state of the right upper arm to be vertically downward along the y-axis. Taking the right shoulder joint as the origin, the initial position of the right elbow joint , and the actual position is ;

[0028] S403. Set the range of the angle of the shoulder joint lifted as RXstart = 0, RXend = 90. If y4 - y3 ≥ 0, then set RXstart = 90, RXend = 180; and set Rz = 0, Rx = RXstart, minErr = 100;

[0029] S404. Calculate the predicted position of the right elbow joint , where the rotation matrices MR R1 and MR R2 are respectively: , ;

[0030] S405. Calculate the error between the predicted position and the actual position of the elbow joint. , if E R < minErr, then set minErr = E R , the angle R1 of the right shoulder joint lifted forward and backward is R1 = Rx, and the angle R2 of the left and right lift is R2 = Rz. At this time, if E R < 0.01, then jump to S408; if E R ≥ minErr, then jump to S406;

[0031] S406. Set Rx = Rx + 1. If Rx ≥ RXend, jump to S407; otherwise, jump to S404.

[0032] S407. Set Rz = Rz + 1. If Rz ≥ 90, then jump to S408; otherwise, set Rx = RXstart and jump to S404.

[0033] S408. Obtain the angle R1 of the right shoulder joint lifted forward and backward and the angle R2 of the left and right lift.

[0034] Furthermore, in step S5, assume that the spatial coordinates of the left shoulder J7, left elbow J8, and left wrist J9 are ([[]] x 7, y 7, z 7), ([[]] x 8, y 8, z 8), ([[]] x 9, y 9, z 9), then the included angle at the elbow joint of the left arm , where , .

[0035] Furthermore, in step S6, assume that the spatial coordinates of the right shoulder J3, right elbow J4, and right wrist J5 are ([[]] x 3, y 3, z 3), ([[]] x 4, y 4, z 4), ([[]] x 5, y 5, z 5), then the included angle at the elbow joint of the right arm , where , .

[0036] Furthermore, in step S7, assume that the spatial coordinates of the left shoulder J7, left elbow J8, and left wrist J9 are ([[]] x 7, y 7,z 7), ( x 8, y 8, z 8), ( x 9, y 9, z 9), then the calculation steps for the axial rotation angle of the upper arm of the left arm are as follows:

[0037] S701. Calculate the length of the left forearm ;

[0038] S702. Set the initial state of the left forearm to be vertically downward along the y-axis, with the left shoulder joint as the origin and the initial position of the left wrist joint , and the actual position is ;

[0039] S703. Set the axial rotation angle range RAstart = -45, RAend = 45, and set RA = RAstart, minErr = 100;

[0040] S704. Calculate the predicted position of the wrist joint of the left arm:

[0041] ,

[0042] where the translation matrix , the rotation matrix , the rotation matrix , the rotation matrix , the rotation matrix ;

[0043] S705. Calculate the error between the predicted position and the actual position of the elbow joint , if E l < minErr, then let minErr = E l , the axial rotation angle of the left upper arm L3 = RA, and at this time if E l < 0.01, then jump to S707; if E l ≥ minErr, then jump to S706;

[0044] S706. Let RA = RA + 1; if RA ≥ RAend, then jump to S707, otherwise jump to S704;

[0045] S707. Obtain the axial rotation angle L3 of the left upper arm.

[0046] Furthermore, in step S8, assume that the spatial coordinates of the right shoulder J3, right elbow J4, and right wrist J5 are respectively ( x 3, y 3, z 3), ( x 4,y 4, z 4), ( x 5, y 5, z 5), then the calculation steps for the axial rotation angle of the right upper arm are as follows:

[0047] S801. Calculate the length of the right forearm ;

[0048] S802. Set the initial state of the right forearm to be vertically downward along the y-axis, with the right shoulder joint as the origin and the initial position of the right wrist joint , and the actual position is ;

[0049] S803. Set the axial rotation angle range RAstart = -45, RAend = 45, and set RA = RAstart, minErr = 100;

[0050] S804. Calculate the predicted position of the right-arm wrist joint:

[0051] , where the translation matrix , the rotation matrix , the rotation matrix , the rotation matrix , the rotation matrix ;

[0052] S805. Calculate the error between the predicted position and the actual position of the elbow joint , if E l < minErr, then let minErr = E l , the axial rotation angle of the left upper arm L3 = RA, and at this time if E l < 0.01, then jump to S807; if E l ≥ minErr, then jump to S806;

[0053] S806. Let RA = RA + 1; if RA ≥ RAend, then jump to S807, otherwise jump to S804;

[0054] S807. Obtain the axial rotation angle R3 of the right upper arm.

[0055] A humanoid robot double-arm follow-up control system based on the above method, including:

[0056] An image acquisition module for real-time acquisition of human body image data;

[0057] A human body joint point positioning module for obtaining the spatial position information of human body joint points from the acquired image data;

[0058] The left - arm control parameter acquisition module is used to calculate the rotational degrees of freedom at the shoulder joint of the left upper arm, the angle at the elbow joint of the left arm, and the axial rotation angle of the left upper arm according to the acquired spatial position information of the human body joint points.

[0059] The right - arm control parameter acquisition module is used to calculate the rotational degrees of freedom at the shoulder joint of the right upper arm, the angle at the elbow joint of the right arm, and the axial rotation angle of the right upper arm according to the acquired spatial position information of the human body joint points.

[0060] The control parameter output module is used to linearly interpolate the calculated rotational degrees of freedom, angles, and axial rotation angles and send them to the corresponding joint control modules of the humanoid robot at a set frequency to achieve the follow - up control of the two arms of the humanoid robot.

[0061] A humanoid robot includes a head, hands, forearms, upper arms, shoulders, eyes, mouth, front chest shell, back shell, base, power switch, power cord, headlamp, earlamp, chest lamp, soft - rubber neck, display screen, microphone, and speaker; the shoulder joint of the upper arm has three degrees of freedom of forward - backward movement, left - right movement, and axial rotation, the wrist joint of the forearm has two degrees of freedom of rotation around the forearm axis and forward - backward movement along the direction perpendicular to the palm, the hand has five separately movable fingers, and the head has two degrees of freedom of nodding up - down and shaking left - right; the humanoid robot is also configured with the above - mentioned two - arm follow - up control system, including an image acquisition module, a human body joint point positioning module, a left - arm control parameter acquisition module, a right - arm control parameter acquisition module, and a control parameter output module that outputs control parameters to the joint control module.

[0062] Compared with the prior art, the present invention has at least the following beneficial effects:

[0063] It can achieve the follow - up control of the two arms of the humanoid robot through vision, which can well replace the traditional wearable devices; by using a resolution of 1 degree for the rotation angle to calculate the error in a loop, it can well solve the problem of small - range error jitter in the process of skeleton recognition by vision; the rotation calculation order of each axis is specified, and there is no problem of multiple solutions compared with the traditional direct inverse solution; it has good real - time performance and can reach 25fps, and has a wide application prospect in the visual follow - up control of teleoperated robots; it has good smoothness, and the data synchronization frequency is greater than the visual detection frequency, enabling the humanoid robot to have good smoothness in the visual follow - up process. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] The drawings are used to provide further understanding of the present invention, and constitute a part of the specification. They are used to explain the present invention together with the embodiments of the present invention, and do not constitute a limitation to the present invention.

[0065] Figure 1Schematic diagram of the visual double-arm follow-up control method of a humanoid robot provided by an embodiment of the present invention;

[0066] Figure 2 Schematic diagram of 20 joint points of the Kinect human skeleton provided by an embodiment of the present invention;

[0067] Figure 3 Front view of a humanoid robot provided by an embodiment of the present invention;

[0068] Figure 4 Side view of a humanoid robot provided by an embodiment of the present invention;

[0069] Figure 5 Rear view of a humanoid robot provided by an embodiment of the present invention.

[0070] Among them, each reference numeral represents:

[0071] 1 - hand, 2 - forearm, 3 - upper arm, 4 - shoulder, 5 - eye, 6 - mouth, 7 - high-definition camera, 8 - front chest shell, 9 - back shell, 10 - base, 11 - power switch, 12 - power cord, 13 - headlamp, 14 - ear lamp, 15 - chest lamp, 16 - soft rubber neck, 17 - display screen, 18 - microphone and speaker. Specific embodiments

[0072] To make the objectives, 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. Apparently, the described embodiments are some, but not all, of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0073] Embodiment 1

[0074] This embodiment provides a vision-based double-arm follow-up control method for a humanoid robot, as Figure 1 shown, which mainly includes the following steps:

[0075] Step S1: Through the Kinect vision sensor, the depth image data and RGB image data of the human body are collected in real time.

[0076] Step S2: For the real-time image data obtained in step S1, based on the Kinect human skeleton recognition technology, the human skeleton within the field of view is recognized, and the position information of the 20 joint points of the skeleton in the three-dimensional space is recorded. The position information is represented by the three-dimensional space coordinates (x, y, z). These 20 joint points are respectively represented by J1, J2, J3... J20, as Figure 2As shown in the figure, specifically: head J1, shoulder center J2, right shoulder J3, right elbow J4, right wrist J5, right hand J6, left shoulder J7, left elbow J8, left wrist J9, left hand J10, spine J11, hip center J12, right hip J13, right knee J14, right ankle J15, right foot J16, left hip J17, left knee J18, left ankle J19, left foot J20. The coordinates of J1 in space are represented by (x1, y1, z1), the coordinates of J2 in space are represented by (x2, y2, z2), and so on. The coordinates of J20 in space are (x20, y20, z20).

[0077] Step S3: Based on the information of two joint positions J7(x7, y7, z7) and J8(x8, y8, z8) in step S2, calculate two rotational degrees of freedom at the shoulder joint of the left upper arm of the human body. The calculation process is as follows:

[0078] (1) Calculate the length of the left upper arm, and the calculation method is:

[0079] ;

[0080] (2) Set the initial state of the left upper arm to be vertically downward along the y-axis, with the left shoulder joint as the origin and the initial position of the left elbow joint , and the actual position is

[0081] ;

[0082] (3) Set RXstart = 0, RXend = 90; if y8 - y7 ≥ 0, then set RXstart = 90, RXend = 180, and set Rz = 0, Rx = RXstart, minErr = 100 (the angle range for the front and back lift of the left upper arm shoulder joint of the robot is [0, 180], and the angle range for the left and right lift of the left upper arm shoulder joint is [0, 90]);

[0083] (4) Generate two rotation matrices MR L1 and MR L2 , respectively:

[0084] ,

[0085] Calculate the predicted position of the elbow joint ;

[0086] (5) Calculate the error between the predicted position and the actual position of the elbow joint , if E L < minErr, then let minErr = E L , L1 = Rx, L2 = Rz, at this time if E LIf <0.01, then jump to (8); if E L ≥minErr, then jump to (6);

[0087] (5) Let Rx = Rx + 1. If Rx ≥ RXend, jump to (7); otherwise, jump to (4);

[0088] (7) Let Rz = Rz + 1. If Rz ≥ 90, then jump to (8); otherwise, let Rx = RXstart and jump to (4);

[0089] (8) Obtain the angle L1 of the shoulder joint lifted forward and backward and the angle L2 of the shoulder joint lifted left and right.

[0090] Step S4: Based on the two joint position information of J3(x3, y3, z3) and J4(x4, y4, z4) in Step S2, calculate the two rotational degrees of freedom at the shoulder joint of the right upper arm of the human body. The calculation process is as follows:

[0091] (1) Calculate the length of the right upper arm. The calculation method is:

[0092] ;

[0093] (2) Set the initial state of the right upper arm to be vertically downward along the y-axis. Taking the right shoulder joint as the origin, the initial position of the right elbow joint , and the actual position is

[0094] ;

[0095] (3) Set RXstart = 0, RXend = 90; if y4 - y3 ≥ 0, then set RXstart = 90, RXend = 180; set Rz = 0, Rx = RXstart, minErr = 100 (the range of the angle of the right upper arm shoulder joint lifted forward and backward is [0, 180], and the range of the angle of the right upper arm shoulder joint lifted left and right is [0, 90]);

[0096] (4) Generate two rotation matrices MR R1 and MR R2 , respectively:

[0097] ,

[0098] Calculate the predicted position of the elbow joint ;

[0099] (5) Calculate the error between the predicted position and the actual position of the elbow joint , if E R <minErr, then let minErr = E R, R1 = Rx, R2 = Rz; At this time, if E R < 0.01, then jump to (8), if E R ≥ minErr, then jump to (6);

[0100] (6) Let Rx = Rx + 1; if Rx ≥ RXend, jump to (7), otherwise jump to (4);

[0101] (7) Let Rz = Rz + 1; if Rz ≥ 90, then jump to (8), otherwise let Rx = RXstart, jump to (4);

[0102] (8) Obtain the angle R1 of the right shoulder joint lifted forward and backward, and the angle R2 of the left and right lift.

[0103] Step S5: Based on the three joint position information of J7(x7, y7, z7), J8(x8, y8, z8) and J9(x9, y9, z9) in step S2, calculate the included angle at the elbow joint of the left arm: Calculate the vector , , to obtain the elbow angle of the left arm .

[0104] Step S6: Based on the three joint position information of J3(x3, y3, z3), J4(x4, y4, z4) and J5(x5, y5, z5) in step S2, calculate the included angle at the elbow joint of the right arm: Calculate the vector , , to obtain the elbow angle of the right arm .

[0105] Step S7: Based on the three joint position information of J7(x7, y7, z7), J8(x8, y8, z8) and J9(x9, y9, z9) in step S2, calculate the axial rotation angle of the left upper arm. The calculation process is as follows:

[0106] (1) Calculate the length of the left forearm, and its calculation method is:

[0107] ;

[0108] (2) Set the initial state of the left forearm to be vertically downward along the y-axis. Taking the left shoulder joint as the origin, the initial position of the left wrist joint , the actual position is

[0109] ;

[0110] (3) Set RAstart = -45, RAend = 45; and set RA = RAstart; minErr = 100 (the axial rotation angle range of the robot's left upper arm is [-45, 45]);

[0111] (4) Generate a translation matrix MT L1 and four rotation matrices MR L01 、MR L02 、MR L03 、MR L04 , which are respectively:

[0112] ,

[0113] ,

[0114] ,

[0115] Calculate the predicted position of the wrist joint of the left arm:

[0116] ;

[0117] (5)Calculate the error between the predicted position and the actual position of the elbow joint , if E l < minErr, then let minErr = E l , L3 = RA, at this time if E l < 0.01, then jump to (7); if E l ≥ minErr, then jump to (6);

[0118] (6)Let RA = RA + 1; if RA ≥ RAend, then jump to (7), otherwise jump to (4);

[0119] (7)Obtain the angle L3 of the axial rotation of the left upper arm.

[0120] Step S8: Based on the three joint position information of J3(x3, y3, z3), J4(x4, y4, z4) and J5(x5, y5, z5) in step S2, calculate the axial rotation angle of the right upper arm. The calculation process is as follows:

[0121] (1)Calculate the length of the right forearm, and its calculation method is:

[0122] ;

[0123] (2)Set the initial state of the right forearm to be vertically downward along the y-axis, with the right shoulder joint as the origin and the initial position of the right wrist joint , and the actual position is

[0124] ;

[0125] (3)Set RAstart = -45, RAend = 45, and set RA = RAstart, minErr = 100 (the angular range of the axial rotation of the robot's right upper arm is [-45, 45]);

[0126] (4)Generate a translation matrix MT R1 and four rotation matrices MR R01 、MR R02 、MR R03 、MR R04 , which are respectively:

[0127] ,

[0128] ,

[0129] ,

[0130] Calculate the predicted position of the left arm wrist joint:

[0131] ;

[0132] (5)Calculate the error between the predicted position and the actual position of the elbow joint . If E r < minErr, then set minErr = E r , R3 = RA. At this time, if E r < 0.01, then jump to (7); if E r ≥ minErr, then jump to (6);

[0133] (6)Set RA = RA + 1. If RA ≥ RAend, then jump to (7); otherwise, jump to (4);

[0134] (7)Obtain the angle R3 of the axial rotation of the right upper arm.

[0135] Step S9: Output the 8 rotation angles L1, L2, L3, L4, R1, R2, R3, R4 corresponding to the human arm and the humanoid robot arm.

[0136] Step S10: Send the 8 rotation angles L1, L2, L3, L4, R1, R2, R3, R4 to the humanoid robot as synchronous joint data at a frequency of 50 Hz in a linear interpolation manner to achieve the function of the visual double - arm follow - up of the humanoid robot.

[0137] This embodiment completely determines the joint information of the human body in the three-dimensional space based on robot vision, and converts this kind of joint information into the joint rotation angle information of the humanoid robot according to the joint model of the humanoid robot, so as to realize the visual servo control of the humanoid robot. Without additional wearable devices, the implementation is simple and the judgment accuracy is high.

[0138] Embodiment 2

[0139] Based on the above-mentioned double-arm servo control method, this embodiment provides a double-arm servo control system for a humanoid robot, which mainly includes the following modules:

[0140] An image acquisition module, which is used to collect human body image data in real time;

[0141] A human body joint point positioning module, which is used to obtain the spatial position information of human body joint points from the collected image data;

[0142] A left arm control parameter acquisition module, which is used to calculate the rotational degree of freedom at the shoulder joint of the left upper arm, the included angle at the elbow joint of the left arm, and the axial rotation angle of the left upper arm according to the obtained spatial position information of human body joint points;

[0143] A right arm control parameter acquisition module, which is used to calculate the rotational degree of freedom at the shoulder joint of the right upper arm, the included angle at the elbow joint of the right arm, and the axial rotation angle of the right upper arm according to the obtained spatial position information of human body joint points;

[0144] A control parameter output module, which is used to linearly interpolate the calculated rotational degree of freedom, included angle, and axial rotation angle, and send them to the corresponding joint control module of the humanoid robot at a set frequency to realize the servo control of the two arms of the humanoid robot.

[0145] The above product can execute the vision-based double-arm servo control method for a humanoid robot described in Embodiment 1, and has the corresponding functional modules and beneficial effects of the method. For the technical details not described in detail in this embodiment, reference can be made to the detailed content provided in Embodiment 1 of the present invention.

[0146] Embodiment 3

[0147] Based on the above-mentioned double-arm servo control method and its corresponding system, this embodiment provides a humanoid robot, whose main components are as Figure 3 、 Figure 4 and Figure 5As shown in the figure, the components shown in the figure are respectively a five-fingered dexterous hand 1, a forearm 2, a upper arm 3, a shoulder 4, eyes 5, a mouth 6, a high-definition camera 7, a front chest housing 8, a back housing 9, a base 10, a power switch 11, a power cord 12, a headlight 13, an earlight 14, a chest light 15, a soft rubber neck 16, a display screen 17, a microphone and a speaker 18. Among them, the joint layouts of the left arm and the right arm are the same. The shoulder joint of the upper arm has three degrees of freedom. The upper arm moves back and forth, left and right around the shoulder joint, and the upper arm can rotate axially; the elbow joint has one degree of freedom; the wrist joint has two degrees of freedom, rotating around the axis of the forearm and moving back and forth along the direction perpendicular to the palm; the palms of both the left and right hands each have five fingers that can move independently; the head has two degrees of freedom, nodding up and down and shaking left and right. This humanoid robot has various modules described in Embodiment 2 and can perform double-arm follow-up control through the method described in Embodiment 1. The specific control steps are not elaborated here. For the specific content, please refer to the corresponding descriptions in Embodiment 1 and Embodiment 2.

[0148] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit them; under the idea of the present invention, the technical features in the above embodiments or different embodiments can also be combined, and the steps can be implemented in any order, and there are many other variations in different aspects of the present invention as described above. For the sake of brevity, they are not provided in detail; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions recorded in the foregoing embodiments, or perform equivalent replacements for some of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of each embodiment of the present application.

Claims

1. A vision-based humanoid robot dual-arm follow-up control method, characterized in that: Includes steps: S1, real-time collection of human image data, including depth images and RGB images; S2. Based on the collected image data, spatial position information of human joints is obtained, where the joints include head J1, shoulder center J2, right shoulder J3, right elbow J4, right wrist J5, right hand J6, left shoulder J7, left elbow J8, left wrist J9, left hand J10, spine J11, hip center J12, right hip J13, right knee J14, right ankle J15, right foot J16, left hip J17, left knee J18, left ankle J19 and left foot J20; S3. Calculate the rotational freedom of the left upper arm shoulder joint according to the position information of the left shoulder J7 and the left elbow J8: Assume that the spatial coordinates of the left shoulder J7 and the left elbow J8 are ( x 7, y 7, z 7) and ( x 8, y 8, z 8), then: S301, calculate the length of the left upper arm ; S302: Set the initial state of the left upper arm vertically downward along the y-axis, with the left shoulder joint as the origin. The initial position of the left elbow joint is , the actual location is ; S303, set the angle range of the shoulder joint lifting RXstart=0, RXend=90, if y8-y7≥0, set RXstart=90, RXend=180; and set Rz=0, Rx=RXstart, minErr=100; S304: Calculate the predicted position of the left elbow joint , where the rotation matrix MR L1 and MR L2 They are: , ; S305, calculating the error between the predicted position and the actual position of the elbow joint , if E L < minErr, then let minErr = E L , the left shoulder joint lift angle L1 = Rx, the left and right lift angle L2 = Rz, at this time if E L <0.01, then jump to S308; if E L ≥minErr, jump to S306; S306, let Rx = Rx + 1, if Rx ≥ RXend, jump to S307, otherwise jump to S304; S307, set Rz = Rz + 1, if Rz ≥ 90, jump to S308, otherwise set Rx = RXstart, jump to S304; S308, obtaining the front-to-back lifting angle L1 and the left-to-right lifting angle L2 of the left shoulder joint; S4, calculating the rotational freedom at the shoulder joint of the right upper arm of the human body according to the position information of the right shoulder J3 and the right elbow J4; S5. Calculate the angle at the left arm elbow joint according to the position information of the left shoulder J7, the left elbow J8 and the left wrist J9; S6. Calculate the angle at the elbow joint of the right arm according to the position information of the right shoulder J3, the right elbow J4 and the right wrist J5; S7. Calculate the axial rotation angle of the left upper arm according to the position information of the left shoulder J7, the left elbow J8 and the left wrist J9: Assume that the spatial coordinates of the left shoulder J7, left elbow J8 and left wrist J9 are ( x 7, y 7, z 7) x 8, y 8, z 8) x 9, y 9, z 9), the calculation steps of the axial rotation angle of the left upper arm are: S701. Calculate the length of the left forearm ; S702, set the initial state of the left forearm vertically downward along the y-axis, with the left shoulder joint as the origin, and the initial position of the left wrist joint , the actual location is ; S703, set the axial rotation angle range RAstart=-45, RAend=45, and set RA=RAstart, minErr=100; S704, calculating the predicted position of the left arm wrist joint: ， Among them, the translation matrix , the rotation matrix , the rotation matrix , the rotation matrix , the rotation matrix ; S705. Calculate the error between the predicted position and the actual position of the elbow joint , if E l < minErr, then let minErr=E l , the axial rotation angle of the left upper arm is L3 = RA. At this time, if E l <0.01, then jump to S707; if E l ≥minErr, jump to S706; S706, let RA = RA + 1; if RA ≥ RAend, jump to S707, otherwise jump to S704; S707, obtaining the axial rotation angle L3 of the left upper arm; S8, calculating the axial rotation angle of the right upper arm according to the position information of the right shoulder J3, the right elbow J4 and the right wrist J5; S9, linearly interpolate the rotational degrees of freedom, angles, and axial rotation angles calculated by S3-S8, and send them to the corresponding joint control modules of the humanoid robot at a set frequency to achieve follow-up control of the arms of the humanoid robot.

2. The humanoid robot dual-arm follow-up control method according to claim 1, characterized in that: In step S1, the image data of the human body is acquired in real time through the Kinect visual sensor. In step S2, the human skeleton in the image field is identified based on the Kinect human skeleton recognition technology, and then the position information of each joint point of the skeleton in the three-dimensional space is acquired.

3. The method for controlling the dual arms of a humanoid robot according to claim 1, wherein: In step S4, it is assumed that the spatial coordinates of the right shoulder J3 and the right elbow J4 are respectively ( x 3, y 3, z 3) and ( x 4, y 4, z 4), then: S401, calculate the length of the right upper arm ; S402: Set the initial state of the right upper arm vertically downward along the y-axis, with the right shoulder joint as the origin. The initial position of the right elbow joint is , the actual location is ; S403, set the angle range of the shoulder joint lifting RXstart=0, RXend=90, if y4-y3≥0, set RXstart=90, RXend=180; and set Rz=0, Rx=RXstart, minErr=100; S404: Calculate the predicted position of the right elbow joint , where the rotation matrix MR R1 and MR R2 They are: ， ； S405, calculating the error between the predicted position and the actual position of the elbow joint , if E R < minErr, then let minErr = E R , the right shoulder joint lift angle R1 = Rx, the left and right lift angle R2 = Rz, at this time if E R <0.01, then jump to S408; if E R ≥minErr, jump to S406; S406, let Rx = Rx + 1, if Rx ≥ RXend, jump to S407, otherwise jump to S404; S407, set Rz = Rz + 1, if Rz ≥ 90, jump to S408, otherwise set Rx = RXstart, jump to S404; S408, obtaining the front-to-back lifting angle R1 and the left-to-right lifting angle R2 of the right shoulder joint.

4. The method for controlling the dual arms of a humanoid robot according to claim 1, wherein: In step S5, it is assumed that the spatial coordinates of the left shoulder J7, the left elbow J8 and the left wrist J9 are respectively ( x 7, y 7, z 7) x 8, y 8, z 8) x 9, y 9, z 9), then the angle at the left arm elbow joint ,in , .

5. The method for controlling the dual arms of a humanoid robot according to claim 1, wherein: In step S6, it is assumed that the spatial coordinates of the right shoulder J3, the right elbow J4 and the right wrist J5 are respectively ( x 3, y 3, z 3) x 4, y 4, z 4) x 5, y 5, z 5), then the angle at the elbow joint of the right arm ,in , .

6. The method for controlling the dual arms of a humanoid robot according to claim 1, wherein: In step S8, it is assumed that the spatial coordinates of the right shoulder J3, the right elbow J4 and the right wrist J5 are respectively ( x 3, y 3, z 3) x 4, y 4, z 4) x 5, y 5, z 5), then the calculation steps of the axial rotation angle of the right upper arm are: S801. Calculate the length of the right forearm ; S802, set the initial state of the right forearm vertically downward along the y-axis, with the right shoulder joint as the origin, and the initial position of the right wrist joint , the actual location is ; S803, set the axial rotation angle range RAstart=-45, RAend=45, and set RA=RAstart, minErr=100; S804, calculating the predicted position of the right arm wrist joint: ， Among them, the translation matrix , the rotation matrix , the rotation matrix , the rotation matrix , the rotation matrix ; S805. Calculate the error between the predicted position and the actual position of the elbow joint , if E l < minErr, then let minErr=E l , the axial rotation angle of the left upper arm is L3 = RA. At this time, if E l <0.01, then jump to S807; if E l ≥minErr, jump to S806; S806, let RA = RA + 1; if RA ≥ RAend, jump to S807, otherwise jump to S804; S807, obtaining the axial rotation angle R3 of the right upper arm.

7. A humanoid robot dual-arm follow-up control system based on the method according to any one of claims 1 to 6, characterized in that: include: An image acquisition module, used for collecting human body image data in real time; A human joint point positioning module is used to obtain the spatial position information of human joint points from the collected image data; The left arm control parameter acquisition module is used to calculate the rotational freedom of the left upper arm shoulder joint, the angle of the left arm elbow joint and the axial rotation angle of the left upper arm according to the acquired spatial position information of the human body joint points; The right arm control parameter acquisition module is used to calculate the rotational freedom at the shoulder joint of the right upper arm, the angle at the elbow joint of the right arm and the axial rotation angle of the right upper arm according to the spatial position information of the acquired human body joint points; The control parameter output module is used to linearly interpolate the calculated rotational degrees of freedom, angles, and axial rotation angles, and send them to the corresponding joint control modules of the humanoid robot at a set frequency to achieve follow-up control of the humanoid robot's arms.

8. A humanoid robot, characterized in that: It includes a head, hands, forearms, upper arms, shoulders, eyes, mouth, front chest shell, back shell, base, power switch, power cord, headlamp, ear lamp, chest lamp, soft rubber neck, display screen, microphone and speakers; the shoulder joint of the upper arm has three degrees of freedom of forward and backward movement, left and right movement and axial rotation, the wrist joint of the forearm has two degrees of freedom of rotation around the axial direction of the forearm and forward and backward rotation along the direction perpendicular to the palm, the hand has five independently movable fingers, and the head has two degrees of freedom of nodding up and down and shaking left and right; the humanoid robot is also equipped with a double-arm follow-up control system as described in claim 7, including an image acquisition module, a human joint point positioning module, a left arm control parameter acquisition module, a right arm control parameter acquisition module and a control parameter output module that outputs control parameters to the joint control module.

Images