CN110328689B - Robot balance detection method, device and equipment and robot - Google Patents
Robot balance detection method, device and equipment and robot Download PDFInfo
- Publication number
- CN110328689B CN110328689B CN201910614249.4A CN201910614249A CN110328689B CN 110328689 B CN110328689 B CN 110328689B CN 201910614249 A CN201910614249 A CN 201910614249A CN 110328689 B CN110328689 B CN 110328689B
- Authority
- CN
- China
- Prior art keywords
- connecting rod
- base
- robot
- moment
- force
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
- B25J19/0095—Means or methods for testing manipulators
Abstract
The embodiment of the invention relates to the technical field of robots, and discloses a robot balance detection method, device, equipment and a robot. The robot balance detection method comprises the following steps: establishing a recursion Newton Euler equation expressing the relationship among the adjacent connecting rods of the robot and the self force, moment, mass, inertia and acceleration of the connecting rods; calculating the force and theoretical moment which are required to be provided by the base of the robot to a connecting rod connected with the base according to the recursive Newton Euler equation; calculating the maximum moment provided by the base according to the force of the base on a connecting rod connected with the base, the gravity of the base and the force arm from the gravity center of the base to the connecting rod connected with the base; and judging whether the robot is unbalanced or not according to the theoretical moment and the maximum moment. Through the mode, the embodiment of the invention can prejudge the balance of the robot, so that the robot is prevented from falling.
Description
Technical Field
The embodiment of the invention relates to the technical field of robots, in particular to a robot balance detection method, a device, equipment and a robot.
Background
With the development of science and technology, robots have been gradually applied to various fields. A bionic robot is an intelligent robot with the external characteristics of a person, and most of the bionic robots have a head, a trunk, arms and the like.
At present, compared with other robots, the bionic robot has more degrees of freedom, higher gravity center and smaller support area on the ground, so that the bionic robot is easy to fall down in the motion process.
Disclosure of Invention
An object of an embodiment of the present invention is to provide a robot balancing method, apparatus, device, and robot, which can pre-judge the balance of the robot, so as to avoid the robot from falling.
According to an aspect of an embodiment of the present invention, there is provided a robot balance detection method, including: establishing a recursion Newton Euler equation expressing the relationship among the adjacent connecting rods of the robot and the self force, moment, mass, inertia and acceleration of the connecting rods; calculating the force and theoretical moment which are required to be provided by the base of the robot to a connecting rod connected with the base according to the recursive Newton Euler equation; calculating the maximum moment provided by the base according to the force of the base on a connecting rod connected with the base, the gravity of the base and the force arm from the gravity center of the base to the connecting rod connected with the base; and judging whether the robot is unbalanced or not according to the theoretical moment and the maximum moment.
In an alternative mode, the calculating the maximum moment that the base can provide according to the force of the base on the connecting rod connected with the base, the gravity of the base and the moment arm from the center of gravity of the base to the connecting rod connected with the base specifically includes:
calculating the maximum torque that the base can provide according to the following formula:
τd＝(f0,1+G)d0
wherein, taudMaximum moment available for the base, f0,1The force of the base of the robot on a connecting rod connected with the base, G is the gravity of the base, d0The moment arm is from the center of gravity of the base to a connecting rod connected with the base.
In an optional manner, the determining whether the robot is unbalanced according to the theoretical moment and the maximum moment specifically includes: comparing the theoretical moment with the maximum moment; if the maximum moment is greater than or equal to the theoretical moment, determining that the robot is not unbalanced; and if the maximum moment is smaller than the theoretical moment, determining that the robot is unbalanced.
In an optional manner, the method further comprises: if it is determined that the robot is not out of balance, allowing the robot to perform a next action.
In an optional manner, the calculating, according to the recursive newton euler equation, a force and a theoretical moment that a base of the robot needs to provide to a link connected to the base specifically includes: according to a forward recursion equation of the recursion Newton Euler equation, recursion is carried out from a base to each connecting rod sequentially connected with the base so as to determine the kinematic parameters of each connecting rod of the robot; and according to the kinematic parameters of each connecting rod of the robot and a reverse recursion equation of the recursion Newton Euler equation, recursion is carried out from the tail end connecting rod to each connecting rod sequentially connected with the tail end connecting rod to determine the force and the theoretical moment required by each connecting rod of the robot, so that the force and the theoretical moment required by the base of the robot to the connecting rod connected with the base are determined.
In an optional manner, the kinematic parameters include angular velocity, angular acceleration and linear acceleration, and the determining the kinematic parameters of each link of the robot by recursion from the base to each link connected to the base in sequence according to a forward recursion equation of the recursive newton euler equation specifically includes:
calculating kinematic parameters of each link of the robot according to the following formula:
wherein, ω isi、
Angular velocity, angular acceleration and linear acceleration, theta, of the connecting rod i relative to the base, respectivelyiIs the angle of rotation of link i relative to link i-1,
is the amplitude of the angular velocity of the connecting rod i,
is the angular acceleration amplitude, c, of the connecting rod iiIs the position of the center of mass of the connecting rod i relative to the base, FiFor a resultant force acting on the centre of mass of the connecting rod i, miIs the mass of the connecting rod i, NiI is an integer greater than 0, which is the resultant moment acting on the center of mass of the connecting rod i.
In an optional manner, the determining the force and the theoretical moment of each link of the robot by recursion from the end link to each link connected to the end link sequentially according to the kinematic parameter of each link of the robot and the reverse recursion equation of the recursive newton euler equation specifically includes:
calculating the force and theoretical moment of each link of the robot according to the following formulas:
fi-1,i＝fi,i+1-fi+Fi
ni-1,i＝fi,i+1×(li+hi)+ni,i+1-fi×li-ni+Fi×li+Ni
τi＝ni-1,i
wherein f isi-1,iFor the force of the connecting rod i-1 on the connecting rod i, fi,i+1For the force of link i acting on link i +1, fiIs the connecting rod i self gravity and friction force, ni-1,iFor the moment of action of the connecting rod i-1 on the connecting rod i, ni,i+1For the moment of action of link i on link i +1, niFor moment produced by the connecting rod i itself,/iArm of force of the action force of the connecting rod i-1 on the connecting rod i, hiArm of force, τ, of the force of connecting rod i on connecting rod i +1iThe input theoretical moment of the connecting rod i;
when i is 1, f0,1The force, τ, required to be provided by the base of the robot to a link connected to the base1The theoretical moment that the base of the robot needs to provide to the connecting rod connected with the base.
According to another aspect of the embodiments of the present invention, there is provided a robot balance detecting apparatus including: the establishing module is used for establishing a recursive Newton Euler equation expressing the relationship among the adjacent connecting rods of the robot and the force, moment, mass, inertia and acceleration of the connecting rods; the first calculation module is used for calculating the force and theoretical moment which are required to be provided by the base of the robot to the connecting rod connected with the base according to the recursive Newton Euler equation; the second calculation module is used for calculating the maximum moment provided by the base according to the force of the base on the connecting rod connected with the base, the gravity of the base and the force arm from the gravity center of the base to the connecting rod connected with the base; and the judging module is used for judging whether the robot is unbalanced or not according to the theoretical moment and the maximum moment.
According to still another aspect of an embodiment of the present invention, there is provided a robot balance detecting apparatus including: the system comprises a processor, a memory, a communication interface and a communication bus, wherein the processor, the memory and the communication interface complete mutual communication through the communication bus; the memory is configured to store at least one executable instruction that causes the processor to perform the steps according to the robot balance detection method as described above.
According to a further aspect of embodiments of the present invention, there is provided a computer storage medium having stored therein at least one executable instruction for causing a processor to perform the steps according to the robot balance detection method as described above.
According to a further aspect of embodiments of the present invention, there is provided a robot including the robot balance detecting apparatus as described above.
According to the embodiment of the invention, a recursion Newton Euler equation expressing the relationship among adjacent connecting rods of the robot and the force, moment, mass, inertia and acceleration of the connecting rods is established, the force and theoretical moment which are required to be provided by the base of the robot to the connecting rods connected with the base are calculated according to the recursion Newton Euler equation, the maximum moment which can be provided by the base is calculated according to the force of the base to the connecting rods connected with the base, the gravity of the base and the moment arm of the connecting rods connected from the gravity center of the base to the base, and whether the robot is unbalanced or not is judged according to the theoretical moment and the maximum moment, so that the balance of the robot can be judged in advance, and the robot is prevented from falling down.
The foregoing description is only an overview of the technical solutions of the embodiments of the present invention, and the embodiments of the present invention can be implemented according to the content of the description in order to make the technical means of the embodiments of the present invention more clearly understood, and the detailed description of the present invention is provided below in order to make the foregoing and other objects, features, and advantages of the embodiments of the present invention more clearly understandable.
Drawings
Various other advantages and benefits will become apparent to those of ordinary skill in the art upon reading the following detailed description of the preferred embodiments. The drawings are only for purposes of illustrating the preferred embodiments and are not to be construed as limiting the invention. Also, like reference numerals are used to refer to like parts throughout the drawings. In the drawings:
fig. 1 is a schematic flow chart illustrating a robot balance detection method according to an embodiment of the present invention;
FIG. 2 shows a schematic flow chart of step 120;
fig. 3 is a schematic structural diagram of a robot to which a robot balance detection method according to an embodiment of the present invention is applied;
fig. 4 is a schematic structural diagram of a robot balance detection device according to an embodiment of the present invention;
fig. 5 shows a schematic structural diagram of a robot balance detection device provided by an embodiment of the present invention.
Detailed Description
Exemplary embodiments of the present invention will be described in more detail below with reference to the accompanying drawings. While exemplary embodiments of the invention are shown in the drawings, it should be understood that the invention can be embodied in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the scope of the invention to those skilled in the art.
Fig. 1 shows a schematic flow chart of a robot balance detection method according to an embodiment of the present invention. The method is applied to the robot, and is particularly suitable for the bionic robot. As shown in fig. 1, the method includes:
and 110, establishing a recursive Newton Euler equation expressing the relationship among all adjacent connecting rods of the robot and the force, moment, mass, inertia and acceleration of the connecting rods.
The robot is composed of a plurality of connecting rods, joints of the connecting rods are used as joints of the robot, and the connecting rods move by controlling the movement of the joints, so that the robot moves. For example, as shown in fig. 2, the links of the robot 10 include a base 0, a first link 1, a second link 2, a third link 3, a fourth link 4, and a fifth link 4 ', the first link 1 is connected to the base 0, the second link 2 is connected to the first link 1, the third link 3 is connected to the second link 2, the fourth link 4 is connected to the third link 3, the fifth link 4 ' is connected to the third link 3, the base 0 supports the robot 10, the first link 1, the second link 2, and the third link 3 serve as a trunk of the robot 10, and the fourth link 4 and the fifth link 4 ' serve as arms of the robot 10.
In this embodiment, step 110 specifically includes: and establishing a motion coordinate system and a basic coordinate system, and establishing a recursive Newton Euler equation according to the motion coordinate system and the basic coordinate system. The specific implementation mode can be as follows: establishing a basic coordinate system on a base of the robot, and respectively establishing a motion coordinate system on each joint of a first connecting rod, a second connecting rod, a third connecting rod, a fourth connecting rod and a fifth connecting rod, wherein the established recursive Newton Euler equation is as follows:
forward recursion equation: i is 0 → n
Reverse recursion equation: i ═ n → 1
fi-1,i＝fi,i+1-fi+Fi
ni-1,i＝fi,i+1×(li+hi)+ni,i+1-fi×li-ni+Fi×li+Ni
τi＝ni-1,i
Wherein, ω isi、
Angular velocity, angular acceleration and linear acceleration, theta, of the connecting rod i relative to the base, respectivelyiIs the angle of rotation of link i relative to link i-1,
is the amplitude of the angular velocity of the connecting rod i,
is the amplitude of the angular acceleration of the connecting rod i,
linear velocity of the connecting rod i-1 relative to the base, ciThe position of the centroid of the link i relative to the base, FiFor a resultant force acting on the centre of mass of the connecting rod i, miIs the mass of the connecting rod i, NiFor resultant moment acting on the centre of mass of the connecting rod I, IciIs an inertia matrix of the connecting rod i, fi-1,iFor the force of the connecting rod i-1 on the connecting rod i, fi,i+1For the force of link i acting on link i +1, fiIs the connecting rod i self gravity and friction force, ni-1,iFor the moment of action of the connecting rod i-1 on the connecting rod i, ni,i+1For the moment of action of link i on link i +1, niFor moment produced by the connecting rod i itself,/iArm of force of the action force of the connecting rod i-1 on the connecting rod i, hiArm of force, τ, of the force of connecting rod i on connecting rod i +1iThe input theoretical moment of the connecting rod i; i is an integer greater than 0.
The angular velocity, the angular acceleration and the linear acceleration of the link i relative to the base are the angular velocity, the angular acceleration and the linear acceleration of the link i relative to the base coordinate system in the motion coordinate system of the link i, the rotation angle of the link i relative to the link i-1 is the rotation angle of the link i relative to the motion coordinate system of the link i-1, and the position of the centroid of the link i relative to the base is the position of the centroid of the link i in the base coordinate system.
In this embodiment, when i is equal to 0, the link i indicates a base of the robot (e.g., the base 0 in fig. 2), when i is equal to 1, the link i indicates a link connected to the base (e.g., the first link 1 in fig. 2), when i is equal to 2, the link i indicates a link connected to the base (e.g., the second link 2 in fig. 2), and so on.
And 120, calculating the force and theoretical moment which are required to be provided by the base of the robot to a connecting rod connected with the base according to the recursive Newton Euler equation.
After the recursive Newton Euler equation is established, data such as angular velocity, angular acceleration and inertia matrix of the base and each connecting rod are obtained, the data are substituted into the recursive Newton Euler equation, and the force and theoretical moment which are required to be provided by the base of the robot to the connecting rod connected with the base can be obtained.
As shown in fig. 3, step 120 specifically includes:
and step 122, recursion is carried out from the tail end connecting rod to the connecting rods sequentially connected with the tail end connecting rod according to the kinematic parameters of the connecting rods of the robot and the reverse recursion equation of the recursion Newton Euler equation to determine the force and the theoretical moment required by the connecting rods of the robot, so that the force and the theoretical moment required by the base of the robot on the connecting rods connected with the base are determined.
Wherein the kinematic parameters include angular velocity, angular acceleration, and linear acceleration. In step 121, according to the forward recursion equation of the recursive newton euler equation, when i is 1, there is
Because the data of angular velocity, angular acceleration, inertia matrix and the like of the base and each connecting rod by taking the motion coordinate system of the base and each connecting rod as reference can be obtained, namely omega is known0、θ1、Ic1Substituting into the calculation to obtain omega1、
And F1、N1. The angular velocity, angular acceleration and linear acceleration of each link of the robot are obtained by recursion.
In step 122, according to the inverse recursion equation of the recursive newton euler equation, when i is 4, there is
f3,4＝f4,5-f4+F4
n3,4＝f4,5×(l4+h4)+n4,5-f4×l4-n4+F4×l4+N4
τ4＝n3,4
Since F can be determined in step 1214、N4Substituting can find f3,4、n3,4. And the force and the theoretical moment required by each connecting rod of the robot are obtained by recursion. When i is 1, f0,1The force, τ, required to be provided to the base of the robot to the link connected to the base1The base of the robot needs a theoretical moment to be provided for the connecting rod connected with the base.
When the robot is about to fall down, the support force provided by the ground surface of the robot about to fall down is on the rotating shaft of the robot about to fall down in consideration of critical conditions, at the moment, the base of the robot can provide the force of reverse moment (including the force of the base and the gravity of the base which are connected with the base), and the maximum moment provided by the base before unbalance can be obtained according to the force of the reverse moment and the force arm from the force to the falling rotating shaft.
Wherein, step 130 specifically includes: the maximum moment that can be provided by the base is calculated according to the following formula:
τd＝(f0,1+G)d0
wherein, taudMaximum moment available for the base, f0,1Is a base pair of a robotThe force of a connecting rod connected to the base, G being the weight of the base, d0The moment arm from the gravity center of the base to the connecting rod connected with the base. The moment arm of the link connecting the center of gravity of the base to the base means the shortest distance between the center of gravity of the base and the link connecting the base.
And 140, judging whether the robot is unbalanced or not according to the theoretical moment and the maximum moment.
Wherein, step 140 specifically includes: comparing the theoretical moment with the maximum moment; if the maximum moment is greater than or equal to the theoretical moment, determining that the robot is not unbalanced; and if the maximum moment is smaller than the theoretical moment, determining that the robot is unbalanced.
In some embodiments, the method further comprises: if it is determined that the robot is not out of balance, the robot is allowed to perform the next action. The balance detection of the robot may be performed at intervals of a preset detection time, for example, when the robot completes one action, it is determined whether the robot is unbalanced, and after it is determined that the robot is not unbalanced, the robot is allowed to perform the next action. Alternatively, the detection time is preset at regular intervals, for example, the balance detection is performed every 5 seconds.
In some embodiments, the method further comprises: and if the robot is determined to be unbalanced, adjusting the motion of the robot until the maximum moment of the robot is greater than or equal to the theoretical moment. Wherein, the motion of adjusting the robot can be: and adjusting the rotation angle of each joint of the robot so as to adjust the position of each connecting rod.
The robot balance detection method of the embodiment calculates the force and the theoretical moment which need to be provided by the base of the robot to the connecting rod connected with the base by establishing the recursion Newton Euler equation expressing the relationship among the adjacent connecting rods of the robot and the self force, moment, mass, inertia and acceleration of the connecting rods according to the recursion Newton Euler equation, calculates the maximum moment which can be provided by the base according to the force of the base to the connecting rod connected with the base, the gravity of the base and the moment arm of the connecting rod which is connected from the gravity center of the base to the base, and judges whether the robot is unbalanced or not according to the theoretical moment and the maximum moment, so that the balance of the robot can be judged in advance, and the robot is prevented from falling down.
The embodiment of the invention provides a flow of an application example of a robot balance detection method, wherein the method is applied to a robot 10 shown in fig. 2, the robot 10 includes a base 0, a first connecting rod 1, a second connecting rod 2, a third connecting rod 3, a fourth connecting rod 4 and a fifth connecting rod 4 ', the base 0 is connected with the first connecting rod 1, the first connecting rod 1 is connected with the second connecting rod 2, the second connecting rod 2 is connected with the third connecting rod 3, the third connecting rod 3 is respectively connected with the fourth connecting rod 4 and the fifth connecting rod 4', and joints are arranged at the joints among the connecting rods. The base 0 is placed on the ground, the base 0 serves as a support, the first connecting rod 1, the second connecting rod 2 and the third connecting rod 3 are the trunk of the robot 10, and the fourth connecting rod 4 and the fifth connecting rod 4' are the arms of the robot 10. The angular velocity and angular acceleration of each link are read by an encoder, and the inertia matrix of each link is obtained by mechanical modeling.
The method comprises the following steps:
step 210, establishing a recursive Newton Euler equation expressing the relationship between each adjacent connecting rod of the robot 10 and the force, moment, mass, inertia and acceleration of the connecting rod.
Step 220, calculating the force f required by the base 0 to be provided for the first connecting rod 1 according to the recursive Newton Euler equation0,1And theoretical moment τ1；
Step 230, according to the force f of the base 0 to the first connecting rod 10,1The gravity G of the base 0 and the arm d from the gravity center of the base 0 to the first connecting rod 10Calculating the maximum torque τ provided by the base 0d；
Step 240, comparing the theoretical moment tau1And maximum moment τdIt is determined whether the robot 10 is out of balance.
The robot balance detection method of the embodiment calculates the force and the theoretical moment which need to be provided by the base of the robot to the connecting rod connected with the base by establishing the recursion Newton Euler equation expressing the relationship among the adjacent connecting rods of the robot and the self force, moment, mass, inertia and acceleration of the connecting rods according to the recursion Newton Euler equation, calculates the maximum moment which can be provided by the base according to the force of the base to the connecting rod connected with the base, the gravity of the base and the moment arm of the connecting rod which is connected from the gravity center of the base to the base, and judges whether the robot is unbalanced or not according to the theoretical moment and the maximum moment, so that the balance of the robot can be judged in advance, and the robot is prevented from falling down.
Fig. 4 shows a schematic structural diagram of a robot balance detection device provided by an embodiment of the present invention. As shown in fig. 4, the apparatus 300 includes: a setup module 310, a first calculation module 320, a second calculation module 330, and a determination module 340.
The establishing module 310 is configured to establish a recursive newton euler equation expressing relationships between adjacent links of the robot and between the force, moment, mass, inertia, and acceleration of the links; the first calculation module 320 is used for calculating the force and theoretical moment which are required to be provided by the base of the robot to the connecting rod connected with the base according to the recursive Newton Euler equation; the second calculating module 330 is configured to calculate a maximum moment that the base can provide according to a force of the base on a connecting rod connected to the base, a gravity of the base, and a moment arm from a center of gravity of the base to the connecting rod connected to the base; the determining module 340 is configured to determine whether the robot is unbalanced according to the theoretical moment and the maximum moment.
In an optional manner, the second calculation module 330 is further configured to: calculating the maximum torque that the base can provide according to the following formula:
τd＝(f0,1+G)d0
wherein, taudMaximum moment available for the base, f0,1The force of the base of the robot on a connecting rod connected with the base, G is the gravity of the base, d0The moment arm is from the center of gravity of the base to a connecting rod connected with the base.
In an optional manner, the determining module 340 is further configured to: comparing the theoretical moment with the maximum moment; if the maximum moment is greater than or equal to the theoretical moment, determining that the robot is not unbalanced; and if the maximum moment is smaller than the theoretical moment, determining that the robot is unbalanced.
In an optional manner, the apparatus 300 further comprises: and continuing to move the module. The motion continuation module is used for allowing the robot to execute the next action if the robot is determined not to be unbalanced.
In an alternative manner, the first calculation module 320 includes: a forward recursion unit and a reverse recursion unit. The forward recursion unit is used for recursing from the base to each connecting rod sequentially connected with the base according to a forward recursion equation of the recursion Newton Euler equation so as to determine kinematic parameters of each connecting rod of the robot; and the reverse recursion unit is used for recursing from the tail end connecting rod to the connecting rods sequentially connected with the tail end connecting rod according to the kinematic parameters of the connecting rods of the robot and the reverse recursion equation of the recursion Newton Euler equation to determine the force and the theoretical moment required by the connecting rods of the robot, so that the force and the theoretical moment required by the base of the robot to the connecting rods connected with the base are determined.
In an alternative mode, the kinematic parameters include angular velocity, angular acceleration and linear acceleration, and the forward recursion unit is specifically configured to: calculating kinematic parameters of each link of the robot according to the following formula:
wherein, ω isi、
Angular velocity, angular acceleration and linear acceleration, theta, of the connecting rod i relative to the base, respectivelyiIs the angle of rotation of link i relative to link i-1,
is the amplitude of the angular velocity of the connecting rod i,
is the angular acceleration amplitude, c, of the connecting rod iiIs the position of the center of mass of the connecting rod i relative to the base, FiFor a resultant force acting on the centre of mass of the connecting rod i, miIs the mass of the connecting rod i, NiI is an integer greater than 0, which is the resultant moment acting on the center of mass of the connecting rod i.
In an alternative approach, the reverse recursion unit is specifically configured to: calculating the force and theoretical moment of each link of the robot according to the following formulas:
fi-1,i＝fi,i+1-fi+Fi
ni-1,i＝fi,i+1×(li+hi)+ni,i+1-fi×li-ni+Fi×li+Ni
τi＝ni-1,i
wherein f isi-1,iFor the force of the connecting rod i-1 on the connecting rod i, fi,i+1For the force of link i acting on link i +1, fiIs the connecting rod i self gravity and friction force, ni-1,iFor the moment of action of the connecting rod i-1 on the connecting rod i, ni,i+1For the moment of action of link i on link i +1, niFor moment, τ, generated by the connecting rod i itselfiThe input theoretical moment of the connecting rod i;
when i is 1, f0,1For the base of the robot to be coupled to the baseForce, τ, provided by a connecting rod1The theoretical moment that the base of the robot needs to provide to the connecting rod connected with the base.
It should be noted that the robot balance detection apparatus provided in the embodiments of the present invention is an apparatus capable of executing the robot balance detection method, and all embodiments based on the robot balance detection method are applicable to the apparatus and can achieve the same or similar beneficial effects.
The robot balance detection device of the embodiment calculates the force and the theoretical moment which need to be provided by the base of the robot to the connecting rod connected with the base by establishing the recursion Newton Euler equation expressing the relationship among the adjacent connecting rods of the robot and the self force, moment, mass, inertia and acceleration of the connecting rods according to the recursion Newton Euler equation, calculates the maximum moment which can be provided by the base according to the force of the base to the connecting rod connected with the base, the gravity of the base and the moment arm of the connecting rod which is connected from the gravity center of the base to the base, and judges whether the robot is unbalanced or not according to the theoretical moment and the maximum moment, thereby being capable of prejudging the balance of the robot and avoiding falling down.
An embodiment of the present invention provides a computer storage medium, where at least one executable instruction is stored in the storage medium, and the executable instruction causes a processor to execute the robot balance detection method in any of the above method embodiments.
Embodiments of the present invention provide a computer program product comprising a computer program stored on a computer storage medium, the computer program comprising program instructions which, when executed by a computer, cause the computer to perform a robot balance detection method in any of the above-mentioned method embodiments.
The embodiment of the invention also provides a robot. Referring to fig. 2, the robot is provided with a robot balance detecting device 300 according to the above embodiment.
The robot of the embodiment calculates the force and the theoretical moment which need to be provided by the base of the robot to the connecting rod connected with the base by establishing the recursion Newton Euler equation expressing the relationship among the adjacent connecting rods of the robot and the self force, moment, mass, inertia and acceleration of the connecting rods according to the recursion Newton Euler equation, calculates the maximum moment which can be provided by the base according to the force of the base to the connecting rod connected with the base, the gravity of the base and the moment arm of the connecting rod connected from the gravity center of the base to the base, and judges whether the robot is unbalanced or not according to the theoretical moment and the maximum moment, so that the balance of the robot can be judged in advance, and the robot is prevented from falling down.
Fig. 5 is a schematic structural diagram of a robot balance detection apparatus according to an embodiment of the present invention, and the specific embodiment of the present invention does not limit the specific implementation of the robot balance detection apparatus.
As shown in fig. 5, the robot balance detecting apparatus may include: a processor (processor)502, a Communications Interface 504, a memory 506, and a communication bus 508.
Wherein: the processor 502, communication interface 504, and memory 506 communicate with one another via a communication bus 508. A communication interface 504 for communicating with network elements of other devices, such as clients or other servers. The processor 502 is configured to execute the program 510, and may specifically execute the robot balance detection method in any of the method embodiments described above.
In particular, program 510 may include program code that includes computer operating instructions.
The processor 502 may be a central processing unit CPU, or an Application Specific Integrated Circuit (ASIC), or one or more Integrated circuits configured to implement an embodiment of the present invention. The computing device includes one or more processors, which may be the same type of processor, such as one or more CPUs; or may be different types of processors such as one or more CPUs and one or more ASICs.
And a memory 506 for storing a program 510. The memory 506 may comprise high-speed RAM memory, and may also include non-volatile memory (non-volatile memory), such as at least one disk memory.
The robot balance detection equipment of the embodiment calculates the force and the theoretical moment which need to be provided by the base of the robot to the connecting rod connected with the base by establishing the recursion Newton Euler equation expressing the relationship among the adjacent connecting rods of the robot and the self force, moment, mass, inertia and acceleration of the connecting rods according to the recursion Newton Euler equation, calculates the maximum moment which can be provided by the base according to the force of the base to the connecting rod connected with the base, the gravity of the base and the moment arm of the connecting rod which is connected from the gravity center of the base to the base, and judges whether the robot is unbalanced or not according to the theoretical moment and the maximum moment, thereby being capable of prejudging the balance of the robot and avoiding falling down.
The algorithms or displays presented herein are not inherently related to any particular computer, virtual system, or other apparatus. Various general purpose systems may also be used with the teachings herein. The required structure for constructing such a system will be apparent from the description above. In addition, embodiments of the present invention are not directed to any particular programming language. It is appreciated that a variety of programming languages may be used to implement the teachings of the present invention as described herein, and any descriptions of specific languages are provided above to disclose the best mode of the invention.
In the description provided herein, numerous specific details are set forth. It is understood, however, that embodiments of the invention may be practiced without these specific details. In some instances, well-known methods, structures and techniques have not been shown in detail in order not to obscure an understanding of this description.
Similarly, it should be appreciated that in the foregoing description of exemplary embodiments of the invention, various features of the embodiments of the invention are sometimes grouped together in a single embodiment, figure, or description thereof for the purpose of streamlining the invention and aiding in the understanding of one or more of the various inventive aspects. However, the disclosed method should not be interpreted as reflecting an intention that: that the invention as claimed requires more features than are expressly recited in each claim. Rather, as the following claims reflect, inventive aspects lie in less than all features of a single foregoing disclosed embodiment. Thus, the claims following the detailed description are hereby expressly incorporated into this detailed description, with each claim standing on its own as a separate embodiment of this invention.
Those skilled in the art will appreciate that the modules in the device in an embodiment may be adaptively changed and disposed in one or more devices different from the embodiment. The modules or units or components of the embodiments may be combined into one module or unit or component, and furthermore they may be divided into a plurality of sub-modules or sub-units or sub-components. All of the features disclosed in this specification (including any accompanying claims, abstract and drawings), and all of the processes or elements of any method or apparatus so disclosed, may be combined in any combination, except combinations where at least some of such features and/or processes or elements are mutually exclusive. Each feature disclosed in this specification (including any accompanying claims, abstract and drawings) may be replaced by alternative features serving the same, equivalent or similar purpose, unless expressly stated otherwise.
Furthermore, those skilled in the art will appreciate that while some embodiments herein include some features included in other embodiments, rather than other features, combinations of features of different embodiments are meant to be within the scope of the invention and form different embodiments. For example, in the following claims, any of the claimed embodiments may be used in any combination.
It should be noted that the above-mentioned embodiments illustrate rather than limit the invention, and that those skilled in the art will be able to design alternative embodiments without departing from the scope of the appended claims. In the claims, any reference signs placed between parentheses shall not be construed as limiting the claim. The word "comprising" does not exclude the presence of elements or steps not listed in a claim. The word "a" or "an" preceding an element does not exclude the presence of a plurality of such elements. The invention may be implemented by means of hardware comprising several distinct elements, and by means of a suitably programmed computer. In the unit claims enumerating several means, several of these means may be embodied by one and the same item of hardware. The usage of the words first, second and third, etcetera do not indicate any ordering. These words may be interpreted as names. The steps in the above embodiments should not be construed as limiting the order of execution unless specified otherwise.
Claims (8)
1. A robot balance detection method is characterized by comprising the following steps:
establishing a recursion Newton Euler equation expressing the relationship among the adjacent connecting rods of the robot and the self force, moment, mass, inertia and acceleration of the connecting rods; wherein the recursive Newton Euler equation comprises a forward recursion equation and a reverse recursion equation;
the forward recursion equation of the recursive Newton Euler equation is as follows:
wherein the kinematic parameters include angular velocity, angular acceleration and linear acceleration, ωi、
Angular velocity, angular acceleration and linear acceleration, theta, of the connecting rod i relative to the base, respectivelyiIs the angle of rotation of link i relative to link i-1,
is the amplitude of the angular velocity of the connecting rod i,
is the angular acceleration amplitude, c, of the connecting rod iiIs the position of the center of mass of the connecting rod i relative to the base, FiFor a resultant force acting on the centre of mass of the connecting rod i, miIs the mass of the connecting rod i, NiI is an integer greater than 0, which is the resultant moment acting on the center of mass of the connecting rod i;
the reverse recursion equation of the recursion Newton Euler equation is as follows:
fi-1,i＝fi,i+1-fi+Fi
ni-1,i＝fi,i+1×(li+hi)+ni,i+1-fi×li-ni+Fi×li+Ni
τi＝ni-1,i
wherein f isi-1,iFor the force of the connecting rod i-1 on the connecting rod i, fi,i+1For the force of link i acting on link i +1, fiIs the connecting rod i self gravity and friction force, ni-1,iFor the moment of action of the connecting rod i-1 on the connecting rod i, ni,i+1For the moment of action of link i on link i +1, niFor moment produced by the connecting rod i itself,/iArm of force of the action force of the connecting rod i-1 on the connecting rod i, hiArm of force, τ, of the force of connecting rod i on connecting rod i +1iThe input theoretical moment of the connecting rod i; when i is 1, f0,1The force, τ, required to be provided by the base of the robot to a link connected to the base1For the base of the robot to be connected with the baseThe theoretical force provided by the connecting rod of (1);
according to a forward recursion equation of the recursion Newton Euler equation, recursion is carried out from a base to each connecting rod sequentially connected with the base so as to determine the kinematic parameters of each connecting rod of the robot;
according to kinematic parameters of each connecting rod of the robot and a reverse recursion equation of the recursion Newton Euler equation, recursion is carried out from a tail end connecting rod to each connecting rod sequentially connected with the tail end connecting rod to determine force and theoretical moment required by each connecting rod of the robot, so that force and theoretical moment required by a base of the robot to be provided for the connecting rod connected with the base are determined;
calculating the maximum moment provided by the base according to the force of the base on a connecting rod connected with the base, the gravity of the base and the force arm from the gravity center of the base to the connecting rod connected with the base;
and judging whether the robot is unbalanced or not according to the theoretical moment and the maximum moment.
2. The method of claim 1, wherein calculating the maximum moment that the base can provide based on the force of the base on the link connected to the base, the weight of the base, and the moment arm from the center of gravity of the base to the link connected to the base comprises:
calculating the maximum torque that the base can provide according to the following formula:
τd＝(f0,1+G)d0
wherein, taudMaximum moment available for the base, f0,1The force of the base of the robot on a connecting rod connected with the base, G is the gravity of the base, d0The moment arm is from the center of gravity of the base to a connecting rod connected with the base.
3. The method according to claim 1, wherein the determining whether the robot is out of balance according to the theoretical moment and the maximum moment comprises:
comparing the theoretical moment with the maximum moment;
if the maximum moment is greater than or equal to the theoretical moment, determining that the robot is not unbalanced;
and if the maximum moment is smaller than the theoretical moment, determining that the robot is unbalanced.
4. The method of claim 3, further comprising:
if it is determined that the robot is not out of balance, allowing the robot to perform a next action.
5. A robot balance detecting device, comprising:
the establishing module is used for establishing a recursive Newton Euler equation expressing the relationship among the adjacent connecting rods of the robot and the force, moment, mass, inertia and acceleration of the connecting rods;
the first calculation module is used for determining the kinematic parameters of each connecting rod of the robot by recursion from a base to each connecting rod sequentially connected with the base according to a forward recursion equation of the recursion Newton Euler equation;
the forward recursion equation of the recursive Newton Euler equation is as follows:
wherein the kinematic parameters include angular velocity, angular acceleration and linear acceleration, ωi、
Angular velocity, angular acceleration and linear acceleration, theta, of the connecting rod i relative to the base, respectivelyiIs the angle of rotation of link i relative to link i-1,
is the amplitude of the angular velocity of the connecting rod i,
is the angular acceleration amplitude, c, of the connecting rod iiIs the position of the center of mass of the connecting rod i relative to the base, FiFor a resultant force acting on the centre of mass of the connecting rod i, miIs the mass of the connecting rod i, NiI is an integer greater than 0, which is the resultant moment acting on the center of mass of the connecting rod i;
according to kinematic parameters of each connecting rod of the robot and a reverse recursion equation of the recursion Newton Euler equation, recursion is carried out from a tail end connecting rod to each connecting rod sequentially connected with the tail end connecting rod to determine force and theoretical moment required by each connecting rod of the robot, so that force and theoretical moment required by a base of the robot to be provided for the connecting rod connected with the base are determined;
the reverse recursion equation of the recursion Newton Euler equation is as follows:
fi-1,i＝fi,i+1-fi+Fi
ni-1,i＝fi,i+1×(li+hi)+ni,i+1-fi×li-ni+Fi×li+Ni
τi＝ni-1,i
wherein f isi-1,iFor the force of the connecting rod i-1 on the connecting rod i, fi,i+1For the force of link i acting on link i +1, fiIs the connecting rod i self gravity and friction force, ni-1,iFor the moment of action of the connecting rod i-1 on the connecting rod i, ni,i+1For the moment of action of link i on link i +1, niFor moment produced by the connecting rod i itself,/iArm of force of the action force of the connecting rod i-1 on the connecting rod i, hiArm of force, τ, of the force of connecting rod i on connecting rod i +1iThe input theoretical moment of the connecting rod i;
when i is 1, f0,1The force, τ, required to be provided by the base of the robot to a link connected to the base1Theoretical force which is required to be provided for a base of the robot to a connecting rod connected with the base;
the second calculation module is used for calculating the maximum moment provided by the base according to the force of the base on the connecting rod connected with the base, the gravity of the base and the force arm from the gravity center of the base to the connecting rod connected with the base;
and the judging module is used for judging whether the robot is unbalanced or not according to the theoretical moment and the maximum moment.
6. A robot balance detection apparatus, comprising: the system comprises a processor, a memory, a communication interface and a communication bus, wherein the processor, the memory and the communication interface complete mutual communication through the communication bus;
the memory is configured to store at least one executable instruction that causes the processor to perform the steps of the robot balance detection method according to any of claims 1-4.
7. A computer storage medium having stored therein at least one executable instruction causing a processor to perform the steps of the robot balance detection method according to any of claims 1-4.
8. A robot comprising the robot balance detecting device according to claim 5.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910614249.4A CN110328689B (en) | 2019-07-09 | 2019-07-09 | Robot balance detection method, device and equipment and robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910614249.4A CN110328689B (en) | 2019-07-09 | 2019-07-09 | Robot balance detection method, device and equipment and robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110328689A CN110328689A (en) | 2019-10-15 |
| CN110328689B true CN110328689B (en) | 2021-01-08 |
Family
ID=68144472
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910614249.4A Active CN110328689B (en) | 2019-07-09 | 2019-07-09 | Robot balance detection method, device and equipment and robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110328689B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111098303A (en) * | 2019-12-25 | 2020-05-05 | 深圳前海达闼云端智能科技有限公司 | Robot and flexible control method and device thereof |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP1084943A2 (en) * | 1999-09-20 | 2001-03-21 | Sony Corporation | Robot ambulation control apparatus and method |
| RU2296668C2 (en) * | 2001-08-01 | 2007-04-10 | Хонда Гикен Когьо Кабусики Кайся | Method for evaluating reaction of support and method for evaluating moments of two-legged walking body's joints |
Family Cites Families (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP3726032B2 (en) * | 2001-04-27 | 2005-12-14 | 本田技研工業株式会社 | Target motion generator for legged mobile robot |
| DE602005016580D1 (en) * | 2004-08-02 | 2009-10-22 | Honda Motor Co Ltd | CONTROL PROCEDURE FOR SCREED ROBOTS |
| JP2007044836A (en) * | 2005-08-11 | 2007-02-22 | Victor Co Of Japan Ltd | Leg type biped robot |
| US9367795B2 (en) * | 2010-02-25 | 2016-06-14 | Honda Motor Co., Ltd. | Momentum-based balance controller for humanoid robots on non-level and non-stationary ground |
| CN108372505A (en) * | 2018-01-31 | 2018-08-07 | 国机智能技术研究院有限公司 | A kind of collision detection algorithm and system based on dynamic (dynamical) robot |
| CN108638072B (en) * | 2018-05-25 | 2021-07-30 | 珞石（山东）智能科技有限公司 | High-precision control device and method for industrial robot |
| CN108858208B (en) * | 2018-09-05 | 2022-06-17 | 鲁东大学 | Self-adaptive balance control method, device and system for humanoid robot in complex terrain |
| CN109634100B (en) * | 2018-12-30 | 2021-11-02 | 深圳市优必选科技有限公司 | Humanoid robot walking acceleration compensation method and device and humanoid robot |
-
2019
- 2019-07-09 CN CN201910614249.4A patent/CN110328689B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP1084943A2 (en) * | 1999-09-20 | 2001-03-21 | Sony Corporation | Robot ambulation control apparatus and method |
| RU2296668C2 (en) * | 2001-08-01 | 2007-04-10 | Хонда Гикен Когьо Кабусики Кайся | Method for evaluating reaction of support and method for evaluating moments of two-legged walking body's joints |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110328689A (en) | 2019-10-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11579591B2 (en) | Feedforward control method for floating base dynamics, computer-readable storage medium and robot | |
| Kim et al. | Rolling locomotion of cable-driven soft spherical tensegrity robots | |
| CN111506100B (en) | Multi-legged robot joint control method and device and multi-legged robot | |
| US11346495B2 (en) | Control method for non-orthogonal gimbal, gimbal thereof, and storage device | |
| CN109676606B (en) | Method for calculating arm angle range of mechanical arm, mechanical arm and robot | |
| US11833692B2 (en) | Method and device for controlling arm of robot | |
| WO2022135011A1 (en) | Method and apparatus for obtaining combined centroid state, humanoid robot, and readable storage medium | |
| CN106202713A (en) | A kind of biasing mechanism arm inverse kinematics method | |
| WO2023024317A1 (en) | Robot obstacle avoidance method and apparatus, and robot | |
| CN110328689B (en) | Robot balance detection method, device and equipment and robot | |
| JP2018030210A (en) | Simulation device, control system, robot system, simulation method, program and recording medium | |
| CN113119096B (en) | Mechanical arm space position adjusting method and device, mechanical arm and storage medium | |
| CN113031579A (en) | Walking control method and device of biped robot and biped robot | |
| CN113119104A (en) | Mechanical arm control method, mechanical arm control device, computing equipment and system | |
| CN112140111B (en) | Method for determining center of mass of tail end load of robot | |
| CN112199827B (en) | Mechanical arm dynamics simulation method and device, electronic equipment and storage medium | |
| CN114454981B (en) | Jumping motion control method and device of biped robot and biped robot | |
| Ono et al. | Development of equations of motion for a Stewart platform under prescribed mount motion | |
| CN108073283A (en) | The computational methods and gloves of hand joint | |
| CN113858207A (en) | Method and device for controlling falling of humanoid robot, electronic device and storage medium | |
| CN114161402A (en) | Robot stability control method, model construction method and device and robot | |
| TANG et al. | Kinematic analysis of a Stewart platform based on AFSA | |
| CN112784428B (en) | DH parameter-based Lagrange dynamics modeling method for concrete pump truck | |
| CN116197917B (en) | Self-adaptive maximum acceleration calculation method and device, storage medium and electronic equipment | |
| CN113127974B (en) | Kinetic parameter identification method, parameter identification device and storage medium |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |