A control method for a robot full-body imitation system

Abstract

The present invention discloses a control method for a robot full-body imitation system. The method comprises the following steps: Step 1, based on the coordinates of the human skeleton points captured by the somatosensory camera Kinect, the coordinates of the human skeleton points in the Kinect coordinate system are transformed into representations in the human coordinate system; Step 2, the human joint angles and the connecting rod vectors are solved using the vector method; Step 3, a robot kinematic model is established to obtain the robot's center of mass position, the position of each effector end and its constraint range and the robot's connecting rod vector; Step 4, an objective function is established, the optimal solution is iteratively solved as the robot's control parameters, and finally the control parameters are passed to the robot for execution. The present invention models the robot's imitation and balance control as an optimization problem, obtains the optimal control parameters through real-time iterative solution, and thus takes into account the similarity of balance and imitation.

Description

Technical Field

[0001] The invention relates to the field of human-machine interaction and robot control, and in particular to a control method for a robot full-body imitation system. Background Art

[0002] Nowadays, a large number of humanoid robots have been developed, and they are expected to be as similar to humans as possible. In traditional human-computer interaction, buttons and handles are mostly used, which limits the degree of freedom of operation. People need a freer and more natural way to control robots. Training robots by imitating human movements has attracted more and more attention due to the many benefits it brings. First, it can greatly simplify the human-computer interaction mode; second, imitation is also very helpful in improving the rationality of the robot's posture and movements and the expression of body language; third, it also helps to understand the research on human movement mechanism.

[0003] In the motion imitation system, when humans demonstrate a series of actions, the robot should reproduce these actions as quickly as possible and ensure their similarity. The technology can be specifically divided into three parts: motion capture, motion redirection and robot drive.

[0004] There are many mature solutions for motion capture, most of which use wearable sensors or optical depth sensors to capture human motion. Wearable sensors have high accuracy and reliability; optical depth sensors (such as Microsoft Kinect) have certain disadvantages in accuracy due to self-occlusion and depth measurement errors, but their non-invasiveness and low cost bring great convenience. There are also some other methods that use multi-sensor fusion to obtain more modalities and higher-precision data.

[0005] For motion redirection, considering the many differences between humans and humanoid robots (such as body structure, mass distribution, joint freedom, drive mode, etc.), the captured human motion parameters cannot be directly reproduced by the robot, and they need to be represented as data in the robot's operating space. Therefore, researchers have done a lot of work on motion representation to transfer human motion to the robot. Generally speaking, most methods are based on inverse kinematics (IK) or vector mapping. In the human motion space, the human link is represented by a vector, and the angle between the vectors is calculated to obtain the rotation in the robot joint space. It should be noted that the points captured by Kinect are all 3D vectors with the camera as the reference system, and the rotation around the link axis cannot be obtained. This makes the calculation of joint angles must be based on the human body as the reference system. In some works, the joint angles of the human body are represented as the angle between the link and the Kinect coordinate axis, which is only applicable when the human body faces Kinect. Once a rotation occurs between the two, this method is no longer applicable.

[0006] In terms of robot driving, the robot must ensure its balance and smoothness in full-body motion simulation, so it is necessary to optimize and constrain motion parameters to maintain balance control, and use a series of communication mechanisms and control strategies to drive the robot. In this regard, most methods assume that the robot moves at a relatively low speed, equating the zero moment point (ZMP) with the projection of the center of mass (CoM) on the ground, and the balance problem is thus simplified to keeping the projection of the center of mass within the support polygon of the foot.

[0007] For the above tasks, researchers have proposed a series of methods to capture the movements of human instructors and pass them to robots for execution. However, how to convert human motion information into data suitable for robot motion and ensure the balance and similarity of the robot's imitation process is still a challenging task. Summary of the invention

[0008] In view of the above-mentioned defects of the prior art, the present invention provides a control method for a robot full-body imitation system.

[0009] In order to achieve the above-mentioned invention object, the technical solution adopted by the present invention is as follows:

[0010] A control method for a robot full body imitation system, characterized in that the method comprises the following steps:

[0011] Step 1, based on the coordinates of the human skeleton points captured by the somatosensory camera Kinect, the coordinates of each human skeleton point in the Kinect coordinate system are transformed into representations in the human coordinate system;

[0012] Step 2: Use the vector method to solve the human body joint angles and the connecting rod vectors, and estimate the current human body support mode based on the data captured by the Kinect body sensing camera;

[0013] Step 3, establish the robot kinematic model, constrain the joint angle value range according to the rotation range of the motor of each joint of the robot; according to the current human body support mode, determine the support polygon of the robot in the corresponding mode to constrain the center of mass range; input the joint angle into the robot's DH parameter coordinate transformation matrix, so as to obtain the robot's center of mass position, the position of each effector end and its constraint range and the robot's link vector;

[0014] Step 4, establish the objective function according to the human joint angles and connecting rod vectors obtained in step 2; at the same time, obtain the joint angles of the robot in the current state, and combine them with the input human target joint angles as the initial values; continuously send the joint angle parameters to the robot kinematic model to detect whether they meet the constraint range obtained in step 3, iterate along the maximum gradient direction of the objective function, and solve the optimal solution as the control parameter of the robot, and finally pass the control parameter to the robot for execution.

[0015] Furthermore, in step 1, the method for establishing the human body coordinate system is: the plane normal vector composed of the right shoulder joint point RShoulder, the left shoulder joint point LShoulder and the hip joint point Torso extracted by the body sensing camera Kinect is represented as p Torso , which points to the front of the human body, and its direction is defined as the x-axis in the human body coordinate system; then the vector from the hip joint point Torso to the neck joint point Neck is defined as Perpendicular to vector p Torso and And the direction pointing to the left side of the object is defined as the y-axis.

[0016] Furthermore, in step 1, in the human body coordinate system, the coordinates of all bone points are written as r H ; Through the formula: Convert all other link vectors from the Kinect coordinate system to the human coordinate system, where r K Represents all vectors in the Kinect coordinate system of the somatosensory camera. Represents the transformation matrix from the Kinect camera to the human body coordinate system.

[0017] Furthermore, in step 2, the coordinates of the corresponding bone points transformed in step 1 are subtracted to obtain the human connecting rod vector; and the human joint angle is calculated according to the method for solving the vector angle.

[0018] Furthermore, in the step 3, the robot kinematic model includes the robot's DH parameter coordinate transformation matrix, the rotation range of each joint motor and the support polygon range of the robot under different support modes; the DH parameter coordinate transformation matrix is ​​used to transform the joint point coordinate parameters into representations in the torso and foot coordinate systems respectively; the rotation range of each joint motor is used to limit the upper and lower limits of each joint angle value; the support polygon range of the robot under different support modes is used to constrain the upper and lower limits of the x-coordinate of the robot's center of mass position and the upper and lower limits of the y-coordinate.

[0019] Furthermore, in step 3, the DH parameter coordinate transformation matrix first transforms the local position of each joint in the static position into a representation in the trunk coordinate system; then the center of mass position of each joint is weighted averaged according to the mass, thereby obtaining the representation of the center of mass position of the entire robot in the trunk coordinate system; then, according to different support modes, the corresponding DH parameter transformation matrix is ​​used to obtain the representation of the center of mass position of the robot in the foot coordinate system; finally, the coordinate position of the end of each effector of the robot in the trunk coordinate system is obtained using the DH parameter transformation matrix, and the corresponding coordinates are subtracted to obtain the connecting rod vector of the robot.

[0020] Furthermore, in step 4, the objective function is:

[0021]

[0022]

[0023] Among them, α and β are adjustable hyperparameters; θ is the parameter to be optimized; θ des is the human joint angle; p des is the human connecting rod vector, λ i (θ,p des ) is the connecting rod vector difference function, which can be written as

[0024]

[0025] p i (θ) is the link vector of the ith robot, is the connecting rod vector of the i-th body; A Robot (θ)∈C A and θ∈C θ The robot's center of mass constraint range and the rotation range of each joint motor; Constrains the robot's different effector ends It must be greater than a certain distance threshold h to prevent self-collision, and i≠j represents different effector ends.

[0026] The method proposed by the present invention has the following beneficial effects:

[0027] (1) During the motion capture process, a human body coordinate system is established corresponding to the robot's torso coordinate system. The motion information is converted from the Kinect coordinate system to the human body coordinate system, which can obtain a more accurate representation of human body motion information.

[0028] (2) Combined with the kinematic model, the robot's imitation and balance control are modeled as an optimization problem, and the optimal control parameters are obtained through real-time iterative solution, thereby taking into account the similarities between balance and imitation. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 It is a schematic diagram of the process of the present invention;

[0030] Figure 2 are the coordinates of human skeleton points, (a) is the schematic diagram of the original human joint points extracted by Kinect, and (b) is the schematic diagram of the human joint points after removing some redundant points;

[0031] Figure 3 The coordinate systems of the human body, Kinect, and robot. (a) is a schematic diagram of the original coordinate system of Kinect and the defined human body coordinate system, and (b) is a schematic diagram of the coordinate system of the robot.

[0032] Figure 4 This is a schematic diagram of the similarity after optimization by this method;

[0033] Figure 5 This is a schematic diagram of the similarity comparison before and after optimization;

[0034] Figure 6 The center of mass activity range of the robot after optimization, (a) is a schematic diagram of the activity range in the x-axis direction, and (b) is a schematic diagram of the activity range in the y-axis direction;

[0035] Figure 7 Schematic diagram of the distance between the robot's center of mass projection and the ideal support point during the imitation process. DETAILED DESCRIPTION

[0036] In order to make the purpose, technical scheme and advantages of the embodiments of the present invention clearer, the implementation method of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the drawings described below are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0037] Aiming at the similarity and balance control problems in the robot imitation process, the present invention solves the control parameters in the robot imitation system and controls the system, such as Figure 1 As shown, the specific steps include:

[0038] Step 1: Based on the coordinates of the human skeleton points captured by the somatosensory camera KinectV2, with reference to the torso coordinate system of the Nao V6 robot, the coordinates of each human skeleton point in the Kinect coordinate system are transformed into representations in the human coordinate system;

[0039] Step 2: Use the vector method to solve the human body joint angles and limb link vectors, and estimate the human body's support mode at this time based on the data captured by Kinect;

[0040] Step 3, establish the robot kinematic model, determine the support polygon of the robot in the corresponding mode according to the current support mode of the human body, input the joint angle into the Denavit-Hartenberg (DH) parameter transformation matrix of the robot, and thus obtain the CoM of the robot and the position and constraint range of each effector end, as well as the link vector of the robot;

[0041] Step 4, model the solution of the robot control parameters as an optimization problem: establish the objective function based on the human target joint angles and limb linkage vectors obtained in step 2; set the constraint range according to step 3; at the same time, obtain the joint angles of the robot in the current state, and combine them with the input human target joint angles as the initial values; continuously send the optimization parameters to the robot kinematic model to detect whether the constraints are met, iterate along the maximum gradient direction of the objective function to solve the optimal solution as the robot's control parameters, and finally pass the control parameters to the robot for execution.

[0042] Furthermore, in step 1, Kinect V2 continuously captures the three-dimensional coordinates of 25 skeletal points of the human body at a frequency of 30 Hz, such as Figure 2 As shown in (a). Since some joints (such as fingers and feet) extracted by Kinect have no effect on the robot's motion control, some redundant points are removed and 15 points are retained for calculating joint angles. Figure 2 (b) The "R" before the joint name refers to the right side, and the "L" refers to the left side. The coordinate system of Kinect is as follows Figure 3 (a) (right side), Nao's coordinate system is Figure 3 (b) In order to express the human body link vectors in the human body coordinate system (the directions of the link vectors are as follows Figure 2 (b)), first define the human body coordinate system: the plane normal vector composed of the right shoulder joint point RShoulder, the left shoulder joint point LShoulder and the hip joint point Torso is represented as p Torso , which points to the front of the human body, and its direction is defined as the x-axis in the human body coordinate system; then the vector from the hip joint point Torso to the neck joint point Neck is defined as Perpendicular to vector p Torso and The direction pointing to the left of the object is defined as the y-axis, so we establish Figure 3 (a) (left) shows the right-hand coordinate system, i.e. the human body coordinate system. In this coordinate system, the coordinates of all bone points can be written as r H On this basis, through

[0043]

[0044] Convert all other link vectors from the Kinect coordinate system to the human coordinate system, where r K Represents all vectors in the Kinect coordinate system, Represents the transformation matrix from Kinect to human body coordinate system.

[0045] Furthermore, in step 2, the human body connecting rod vector p can be obtained by subtracting the corresponding bone point coordinates obtained in step 1 after transformation. des ; According to the method of solving the vector angle, the target joint angle θ of the human body can be calculated des .

[0046] Furthermore, in step 3, the robot kinematic model established includes the Denavit-Hartenberg (DH) parameter coordinate transformation matrix of the Nao robot It is a function of the joint angle parameter θ, which transforms the local position of the i-th joint in the static position into a representation in the torso coordinate system. The joint angle parameter θ is input into the robot DH parameter coordinate transformation matrix According to the mass m of each joint in the static position i and local CoM location A i ,pass

[0047]

[0048] The position of the CoM of the robot's i-th joint in the torso coordinate system can be obtained as Then through

[0049]

[0050] Get the position A of the robot's CoM in the torso coordinate system Torso According to different support modes S, the CoM position A of the robot in the foot coordinate system can be obtained: Robot :

[0051]

[0052] Among them, S = LLegSupport, RLegSupport, LegsSupport respectively represent left leg standing, right leg standing and double leg standing. Similarly, the robot DH parameter transformation matrix can also be used to obtain the coordinate position of each effector end of the robot in the torso coordinate system.

[0053]

[0054] Subtracting the corresponding coordinates gives the robot's link vector p i .

[0055] The kinematic model also includes the rotation range C of each joint motor θ , which can be written as

[0056]

[0057] Where N represents the number of joint angle parameters θ, Represents the joint angles θ of the robot i The upper and lower bounds of the range of activity.

[0058] In addition, the kinematic model also includes the support polygon range C of the robot in different support modes A , which can be written as

[0059]

[0060] in, Represents the x coordinate of the robot CoM The upper and lower bounds of Represents the y coordinate of the robot CoM The upper and lower bounds of .

[0061] Furthermore, in step 4, the optimization problem of modeling can be expressed as

[0062]

[0063]

[0064] In the objective function, α and β are adjustable hyperparameters; θ des is the target joint angle of the human body calculated in step 2, so the first term of the objective function represents the bi-norm of the parameter to be optimized and the target joint angle of the human body; des is the human connecting rod vector calculated in step 2, λ i (θ,p des ) is the defined connecting rod vector difference function, which can be written as

[0065]

[0066] p i (θ) is the link vector of the robot obtained in step 3, is the human link vector obtained in step 2. Sum up all the link vector differences. The second term of the objective function measures the difference between the human and robot link vectors. Robot (θ)∈C A and θ∈C θ That is, the robot center of gravity constraint range and the joint angle parameter constraint range obtained in step 3; The robot's different effector ends are constrained to be greater than a certain distance threshold h to prevent self-collision. i≠j represents different effector ends, including head, hand, knee, foot and other joints.

[0067] Part of the initial value of the optimization parameter is the joint angle of the robot in the current state read by the sensor, and the other part is the input human target joint angle.

[0068] Through the evaluation of the similarity index, it can be found that the method proposed in the present invention can ensure good similarity in the imitation of the whole body and the upper body, such as Figure 4 Among them, the similarity index is

[0069]

[0070] Where N represents the number of connecting rod vectors, p i is the robot link vector read by the sensor, To capture the human link vectors, they are all expressed in the world coordinate system.

[0071] By comparing the similarity indicators before and after optimization, such as Figure 5 As shown, it can be found that the method proposed in the present invention sacrifices a certain similarity to ensure a good balance.

[0072] Describe the safe support range of the robot's left and right feet and the trajectory of CoM during the imitation process, such as Figure 6 As shown in Figure 2, it can be found that no matter in the double-foot support or single-foot support stage, the proposed method ensures that the robot's CoM is always within the support range, thereby ensuring the stability of the imitation process; the distance between the robot's CoM projection and the ideal support point during the imitation process is depicted, as shown in Figure 2. Figure 7 As shown, it can be found that compared with before optimization, the distance between the CoM obtained by the proposed method and the ideal support point is smaller overall, and its change is smoother, which effectively prevents falls caused by excessive center of gravity shift.

Claims

1. A control method for a robot full body imitation system, characterized in that: The method comprises the following steps: Step 1, based on the coordinates of the human skeleton points captured by the somatosensory camera Kinect, the coordinates of each human skeleton point in the Kinect coordinate system are transformed into representations in the human coordinate system; Step 2: Use the vector method to solve the human body joint angles and the connecting rod vectors, and estimate the current human body support mode based on the data captured by the Kinect body sensing camera; Step 3, establish the robot kinematic model, constrain the joint angle value range according to the rotation range of the motor of each joint of the robot; according to the current human body support mode, determine the support polygon of the robot in the corresponding mode to constrain the center of mass range; input the joint angle into the robot's DH parameter coordinate transformation matrix, so as to obtain the robot's center of mass position, the position of each effector end and its constraint range and the robot's link vector; Step 4, establish the objective function according to the human joint angles and connecting rod vectors obtained in step 2; at the same time, obtain the joint angles of the robot in the current state, and combine them with the input human target joint angles as initial values; continuously send the joint angle parameters to the robot kinematic model to detect whether they meet the constraint range obtained in step 3, iterate along the maximum gradient direction of the objective function, and solve the optimal solution as the control parameter of the robot, and finally pass the control parameter to the robot for execution; In step 3, the robot kinematic model includes the robot's DH parameter coordinate transformation matrix, the rotation range of each joint motor and the support polygon range of the robot under different support modes; the DH parameter coordinate transformation matrix is ​​used to transform the joint point coordinate parameters into representations in the trunk and foot coordinate systems respectively; the rotation range of each joint motor is used to limit the upper and lower limits of each joint angle value; the support polygon range of the robot under different support modes is used to constrain the upper and lower limits of the x-coordinate and the upper and lower limits of the y-coordinate of the robot's center of mass position; the DH parameter coordinate transformation matrix first transforms the local position of each joint in the static position into a representation in the trunk coordinate system; then the center of mass position of each joint is weighted averaged according to its mass, so as to obtain the representation of the center of mass position of the entire robot in the trunk coordinate system; then according to different support modes, the corresponding DH parameter transformation matrix can be used to obtain the representation of the robot's center of mass position in the foot coordinate system; finally, the coordinate position of each effector end of the robot in the trunk coordinate system is obtained by using the DH parameter transformation matrix, and the corresponding coordinates are subtracted to obtain the robot's connecting rod vector; In step 4, the objective function is: Among them, α and β are adjustable hyperparameters; θ is the parameter to be optimized; θ des is the human joint angle; p des is the human connecting rod vector, λ i (θ,p des ) is the connecting rod vector difference function, which can be written as p i (θ) is the link vector of the ith robot, is the connecting rod vector of the i-th body; A Robot (θ)∈C A and θ∈C θ The robot's center of mass constraint range and the rotation range of each joint motor; Constrains the robot's different effector ends It must be greater than a certain distance threshold h to prevent self-collision, and i≠j represents different effector ends.

2. A control method for a robot full body simulation system according to claim 1, characterized in that: In step 1, the method for establishing the human body coordinate system is as follows: the plane normal vector composed of the right shoulder joint point RShoulder, the left shoulder joint point LShoulder and the hip joint point Torso extracted by the somatosensory camera Kinect is represented as p Torso , which points to the front of the human body, and its direction is defined as the x-axis in the human body coordinate system; then the vector from the hip joint point Torso to the neck joint point Neck is defined as Perpendicular to vector p Torso and And the direction pointing to the left side of the object is defined as the y-axis.

3. A control method for a robot full body simulation system according to claim 2, characterized in that: In step 1, in the human body coordinate system, all the bone point coordinates are written as r H ; Through the formula: Convert all other link vectors from the Kinect coordinate system to the human coordinate system, where r K Represents all vectors in the Kinect coordinate system of the somatosensory camera. Represents the transformation matrix from the Kinect camera to the human body coordinate system.

4. A control method for a robot full body simulation system according to claim 1, characterized in that: In step 2, the coordinates of the corresponding bone points transformed in step 1 are subtracted to obtain the human connecting rod vector; and the human joint angle is calculated according to the method for solving the vector angle.

Images