Humanoid robot self-collision avoidance method, robot, device, and program product

Abstract

This disclosure relates to a self-collision avoidance method, robot, device, and program product for a humanoid robot. The method includes: determining the self-collision avoidance control torque and joint control torque of any link in a target link pair, and determining the self-collision avoidance control torque and joint control torque of the preceding link that affects the movement of the corresponding link; projecting the joint control torque of the corresponding link and the joint control torque of the preceding link of the corresponding link onto the null space of the task corresponding to the self-collision avoidance control torque using a null space projection matrix, to obtain the projected joint control torque of the corresponding link and the projected joint control torque of the preceding link of the corresponding link; and controlling the movement of the corresponding joint motors according to the first superimposed torque and the second superimposed torque.

Description

Technical Field

[0001] This disclosure relates to the field of humanoid robot control technology, and more specifically, to a humanoid robot self-collision avoidance method, robot, device and program product. Background Technology

[0002] With the rapid development of biomimetic motion technology and the demand for human-computer interaction, humanoid robots have evolved into diverse configurations, including bipedal, wheeled, and hybrid motion systems. Unlike traditional industrial robots, humanoid robots generally possess complex motion systems with highly redundant degrees of freedom. During operation, the densely distributed mechanical links are prone to self-collision risks between body components during dynamic movement, posing significant safety challenges.

[0003] Currently, traditional collision avoidance algorithms struggle to guarantee both a compliant system response and high-precision real-time self-collision avoidance. Therefore, a technical solution is needed that can implement a safe and gentle self-collision avoidance strategy in real time. Summary of the Invention

[0004] One objective of this invention is to provide a new technical solution for a self-collision avoidance method for humanoid robots.

[0005] According to a first aspect of the present invention, a self-collision avoidance method for a humanoid robot is provided, comprising:

[0006] Based on any link in the target link pair, determine the self-collision avoidance control torque of the corresponding link and the self-collision avoidance control torque of the preceding link that affects the movement of the corresponding link, as well as determine the joint control torque of the corresponding link and the joint control torque of the preceding link of the corresponding link, wherein the target link pair is a link pair with a collision risk;

[0007] Using the null space projection matrix, the joint control torque of the corresponding link and the joint control torque of the preceding link of the corresponding link are projected onto the null space of the task corresponding to the self-collision avoidance control torque, so as to obtain the joint control torque of the corresponding link and the joint control torque of the preceding link of the corresponding link after projection.

[0008] The movement of the corresponding joint motor is controlled according to the first superimposed torque, and the movement of the corresponding joint motor is controlled according to the second superimposed torque. The first superimposed torque is the torque obtained by superimposing the joint control torque of the corresponding link after projection and the self-collision avoidance control torque of the corresponding link. The second superimposed torque is the torque obtained by superimposing the joint control torque of the front link of the corresponding link after projection and the self-collision avoidance control torque of the front link of the corresponding link.

[0009] Optionally, determining the self-collision avoidance control torque of the corresponding link and the self-collision avoidance control torque of the preceding link affecting the motion of the corresponding link includes:

[0010] Based on the motion state information of the target link pair, a mutual repulsion force is determined, wherein the mutual repulsion force is used to limit the force that reduces the distance between the two links in the target link pair;

[0011] Based on any link in the target link pair, determine the Jacobian matrix of the corresponding link according to the position information of the corresponding link and the position information of the preceding link that affects the movement of the corresponding link;

[0012] Based on the mutual repulsion force and the Jacobian matrix of the corresponding link, determine the self-collision avoidance control torque of the corresponding link and the self-collision avoidance control torque of the preceding link of the corresponding link.

[0013] Optionally, determining the repulsive force based on the motion state information of the target link pair includes:

[0014] Based on any link in the target link pair, obtain the distance between the two links in the target link pair, the position information of the two collision points in the target link pair that have a risk of self-collision, the velocity information of the collision points on the corresponding links, the maximum mutual repulsion force value, the safe distance threshold and the damping matrix;

[0015] The mutual repulsion force of the corresponding link is determined based on the distance between the two links of the target link pair, the position information of the two collision points on the target link pair that have a risk of self-collision, the velocity information of the collision point on the corresponding link, the maximum mutual repulsion force value, the safe distance threshold, and the damping matrix.

[0016] Optionally, the method further includes:

[0017] Based on the position information of each link in the humanoid robot, determine the Jacobian matrix corresponding to the self-collision avoidance task;

[0018] The pseudo-inverse matrix of the Jacobian matrix corresponding to the self-collision avoidance task is determined based on the Jacobian matrix corresponding to the self-collision avoidance task.

[0019] The null projection matrix is ​​determined based on the unit matrix, the Jacobian matrix corresponding to the self-collision avoidance task, and the pseudo-inverse matrix of the Jacobian matrix corresponding to the self-collision avoidance task.

[0020] Optionally, determining the pseudo-inverse matrix of the Jacobian matrix corresponding to the self-collision avoidance task based on the Jacobian matrix corresponding to the self-collision avoidance task includes:

[0021] The mass matrix of the humanoid robot is determined based on the mass and moment of inertia of each link in the humanoid robot.

[0022] Based on the Jacobian matrix corresponding to the self-collision avoidance task and the mass matrix of the humanoid robot, determine the pseudo-inverse matrix of the Jacobian matrix corresponding to the self-collision avoidance task.

[0023] Optionally, determining the joint control torque of the corresponding link and the joint control torque of the preceding link of the corresponding link includes:

[0024] Based on any link in the target link pair, the joint control torque of the corresponding link is determined according to the expected motion state information and the actual motion state information of the corresponding joint. Furthermore, the joint control torque of the preceding link of the corresponding link is determined according to the expected motion state information and the actual motion state information of the corresponding joint of the preceding link of the corresponding link.

[0025] Optionally, before controlling the movement of the corresponding joint motor according to the first superimposed torque and the control of the movement of the corresponding joint motor according to the second superimposed torque, the method further includes:

[0026] When one link in the target link pair is a force-controlled joint and the other link is a position-controlled joint, the self-collision avoidance control torque of the link corresponding to the force-controlled joint and the self-collision avoidance control torque of the link preceding the link corresponding to the force-controlled joint are amplified based on a multiplier threshold, resulting in amplified self-collision avoidance control torque of the link corresponding to the force-controlled joint and amplified self-collision avoidance control torque of the link preceding the link corresponding to the force-controlled joint.

[0027] Optionally, the method further includes:

[0028] Based on each link in the humanoid robot, a corresponding sphere model is constructed, wherein each link is enveloped by at least one sphere.

[0029] Based on each ball in each link, determine the distance to each ball that makes up the other links;

[0030] The link pair that corresponds to a distance between two spheres that is less than a safe distance threshold is identified as the target link pair.

[0031] Optionally, the method further includes:

[0032] Obtain a list of link pairs to be detected, wherein the list of link pairs to be detected has excluded link pairs to be detected without collision.

[0033] Based on the link pair detection list, the target link pair in the humanoid robot is determined.

[0034] According to a second aspect of the present invention, a humanoid robot is provided, comprising a control module, a plurality of links, a plurality of joints and a plurality of joint motors, wherein adjacent links are connected by joints, and the control module is used to perform the method described in any one of the first aspects.

[0035] According to a third aspect of the present invention, a computer device is provided, including a memory and a processor, the memory storing a computer program for controlling the processor to operate in order to perform the method according to any one of the first aspects.

[0036] According to a fourth aspect of the present invention, a computer program product is provided, the computer program product comprising a computer program that, when executed by a processor of a computer device, enables the computer device to perform the method as described in any one of the first aspects.

[0037] This disclosure provides a self-collision avoidance method for humanoid robots. By using a null space projection matrix, the joint control torque is projected onto the null space of the task corresponding to the self-collision avoidance control torque, so that the self-collision avoidance task and the driving of each link to achieve the desired motion task can be carried out simultaneously. In addition, it ensures that the joint control torque required to drive each link to achieve the desired motion task will not affect the self-collision avoidance control torque required to achieve the self-collision avoidance task.

[0038] The features and advantages of the embodiments of this specification will become clear from the following detailed description of exemplary embodiments with reference to the accompanying drawings. Attached Figure Description

[0039] The accompanying drawings, which are incorporated in and form a part of this specification, illustrate embodiments of this specification and, together with their description, serve to explain the principles of these embodiments.

[0040] Figure 1 This is a flowchart illustrating a self-collision avoidance method for a humanoid robot according to an embodiment of the present invention.

[0041] Figure 2 This is a schematic diagram of a sphere model constructed based on a linkage, according to an embodiment of the present invention.

[0042] Figure 3 This is a schematic diagram of a structure for determining the distance between two connecting rods based on a sphere model according to an embodiment of the present invention.

[0043] Figure 4 This is another schematic flowchart of a self-collision avoidance method for a humanoid robot according to an embodiment of the present invention.

[0044] Figure 5This is a schematic block diagram of a humanoid robot self-collision avoidance device according to an embodiment of the present invention.

[0045] Figure 6 This is a schematic diagram of the structure of a computer device according to an embodiment of the present invention. Detailed Implementation

[0046] Various exemplary embodiments of this specification will now be described in detail with reference to the accompanying drawings.

[0047] The following description of at least one exemplary embodiment is merely illustrative and is in no way intended to limit the embodiments of this specification or their application or use.

[0048] It should be noted that similar labels and letters in the following figures indicate similar items; therefore, once an item is defined in one figure, it does not need to be discussed further in subsequent figures.

[0049] Each component (e.g., a robotic arm, a mobile body) in the humanoid robot disclosed herein includes at least one link. Adjacent links are rotatably connected by joints, and the joints are controlled by motors. During the operation of the humanoid robot, the movement of a link is related not only to the movement of the joint corresponding to that link, but also to the movement of the joint corresponding to the upstream link of that link. The upstream link is the link upstream of the current link. In other words, the movement of a joint will not only drive the movement of the corresponding link, but also drive the movement of the downstream link.

[0050] To address the aforementioned technical problems, this disclosure provides a self-collision avoidance method for humanoid robots. By utilizing a null space projection matrix, the joint control torque is projected onto the null space of the task corresponding to the self-collision avoidance control torque. This allows the self-collision avoidance task and the task of driving each link to achieve the desired motion to be performed simultaneously. Furthermore, it ensures that the joint control torque required to drive each link to achieve the desired motion task will not affect the self-collision avoidance control torque required to achieve the self-collision avoidance task.

[0051] In one embodiment of the present invention, a self-collision avoidance method for a humanoid robot is provided. According to... Figure 1 As shown, the humanoid robot self-collision avoidance method of this embodiment includes the following steps S110 to S130.

[0052] Step S110: Based on any link in the target link pair, determine the self-collision avoidance control torque of the corresponding link and the self-collision avoidance control torque of the preceding link that affects the movement of the corresponding link, and determine the joint control torque of the corresponding link and the joint control torque of the preceding link of the corresponding link, wherein the target link pair is a link pair with a collision risk.

[0053] Step S120: Using the null space projection matrix, the joint control torque of the corresponding link and the joint control torque of the preceding link of the corresponding link are projected onto the null space of the task corresponding to the self-collision avoidance control torque, so as to obtain the joint control torque of the corresponding link and the joint control torque of the preceding link of the corresponding link after projection.

[0054] Step S130: Control the movement of the corresponding joint motor according to the first superimposed torque, and control the movement of the corresponding joint motor according to the second superimposed torque, wherein the first superimposed torque is the torque obtained by superimposing the joint control torque of the corresponding link after projection and the self-collision avoidance control torque of the corresponding link, and the second superimposed torque is the torque obtained by superimposing the joint control torque of the front link of the corresponding link after projection and the self-collision avoidance control torque of the front link of the corresponding link.

[0055] In some embodiments, the method further includes: constructing a corresponding sphere model based on each link in the humanoid robot, wherein the sphere model of each link is such that each link is enveloped by at least one sphere; determining the distance to each sphere constituting other links based on each sphere of each link; and obtaining the link pair corresponding to the distance between two spheres being less than a safe distance threshold as the target link pair.

[0056] Each link in this invention is connected to a joint. When constructing a sphere model based on any link, a link and its corresponding joint are used as a basic unit to construct the sphere model. Each basic unit is enveloped by at least one sphere. The spheres can be the same size or different sizes. Figure 2 The sphere model shown consists of five spheres: sphere 1, sphere 2, sphere 3, sphere 4, and sphere 5.

[0057] The steps for determining the distance between each sphere and the spheres that make up the other links specifically include: based on each sphere, determining the distance between the center of the sphere and the center of the spheres that make up the other links respectively; and determining the distance between the sphere and the spheres that make up the other links respectively based on the radius of the sphere, the radius of the spheres that make up the other links, and the distance between the center of the sphere and the center of the spheres that make up the other links respectively.

[0058] See Figure 3 ,based on Figure 2Taking sphere 2 and sphere 6 on other connecting rods as examples, the distance between the center of sphere 2 and the center of sphere 6 is determined based on the center position information of sphere 2 and sphere 6. The radius of sphere 2 and the radius of sphere 6 are obtained. Based on the radius of sphere 2, the radius of sphere 6, and the distance between the center of sphere 2 and the center of sphere 6, the distance between sphere 2 and sphere 6 is determined by subtracting the radius of sphere 2 and the radius of sphere 6 from the distance between the center of sphere 2 and the center of sphere 6.

[0059] In this embodiment, the link is described using a sphere model, which simplifies the model construction method for the link in collision mode. Furthermore, using the sphere model to detect the distance between any two links facilitates the calculation of the collision distance while providing sufficient safety redundancy to achieve self-collision avoidance.

[0060] In some embodiments, the method further includes: obtaining a link pair detection list, wherein the link pair detection list has excluded collision-free detection link pairs; and determining a target link pair in the humanoid robot based on the link pair detection list.

[0061] Collision-free detection link pairs are those selected manually. These are link pairs that will not collide with the humanoid robot during any task, such as the link corresponding to the left upper arm and the link corresponding to the right upper arm.

[0062] This reduces the number of link pairs to be detected and improves computational efficiency during the process of identifying target link pairs.

[0063] In some embodiments, step S110 specifically includes: determining a mutual repulsion force based on the motion state information of the target link pair, wherein the mutual repulsion force is used to limit the decrease in distance between the two links in the target link pair; determining the Jacobian matrix of the corresponding link based on the position information of the corresponding link and the position information of the preceding link that affects the motion of the corresponding link, based on any link in the target link pair; and determining the self-collision avoidance control torque of the corresponding link and the self-collision avoidance control torque of the preceding link of the corresponding link based on the mutual repulsion force and the Jacobian matrix of the corresponding link.

[0064] In some embodiments, based on any link in the target link pair, the distance between the two links in the target link pair, the position information of the two collision points on the target link pair that have a risk of self-collision, the velocity information of the collision point on the corresponding link, the maximum mutual repulsion force value, the safety distance threshold, and the damping matrix are obtained; based on the distance between the two links in the target link pair, the position information of the two collision points on the target link pair that have a risk of self-collision, the velocity information of the collision point on the corresponding link, the maximum mutual repulsion force value, the safety distance threshold, and the damping matrix, the mutual repulsion force of the corresponding link is determined.

[0065] This embodiment provides a mutual repulsion force algorithm, which is based on the motion state information of the link pair and configured with damping matrix, safe collision threshold and maximum mutual repulsion force to obtain mutual repulsion force. This mutual repulsion force can achieve compliant and smooth self-collision avoidance.

[0066] The distance between the two links in the target link pair is the distance between two collision points in the target link pair that have a risk of self-collision. The distance between the two links in the target link pair is determined based on the sphere model provided in the above embodiment, specifically including: determining the distance to each sphere of the other link in the target link pair based on each sphere of one link in the target link pair, obtaining multiple distance values, and obtaining the minimum distance value from these multiple distance values ​​as the distance between the two links in the target link pair.

[0067] The determination of the two collision points on the target link pair that have a risk of self-collision specifically includes: based on each ball of one link in the target link pair, determining the distance to each ball of the other link in the target link pair, obtaining multiple distance values, obtaining the minimum distance value from these multiple distance values, determining the two balls corresponding to the minimum distance value, and determining the intersection of the line connecting the centers of the two balls corresponding to the minimum distance value and the surface of the two balls as the two collision points on the target link pair that have a risk of self-collision.

[0068] The maximum repulsive force value is determined based on the joint motor performance. The damping matrix is ​​used to smooth the repulsive motion process. The maximum repulsive force value, safety distance threshold, and damping matrix are all pre-stored values ​​and can be obtained directly.

[0069] Taking a pair of target links (link A and link B) as an example, the two collision points on the target link pair that have the risk of self-collision are collision point i and collision point j, respectively. Collision point i is located on link A, and collision point j is located on link B.

[0070] The repulsive force of link A is determined based on the following formula. Repulsive force of link B

[0071]

[0072] Among them, F max For the maximum repulsive force, d threshold For safe distance threshold, Let x be the distance between collision point i and collision point j, D be the damping matrix, and x be the distance between collision point i and collision point j. i For the location information of collision point i, x j For the location information of collision point j, For the velocity information of collision point i, This provides the velocity information at the collision point j.

[0073] It should be noted that the mutual repulsion force corresponding to any link in the target link pair is a matrix, and each element of the matrix is ​​the mutual repulsion force corresponding to the link and the mutual repulsion force corresponding to the link preceding the link.

[0074] The self-collision avoidance control torque of link A and the self-collision avoidance control torque of the link preceding link A are determined based on the following calculation formulas.

[0075]

[0076] in, This is a mutual repulsion force matrix, where each element represents the mutual repulsion force corresponding to link A and the mutual repulsion force corresponding to the preceding link of link A, J. i Let be the Jacobian matrix of link A. Let J be the self-collision avoidance control torque matrix, where each element represents the self-collision avoidance control torque of link A and the self-collision avoidance control torque of the link preceding link A. The Jacobian matrix J of link A is also shown. i Determined based on the position information of link A and its preceding link.

[0077] The self-collision avoidance control torque of link B and the self-collision avoidance control torque of the link preceding link B are determined based on the following calculation formulas.

[0078]

[0079] in, This is a mutual repulsion force matrix, where each element represents the mutual repulsion force corresponding to link B and the mutual repulsion force corresponding to the preceding link of link B, J. j Let be the Jacobian matrix of link B. Let J be the self-collision avoidance control torque matrix, where each element represents the self-collision avoidance control torque of link B and the self-collision avoidance control torque of the link preceding link B. The Jacobian matrix J of link B is also shown. j Determined based on the position information of link B and its preceding link.

[0080] In some embodiments, before step S130, the method further includes: when one link in the target link pair is a force-controlled joint and the other link is a position-controlled joint, based on a multiplier threshold, amplifying the self-collision avoidance control torque of the link corresponding to the force-controlled joint and the self-collision avoidance control torque of the link preceding the link corresponding to the force-controlled joint, to obtain the amplified self-collision avoidance control torque of the link corresponding to the force-controlled joint and the amplified self-collision avoidance control torque of the link preceding the link corresponding to the force-controlled joint.

[0081] Position-controlled joints can only execute position commands, not torque commands. Therefore, the position-controlled joint in the target link pair still moves according to the position command, driving the corresponding link to move, without providing any assistance for the self-collision avoidance of the target link pair. To address this, in this embodiment, based on a multiplier threshold, the self-collision avoidance control torque of the link corresponding to the force-controlled joint and the self-collision avoidance control torque of the link preceding the force-controlled joint are amplified. This compensates for the lack of assistance from the movement of the position-controlled joint in the target link pair for self-collision avoidance, making the self-collision avoidance algorithm provided in this disclosure more widely applicable. It can achieve self-collision avoidance when the joints corresponding to link pairs with collision risk are all force-controlled joints, and it can also achieve self-collision avoidance when the joints corresponding to link pairs with collision risk include both force-controlled and position-controlled joints.

[0082] The multiplier threshold can be set according to requirements, such as 2x, 3x, 5x, etc.

[0083] When one link in a target linkage is a force-controlled joint and the other is a position-controlled joint, the following methods are used: First, based on the link corresponding to the force-controlled joint, the projected joint control torque and the amplified self-collision avoidance control torque of the corresponding link are superimposed to obtain a superimposed torque. The motor of the corresponding joint is then controlled based on this superimposed torque. Second, based on the preceding link of the force-controlled joint, the projected joint control torque and the amplified self-collision avoidance control torque of the preceding link of the force-controlled joint are superimposed to obtain a superimposed torque. The motor of the corresponding joint is then controlled based on this superimposed torque. Third, based on the corresponding link of the position-controlled joint, the motor of the corresponding joint is controlled according to position commands.

[0084] In some embodiments, step S110 specifically includes: determining the joint control torque of the corresponding link based on any link in the target link pair, according to the expected motion state information and actual motion state information of the corresponding joint; and determining the joint control torque of the preceding link of the corresponding link based on the expected motion state information and actual motion state information of the corresponding joint of the preceding link of the corresponding link.

[0085] The desired motion state information includes the desired position and desired velocity. The actual motion state information includes the actual position and actual velocity. The actual position and actual velocity of each joint are measured by sensors.

[0086] In this embodiment, based on any link, the state difference is determined according to the desired motion state information and the actual motion state information. The corresponding joint control torque is determined according to the state difference to drive the corresponding link to achieve the desired motion.

[0087] Specifically, the joint control torque T of any link is determined based on the following formula.c ,

[0088]

[0089] q e =q d -q c

[0090]

[0091] Where, q d Let q be the desired position of one of the joints. c q represents the actual position of the joint. e This represents the positional difference of the joint. The desired velocity of this joint, This is the actual speed of the joint. K represents the velocity difference of this joint. p For the desired joint stiffness, K d To achieve desired joint damping, The compensation value for the first nonlinear term, g(q) c ) represents the compensation value for the second nonlinear term. It should be noted that K... p K d Both are pre-stored information and can be obtained directly. g(q c Each of these is a matrix, and the compensation value of the first nonlinear term is... The compensation value of the second nonlinear term is determined based on the actual position and actual velocity of the joint.

[0092] In some embodiments, determining the null projection matrix specifically includes the following steps: determining the Jacobian matrix corresponding to the self-collision avoidance task based on the position information of each link in the humanoid robot; determining the pseudo-inverse matrix of the Jacobian matrix corresponding to the self-collision avoidance task based on the Jacobian matrix corresponding to the self-collision avoidance task; and determining the null projection matrix based on the element matrix, the Jacobian matrix corresponding to the self-collision avoidance task, and the pseudo-inverse matrix of the Jacobian matrix corresponding to the self-collision avoidance task.

[0093] The null space projection matrix N is determined based on the following formula. p ,

[0094] N p =IJ T J #T

[0095] Where I is the unit matrix, J is the Jacobian matrix corresponding to the self-collision avoidance task, and J # Let T be the pseudo-inverse of the Jacobian matrix corresponding to the self-collision avoidance task, and T be the matrix transpose symbol.

[0096] In some embodiments, the step of determining the pseudo-inverse matrix of the Jacobian matrix corresponding to the self-collision avoidance task specifically includes: determining the mass matrix of the humanoid robot based on the mass and moment of inertia of each link in the humanoid robot; and determining the pseudo-inverse matrix of the Jacobian matrix corresponding to the self-collision avoidance task based on the Jacobian matrix corresponding to the self-collision avoidance task and the mass matrix of the humanoid robot.

[0097] The pseudo-inverse matrix J of the Jacobian matrix corresponding to the self-collision avoidance task is determined based on the following formula. # ,

[0098] J # =M -1 J T (JM -1 J T ) -1

[0099] Where M is the mass matrix of the humanoid robot, J is the Jacobian matrix corresponding to the self-collision avoidance task, -1 is the inverse matrix symbol, and T is the matrix transpose symbol.

[0100] During the operation of a humanoid robot, the priority of the self-collision avoidance task is higher than the priority of driving the links to achieve the desired motion. The torque required to achieve the self-collision avoidance task is the self-collision avoidance control torque, while the torque required to drive the links to achieve the desired motion task is the joint control torque. The null space projection matrix is ​​used to project the joint control torque onto the null space of the task corresponding to the self-collision avoidance control torque. In this way, the lower-priority joint control torque will not affect the higher-priority self-collision avoidance control torque. This can be verified through the following calculation formula.

[0101] F = J #1 (N p T c ) = J #T (IJ T J #T )T c =0

[0102] Where F represents the influence of low-priority joint control torque on high-priority self-collision avoidance control torque, and J is the Jacobian matrix corresponding to the self-collision avoidance task. # N is the pseudo-inverse of the Jacobian matrix corresponding to the self-collision avoidance task. p T is the null projection matrix. c The joint control torque T of a link cI is the identity matrix, and T is the matrix transpose. According to this formula, when the joint control torque of any link is projected onto the null space of the task corresponding to the self-collision avoidance control torque, the joint control torque of that link will not affect the self-collision avoidance task.

[0103] The joint control torque of the projected link A and the torque resulting from the superposition of the self-collision avoidance control torque of link A are determined based on the following calculation formula.

[0104]

[0105] Among them, T 总i Let N be a matrix, where each element represents the sum of the joint control torque and the self-collision avoidance control torque of the projected link A, and the sum of the joint control torque and the self-collision avoidance control torque of the preceding link of the projected link A. p T ci This is a matrix, where each element represents the joint control torque of the projected link A and the joint control torque of the preceding link of the projected link A. This is the self-collision avoidance control torque matrix, where each element represents the self-collision avoidance control torque of link A and the self-collision avoidance control torque of the link preceding link A.

[0106] The joint control torque of the projected link B and the torque resulting from the superposition of the self-collision avoidance control torque of link B are determined based on the following calculation formula.

[0107]

[0108] Among them, T 总j Let N be a matrix, where each element represents the sum of the joint control torque and the self-collision avoidance control torque of the projected link B, and the sum of the joint control torque and the self-collision avoidance control torque of the preceding link of the projected link B. p T cj This is a matrix, where each element represents the joint control torque of the projected link B and the joint control torque of the preceding link of the projected link B. This is the self-collision avoidance control torque matrix, where each element represents the self-collision avoidance control torque of link B and the self-collision avoidance control torque of the link preceding link B.

[0109] Figure 4 This is a schematic flowchart of a self-collision avoidance method according to an embodiment of the present invention. Figure 4As shown, based on the actual position of each link, link pairs with collision risk are identified as target link pairs. Based on the actual position, actual velocity, desired position, and desired velocity of any link in the target link pair, the joint control torque of the corresponding link is determined. Each joint control torque is projected onto the null space of the task corresponding to the self-collision avoidance control torque to obtain the projected joint control torque. Each projected joint control torque is superimposed with the self-collision avoidance torque of the corresponding joint to obtain the superimposed torque. The motors of the corresponding joints are controlled according to the superimposed torque to achieve the desired motion of each link while also achieving self-collision avoidance.

[0110] One embodiment of the present invention provides a humanoid robot. The humanoid robot includes a control module, multiple links, multiple joints, and multiple joint motors. Adjacent links are connected via joints. The control module is used to execute the method described in any of the above embodiments.

[0111] In some embodiments, each joint in the humanoid robot is a force-controlled joint.

[0112] In some embodiments, the joints of the humanoid robot are partly force-controlled joints and partly position-controlled joints.

[0113] One embodiment of the present invention provides a self-collision avoidance device for a humanoid robot.

[0114] according to Figure 5 As shown, the humanoid robot self-collision avoidance device includes a torque determination module 510, a torque projection module 520, and a control module 530.

[0115] The torque determination module 510 is used to determine the self-collision avoidance control torque of the corresponding link and the self-collision avoidance control torque of the preceding link that affects the movement of the corresponding link, based on any link in the target link pair, as well as to determine the joint control torque of the corresponding link and the joint control torque of the preceding link of the corresponding link. The target link pair is a link pair with a collision risk.

[0116] The torque projection module 520 is used to project the joint control torque of the corresponding link and the joint control torque of the preceding link of the corresponding link to the zero space of the task corresponding to the self-collision avoidance control torque using the zero space projection matrix, so as to obtain the joint control torque of the corresponding link and the joint control torque of the preceding link of the corresponding link after projection.

[0117] The control module 530 is used to control the movement of the corresponding joint motor according to the first superimposed torque and to control the movement of the corresponding joint motor according to the second superimposed torque. The first superimposed torque is the torque obtained by superimposing the joint control torque of the corresponding link after projection and the self-collision avoidance control torque of the corresponding link. The second superimposed torque is the torque obtained by superimposing the joint control torque of the front link of the corresponding link after projection and the self-collision avoidance control torque of the front link of the corresponding link.

[0118] The self-collision avoidance device provided in this embodiment of the invention utilizes a null space projection matrix to project the joint control torque onto the null space of the task corresponding to the self-collision avoidance control torque, thereby enabling the self-collision avoidance task and the driving of each link to achieve the desired motion task to be performed simultaneously. In addition, it ensures that the joint control torque required to drive each link to achieve the desired motion task will not affect the self-collision avoidance control torque required to achieve the self-collision avoidance task.

[0119] In some embodiments, the torque determination module 510 is further configured to determine a mutual repulsion force based on the motion state information of the target link pair, wherein the mutual repulsion force is used to limit the decrease in the distance between the two links in the target link pair; based on any link in the target link pair, determine the Jacobian matrix of the corresponding link according to the position information of the corresponding link and the position information of the preceding link that affects the motion of the corresponding link; and determine the self-collision avoidance control torque of the corresponding link and the self-collision avoidance control torque of the preceding link of the corresponding link according to the mutual repulsion force and the Jacobian matrix of the corresponding link.

[0120] In some embodiments, the torque determination module 510 is further configured to, based on any link in the target link pair, acquire the distance between the two links in the target link pair, the position information of the two collision points on the target link pair that have a risk of self-collision, the velocity information of the collision point on the corresponding link, the maximum mutual repulsion force value, the safety distance threshold, and the damping matrix; and determine the mutual repulsion force of the corresponding link based on the distance between the two links in the target link pair, the position information of the two collision points on the target link pair that have a risk of self-collision, the velocity information of the collision point on the corresponding link, the maximum mutual repulsion force value, the safety distance threshold, and the damping matrix.

[0121] In some embodiments, the device further includes a null projection matrix determination module. The null projection matrix determination module is used to determine the Jacobian matrix corresponding to the self-collision avoidance task based on the position information of each link in the humanoid robot; determine the pseudo-inverse matrix of the Jacobian matrix corresponding to the self-collision avoidance task based on the Jacobian matrix corresponding to the self-collision avoidance task; and determine the null projection matrix based on the element matrix, the Jacobian matrix corresponding to the self-collision avoidance task, and the pseudo-inverse matrix of the Jacobian matrix corresponding to the self-collision avoidance task.

[0122] In some embodiments, the spatial projection matrix determination module is further configured to determine the mass matrix of the humanoid robot based on the mass and moment of inertia of each link in the humanoid robot; and to determine the pseudo-inverse matrix of the Jacobian matrix corresponding to the self-collision avoidance task based on the Jacobian matrix corresponding to the self-collision avoidance task and the mass matrix of the humanoid robot.

[0123] In some embodiments, the torque determination module 510 is used to determine the joint control torque of the corresponding link based on any link in the target link pair, according to the expected motion state information and actual motion state information of the corresponding joint, and to determine the joint control torque of the preceding link of the corresponding link based on the expected motion state information and actual motion state information of the corresponding joint of the preceding link of the corresponding link.

[0124] In some embodiments, the device further includes a self-collision avoidance control torque processing module. The self-collision avoidance control torque processing module is used to amplify the self-collision avoidance control torque of the link corresponding to the force-controlled joint and the self-collision avoidance control torque of the link preceding the force-controlled joint, based on a multiplier threshold, when one link in the target link pair has a force-controlled joint and the other has a position-controlled joint, to obtain amplified self-collision avoidance control torques of the link corresponding to the force-controlled joint and the link preceding the force-controlled joint.

[0125] In some embodiments, the device further includes a target link pair determination module. The target link pair determination module is used to construct corresponding sphere models based on each link in the humanoid robot, wherein the sphere model of each link is such that each link is enveloped by at least one sphere; determine the distance to each sphere constituting other links based on each sphere of each link; and obtain link pairs corresponding to a distance between two spheres that is less than a safe distance threshold, as target link pairs.

[0126] In some embodiments, the apparatus further includes a target link pair determination module. The target link pair determination module is used to acquire a link pair detection list, wherein the link pair detection list has excluded collision-free detection link pairs; and to determine target link pairs in the humanoid robot based on the link pair detection list.

[0127] One embodiment of the present invention provides a computer device. According to... Figure 6 As shown, the computer device includes a memory 620 and a processor 610. The memory 620 stores a computer program that controls the processor 610 to operate and execute the methods provided according to any of the above embodiments.

[0128] The processor 610 is used to execute computer instructions, which can be written using instruction sets of architectures such as x86, Arm, RISC, MIPS, and SSE. The memory 620 includes, for example, ROM (Read-Only Memory), RAM (Random Access Memory), and non-volatile memory such as a hard disk, etc., and is not limited thereto.

[0129] One embodiment of the present invention provides a computer program product comprising a computer program that, when executed by a processor of a computer device, enables the computer device to perform the method provided in any of the above embodiments.

[0130] This invention can be a system, method, and / or computer program product. A computer program product may include a computer-readable storage medium having computer-readable program instructions loaded thereon for causing a processor to implement various aspects of the invention.

[0131] Computer-readable storage media can be tangible devices capable of holding and storing instructions for use by an instruction execution device. Computer-readable storage media can be, for example, but not limited to, electrical storage devices, magnetic storage devices, optical storage devices, electromagnetic storage devices, semiconductor storage devices, or any suitable combination thereof. More specific examples (a non-exhaustive list) of computer-readable storage media include: portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), static random access memory (SRAM), portable compact disc read-only memory (CD-ROM), digital multifunction disc (DVD), memory sticks, floppy disks, mechanical encoding devices, such as punch cards or recessed protrusions storing instructions thereon, and any suitable combination thereof. The computer-readable storage media used herein are not to be construed as transient signals themselves, such as radio waves or other freely propagating electromagnetic waves, electromagnetic waves propagating through waveguides or other transmission media (e.g., light pulses through fiber optic cables), or electrical signals transmitted through wires.

[0132] The computer-readable program instructions described herein can be downloaded from computer-readable storage media to various computing / processing devices, or downloaded via a network, such as the Internet, local area network, wide area network, and / or wireless network, to an external computer or external storage device. The network may include copper transmission cables, fiber optic transmission, wireless transmission, routers, firewalls, switches, gateway computers, and / or edge servers. A network adapter card or network interface in each computing / processing device receives the computer-readable program instructions from the network and forwards them to the computer-readable storage media in the respective computing / processing device.

[0133] The computer program instructions used to perform the operations of this invention may be assembly instructions, instruction set architecture (ISA) instructions, machine instructions, machine-dependent instructions, microcode, firmware instructions, status setting data, or source code or object code written in any combination of one or more programming languages. Programming languages ​​include object-oriented programming languages ​​such as Smalltalk, C++, etc., and conventional procedural programming languages ​​such as the "C" language or similar programming languages. The computer-readable program instructions may execute entirely on the user's computer, partially on the user's computer, as a standalone software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In cases involving remote computers, the remote computer may be connected to the user's computer via any type of network—including a local area network (LAN) or a wide area network (WAN)—or may be connected to an external computer (e.g., via the Internet using an Internet service provider). In some embodiments, electronic circuits, such as programmable logic circuits, field-programmable gate arrays (FPGAs), or programmable logic arrays (PLAs), are personalized by utilizing state information from computer-readable program instructions. These electronic circuits can execute computer-readable program instructions to implement various aspects of the present invention.

[0134] Various aspects of the present invention are described herein with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It should be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer-readable program instructions.

[0135] These computer-readable program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing apparatus to produce a machine such that, when executed by the processor of the computer or other programmable data processing apparatus, they create means for implementing the functions / actions specified in one or more blocks of the flowchart and / or block diagram. These computer-readable program instructions can also be stored in a computer-readable storage medium that causes a computer, programmable data processing apparatus, and / or other device to operate in a particular manner; thus, the computer-readable medium storing the instructions comprises an article of manufacture that includes instructions for implementing aspects of the functions / actions specified in one or more blocks of the flowchart and / or block diagram.

[0136] Computer-readable program instructions may also be loaded onto a computer, other programmable data processing apparatus, or other device to cause a series of operational steps to be performed on the computer, other programmable data processing apparatus, or other device to produce a computer-implemented process, thereby causing the instructions executed on the computer, other programmable data processing apparatus, or other device to perform the functions / actions specified in one or more boxes of a flowchart and / or block diagram.

[0137] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of the present invention. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of an instruction, which contains one or more executable instructions for implementing a specified logical function. In some alternative implementations, the functions marked in the blocks may occur in a different order than those marked in the drawings. For example, two consecutive blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in the block diagrams and / or flowcharts, and combinations of blocks in the block diagrams and / or flowcharts, can be implemented using a dedicated hardware-based system that performs the specified function or action, or using a combination of dedicated hardware and computer instructions. It will be known to those skilled in the art that implementation in hardware, implementation in software, and implementation using a combination of software and hardware are equivalent.

[0138] The various embodiments of the present invention have been described above. These descriptions are exemplary and not exhaustive, and are not limited to the disclosed embodiments. Many modifications and variations will be apparent to those skilled in the art without departing from the scope and spirit of the described embodiments. The terminology used herein is chosen to best explain the principles, practical application, or technical improvements to the embodiments in the market, or to enable others skilled in the art to understand the embodiments disclosed herein. The scope of the invention is defined by the appended claims.

Claims

1. A self-collision avoidance method for humanoid robots, characterized in that, include: Based on any link in the target link pair, determine the self-collision avoidance control torque of the corresponding link and the self-collision avoidance control torque of the preceding link that affects the movement of the corresponding link, as well as determine the joint control torque of the corresponding link and the joint control torque of the preceding link of the corresponding link, wherein the target link pair is a link pair with a collision risk; Using the null space projection matrix, the joint control torque of the corresponding link and the joint control torque of the preceding link of the corresponding link are projected onto the null space of the task corresponding to the self-collision avoidance control torque, so as to obtain the joint control torque of the corresponding link and the joint control torque of the preceding link of the corresponding link after projection. The movement of the corresponding joint motor is controlled according to the first superimposed torque, and the movement of the corresponding joint motor is controlled according to the second superimposed torque. The first superimposed torque is the torque obtained by superimposing the joint control torque of the corresponding link after projection and the self-collision avoidance control torque of the corresponding link. The second superimposed torque is the torque obtained by superimposing the joint control torque of the front link of the corresponding link after projection and the self-collision avoidance control torque of the front link of the corresponding link.

2. The method according to claim 1, characterized in that, The determination of the self-collision avoidance control torque of the corresponding link and the self-collision avoidance control torque of the preceding link affecting the motion of the corresponding link includes: Based on the motion state information of the target link pair, a mutual repulsion force is determined, wherein the mutual repulsion force is used to limit the force that reduces the distance between the two links in the target link pair; Based on any link in the target link pair, determine the Jacobian matrix of the corresponding link according to the position information of the corresponding link and the position information of the preceding link that affects the movement of the corresponding link; Based on the mutual repulsion force and the Jacobian matrix of the corresponding link, determine the self-collision avoidance control torque of the corresponding link and the self-collision avoidance control torque of the preceding link of the corresponding link.

3. The method according to claim 2, characterized in that, The step of determining the repulsive force based on the motion state information of the target link pair includes: Based on any link in the target link pair, obtain the distance between the two links in the target link pair, the position information of the two collision points in the target link pair that have a risk of self-collision, the velocity information of the collision points on the corresponding links, the maximum mutual repulsion force value, the safe distance threshold and the damping matrix; The mutual repulsion force of the corresponding link is determined based on the distance between the two links of the target link pair, the position information of the two collision points on the target link pair that have a risk of self-collision, the velocity information of the collision point on the corresponding link, the maximum mutual repulsion force value, the safe distance threshold, and the damping matrix.

4. The method according to claim 1, characterized in that, The method further includes: Based on the position information of each link in the humanoid robot, determine the Jacobian matrix corresponding to the self-collision avoidance task; The pseudo-inverse matrix of the Jacobian matrix corresponding to the self-collision avoidance task is determined based on the Jacobian matrix corresponding to the self-collision avoidance task. The null projection matrix is ​​determined based on the unit matrix, the Jacobian matrix corresponding to the self-collision avoidance task, and the pseudo-inverse matrix of the Jacobian matrix corresponding to the self-collision avoidance task.

5. The method according to claim 4, characterized in that, The step of determining the pseudo-inverse matrix of the Jacobian matrix corresponding to the self-collision avoidance task based on the Jacobian matrix corresponding to the self-collision avoidance task includes: The mass matrix of the humanoid robot is determined based on the mass and moment of inertia of each link in the humanoid robot. Based on the Jacobian matrix corresponding to the self-collision avoidance task and the mass matrix of the humanoid robot, determine the pseudo-inverse matrix of the Jacobian matrix corresponding to the self-collision avoidance task.

6. The method according to claim 1, characterized in that, The determination of the joint control torque of the corresponding link and the joint control torque of the preceding link of the corresponding link includes: Based on any link in the target link pair, the joint control torque of the corresponding link is determined according to the expected motion state information and the actual motion state information of the corresponding joint. Furthermore, the joint control torque of the preceding link of the corresponding link is determined according to the expected motion state information and the actual motion state information of the corresponding joint of the preceding link of the corresponding link.

7. The method according to claim 1, characterized in that, Before controlling the movement of the corresponding joint motor according to the first superimposed torque and the control of the movement of the corresponding joint motor according to the second superimposed torque, the method further includes: When one link in the target link pair is a force-controlled joint and the other link is a position-controlled joint, the self-collision avoidance control torque of the link corresponding to the force-controlled joint and the self-collision avoidance control torque of the link preceding the link corresponding to the force-controlled joint are amplified based on a multiplier threshold, resulting in amplified self-collision avoidance control torque of the link corresponding to the force-controlled joint and amplified self-collision avoidance control torque of the link preceding the link corresponding to the force-controlled joint.

8. The method according to claim 1, characterized in that, The method further includes: Based on each link in the humanoid robot, a corresponding sphere model is constructed, wherein each link is enveloped by at least one sphere. Based on each ball in each link, determine the distance to each ball that makes up the other links; The link pair that corresponds to a distance between two spheres that is less than a safe distance threshold is identified as the target link pair.

9. The method according to claim 1, characterized in that, The method further includes: Obtain a list of link pairs to be detected, wherein the list of link pairs to be detected has excluded link pairs to be detected without collision. Based on the link pair detection list, the target link pair in the humanoid robot is determined.

10. A humanoid robot, characterized in that, It includes a control module, multiple links, multiple joints and multiple joint motors, with adjacent links connected by joints, and the control module is used to perform the method described in any one of claims 1 to 9.

11. A computer device, characterized in that, It includes a memory and a processor, the memory storing a computer program for controlling the processor to operate in order to perform the method according to any one of claims 1 to 9.

12. A computer program product, characterized in that, The computer program product includes a computer program that, when executed by a processor of a computer device, enables the computer device to perform the method as described in any one of claims 1 to 9.

Images