Method for avoiding autologous interference of hip joint of humanoid robot

Abstract

The invention provides a humanoid robot hip joint autologous interference avoidance method, which effectively solves the problem that autologous interference is easy to occur when a humanoid robot hip joint executes large-amplitude or complex actions by arranging a data communication layer, an interference inspection layer and a joint execution layer, the data communication layer is used for acquiring the position, speed and torque measured values of a hip joint motor through a bottom-layer hip joint motor communication module and acquiring the position, speed and torque expected values of the hip joint motor through a high-layer operation control algorithm module; the interference inspection layer is used for comprehensively solving the self-interference problem of the hip joint and improving the movement efficiency and flexibility of the humanoid robot through single-side working space planning, three-dimensional safety boundary establishment and real-time dynamic monitoring, and the joint execution layer is used for generating hip joint position, speed and torque instructions in real time through dynamic monitoring and transmitting the hip joint position, speed and torque instructions to a hip joint PD controller. And the hip joint PD controller generates a hip joint motor torque instruction and controls the tail end of the hip joint to generate interference-free motion.

Description

Technical Field

[0001] The invention relates to the technical field of humanoid robots, and in particular to a method for avoiding self-interference of hip joints of a humanoid robot. Background Art

[0002] In the field of humanoid robots, the movement performance of the hip joint is particularly important. It connects the torso and the entire leg, transmitting the force and torque between the two. Due to the complexity and danger of the force conditions, it often has a larger joint volume and a more complex mechanical structure. Humanoid robots usually need to complete actions such as walking, running, turning, and stepping in various complex environments. The hip joint has a large range of motion and high flexibility. However, when existing humanoid robots perform large movements, due to the compact structure of the hip joint and the complex force conditions, they are prone to structural interference with themselves, resulting in movement failure, discontinuous movements, and even damage to the hardware structure.

[0003] To solve this problem, traditional methods mostly use mechanical limit or simple motion planning algorithm. These methods often limit the range of motion of hip joint through simple rules. Although it reduces the interference problem to a certain extent, it often leads to excessive restriction, resulting in a greatly reduced range of motion, and also reduces the flexibility and motion efficiency of the robot, which cannot meet the needs of fast and complex motion of humanoid robots.

[0004] In the prior art, the hip joint of a humanoid robot is prone to structural interference with itself when performing large or complex movements, resulting in incoherent movement of the robot's hip joint and even possible hardware damage.

[0005] And the humanoid robot in the prior art still has the following problems when in use:

[0006] (1) The mechanical limitation method restricts the freedom of the hip joint, reduces the flexibility of the robot, and makes it impossible to complete more complex movements.

[0007] (2) Simple motion planning algorithms cannot dynamically adapt to the real-time requirements of complex motions and are unable to cope with emergencies or interference avoidance problems in complex motions.

[0008] (3) The output results of the interference avoidance method based on artificial neural networks are random and unexplainable, and there is a large difference between the effects in the simulation and the real environment. Summary of the invention

[0009] In order to solve the above technical problems, the present invention proposes a method for avoiding self-interference of the hip joint of a humanoid robot, which can effectively solve the problem that the hip joint of a humanoid robot is prone to self-interference when performing large or complex movements.

[0010] To achieve the above object, the technical solution adopted by the present invention is:

[0011] A method for avoiding self-interference of the hip joint of a humanoid robot, characterized by comprising the following steps:

[0012] S1. Data collection:

[0013] Construct a data communication layer, in which a bottom-level hip joint motor communication module and a high-level operation and control algorithm module are set, and the position, speed, and torque measurement values ​​of the hip joint motor are obtained through the bottom-level hip joint motor communication module, and the position, speed, and torque expected values ​​of the hip joint motor are obtained through the high-level operation and control algorithm module;

[0014] S2. Interference inspection:

[0015] S21, Single-side workspace planning:

[0016] Ensure hip space accessibility,

[0017] Since each hip joint actually has a certain limit, a set of hip joint accessibility points can be obtained based on this:

[0018] S joint ={(x, y, z)|f IK (x, y, z) = q sol ∈[q min ,q max ]};

[0019] Ensure accessibility in the workspace,

[0020] First, the structure and shape of the hip joint and even the entire humanoid robot are modeled. Then, the optimization method is used to perform numerical solution, and its workspace is conservatively planned. Combined with the overall humanoid robot structure, it is designed as follows:

[0021] S end ={(x, y, z)|x min <x<x max ,y min <y<y max , z min <z<z max}

[0022] The planned unilateral hip joint working space is:

[0023] single S op =S op =S theory ∩S joint ∩S end

[0024] Therefore, the working space of the left and right hip joints can be obtained according to the above formula, which can be expressed as L S op , R S op ;

[0025] S22. Establish a three-dimensional security boundary:

[0026] According to the degree of freedom and range of motion of the humanoid robot's hip joint, a three-dimensional virtual boundary of the hip joint is established. The three-dimensional virtual boundary is calculated according to the motion limit of the humanoid robot's hip joint. During the motion planning process, it is detected in real time whether the motion trajectory of the hip joint exceeds the three-dimensional virtual boundary, and the motion path is adjusted when necessary.

[0027] According to the planned unilateral hip joint working space, the theoretical bilateral hip joint working space is:

[0028] double S theory ={(x L ,y L , z L , x R ,y R , z R )|(x L ,y L , z L )∈ L S op ,(x R ,y R , z R )∈ R S op}

[0029] When the end positions of the hip joints on both sides are in their respective workspaces, different types of structural interferences will still occur in the hip joints during movement, namely two hip_yaw direction interferences, two hip_roll direction interferences, and one joint end interference, which are recorded as err 1 , err 2 , err 3 , err 4 , err 5 ,

[0030] To this end, the points in the workspace are removed.

[0031] For err 1 To solve the problem, it is stipulated that if the yaw angles of the hip joints on both sides are very close, a set of angle soft limits are set:

[0032]

[0033] For err2 To solve the problem, it is stipulated that if the difference between the yaw angles of the hip joints on both sides is large, another set of angle soft limits is set:

[0034]

[0035] For err 3 To solve the problem, it is stipulated that if the roll angle difference of the hip joints on both sides is large, a set of angle soft limits are set:

[0036]

[0037] For err 4 To solve the problem, it is stipulated that if the roll angles of the hip joints on both sides are close, a set of angle soft limits are set:

[0038]

[0039] For err 5 The problem is that the distance between the ends of the hip joints on both sides cannot be less than a certain threshold:

[0040]

[0041] From the above, we can see that the first four problems are limited to the joint space, and the fifth problem is limited to the workspace, unifying all the rules into the workspace:

[0042] double S joint ={(x L ,y L , z L , x R ,y R , z R )|(f IK (x L ,y L , z L ), f IK (x R ,y R , z R ))∈S er1 ∩S err2 ∩S Err3 ∩S err4} double S end =S err5

[0043] Finally, the reachable workspace after trimming is obtained, that is, the three-dimensional virtual boundary of the hip joint end when no interference occurs:

[0044] double S op = doubleS theory ∩ double S joint ∩ double S end ;

[0045] S23, real-time dynamic monitoring:

[0046] The three hip joint motors in the hip joint estimate the torque of the hip joint motor through current loop feedback, and obtain the real-time angle of the hip joint through the output encoder, and then obtain the angular velocity and angular acceleration of the joint through the first-order and second-order derivatives, so as to monitor the angle, velocity, acceleration and torque of the joint in real time.

[0047] When the hip joint is about to approach or enter the interference area, immediately adjust the movement trajectory of the hip joint, adjust the expected position, speed, and torque to a safe range,

[0048] For the hip joint, first generate the hip joint torque command based on the hip joint PD controller

[0049]

[0050] When interference occurs, a reverse resistance force is generated to correct the hip joint torque command.

[0051] τ out =τ in -τ resist

[0052] The resisting moment is:

[0053] τ resist =f(q mea -q lim_max )+f(q lim_min -q mea )

[0054] in

[0055]

[0056] The correction strength of real-time dynamic monitoring is set separately to adapt to the flexibility requirements of the hip joints for different types of movements of the lower limbs of the humanoid robot. In the calculation formula of the hip joint resistance torque, the coefficient vector k = [k 1 ·k 2 ] T Expanded to cover multiple action types of coefficient matrices:

[0057]

[0058] Where m means there are m types of hip joint movements, k i，jrepresents the i-th coefficient under the j-th action type. By independently setting the coefficient vectors under different action types, the flexibility of real-time dynamic monitoring can meet the requirements of different action types.

[0059] S3, Joint execution:

[0060] Through dynamic monitoring, the hip joint position, speed, and torque instructions are generated in real time and transmitted to the hip joint PD controller. The hip joint PD controller generates hip joint motor torque instructions, thereby controlling the hip joint end to produce interference-free movement.

[0061] As a preferred technical solution of the present invention: the hip joint includes two branches, a left leg hip joint and a right leg hip joint. The left leg hip joint branch and the right leg hip joint branch are respectively fixed to the floating base of the humanoid robot. The coordinate system is denoted by {B}, the origin is the floating matrix center, and there are three rotating joints, hip_roll, hip_yaw, and hip_pitch, from the floating base to the end of the hip joint, and the end is denoted by {E}.

[0062] As a preferred technical solution of the present invention: the hip joint motor adopts a quasi-direct drive DC brushless servo motor.

[0063] In the above structure: there will be an error problem when modeling the three-dimensional virtual boundary: the kinematic and dynamic models of the humanoid robot may have certain measurement errors compared with the real machine, and this error may cause the interference avoidance mechanism to fail to be triggered in time.

[0064] Solution to the above problem: The hip joint motor in the present invention adopts a quasi-direct drive DC brushless servo motor (QDD, Quasi-Direct Drive). Due to its quasi-direct drive principle with a small reduction ratio, it can reversely calculate the motor torque in real time through the real-time current size. Once interference occurs, the interference avoidance program can be triggered by the change of motor torque.

[0065] As a preferred technical solution of the present invention: in step S22, before the whole machine is operated, a three-dimensional virtual boundary is generated by pre-calculation, and the safety boundary information is statically stored.

[0066] In the above structure: the three-dimensional virtual boundary has high real-time requirements: the movement of the humanoid robot is carried out in real time, and the system needs to make decisions in a very short time.

[0067] Solution to the above problem: Before the whole machine is run, the present invention uses a pre-calculation method to generate a three-dimensional safety boundary, and stores the safety boundary information statically without real-time calculation, thereby greatly improving the real-time performance of the algorithm.

[0068] As a preferred technical solution of the present invention: in step S23, different dynamic monitoring parameters are specifically applied to action types with different dynamic performances.

[0069] In the above structure: there is a contradiction between the strictness of interference avoidance and the flexibility of movement: due to the existence of processing errors, measurement errors, and clearance fit in the real physical world, robot self-interference in some high-dynamic movements is difficult to completely avoid and will greatly reduce the flexibility of movement.

[0070] Solution to the above problem: The present invention applies different dynamic monitoring parameters to action types with different dynamic performances in a targeted manner, thereby achieving the effect of balancing the interference avoidance strictness and action flexibility of different action types.

[0071] Compared with the prior art, the present invention has the following beneficial effects:

[0072] The present invention can effectively solve the problem of humanoid robot hip joint self-interference and achieve the following technical effects:

[0073] (1) Avoiding self-interference of the hip joint: The three-dimensional virtual boundary method can prevent the hip joint from self-interference in any movement situation.

[0074] (2) Ensure the real-time performance of the algorithm: Make full use of the static information of the robot model to pre-generate the safety boundary and improve the real-time performance of the algorithm. At the same time, use real-time dynamic monitoring to provide additional interference avoidance for the entire process of hip joint movement.

[0075] (4) Applicable to various types of actions: Whether walking on flat ground, climbing complex terrain, or running at high speed, the present invention can provide the robot with interference avoidance effects with different degrees of flexibility to adapt to various action requirements. BRIEF DESCRIPTION OF THE DRAWINGS

[0076] Figure 1 is a flow chart of the present invention;

[0077] Figure 2 It is a schematic diagram of the hip joint structure;

[0078] Figure 3 It is the coordinate system of the right leg hip joint and the floating base of the humanoid robot;

[0079] Figure 4 It is a schematic diagram of the structural interference that occurs during the movement of the hip joint;

[0080] Figure 5 It is a schematic diagram of joint space limitation for structural interference. DETAILED DESCRIPTION

[0081] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments:

[0082] like Figure 1 As shown, a method for avoiding self-interference of the hip joint of a humanoid robot proposed in the present invention comprises the following steps:

[0083] S1. Data collection:

[0084] Construct a data communication layer, in which a bottom-level hip joint motor communication module and a high-level operation and control algorithm module are set, and the position, speed, and torque measurement values ​​of the hip joint motor are obtained through the bottom-level hip joint motor communication module, and the position, speed, and torque expected values ​​of the hip joint motor are obtained through the high-level operation and control algorithm module;

[0085] S2. Interference inspection:

[0086] S21, Single-side workspace planning:

[0087] Ensure hip space accessibility,

[0088] Since each hip joint actually has a certain limit, a set of hip joint accessibility points can be obtained based on this:

[0089] S joint ={(x, y, z)|f IK (x, y, z) = q sol ∈[q min ,q max ]};

[0090] Ensure accessibility in the workspace,

[0091] First, the structure and shape of the hip joint and even the entire humanoid robot are modeled. Then, the optimization method is used to perform numerical solution, and its workspace is conservatively planned. Combined with the overall humanoid robot structure, it is designed as follows:

[0092] S end ={(x, y, z)|x min <x<x max ,y min <y<y max , z min <z<z max}

[0093] The planned unilateral hip joint working space is:

[0094] single S op =S op =S theory ∩S joint ∩S end

[0095] Therefore, the working space of the left and right hip joints can be obtained according to the above formula, which can be expressed as L S op , R S op ;

[0096] S22. Establish a three-dimensional security boundary:

[0097] According to the degree of freedom and range of motion of the humanoid robot's hip joint, a three-dimensional virtual boundary of the hip joint is established. The three-dimensional virtual boundary is calculated according to the motion limit of the humanoid robot's hip joint. During the motion planning process, it is detected in real time whether the motion trajectory of the hip joint exceeds the three-dimensional virtual boundary, and the motion path is adjusted when necessary.

[0098] According to the planned unilateral hip joint working space, the theoretical bilateral hip joint working space is:

[0099] double S theory ={(x L ,y L , z L , x R ,y R , z R )|(x L ,y L , z L )∈ L S op ,(x R ,y R , z R )∈ R S op}

[0100] When the end positions of the hip joints on both sides are in their respective workspaces, different types of structural interferences will still occur in the hip joints during movement, namely two hip_yaw direction interferences, two hip_roll direction interferences, and one joint end interference, which are recorded as err 1 , err 2 , err 3 , err 4 , err 5 ,

[0101] To this end, the points in the workspace are removed.

[0102] For err 1 To solve the problem, it is stipulated that if the yaw angles of the hip joints on both sides are very close, a set of angle soft limits are set:

[0103]

[0104] For err2 To solve the problem, it is stipulated that if the difference between the yaw angles of the hip joints on both sides is large, another set of angle soft limits is set:

[0105]

[0106] For err 3 To solve the problem, it is stipulated that if the roll angle difference of the hip joints on both sides is large, a set of angle soft limits are set:

[0107]

[0108] For err 4 To solve the problem, it is stipulated that if the roll angles of the hip joints on both sides are close, a set of angle soft limits are set:

[0109]

[0110] For err 5 The problem is that the distance between the ends of the hip joints on both sides cannot be less than a certain threshold:

[0111]

[0112] From the above, we can see that the first four problems are limited to the joint space, and the fifth problem is limited to the workspace, unifying all the rules into the workspace:

[0113] double S joint ={(x L ,y L , z L , x R ,y R , z R )|(f IK (x L ,y L , z L ), f IK (x R ,y R , z R ))∈S err1 ∩S err2 ∩s err3 ∩S err4} double S end =S err5

[0114] Finally, the reachable workspace after trimming is obtained, that is, the three-dimensional virtual boundary of the hip joint end when no interference occurs:

[0115] double S op = doubleS theory ∩ double S joint ∩ double S end ;

[0116] S23, real-time dynamic monitoring:

[0117] The three hip joint motors in the hip joint estimate the torque of the hip joint motor through current loop feedback, and obtain the real-time angle of the hip joint through the output encoder, and then obtain the angular velocity and angular acceleration of the joint through the first-order and second-order derivatives, so as to monitor the angle, velocity, acceleration and torque of the joint in real time.

[0118] When the hip joint is about to approach or enter the interference area, immediately adjust the movement trajectory of the hip joint, adjust the expected position, speed, and torque to a safe range,

[0119] For the hip joint, first generate the hip joint torque command based on the hip joint PD controller

[0120]

[0121] When interference occurs, a reverse resistance force is generated to correct the hip joint torque command.

[0122] τ out =τ in -τ resist

[0123] The resisting moment is:

[0124] τ resist =f(q mea -q lim_max )+f(q lim_min -q mea )

[0125] in

[0126]

[0127] The correction strength of real-time dynamic monitoring is set separately to adapt to the flexibility requirements of the hip joints for different types of movements of the lower limbs of the humanoid robot. In the calculation formula of the hip joint resistance torque, the coefficient vector k = [k 1 ·k 2 ] T Expanded to cover multiple action types of coefficient matrices:

[0128]

[0129] Where m means there are m types of hip joint movements, k i，jrepresents the i-th coefficient under the j-th action type. By independently setting the coefficient vectors under different action types, the flexibility of real-time dynamic monitoring can meet the requirements of different action types.

[0130] S3, Joint execution:

[0131] Through dynamic monitoring, the hip joint position, speed, and torque instructions are generated in real time and transmitted to the hip joint PD controller. The hip joint PD controller generates hip joint motor torque instructions, thereby controlling the hip joint end to produce interference-free movement.

[0132] like Figure 2 As shown, the hip joint includes two branches, namely, a left leg hip joint and a right leg hip joint. The left leg hip joint branch and the right leg hip joint branch are respectively fixed on the floating base of the humanoid robot. The coordinate system is denoted by {B}, and the origin is the floating matrix center. There are three rotational joints from the floating base to the end of the hip joint, namely, hip_roll, hip_yaw, and hip_pitch, and the end is denoted by {E}.

[0133] like Figure 3 As shown in the figure, taking the right leg hip joint as an example, the entire branch chain is fixed on the floating base of the humanoid robot, the coordinate system is denoted by {B}, and the origin is the floating matrix center; from the floating base to the end of the hip joint, there are three rotation joints, hip_roll, hip_yaw, and hip_pitch, and the end is denoted by {E}.

[0134] Taking the hip joint as an example, the technical solution of the present invention is explained.

[0135] (1) Single-sided workspace planning

[0136] Taking into account the actual structural limitations of the hip joint, the theoretical working space of the unilateral hip joint is a hemisphere with the origin at the intersection of the coplanar axes z_1 and z_2, and its radius is the maximum distance from the origin E to the origin of the workspace. The theoretical working space equation is:

[0137]

[0138] In fact, due to structural limitations, there are inaccessible parts in the workspace, which are removed to ensure the accessibility of the workspace.

[0139] First, we need to ensure the spatial accessibility of the hip joint. Due to the structural design or the characteristics of the motor itself, each hip joint actually has a certain limit. Based on this, we can get a set of points with good hip joint accessibility.

[0140] S joint ={(x, y, z)|f IN (x, y, z) = q sol ∈[q min,q max ]};

[0141] Then, to ensure accessibility in the workspace, it is usually necessary to make a relatively complete model of the hip joint and even the entire machine structure, and then use the optimization method to perform numerical solution. The algorithm complexity is relatively high. At the same time, considering that the range of motion of the end position of the hip joint of the humanoid robot under most working conditions is small, its workspace can be reasonably and conservatively planned, and designed in combination with the structure of the whole machine.

[0142] S end ={(x, y, z)|x min <x<x max ,y min <y<y max , z min <z<z max}.

[0143] The planned unilateral hip joint working space is:

[0144] single S op =S op =S theory ∩S joint ∩S end

[0145] Therefore, the working space of the left and right hip joints can be obtained according to the above formula, which can be expressed as L S op , R S op .

[0146] (2) Three-dimensional virtual safety boundary

[0147] According to the degrees of freedom and range of motion of the humanoid robot hip joint, a three-dimensional virtual boundary of the hip joint is established. This virtual boundary is calculated based on the motion limit of the humanoid robot hip joint to ensure that the hip joint is within an acceptable range in all movements and avoid structural interference. During the motion planning process, the system will detect whether the hip joint motion trajectory exceeds this boundary and adjust the motion path if necessary to ensure safety.

[0148] Considering the joint working space of both hip joints, according to the planned unilateral hip joint working space, it can be known that the theoretical working space of both hip joints is:

[0149] double S theory ={(x L ,y L , z L , x R ,y R , z R )|(xL ,y L , z L )∈ L S op ,(x R ,y R , z R )∈ R S op}.

[0150] However, in reality, even if the end positions of the hip joints on both sides are in their respective working spaces, structural interference may still occur during the movement of the hip joints, such as Figure 4 As shown in the figure, the hip joint space is restricted for the four most common structural interferences, so as to effectively avoid such problems. There are mainly two hip_yaw direction interferences, two hip_roll direction interferences and one joint end interference, which are respectively denoted as err 1 , err 2 , err 3 , err 4 , err 5 .

[0151] like Figure 5 As shown, for this purpose, the points in the workspace are eliminated.

[0152] For err 1 To solve the problem, it is stipulated that if the yaw angles of the hip joints on both sides are very close, a set of angle soft limits are set:

[0153]

[0154] For err 2 To solve the problem, it is stipulated that if the difference between the yaw angles of the hip joints on both sides is large, another set of angle soft limits is set:

[0155]

[0156] For err 3 To solve the problem, it is stipulated that if the roll angle difference of the hip joints on both sides is large, a set of angle soft limits are set:

[0157]

[0158] For err 4 To solve the problem, it is stipulated that if the roll angles of the hip joints on both sides are close, a set of angle soft limits are set:

[0159]

[0160] For err 5 The problem is that the distance between the ends of the hip joints on both sides cannot be less than a certain threshold:

[0161]

[0162] It can be seen that the first four problems are mainly limited to the joint space, and the fifth problem is mainly limited to the workspace, unifying all the rules into the workspace:

[0163] double S joint ={(x L ,y L , z L , x R ,y R , z R )|(f IK (x L ,y L , z L ), f IK (x R ,y R , z R ))∈S err1 ∩S err2 ∩S err3 ∩S err4} double S end =S err5

[0164] Finally, the reachable workspace after trimming is obtained, that is, the three-dimensional virtual boundary of the hip joint end when no interference occurs:

[0165] double S op = double S theory ∩ double S joint ∩ double S end

[0166] In order to reduce computing power waste and improve real-time performance, the three-dimensional virtual boundary is pre-calculated and can be generated before the entire machine is running, without the need for continuous calculation throughout the movement.

[0167] (3) Real-time kinetic monitoring

[0168] The three hip joint motors in the hip joint estimate the hip joint motor torque through current loop feedback, and obtain the real-time angle of the hip joint through the output encoder, and then obtain the angular velocity and angular acceleration of the hip joint through the first-order derivative and the second-order derivative, so as to monitor the angle, speed, acceleration and torque of the hip joint in real time. When the hip joint is about to approach or enter the interference area, the system will immediately adjust the joint's motion trajectory, adjust the expected position, speed, and torque to a safe range, so as to avoid possible interference and collision. For the hip joint, first generate a hip joint torque command based on the PD controller.

[0169]

[0170] When interference occurs, a reverse resistance force is generated to correct the hip joint torque command.

[0171] τ out =τ in -τ resist

[0172] The resisting torque is

[0173] τ resist =f(q mea -q lim_max )+f(q lim_min -q mea )

[0174] in

[0175]

[0176] The lower limbs of humanoid robots can perform a variety of actions, including balanced standing, stepping on the spot, anthropomorphic walking, standing high jump, forward long jump, rotation jump, etc. Different action types often require different flexibility of the hip joint. In order to adapt to different action types, the correction strength of real-time dynamic monitoring is set separately. In the calculation formula of the hip joint resistance torque, the coefficient vector k=[k 1 ·k 2 ] T Expanded to cover multiple action types

[0177]

[0178] Where m means there are m types of hip joint movements, k i，j Represents the i-th coefficient under the i-th action type. By independently setting the coefficient vectors under different action types, the flexibility of real-time dynamic monitoring can meet the requirements of different action types.

[0179] There will be error problems when modeling three-dimensional virtual boundaries: the kinematic and dynamic models of the humanoid robot may have certain measurement errors compared with the real machine, and this error may cause the interference avoidance mechanism to fail to be triggered in time.

[0180] Solution to the above problem: The hip joint motor in the present invention adopts a quasi-direct drive DC brushless servo motor (QDD, Quasi-Direct Drive). Due to its quasi-direct drive principle with a small reduction ratio, it can reversely calculate the motor torque in real time through the real-time current size. Once interference occurs, the interference avoidance program can be triggered by the change of motor torque.

[0181] The three-dimensional virtual boundary has high real-time requirements: the movement of the humanoid robot is carried out in real time, and the system needs to make decisions in a very short time.

[0182] Solution to the above problem: Before the whole machine is run, the present invention uses a pre-calculation method to generate a three-dimensional safety boundary, and stores the safety boundary information statically without real-time calculation, thereby greatly improving the real-time performance of the algorithm.

[0183] There is a contradiction between the strictness of interference avoidance and the flexibility of movement: due to the existence of processing errors, measurement errors, and clearance fit in the real physical world, robot self-interference in some high-dynamic movements is difficult to completely avoid and will greatly reduce the flexibility of movement.

[0184] Solution to the above problem: The present invention applies different dynamic monitoring parameters to action types with different dynamic performances in a targeted manner, thereby achieving the effect of balancing the interference avoidance strictness and action flexibility of different action types.

[0185] The present invention ensures that the movement trajectory of the hip joint is always within a safe range by setting a three-dimensional virtual boundary.

[0186] The present invention uses real-time dynamic monitoring to timely detect the movement of the hip joint and perform dynamic adjustments.

[0187] The present invention can adapt to the adjustment of movement flexibility for different action types: the movement flexibility can be adjusted in real time according to the action type instruction to adapt to the interference avoidance requirements in different situations.

[0188] The above description is only a preferred embodiment of the present invention and does not constitute any other form of limitation to the present invention. Any modification or equivalent change made based on the technical essence of the present invention still falls within the scope of protection required by the present invention.

Claims

1. A method for avoiding self-interference of the hip joint of a humanoid robot, characterized in that: The steps include: S1. Data collection: Construct a data communication layer, in which a bottom-level hip joint motor communication module and a high-level operation and control algorithm module are set, and the position, speed, and torque measurement values ​​of the hip joint motor are obtained through the bottom-level hip joint motor communication module, and the position, speed, and torque expected values ​​of the hip joint motor are obtained through the high-level operation and control algorithm module; S2. Interference inspection: S21, Single-side workspace planning: Ensure hip space accessibility, Since each hip joint actually has a certain limit, a set of hip joint accessibility points can be obtained based on this: S joint ={(x,y,z)|f IK (x,y,z)=q sol ∈[q min ,q max ]}; Ensure accessibility in the workspace, First, the structure and shape of the hip joint and even the entire humanoid robot are modeled. Then, the optimization method is used to perform numerical solution, and its workspace is conservatively planned. Combined with the overall humanoid robot structure, it is designed as follows: S end ={(x,y,z)|x min <x<x max ,y min <y<y max ,z min <z<z max } The planned unilateral hip joint working space is: single S op =S op =S theory ∩S joint ∩S end Therefore, the working space of the left and right hip joints can be obtained according to the above formula, which can be expressed as L S op , R S op ; S22. Establish a three-dimensional security boundary: According to the degree of freedom and range of motion of the humanoid robot's hip joint, a three-dimensional virtual boundary of the hip joint is established. The three-dimensional virtual boundary is calculated according to the motion limit of the humanoid robot's hip joint. During the motion planning process, it is detected in real time whether the motion trajectory of the hip joint exceeds the three-dimensional virtual boundary, and the motion path is adjusted when necessary. According to the planned unilateral hip joint working space, the theoretical bilateral hip joint working space is: double S theory ={(x L ,y L ,z L ,x R ,y R ,z R )|(x L ,y L ,z L ))∈ L S op ,(x R ,y R ,z R )∈ R S op } When the end positions of the hip joints on both sides are in their respective workspaces, different types of structural interferences will still occur in the hip joints during movement, including two hip_yaw interferences, two hip_roll interferences, and one joint end interference, which are recorded as err1, err2, err3, err4, err5, respectively. To this end, the points in the workspace are removed. In response to the err1 issue, if the yaw angles of the hip joints on both sides are very close, a set of angle soft limits are set: S err1 ={ L q, R q)| L q2∈[ err1 q min , err1 q max ], R q2∈[ err1 q min , err1 q max ],if| L q2- Q q2- R q2|< err1 Δ q2 } In response to the err2 problem, it is stipulated that if the difference between the yaw angles of the hip joints on both sides is large, another set of angle soft limits should be set: S err2 ={( L q, R q)| L q2∈[ err2 q min , err2 q max ], R q2∈[ err2 q min , err2 q max ],if| L q2- R q2|> err2 Δ q2 } In response to the err3 problem, it is stipulated that if the roll angle difference between the two hip joints is large, a set of angle soft limits should be set: S err3 ={( L q, R q)| L q1∈[ err3 q min , err3 q max ], R q1∈[ err3 q min , err3 q max ],if| L q1- R q1|< err3 Δ q1 } In response to the err4 issue, if the roll angles of the hip joints on both sides are close, a set of angle soft limits are set: S err4 ={( L q, R q)| L q1∈[ err4 q min , err4 q max ], R q1∈[ err4 q min , err4 q max ],if| L q1- R q1|> err4 Δ q1 } In response to the err5 problem, it is stipulated that the distance between the ends of the hip joints on both sides cannot be less than a certain threshold: From the above, we can see that the first four problems are limited to the joint space, and the fifth problem is limited to the workspace, unifying all the rules into the workspace: double S joint ={(x L ,y L ,z L ,x R ,y R ,z R )|(f IK (x L ,y L ,z L ),f IK (x R ,y R ,z R ))∈S err1 ∈S err2 ∈S err3 ∩S err4 } double S end =S err5 Finally, the reachable workspace after trimming is obtained, that is, the three-dimensional virtual boundary of the hip joint end when no interference occurs: double S op = double S theory ∩ double S joint ∩ double S end ; S23, real-time dynamic monitoring: The three hip joint motors in the hip joint estimate the torque of the hip joint motor through current loop feedback, and obtain the real-time angle of the hip joint through the output encoder, and then obtain the angular velocity and angular acceleration of the joint through the first-order and second-order derivatives, so as to monitor the angle, velocity, acceleration and torque of the joint in real time. When the hip joint is about to approach or enter the interference area, immediately adjust the movement trajectory of the hip joint, adjust the expected position, speed, and torque to a safe range, For the hip joint, first generate the hip joint torque command based on the hip joint PD controller When interference occurs, a reverse resistance force is generated to correct the hip joint torque command. t out =t in -t resist The resisting moment is: τ resist =f(q mea -q lim_max )+f(q lim_min -q mea ) in The correction strength of real-time dynamic monitoring is set separately to adapt to the flexibility requirements of the hip joints for different types of movements of the lower limbs of the humanoid robot. In the calculation formula of the hip joint resistance torque, the coefficient vector k = [k1·k2] T Expanded to cover multiple action types of coefficient matrices: Where m means there are m types of hip joint movements, k i，j represents the i-th coefficient under the j-th action type. By independently setting the coefficient vectors under different action types, the flexibility of real-time dynamic monitoring can meet the requirements of different action types. S3, Joint execution: Through dynamic monitoring, the hip joint position, speed, and torque instructions are generated in real time and transmitted to the hip joint PD controller. The hip joint PD controller generates hip joint motor torque instructions, thereby controlling the hip joint end to produce interference-free movement.

2. A method for avoiding self-interference of the hip joint of a humanoid robot according to claim 1, characterized in that: The hip joint includes two branches, a left leg hip joint and a right leg hip joint. The left leg hip joint branch and the right leg hip joint branch are respectively fixed to the floating base of the humanoid robot. The coordinate system is denoted by {B}, and the origin is the floating matrix center. There are three rotating joints, hip_roll, hip_yaw, and hip_pitch, from the floating base to the end of the hip joint, and the end is denoted by {E}.

3. The method for avoiding self-interference of hip joint of a humanoid robot according to claim 1, characterized in that: The hip joint motor adopts a quasi-direct drive DC brushless servo motor.

4. The method for avoiding self-interference of hip joint of a humanoid robot according to claim 1, characterized in that: In step S22, before the whole machine is run, a three-dimensional virtual boundary is generated by pre-calculation, and the safety boundary information is statically stored.

5. The method for avoiding self-interference of hip joint of a humanoid robot according to claim 1, characterized in that: In step S23 , different dynamic monitoring parameters are specifically applied to action types with different dynamic performances.

Images