DE102021112485B3 - Method for balancing a robot, method for whole-body control of a robot, regulator and robot - Google Patents

Abstract

A method for balancing a robot to compensate for external interference, the robot comprising: at least one joint with the number of updated degrees of freedom for the articulated connection of body segments of the robot with at least one actuating device for actively influencing the position q of the at least one joint, and a support surface; the method comprising the steps:a) monitoring and determining the contact torques τext acting externally on the robot;b) checking whether the determined contact torques τext exceed at least one specified limit value τthres;c) if the at least one specified limit value τthresin step b) carrying out the steps Calculation and induction of a required reference centroid angular momentum slcreƒ, and subsequent reduction of the generated reference centroid angular momentum slcreƒ by changing the position q of at least one joint using at least one actuating device of the robot; d) calculating and inducing whole-body movements xcmd using a whole-body movement optimizer by changing the position q of at least one joint using at least one actuating device of the robot.

Description

The present invention relates to a method for balancing a robot to compensate for external disturbances acting on it, and a method for whole-body control of a robot to compensate for external disturbances acting on it, a controller and a robot.

In the prior art according to [1]-[4] control methods for humanoid robots are described, which regulate a desired momentum and angular momentum in balancing scenarios. The desired momentum is chosen based on the desired center of gravity position and velocity, the desired angular momentum is always set to zero. If the robot cannot realize both desired quantities, momentum is always given higher priority than angular momentum. This can mean that in order to impress the system with a desired linear momentum, a value deviating from the desired angular momentum is accepted. This case can occur when the robot is disturbed by external forces and the conditions that guarantee slip and tip-free contact of the feet with the ground do not allow simultaneous imprinting of the desired momentum and angular momentum. As a result, non-zero angular momentum is induced in some situations. In the prior art methods described above, the resultant angular momentum in a humanoid robot is not actively planned, but only generated when the fulfillment of higher priority tasks, such as a linear momentum control task, require the violation of the angular momentum control task. The goal of the angular momentum regulation task is to keep angular momentum at zero or to reduce it as much as possible. Due to this typical control structure, the course of the angular momentum can only take place indirectly by reprioritizing different tasks, which may conflict with each other. This is a high-dimensional optimization problem of the setting parameters, which have to be set with great effort and without direct physical significance. This is problematic for humanoid robots insofar as the angular momentum can only be generated with a certain magnitude and only for a limited period of time, since otherwise the position and/or speed limits in the robot's joints are reached. Reaching joint limits can result in a sudden reduction in angular momentum as the required joint speed can no longer be maintained. However, an abrupt change in the angular momentum generates large horizontal forces which, among other things, affect the center of gravity of the robot and can destabilize it, which in the worst case can lead to the humanoid robot falling over. Furthermore, an angular momentum is usually only generated when the contact conditions, which are intended to ensure a slip-free and tilt-free contact, are achieved. This reduces the robustness of the system against model inaccuracies and control deviations, since even small disturbances can bring down the robot if it operates on the edge of the contact conditions.

In the prior art according to [5] and [6], a two-phase method for generating the time derivative of an angular momentum reference trajectory for balancing scenarios under the influence of external forces (e.g. impacts) is presented. In the first phase, a time derivative of the angular momentum is specified to counteract the destabilizing effect of the external force. The time derivative of the angular momentum is calculated from the difference between the center of gravity and the "Center of Pressure (CoP)". The resulting joint accelerations are calculated using a Moore-Penrose pseudo-inverse. In a second phase, the initial pose of the robot is restored, which is determined as a function of the robot's potential energy and/or the effective joint torques.

The time derivative of the desired angular momentum is calculated from the difference of the CoP and the center of gravity multiplied by a proportional factor k. The disadvantage is that this factor has to be determined heuristically by trial and error in order to exert a horizontal force that is appropriate for the impact. Contact conditions are not explicitly considered. A new k value must be determined for each new point of application of force or new direction of force. For this reason, this method cannot be generalized and is only suitable for use under laboratory conditions. Furthermore, it is not clear from the method when which phase is activated.

From [7] is a control or regulating device for a robot, such as. B. a legged mobile robot known. [8] to [11] disclose devices for generating gaits suitable not only for walking but also for running a legged robot. [12] also discloses a method for generating a dynamically realizable movement of a connection system with a human-like structure. This method can be used for motion generation software for a humanoid robot, real-time control system for a humanoid robot, and motion generation software for computer graphics.

• [1] S.-H. Lee and A. Goswami, "A momentum-based balance controller for humanoid robots on non-level and nonstationary ground," Auton. Robots, vol. 33, pp. 399-414, Nov. 2012
• [2] Lee, Sung-hee, and Ambarish Goswami. "Momentum-based balance controller for humanoid robots on non-level and non-stationary ground." US Patent No. 9,367,795 . 14 Jun 2016. WO2011106543A1
• [3] A Hofmann, M Popovic, and H Herr, "Exploiting angular momentum to enhance bipedal center-of-mass control," in Proc. IEEE Int. conf robot. Auto., May 2009, pp. 4423-4429
• 4] R Hinata and D N Nenchev, "Balance stabilization with angular momentum damping derived from the reaction null-space," Proc. 18th IEEE RAS Int. conf Humanoid Robots, pp. 188-195, 2018
• [5] Abdallah, M., & Goswami, A. "A biomechanically motivated two-phase strategy for biped upright balance control," in Proc. IEEE Int. conf robot. Auto, 2005, April, pp. 1996-2001.
• [6] Goswami, Ambarish, and Muhammad E. Abdallah. "Systems and methods for controlling a legged robot using a two-phase disturbance response strategy." patent no. 7,835,822. 16 Nov 2010. US7835822B2
• [7] DE 10 2010 064 270 B4
• [8th] EP 1 649 983 B1
• [9] EP 1 642 688 B1
• [10] EP 1 642 687 B9
• [11] EP 1 475 196 B1
• [12] EP 1 334 901 B1

Based on the aforementioned disadvantages of the prior art, the present application is based on the object of providing an improved method for balancing a robot under the influence of external disturbances and an improved method for controlling the whole body of a robot under the influence of external disturbances, as well as a corresponding controller or a robot .

The present invention deals with a method to give humanoid robots the ability to maintain upright balance even in involuntary contact situations (e.g. bumps) and to prevent the robot from falling. The center of gravity-related angular momentum of the humanoid robot is actively controlled in order to generate forces and torques that counteract the external forces (induced by contacts with the environment). In contrast to satellites in orbit, humanoid robots can only generate angular momentum up to a certain magnitude and only for a limited period of time, otherwise position and/or speed limits in the robot's joints are reached. These properties must be taken into account when generating angular momentum. Depending on the external forces, the method presented here generates a reference trajectory for the desired angular momentum. It is taken into account that the angular momentum must be reduced again quickly enough to prevent the position and/or speed limits in the joints from being reached. Based on this reference trajectory, whole-body movements are generated, which induce the desired angular momentum in the humanoid robot. A special focus is placed on the kinematic feasibility of the generated trajectories.

1. a) Überwachen und Ermitteln der auf den Roboter extern einwirkenden Kontaktmomente τ_ext;
2. b) Überprüfen, ob die ermittelten Kontaktmomente τ_ext mindestens einen vorgegebenen Grenzwert τ^thres überschreiten;
3. c) bei Überschreiten des mindestens einen vorgegebenen Grenzwertes τ^thres in Schritt b) Ausführen der Teilschritte c1) bis c3):
   • c1) Berechnen eines erforderlichen Referenzschwerpunktdrehimpulses (CAM) $l_c^{reƒ},$
     
     um den Druckmittelpunkt (CoP) des Roboters innerhalb der Aufstützfläche (support area) zu halten,
   • c2) Induzieren des erforderlichen Referenzschwerpunktdrehimpulses $l_c^{reƒ}$
     
     durch Verändern der Stellung q mindestens eines Gelenks mittels mindestens einer Stelleinrichtung des Roboters, sowie c3) Abbauen des generierten Referenzschwerpunktdrehimpulses $l_c^{reƒ}$
     
     durch Verändern der Stellung q mindestens eines Gelenks mittels mindestens einer Stelleinrichtung des Roboters;
4. d) Berechnen von Ganzkörperbewegungen x^cmd mittels eines Ganzkörperbewegungsoptimierers, welcher den Referenzschwerpunktdrehimpuls $l_c^{reƒ}$
   
   und eine Referenzpose x^ref als Eingabegrößen nutzt und dynamisch sowie kinematisch umsetzbare Gelenktrajektorien generiert und sicherstellt, dass der Roboter zu der vorgegebenen Referenzpose x^ref konvergiert und dass im Falle der Ausführung der Schritte c1) bis c3) nachfolgend nur betragsmäßig kleine Drehimpulse mit umgekehrten Vorzeichen zu den in den Schritten c2) und c3) generierten Drehimpulsen generiert werden;
5. e) Induzieren der berechneten Ganzkörperbewegungen x^cmd durch Verändern der Stellung q mindestens eines Gelenks mittels mindestens einer Stelleinrichtung des Roboters; sowie
6. f) kontinuierliches Wiederholen der vorbezeichneten Verfahrensschritte.

According to the first aspect, the present invention relates to a method for balancing a robot to compensate for external disturbances, the robot comprising: at least two body segments, at least one joint with the number of n actuated degrees of freedom for the articulated connection of the body segments with at least one adjusting device for actively influencing the position q of the at least one joint, and a support surface;
the procedure comprising the steps:

1. a) Monitoring and determination of the contact moments τ _ext acting externally on the robot;
2. b) checking whether the determined contact torques τ _ext exceed at least one predetermined limit value τ ^thres ;
3. c) if the at least one predetermined limit value τ ^{thres is exceeded} in step b) execution of sub-steps c1) to c3):
   • c1) Calculating a required reference centroid angular momentum (CAM) $l_c^{\mathrm{right}eƒ},$
     
     to keep the robot's center of pressure (CoP) within the support area,
   • c2) Inducing the required reference centroid angular momentum $l_c^{\mathrm{right}eƒ}$
     
     by changing the position q of at least one joint by means of at least one actuating device of the robot, and c3) reducing the generated reference center of gravity angular momentum $l_c^{\mathrm{right}eƒ}$
     
     by changing the position q of at least one joint by means of at least one actuating device of the robot;
4. d) Calculating whole-body motions x ^cmd using a whole-body motion optimizer that uses the reference centroid angular momentum $l_c^{\mathrm{right}eƒ}$
   
   and uses a reference pose x ^ref as input sizes and dynamic as well kinematically implementable joint trajectories are generated and ensures that the robot converges to the specified reference pose x ^ref and that, if steps c1) to c3) are carried out, angular momentum is only small in terms of magnitude with the opposite sign to the angular momentum generated in steps c2) and c3). to be generated;
5. e) inducing the calculated whole-body movements x ^cmd by changing the position q of at least one joint by means of at least one actuating device of the robot; such as
6. f) continuous repetition of the aforementioned process steps.

The method according to the invention generates an angular momentum reference trajectory for balancing scenarios at runtime, depending on the acting external forces, in order to counteract them and prevent the robot from falling over. The method takes into account the magnitude of the force, the point of application of the force and the direction of the force. The course of the angular momentum reference trajectory can be adjusted by two setting parameters with clear physical interpretation. The angular momentum reference trajectory is generated in three consecutive phases. In the first phase, based on the acting contact forces and torques, a required angular momentum is calculated in order to keep the "Center of Pressure (CoP)" within the "support area". If the CoP reaches the edges of the support surface, this can cause the humanoid to fall over, which must be prevented in the first phase. The first phase is activated as soon as a predetermined limit as a function of the maximum permissible contact torque is exceeded. The first phase can be activated independently of task conflicts. The associated setting parameter can be used to ensure that angular momentum is generated before the contact conditions are reached. In the second phase, the induced angular momentum has to be reduced again. This must not be too abrupt, since the first time derivative of angular momentum is directly related to the deflection of the CoP.

According to the invention, this problem can be solved by using a third order polynomial and adjusting the duration of phase 2 based on the maximum angular momentum generated in phase 1. This ensures angular momentum dissipation without requiring joint velocities and accelerations that could potentially exceed position and/or velocity limitations in the joints. During the 3rd phase, the angular momentum reference is permanently zero. A motion optimizer, which has the angular momentum reference as input and generates kinematically and dynamically convertible joint trajectories, ensures that the robot converges to a specified reference pose and only a small angular momentum with the opposite sign, which is induced in phases 1 and 2. A change to phase 1 is possible at any time if another impact is detected.

A motion optimizer generates kinematic and dynamic whole-body motion based on the angular momentum reference trajectory. The optimization-based approach enables the flexible weighting of the contributions from different body segments to the resulting angular momentum, which can be adjusted at runtime. Different contact configurations can also be selected at runtime (e.g. balancing on one or both legs).

This section gives an overview of the dynamic model of the robot, which is used in the method according to the invention and forms the basis of the calculations.

wird aus der Abweichung der aktuellen Position und/oder Geschwindigkeit von der Soll-Position und/oder -Geschwindigkeit des Roboters abgeleitet bzw. daraus berechnet. Eine unmittelbare Messung der auf den Roboter einwirkenden Stoßkräfte beispielsweise mittels Drucksensoren ist somit erfindungsgemäß nicht notwendig. In dem Fall, dass ${\tau }_v^{ankle}>{\tau }^{thres}$

werden die Phasen aktiviert.According to the invention, the contact moments τ _v in the support surface of the robot can be continuously monitored as externally acting contact moments τ _ext . The value ${\tau }_v^{ankle}$

is derived or calculated from the deviation of the current position and/or speed from the target position and/or speed of the robot. A direct measurement of the impact forces acting on the robot, for example by means of pressure sensors, is therefore not necessary according to the invention. In the event that ${\tau }_v^{ankle}>{\tau }^{tH\mathrm{right}es}$

the phases are activated.

According to the invention, the support surface is formed on a planar base by the convex polygon of at least one contact surface of the robot with the base. The at least one contact surface can be, for example, the support surfaces of the robot that rest on the base.

A. Dynamic model

gestapelt werden. Die Gesamtzahl der Freiheitsgrade (DoF) des Systems wird durch $\overline{n}=n+6$

bezeichnet. Die Dynamik des Systems kann formuliert werden wie folgt: $$M\left(\begin{array}{c}{\dot{\nu }}_c\\ \ddot{q}\end{array}\right)+C\left(\begin{array}{c}{\nu }_c\\ \dot{q}\end{array}\right)+\left(\begin{array}{c}-w_g\\ 0\end{array}\right)=\left(\begin{array}{c}0\\ \tau \end{array}\right)+{\tau }_{ext},$$

wobei $M\in {ℝ}^{\overline{n}\times \overline{n}}$

und $C\in {ℝ}^{\overline{n}\times \overline{n}}$

die positive definite Trägheit bzw. die Coriolis-Matrix sind. Die Schwerkraftauslenkung wird durch $w_g={\left(m{g}_0^T{0}^T\right)}^T$

dargestellt, wobei m die Gesamtmasse des Roboters bezeichnet und g₀ ∈ ℝ³ der Vektor der Erdbeschleunigung ist. Die Gelenkpositionen werden durch q ∈ ℝⁿ dargestellt, und die entsprechenden Gelenkdrehmomente sind τ ∈ ℝⁿ. Die Variable ${\tau }_{ext}\in {ℝ}^{\overline{n}}$

steht für die generalisierten externen Kräfte, die auf das System einwirken. Um eine Modelldarstellung zu erhalten, die für eine Ausbalancierungssteuerung geeignet ist, werden die Gelenkkoordinaten beider Füße durch ihre kartesischen Koordinaten ersetzt. Die folgende Aufgabe Jacobian $J\in {ℝ}^{\overline{n}\times \overline{n}}$

leistet das Mapping auf Aufgabenraumgeschwindigkeiten R6 mit $i\text{ G}\dot{\text{x}}\in {ℝ}^{\overline{n}}$

: $$\underset{\dot{x}}{\underbrace{\left(\begin{array}{c}{\nu }_c\\ \nu \\ {\dot{q}}_{ƒ}\end{array}\right)}}=\underset{J}{\underbrace{\left[\begin{array}{cc}I& 0\\ Ad& J\text{'}\\ 0& S_{ƒ}\end{array}\right]}}\left(\begin{array}{c}{\nu }_c\\ \dot{q}\end{array}\right),$$

wo ${\nu }_i={\left({\dot{x}}_i^T{\omega }_i^T\right)}^T\in {ℝ}^6,$

wobei i ∈ {r, l} die Translations- bzw. Rotationsgeschwindigkeit des linken und des rechten Fußes ist, die zu dem Geschwindigkeitsvektor $\nu ={\left({\nu }_r^T{\nu }_l^T\right)}^T$

gestapelt sind. Die verbliebenen freien DoFs q̇_ƒ ∈ ℝ^n-12 sind im Gelenkraum definiert, wobei S_ƒ ∈ ℝ^(n-12)×n die entsprechenden Gelenke des Gesamtgelenkvektors auswählt. Die gestapelten adjungierten Matrizes für die Beine werden durch Ad ∈ ℝ^12×6 bezeichnet, und J' ∈ ℝ^12×n sind die jeweiligen gestapelten Jacobi Matrizen.A sliding foot dynamics with n torque-controlled joints is used as a system model for a humanoid robot. Instead of the base coordinates, the position of the CoM x _c ∈ ℝ ³ is used together with the hip orientation R _b ∈ SO(3) and with the corresponding translational and rotational velocities ẋ _c ∈ ℝ ³ and ω _b , ∈ ℝ ³ , which correspond to the velocity vector $v_c={\left({\dot{x}}_c^T{\omega }_b^T\right)}^T$

be stacked. The total number of degrees of freedom (DoF) of the system is given by $\overline{n}=n+6$

designated. The dynamics of the system can be formulated as follows: $$M\left(\begin{array}{c}{\dot{v}}_c\\ \ddot{q}\end{array}\right)+C\left(\begin{array}{c}{v}_c\\ \dot{q}\end{array}\right)+\left(\begin{array}{c}-w_G\\ 0\end{array}\right)=\left(\begin{array}{c}0\\ \tau \end{array}\right)+{\tau }_{ext},$$

whereby $M\in {ℝ}^{\overline{n}\times \overline{n}}$

and $C\in {ℝ}^{\overline{n}\times \overline{n}}$

are the positive definite inertia and the Coriolis matrix, respectively. The gravity deflection is through $w_G={\left(m{G}_0^T{0}^T\right)}^T$

where m denotes the total mass of the robot and g ₀ ∈ ℝ ³ is the vector of the acceleration due to gravity. The joint positions are represented by q ∈ ℝ ⁿ and the corresponding joint torques are τ ∈ ℝ ⁿ . The variable ${\tau }_{ext}\in {ℝ}^{\overline{n}}$

stands for the generalized external forces acting on the system. In order to obtain a model representation suitable for balance control, the joint coordinates of both feet are replaced with their Cartesian coordinates. The following task Jacobian $J\in {ℝ}^{\overline{n}\times \overline{n}}$

performs the mapping on task space speeds R6 with $i\text{G}\dot{\text{x}}\in {ℝ}^{\overline{n}}$

: $$\underset{\dot{x}}{\underbrace{\left(\begin{array}{c}{v}_c\\ v\\ {\dot{q}}_{ƒ}\end{array}\right)}}=\underset{J}{\underbrace{\left[\begin{array}{cc}I& 0\\ A\mathrm{i.e}& J\text{'}\\ 0& S_{ƒ}\end{array}\right]}}\left(\begin{array}{c}{v}_c\\ \dot{q}\end{array}\right),$$

Where $v_i={\left({\dot{x}}_i^T{\omega }_i^T\right)}^T\in {ℝ}^6,$

where i ∈ {r, l} is the translational and rotational velocities of the left and right feet, respectively, which correspond to the velocity vector $v={\left(v_{\mathrm{right}}^T{v}_l^T\right)}^T$

are stacked. The remaining free DoFs q̇ _ƒ ∈ ℝ ^n-12 are defined in the joint space, where S _ƒ ∈ ℝ ^(n-12)×n selects the corresponding joints of the total joint vector. The stacked adjoint matrices for the legs are denoted by Ad ∈ ℝ ^12×6 and J' ∈ ℝ ^12×n are the respective stacked Jacobian matrices.

wobei $A\in {ℝ}^{3\times \overline{n}}$

der Rotationsteil der Schwerpunkdrehimpulsmatrix (CMM) ist. Der Schwerpunktdrehimpuls wird in einem Frame dargestellt, der an das CoM angefügt ist und mit dem Trägheitsframe aliniert ist. Er kann auch als Funktion der Aufgabengeschwindigkeiten und eines transformierten CMM ausgedrückt werden, der mit A bezeichnet wird, unter Verwendung einer inversen Kinematik von (2). In dieser Arbeit ist J eine quadratische Matrix, von der angenommen wird, dass sie invertierbar ist; die Behandlung von Redundanz- und singulären Konfigurationen wird als Aufgabe späterer Arbeiten betrachtet. The centroid angular momentum l _c ∈ ℝ ³ depends linearly on the velocity vector $$l_c=A\left(\begin{array}{c}{v}_c\\ \dot{q}\end{array}\right)=\underset{A}{\underbrace{A{J}^{-1}}}\dot{x},$$

whereby $A\in {\mathrm{<figure-callout\; id="3"\; label="R"\; filenames="DE102021112485B3\_0205.png,DE102021112485B3\_0206.png"\; state="\{\{state\}\}">ℝ</figure-callout>}}^{3\times \overline{n}}$

is the rotation part of the centroid angular momentum matrix (CMM). The centroid angular momentum is represented in a frame attached to the CoM and aligned with the inertial frame. It can also be expressed as a function of task velocities and a transformed CMM, denoted A, using inverse kinematics of (2). In this work, J is a square matrix assumed to be invertible; the treatment of redundancy and singular configurations is considered a task for later work.

B. Whole-body control based on passivity

The derivation of the control in this section assumes that the robot balances with double the support surface, but the formulation can be extended to other contact configurations by applying several modifications.

wobei ^w _grf ∈ ℝ¹² die zusammengeführten Kontaktauslenkungen beider Füße bezeichnet und die Abweichung von den kommandierten Trajektorien durch $\mathrm{\Delta }\nu ={\nu }_c-{\nu }_c^{cmd}\text{ und }\mathrm{\Delta }\dot{q}=\dot{q}-{\dot{q}}^{cmd}$

wiedergegeben werden. Wobei es zu beachten gilt, dass die kommandierten Trajektorien im Aufgabenraum erzeugt werden und die entsprechenden kommandierten CoM- und Gelenkwerte über inverse Kinematik berechnet werden: $$\left(\begin{array}{c}{\nu }_c^{cmd}\\ {\dot{q}}^{cmd}\end{array}\right)=J^{-1}{\dot{x}}^{cmd}.$$

Inspired by PD+ control [26], the control loop behavior is formulated as follows: $$M\left(\begin{array}{c}\mathrm{\Delta }{\dot{v}}_c\\ \mathrm{\Delta }\ddot{q}\end{array}\right)+C\left(\begin{array}{c}\mathrm{\Delta }{v}_c\\ \mathrm{\Delta }\dot{q}\end{array}\right)={\tau }_{ext}-J^T\left(\begin{array}{c}{w}_c^{imp}\\ w_{G\mathrm{right}ƒ}\\ {\tau }_{ƒ}^{imp}\end{array}\right),$$

where ^w _grf ∈ ℝ ¹² denotes the combined contact deflections of both feet and the deviation from the commanded trajectories by $\mathrm{\Delta }v=v_c-v_c^{cm\mathrm{i.e}}\text{and}\mathrm{\Delta }\dot{q}=\dot{q}-{\dot{q}}^{cm\mathrm{i.e}}$

be played back. It should be noted that the commanded trajectories are generated in the task space and the corresponding commanded CoM and joint values are calculated via inverse kinematics: $$\left(\begin{array}{c}{v}_c^{cm\mathrm{i.e}}\\ {\dot{q}}^{cm\mathrm{i.e}}\end{array}\right)=J^{-1}{\dot{x}}^{cm\mathrm{i.e}}.$$

wobei die Linear- und Rotationssteifigkeitsmatrizes K_c > 0 und Σ_b > 0 sowie die Linear- und Rotationsdämpfungsmatrizes D_c > 0 und B_b > 0 symmetrisch und positiv definit sind. Die kartesische Ausrichtung der Hüfte wird von einer virtuellen Rotationsfeder ${\tau }_r\left({\mathrm{\Sigma }}_b,{\left(R_b^{cmd}\right)}^T{R}_b\right)$

gesteuert [20], während die Impedanz der Gelenkaufgabe durch $${\tau }_{ƒ}^{imp}=K_{ƒ}\left(q_{ƒ}-q_{ƒ}^{cmd}\right)+D_{ƒ}\left({\dot{q}}_{ƒ}-{\dot{q}}_{ƒ}^{cmd}\right),$$

realisiert wird, mit den positiv definiten, linearen Feder- und Dämpfer-Matrizen K_ƒ > 0 und D_ƒ > 0.The CoM-associated impedances are defined by $$w_c^{imp}=\left(\begin{array}{c}{K}_c\left(x_c-x_c^{cm\mathrm{i.e}}\right)+D_c\left({\dot{x}}_c-{\dot{x}}_c^{cm\mathrm{i.e}}\right)\\ {\tau }_{\mathrm{right}}\left({\mathrm{\Sigma }}_b,{\left(R_b^{cm\mathrm{i.e}}\right)}^T{R}_b\right)+B_b\left({\omega }_b-{\omega }_b^{cm\mathrm{i.e}}\right)\end{array}\right),$$

where the linear and rotational stiffness matrices K _c > 0 and Σ _b > 0 and the linear and rotational damping matrices D _c > 0 and B _b > 0 are symmetric and positive definite. The Cartesian alignment of the hip is driven by a virtual rotation spring ${\tau }_{\mathrm{right}}\left({\mathrm{\Sigma }}_b,{\left(R_b^{cm\mathrm{i.e}}\right)}^T{R}_b\right)$

controlled [20], while the impedance of the joint task through $${\tau }_{ƒ}^{imp}=K_{ƒ}\left(q_{ƒ}-q_{ƒ}^{cm\mathrm{i.e}}\right)+D_{ƒ}\left({\dot{q}}_{ƒ}-{\dot{q}}_{ƒ}^{cm\mathrm{i.e}}\right),$$

is realized with the positive definite, linear spring and damper matrices K _ƒ > 0 and D _ƒ > 0.

mit der CoM-projizierten Kraftwindung(wrench) auf der rechten Seite der Gleichung und der gewünschten Gesamt-CoM-Kraftwindung auf der rechten Seite. Demgemäß werden die Trägheits- und die Coriolis-Matrix in die oberen sechs Zeilen, welche die Schwerpunkdynamik beschreiben, und den unteren Teil, d.h. $M={\left[M_c^T,M_q^T\right]}^T\text{ und }C={\left[C_c^T,C_q^T\right]}^T,$

geteilt. Die Vorsteuerungs-Terme werden in $w_c^{ƒƒ}$

zusammengefasst. Es gilt zu beachten, dass die transponierte adjungierte Matrix, die auch als Kontaktkarte bezeichnet wird, einen vollen Rang von sechs aufweist, während die Größe von ^w _grf bei doppelter Stützfläche 12 ist. Um die Verteilung von w_grf in allen möglichen Konfigurationen zu bestimmen, wird ein beschränkter QP auf Basis von (8) unter Verwendung der folgenden Kontaktbeschränkungen formuliert: Kontaktunilateralität, Coulomb-Reibungsmodell, begrenzte Normalkraft, begrenztes Drehmoment auf der z-Achse und CoP-Beschränkungen. Nach Lösung der Auslenkungsverteilung werden die finalen Steuerungsdrehmomente wie folgt berechnet: $$\tau =M_q\left(\begin{array}{c}{\dot{\nu }}_c^{cmd}\\ {\ddot{q}}^{cmd}\end{array}\right)+C_q\left(\begin{array}{c}{\nu }_c^{cmd}\\ {\dot{q}}^{cmd}\end{array}\right)-{\left(J\text{'}\right)}^T{w}_{grƒ}-S_{ƒ}^T{\tau }_{ƒ}^{imp}.$$

Comparing the system dynamics (1) and the desired control loop behavior (4) considering only the top six rows gives the following equation $$A{\mathrm{i.e}}^T{w}_{G\mathrm{right}ƒ}=\underset{w_c^{ƒƒ}}{\underbrace{M_c\left(\begin{array}{c}{\dot{v}}_c^{cm\mathrm{i.e}}\\ {\ddot{q}}^{cm\mathrm{i.e}}\end{array}\right)+C_c\left(\begin{array}{c}{v}_c^{cm\mathrm{i.e}}\\ {\dot{q}}^{cm\mathrm{i.e}}\end{array}\right)}}-w_G+w_c^{imp},$$

with the CoM projected force-turn (wrench) on the right side of the equation and the desired total CoM force-turn on the right. Accordingly, the inertial and Coriolis matrices are divided into the top six rows, which describe the centroid dynamics, and the bottom part, ie $M={\left[M_c^T,M_q^T\right]}^T\text{and}C={\left[C_c^T,C_q^T\right]}^T,$

divided. The feedforward terms are in $w_c^{ƒƒ}$

summarized. Note that the transposed adjoint matrix, also known as the contact map, has a full rank of six, while the size of ^w _grf is 12 with double the support area. To determine the distribution of w _grf in all possible configurations, a bounded QP is formulated based on (8) using the following contact constraints: contact unilaterality, Coulomb friction model, bounded normal force, bounded z-axis torque, and CoP constraints . After solving the deflection distribution, the final control torques are calculated as follows: $$\tau =M_q\left(\begin{array}{c}{\dot{v}}_c^{cm\mathrm{i.e}}\\ {\ddot{q}}^{cm\mathrm{i.e}}\end{array}\right)+C_q\left(\begin{array}{c}{v}_c^{cm\mathrm{i.e}}\\ {\dot{q}}^{cm\mathrm{i.e}}\end{array}\right)-{\left(J\text{'}\right)}^T{w}_{G\mathrm{right}ƒ}-S_{ƒ}^T{\tau }_{ƒ}^{imp}.$$

Method steps a) and b) are preferably carried out in parallel and continuously during the execution of steps c1) to c3), if at least one predetermined limit value is exceeded in step b), the currently running execution of steps c1) to c3) is aborted and steps c1) to c3) executed again.

• d1) Aufteilen der Gesamtanzahl der n Freiheitsgrade im Aufgabenraum in: k zu beeinflussende Freiheitsgrade mit den zugehörigen Aufgabenvariablen x_a sowie in n — k unveränderte Freiheitsgrade mit den zugehörigen Aufgabenvariablen x_u, mit $k\in \left\{\mathrm{0,}\dots ,\overline{n}\right\};$
  
• d2) Bestimmen der optimierten Geschwindigkeiten unter Erfüllung der Beziehung: $$l_c^{reƒ}={\overline{A}}_a{\dot{x}}_a^{opt}+{\overline{A}}_u{\dot{x}}_u^{reƒ},$$
  
• d3) Lösen der Beziehung in d2) durch Formulieren eines quadratischen Optimierungsproblems mit Nebenbedingungen zu: $$\underset{{\dot{x}}_a^{opt}}{\text{min}}\left(\frac{1}{2}{\delta }_l^T{Q}_l{\delta }_l+\frac{1}{2}{\delta }_p^T{Q}_p{\delta }_p\right),$$
  

wobei Q_L und Q_R > 0 Gewichtungsmatrizen darstellen, welche beide positiv definit und symmetrisch sind mit den Residuen: $${\delta }_l={\overline{A}}_a{\dot{x}}_a^{opt}+{\overline{A}}_u{\dot{x}}_u^{reƒ}-l_c^{reƒ},$$

$${\delta }_p={\dot{x}}_a^{opt}-{\dot{x}}_a^d,$$

unter Beachtung der Randbedingungen: $${\dot{x}}_a^{min}\le {\dot{x}}_a^{opt}\le {\dot{x}}_a^{max}.$$

wobei die gewünschte Geschwindigkeit ${\dot{x}}_a^d$

basierend auf der Abweichung der optimierten Position von ihrer Referenzposition $x_a^{reƒ}$

berechnet wird: $${\dot{x}}_a^d={\dot{x}}_a^{reƒ}+K_p\left(x_a^{reƒ}-x_a^{opt}\right),$$

wobei Referenzgeschwindigkeit ${\dot{x}}_a^{reƒ}$

für die zu optimierenden Variablen und die zugehörige Referenzposition $x_a^{reƒ}$

als Eingabegrößen bekannt sind, wobei die Konvergenz des Roboters zu der Referenzpose durch die Designparameter K_p > 0 beeinflussbar ist, welcher diagonal und positiv definit gewählt wird mit den Positionsbegrenzungen $\left({\underset{\_}{x}}_a,{\overline{x}}_a\right),$

und den Geschwindigkeitsbegrenzungen $\left({\dot{x}}_a,{\overline{\dot{x}}}_a\right),$

$$\begin{array}{l}{\dot{x}}_a^{min}=\text{max}\left(K_a\left({\underset{\_}{x}}_a-x_a^{opt}\right),{\underset{\_}{\dot{x}}}_a\right),\hfill \\ {\dot{x}}_a^{max}=\text{min}\left(K_a\left({\overline{x}}_a-x_a^{opt}\right),{\overline{\dot{x}}}_a\right).\hfill \end{array}$$

mit K_a > 0, wobei K_a diagonal und positiv definit ist;

• d4) Berechnen der optimierten Position $x_a^{opt}$
  
  durch Integrieren der optimierten Geschwindigkeiten ${\dot{x}}_a^{opt}$
  
  über die Zeit; sowie
• d5) Zusammensetzen der optimierten Position $x_a^{opt}$
  
  für die veränderlichen Freiheitsgrade mit der Referenzposition $x_u^{reƒ}$
  
  der unveränderlichen Freiheitsgrade zu ${\left(x^{cmd}\right)}^T=\left({\left(x_a^{opt}\right)}^T,{\left(x_u^{reƒ}\right)}^T\right)$
  
  und als Ganzkörperbewegung.

According to the invention, it can also be provided that method step d) comprises the following steps:

• d1) Dividing the total number of n Degrees of freedom in the task space in: k degrees of freedom to be influenced with the associated task variables x _a and in n — k unchanged degrees of freedom with the associated task variables x _u , with $k\in \left\{\mathrm{0,}...,\overline{n}\right\};$
  
• d2) determining the optimized speeds satisfying the relationship: $$l_c^{\mathrm{right}eƒ}={\overline{A}}_a{\dot{x}}_a^{Opt}+{\overline{A}}_{\mathrm{and}}{\dot{x}}_{\mathrm{and}}^{\mathrm{right}eƒ},$$
  
• d3) Solving the relation in d2) by formulating a quadratic optimization problem with constraints to: $$\underset{{\dot{x}}_a^{Opt}}{\text{at least}}\left(\frac{1}{2}{\delta }_l^T{Q}_l{\delta }_l+\frac{1}{2}{\delta }_p^T{Q}_p{\delta }_p\right),$$
  

where Q _L and Q _R > 0 represent weight matrices, both of which are positive definite and symmetric with the residuals: $${\delta }_l={\overline{A}}_a{\dot{x}}_a^{Opt}+{\overline{A}}_{\mathrm{and}}{\dot{x}}_{\mathrm{and}}^{\mathrm{right}eƒ}-l_c^{\mathrm{right}eƒ},$$

$${\delta }_p={\dot{x}}_a^{Opt}-{\dot{x}}_a^{\mathrm{i.e}},$$

taking into account the boundary conditions: $${\dot{x}}_a^{min}\le {\dot{x}}_a^{Opt}\le {\dot{x}}_a^{max}.$$

where the desired speed ${\dot{x}}_a^{\mathrm{i.e}}$

based on the deviation of the optimized position from its reference position $x_a^{\mathrm{right}eƒ}$

is calculated: $${\dot{x}}_a^{\mathrm{i.e}}={\dot{x}}_a^{\mathrm{right}eƒ}+K_p\left(x_a^{\mathrm{right}eƒ}-x_a^{Opt}\right),$$

where reference speed ${\dot{x}}_a^{\mathrm{right}eƒ}$

for the variables to be optimized and the associated reference position $x_a^{\mathrm{right}eƒ}$

are known as input variables, where the convergence of the robot to the reference pose can be influenced by the design parameters K _p > 0, which is chosen to be diagonal and positive definite with the pose constraints $\left({\underset{\_}{x}}_a,{\overline{x}}_a\right),$

and the speed limits $\left({\dot{x}}_a,{\overline{\dot{x}}}_a\right),$

$$\begin{array}{l}{\dot{x}}_a^{min}=\text{Max}\left(K_a\left({\underset{\_}{x}}_a-x_a^{Opt}\right),{\underset{\_}{\dot{x}}}_a\right),\hfill \\ {\dot{x}}_a^{max}=\text{at least}\left(K_a\left({\overline{x}}_a-x_a^{Opt}\right),{\overline{\dot{x}}}_a\right).\hfill \end{array}$$

with K _a > 0, where K _a is diagonal and positive definite;

• d4) Calculating the optimized position $x_a^{Opt}$
  
  by integrating the optimized speeds ${\dot{x}}_a^{Opt}$
  
  over time; such as
• d5) assembling the optimized position $x_a^{Opt}$
  
  for the variable degrees of freedom with the reference position $x_{\mathrm{and}}^{\mathrm{right}eƒ}$
  
  of the invariable degrees of freedom ${\left(x^{cm\mathrm{i.e}}\right)}^T=\left({\left(x_a^{Opt}\right)}^T,{\left(x_{\mathrm{and}}^{\mathrm{right}eƒ}\right)}^T\right)$
  
  and as a whole body movement.

A. Whole Body Movement Optimization

The entire task space is expressed in k degrees of freedom DoFs that are within the motion opti mierers to be adjusted, and the rest n — split k DoFs that remain unchanged, where $k\in \left\{\mathrm{0,}...,\overline{n}\right\}.$

$${\dot{x}}^T=\left(\underset{{\dot{x}}_a^T\in {ℝ}^k}{\underbrace{{\dot{x}}_1\cdots {\dot{x}}_k}}\underset{{\dot{x}}_u^T\in {ℝ}^{\left(n-k\right)}}{\underbrace{{\dot{x}}_{k+1}\cdots {\dot{x}}_n}}\right),$$

wobei die Indizes a und u angepasste bzw. unveränderte DoFs bezeichnen. Die Optimierungsvariablen können online und abhängig von der Kontaktkonfiguration und dem Planungsziel ausgewählt werden. Wenn beispielsweise nur die Geschwindigkeiten der Gelenkaufgabe in Bezug auf ein Schwerpunktdrehimpuls-Ziel angepasst werden sollen, wird der Aufgabenraum-Geschwindigkeitsvektor wie folgt zugewiesen: ${\dot{x}}_a={\dot{q}}_{ƒ}$

und ${\dot{x}}_u={\left({\nu }_c^T{\nu }^T\right)}^T.$

Accordingly, the transformed CMM and the task space velocity vector (3) can be partitioned as $$\overline{A}=\left[\underset{{\overline{A}}_a\in {ℝ}^{3\times k}}{\underbrace{{\overline{A}}_1\cdots {\overline{A}}_k}}\underset{{\overline{A}}_{\mathrm{and}}\in {ℝ}^{3\times \left(\overline{n}-k\right)}}{\underbrace{{\overline{A}}_{k+1}\cdots {\overline{A}}_{\overline{n}}}}\right],$$

$${\dot{x}}^T=\left(\underset{{\dot{x}}_a^T\in {ℝ}^k}{\underbrace{{\dot{x}}_1\cdots {\dot{x}}_k}}\underset{{\dot{x}}_{\mathrm{and}}^T\in {ℝ}^{\left(n-k\right)}}{\underbrace{{\dot{x}}_{k+1}\cdots {\dot{x}}_n}}\right),$$

where the indices a and u denote adjusted and unmodified DoFs, respectively. The optimization variables can be selected online and depending on the contact configuration and planning objective. For example, if only the joint task velocities are to be adjusted with respect to a centroid angular momentum target, the task space velocity vector is assigned as follows: ${\dot{x}}_a={\dot{q}}_{ƒ}$

and ${\dot{x}}_{\mathrm{and}}={\left(v_c^T{v}^T\right)}^T.$

für k ausgewählten DoFs zu finden, um einen vordefinierten Referenz-Schwerpunktdrehimpuls $l_c^{reƒ}$

für das System zu induzieren. Wie in 1 gezeigt ist, wird der Referenz-Schwerpunktdrehimpuls aus dem Schwerpunktdrehimpuls-Referenzwerterzeuger erhalten, der in den Abschnitten III-B und III-C vorgestellt wird. Durch Umformulieren von (3) werden die optimierten Geschwindigkeiten so bestimmt, dass sie folgendes erfüllen: $$l_c^{reƒ}={\overline{A}}_a{\dot{x}}_a^{opt}+{\overline{A}}_u{\dot{x}}_u^{reƒ},$$

mit den Residuen $${\delta }_l={\overline{A}}_a{\dot{x}}_a^{opt}+{\overline{A}}_u{\dot{x}}_u^{reƒ}-l_c^{reƒ},$$

$${\delta }_p={\dot{x}}_a^{opt}-{\dot{x}}_a^d,$$

und vorbehaltlich der Beschränkungen $${\dot{x}}_a^{min}\le {\dot{x}}_a^{opt}\le {\dot{x}}_a^{max}.$$

The goal is optimized speeds ${\dot{x}}_a^{Opt}$

for k selected DoFs to find a predefined reference centroid angular momentum $l_c^{\mathrm{right}eƒ}$

for the system to induce. As in 1 As shown, the reference centroid angular momentum is obtained from the centroid angular momentum reference generator presented in Sections III-B and III-C. Rephrasing (3), the optimized velocities are determined to satisfy: $$l_c^{\mathrm{right}eƒ}={\overline{A}}_a{\dot{x}}_a^{Opt}+{\overline{A}}_{\mathrm{and}}{\dot{x}}_{\mathrm{and}}^{\mathrm{right}eƒ},$$

with the residuals $${\delta }_l={\overline{A}}_a{\dot{x}}_a^{Opt}+{\overline{A}}_{\mathrm{and}}{\dot{x}}_{\mathrm{and}}^{\mathrm{right}eƒ}-l_c^{\mathrm{right}eƒ},$$

$${\delta }_p={\dot{x}}_a^{Opt}-{\dot{x}}_a^{\mathrm{i.e}},$$

and subject to the restrictions $${\dot{x}}_a^{min}\le {\dot{x}}_a^{Opt}\le {\dot{x}}_a^{max}.$$

in (14b) wird auf Basis der Abweichung der optimierten Haltung von ihrem Referenzwert $${\dot{x}}_a^d={\dot{x}}_a^{reƒ}+K_p\left(x_a^{reƒ}-x_a^{opt}\right),$$

berechnet, wobei die Referenzgeschwindigkeit für die Optimierungsvariablen ${\dot{x}}_a^{reƒ}$

und die entsprechende Position $x_a^{reƒ}$

von dem Trajektorieplaner bereitgestellt werden. Die optimierte Position ${\underset{\_}{x}}_a^{opt}$

wird erhalten aus ${\dot{x}}_a^{opt}$

durch Integration in Bezug auf die Zeit. Das Konvergenzverhalten des Roboters in Richtung auf seine Referenzpose kann durch den Designparameter Kp > 0 beeinflusst werden, der diagonal und positiv definit ist.The desired speed ${\dot{x}}_a^{\mathrm{i.e}}$

in (14b) is based on the deviation of the optimized posture from its reference value $${\dot{x}}_a^{\mathrm{i.e}}={\dot{x}}_a^{\mathrm{right}eƒ}+K_p\left(x_a^{\mathrm{right}eƒ}-x_a^{Opt}\right),$$

calculated using the reference speed for the optimization variables ${\dot{x}}_a^{\mathrm{right}eƒ}$

and the corresponding position $x_a^{\mathrm{right}eƒ}$

be provided by the trajectory planner. The optimized position ${\underset{\_}{x}}_a^{Opt}$

will get off ${\dot{x}}_a^{Opt}$

by integration with respect to time. The convergence behavior of the robot towards its reference pose can be influenced by the design parameter Kp > 0, which is diagonal and positive definite.

Die Kombination eines Drehimpulses und einer Haltungsaufgabe wird gewählt, weil eine Optimierung für nur den Schwerpunktdrehimpuls (14a) zu ungünstigen Körperhaltungen führen kann. Die Referenzhaltung, die von dem Trajektorieplaner bereitgestellt wird, wird auf Basis von Zielen hochrangiger Aufgaben definiert.The QP finds a compromise between an angular momentum task (14a) and a reference posture task (14b). The non-strict priorities between the two tasks can be adjusted based on the choice of the weight matrices Q _l and Q ^p > 0, both of which are positive definite and symmetric. A non-strict task hierarchy is necessary to allow the robot to approach its reference pose quite closely, even if $l_c^{\mathrm{right}eƒ}\equiv 0$

The combination of an angular momentum and a posture task is chosen because an optimization for only the center of gravity angular momentum (14a) can lead to unfavorable body postures. The reference attitude provided by the trajectory planner is defined based on goals of high-level tasks.

und Geschwindigkeitsbegrenzungen $\left({\underset{\_}{\dot{x}}}_a,{\overline{\dot{x}}}_a\right)$

nicht überschritten werden $$\begin{array}{l}{\dot{x}}_a^{min}=\text{max}\left(K_a\left({\underset{\_}{x}}_a-x_a^{opt}\right),{\underset{\_}{\dot{x}}}_a\right),\hfill \\ {\dot{x}}_a^{max}=\text{min}\left(K_a\left({\overline{x}}_a-x_a^{opt}\right),{\overline{\dot{x}}}_a\right),\hfill \end{array}$$

wobei Ka > 0 diagonal und positiv definit ist. Größere Werte von Ka führen zu einem breiteren Bereich, wo die Optimierung (13) unbeeinflusst bleibt, aber sie verlangen auch höhere Beschleunigungen in der Nachbarschaft der Beschränkung. Diese Beschränkungen können auf eine Eigenkollisionsvermeidung oder Singular-Konfigurationsverhinderung auf kartesischer Basis ausgeweitet werden. Auf Basis der optimierten Geschwindigkeiten werden die entsprechenden kommandierten Ganzkörpertrajektorien durch numerische Differenzierung und Integration erzeugt und schließlich mit den unveränderten DoFs zusammengeführt.The kinematic constraints in task space (15) ensure that predefined position constraints $\left({\underset{\_}{x}}_a,{\overline{x}}_a\right)$

and speed limits $\left({\underset{\_}{\dot{x}}}_a,{\overline{\dot{x}}}_a\right)$

not be exceeded $$\begin{array}{l}{\dot{x}}_a^{min}=\text{Max}\left(K_a\left({\underset{\_}{x}}_a-x_a^{Opt}\right),{\underset{\_}{\dot{x}}}_a\right),\hfill \\ {\dot{x}}_a^{max}=\text{at least}\left(K_a\left({\overline{x}}_a-x_a^{Opt}\right),{\overline{\dot{x}}}_a\right),\hfill \end{array}$$

where Ka > 0 is diagonal and positive definite. Larger values of Ka lead to a wider range where the optimization (13) remains unaffected, but they also require higher accelerations in the vicinity of the constraint. These constraints can be extended to Cartesian-based intrinsic collision avoidance or singular configuration avoidance. Based on the optimized velocities, the corresponding commanded whole-body trajectories are generated by numerical differentiation and integration and finally merged with the unchanged DoFs.

• a1) bei dem Vorliegen einer Mehrzahl von Stützflächen, Ersetzen der Mehrzahl von Stützflächen durch eine einzige virtuelle Aufstützfläche, deren Mittelpunkt x_v durch den Mittelpunkt der mehreren Stützflächen gebildet wird und deren Fläche, welche der Summe der mehreren Stützflächen entspricht,
• a2) Berechnen des Kontaktdrehmomentes τ_v ∈ ℝ³ der virtuellen Aufstützfläche zu: $${\tau }_v=\underset{A{d}_{v,rot}^{-T}}{\underbrace{\left[{\widehat{x}}_{v,c}I\right]}}\left(w_c^{ƒƒ}-w_g+w_c^{imp}\right),$$
  
  wobei x̂_v,c die Kreuzproduktmatrix des Vektors x_v,c = x_c - x_v zwischen der Schwerpunktsposition x_c und der Position der Aufstützfläche x_v ist, wobei der Rotationsteil der invertierten adjungierten Matrix der virtuellen Aufstützfläche, die durch Konstruktion einen vollen Rang aufweist, wird dargestellt durch $A{d}_{v,rot}^{-T};$
  
• a3) weiteres Aufteilen der Kontaktdrehmomente in Schritt a2) zu: $${\tau }_v=\underset{{\tau }_v^{hip}}{\underbrace{A{d}_{v,rot}^{-T}{w}_c^{ƒƒ}}}+\underset{{\tau }_v^{ankle}}{\underbrace{A{d}_{v,rot}^{-T}\left(-w_g+w_c^{imp}\right)}}.$$
  
• wobei ${\tau }_v^{hip}$
  
  die Kontaktdrehmomente darstellt, die durch eine gewollte Bewegung des Oberkörpers erzeugt werden, hier in Form der Vorwärtskopplungsterme, und daher einer Hüftstrategie zugeordnet ist. Die Kontaktdrehmomente, die durch Schwerkrafts- und Impedanzterme induziert werden, werden von ${\tau }_v^{ankle}$
  
  zusammengefasst, die als Fußgelenksstrategie betrachtet werden, da sie das resultierende Kontaktdrehmoment angeben, das vorhanden wäre, wenn eine feste Körperhaltung bewahrt werden würde, d.h. wenn $w_c^{ƒƒ}=0;$
  
• a4) Umformulieren der Vorsteuerungs-Terme in (8) die mit erweiterter Trägheits- und Coriolis-Matrix wie folgt umformuliert werden zu: $$\underset{w_c^{ƒƒ}}{\underbrace{\left(\begin{array}{c}{ƒ}_c^{ƒƒ}\\ {\tau }_b^{ƒƒ}\end{array}\right)}}=\underset{M_c}{\underbrace{\left[\begin{array}{ccc}mI& 0& 0\\ 0& M_{\omega \omega }& M_{\omega q}\end{array}\right]}}\left(\begin{array}{c}{\ddot{x}}_c^{cmd}\\ {\dot{\omega }}_b^{cmd}\\ {\ddot{q}}^{cmd}\end{array}\right)+\underset{C_c}{\underbrace{\left[\begin{array}{c}0\\ C_{\omega }\end{array}\right]}}\left(\begin{array}{c}{\dot{x}}_c^{cmd}\\ {\omega }_b^{cmd}\\ {\dot{q}}^{cmd}\end{array}\right).$$
  
  wobei die Vorsteuerungs-Kraft und das -Drehmoment ${ƒ}_c^{ƒƒ}$
  
  bzw. ${\tau }_b^{ƒƒ}$
  
  sind. Das Vorsteuerungs-Drehmoment kann als die Änderungsrate des Schwerpunktdrehimpulses interpretiert werden, der sich aus der kommandierten Trajektorie ergibt, die über inverse Kinematik (5) aus dem Aufgabenraum transformiert wird. Die kommandierte Schwerpunkts-Geschwindigkeit und -Beschleunigung, und daher auch ${ƒ}_c^{ƒƒ},$
  
  ist für Ausbalancierungsszenarios als null definiert. Infolgedessen wird das Hüft-Kontaktdrehmoment vereinfacht zu $${\tau }_v^{hip}={\tau }_b^{ƒƒ}=i_c^{cmd},$$
  
  wo $i_c^{cmd}$
  
  die Änderungsrate des kommandierten Schwerpunktdrehimpulses ist.

It can preferably be provided according to the invention that the following steps are additionally carried out in method step a):

• a1) in the presence of a plurality of support surfaces, replacing the plurality of support surfaces with a single virtual support surface, whose center point x _v is formed by the center point of the multiple support surfaces and whose area corresponds to the sum of the multiple support surfaces,
• a2) Calculation of the contact torque τ _v ∈ ℝ ³ of the virtual contact surface to: $${\tau }_v=\underset{A{\mathrm{i.e}}_{v,\mathrm{right}Ot}^{-T}}{\underbrace{\left[{\widehat{x}}_{v,c}I\right]}}\left(w_c^{ƒƒ}-w_G+w_c^{imp}\right),$$
  
  where x̂ _v,c is the cross product matrix of the vector x _v,c = x _c - x _v between the centroid position x _c and the bearing surface position x _v , where the rotation part of the inverted adjoint matrix of the virtual bearing surface, which by construction has a full rank has is represented by $A{\mathrm{i.e}}_{v,\mathrm{right}Ot}^{-T};$
  
• a3) further splitting of the contact torques in step a2) to: $${\tau }_v=\underset{{\tau }_v^{Hip}}{\underbrace{A{\mathrm{i.e}}_{v,\mathrm{right}Ot}^{-T}{w}_c^{ƒƒ}}}+\underset{{\tau }_v^{ankle}}{\underbrace{A{\mathrm{i.e}}_{v,\mathrm{right}Ot}^{-T}\left(-w_G+w_c^{imp}\right)}}.$$
  
• whereby ${\tau }_v^{Hip}$
  
  which represents contact torques generated by voluntary movement of the upper body, here in terms of the feedforward terms, and is therefore associated with a hip strategy. The contact torques induced by gravity and impedance terms are accounted for by ${\tau }_v^{ankle}$
  
  summarized, which are considered an ankle strategy because they indicate the resultant contact torque that would be present if a fixed body posture were maintained, i.e. if $w_c^{ƒƒ}=0;$
  
• a4) Rewrite the feedforward terms in (8) which, with the extended inertia and Coriolis matrix, are rewritten as follows: $$\underset{w_c^{ƒƒ}}{\underbrace{\left(\begin{array}{c}{ƒ}_c^{ƒƒ}\\ {\tau }_b^{ƒƒ}\end{array}\right)}}=\underset{M_c}{\underbrace{\left[\begin{array}{ccc}mI& 0& 0\\ 0& M_{\omega \omega }& M_{\omega q}\end{array}\right]}}\left(\begin{array}{c}{\ddot{x}}_c^{cm\mathrm{i.e}}\\ {\dot{\omega }}_b^{cm\mathrm{i.e}}\\ {\ddot{q}}^{cm\mathrm{i.e}}\end{array}\right)+\underset{C_c}{\underbrace{\left[\begin{array}{c}0\\ C_{\omega }\end{array}\right]}}\left(\begin{array}{c}{\dot{x}}_c^{cm\mathrm{i.e}}\\ {\omega }_b^{cm\mathrm{i.e}}\\ {\dot{q}}^{cm\mathrm{i.e}}\end{array}\right).$$
  
  where the pilot force and torque ${ƒ}_c^{ƒƒ}$
  
  or. ${\tau }_b^{ƒƒ}$
  
  are. The feedforward torque can be interpreted as the rate of change of the centroid angular momentum resulting from the commanded trajectory transformed from the task space via inverse kinematics (5). The commanded center of gravity velocity and acceleration, and hence also ${ƒ}_c^{ƒƒ},$
  
  is defined as zero for balancing scenarios. As a result, the hip contact torque is simplified to $${\tau }_v^{Hip}={\tau }_b^{ƒƒ}=i_c^{cm\mathrm{i.e}},$$
  
  Where $i_c^{cm\mathrm{i.e}}$
  
  is the rate of change of commanded centroid angular momentum.

B. Hip and ankle strategy in the context of passivity-based whole-body control

Daher wird die Ganzkörperbewegung, die sich aus einem Stoß von außen ergibt, nur durch das erfindungsgemäße Verfahren erzeugt. Da die Kontaktauslenkungen die Kontaktbeschränkungen einhalten müssen, existieren ein oberer und ein unterer Grenzwert für die maximale erzeugbare CoM-Auslenkung. Genauer werden in dem Fall, wo Störungen aufgrund von Stößen vorkommen, die Kontaktauslenkungsbeschränkungen leicht erreicht, d.h. der CoP nähert sich dem Rand der Abstützfläche, und es kann zu einer Drehung oder Kippung des Fußes kommen, die bewirkt, dass der Roboter hinfällt. Um dies zu verhindern, muss durch aktives Erzeugen eines Schwerpunktdrehimpulses, der den vorhandenen Kontaktauslenkungen entgegenwirkt, ein zusätzliches Drehmoment um das CoM induziert werden. Als Folge davon können gewünschte CoM-Auslenkungen mit größeren Werten erzeugt werden, d.h. es können stärkere Stöße ausgeglichen werden.For balancing scenarios, the high-level planner generates trajectories to maintain a fixed attitude, ie that the reference values at the velocity and acceleration levels are zero, ${\dot{x}}^{\mathrm{right}ef}={\ddot{x}}^{\mathrm{right}ef}=0$

Therefore, the whole body movement resulting from an external impact is generated only by the method of the present invention. Since the contact deflections must comply with the contact restrictions, there is an upper and a lower limit for the maximum CoM deflection that can be generated. More specifically, in the case where disturbances occur due to impacts, the contact deflection limitations are easily reached, ie the CoP approaches the edge of the support surface, and foot rotation or tilting may occur, causing the robot to fall. To prevent this, an additional torque must be induced around the CoM by actively generating a centroid angular momentum that counteracts the existing contact deflections. As a result, desired CoM deflections can be generated with larger values, ie stronger shocks can be compensated.

oder der Fußposition des Standfußes bei einfacher Stützung gleich ist. Der virtuelle Standfuß hat eine Größe, die der Stützfläche des Roboters gleich ist. Durch Anwendung dieser Vereinfachung und Umformulierung (8) wird das Kontaktdrehmoment des virtuellen Standfußes τ_v ∈ ℝ³ erhalten als $${\tau }_v=\underset{A{d}_{v,rot}^{-T}}{\underbrace{\left[{\widehat{x}}_{v,c}I\right]}}\left(w_c^{ƒƒ}-w_g+w_c^{imp}\right),$$

wo x̂_v,c die Kreuzproduktmatrix des Vektors x_v,_c = x_c - x_v zwischen dem CoM und der Position des virtuellen Standfußes ist. Der Rotationsteil der invertierten adjungierten Matrix des virtuellen Standfußes, die durch Konstruktion einen vollen Rang aufweist, wird dargestellt durch $A{d}_{v,rot}^{-T}.$

Die Kontaktdrehmomente in (18) können weiter aufgeteilt werden wie folgt: $${\tau }_v=\underset{{\tau }_v^{hip}}{\underbrace{A{d}_{v,rot}^{-T}{w}_c^{ƒƒ}}}+\underset{{\tau }_v^{ankle}}{\underbrace{A{d}_{v,rot}^{-T}\left(-w_g+w_c^{imp}\right)}}.$$

wo ${\tau }_v^{hip}$

die Kontaktdrehmomente darstellt, die durch eine gewollte Bewegung des Oberkörpers erzeugt werden, hier in Form der Vorwärtskopplungsterme, und daher einer Hüftstrategie zugeordnet ist. Die Kontaktdrehmomente, die durch Schwerkrafts- und Impedanzterme induziert werden, werden von ${\tau }_v^{ankle}$

zusammengefasst, die als Fußgelenksstrategie betrachtet werden, da sie das resultierende Kontaktdrehmoment angeben, das vorhanden wäre, wenn eine feste Körperhaltung bewahrt werden würde, d.h. wenn $w_c^{ƒƒ}=0.$

The reference centroid angular momentum is calculated based on the required contact torques. To circumvent the displacement distribution problem and make the adjoint matrix (8) invertible, the contact torques are calculated for a single virtual foot whose center is between the right and left foot position under double support, ie $x_v=\frac{1}{2}\left(x_{\mathrm{right}}+x_l\right),$

or equal to the foot position of the stand in simple support. The virtual foot has a size equal to the robot's support surface. Applying this simplification and reformulation (8), the contact torque of the virtual foot τ _v ∈ ℝ ³ is obtained as $${\tau }_v=\underset{A{\mathrm{i.e}}_{v,\mathrm{right}Ot}^{-T}}{\underbrace{\left[{\widehat{x}}_{v,c}I\right]}}\left(w_c^{ƒƒ}-w_G+w_c^{imp}\right),$$

where x̂ _v,c is the cross product matrix of the vector x _v , _c = x _c - x _v between the CoM and the position of the virtual stand. The rotation part of the inverted adjoint matrix of the virtual pedestal, which is full rank by construction, is represented by $A{\mathrm{i.e}}_{v,\mathrm{right}Ot}^{-T}.$

The contact torques in (18) can be further broken down as follows: $${\tau }_v=\underset{{\tau }_v^{Hip}}{\underbrace{A{\mathrm{i.e}}_{v,\mathrm{right}Ot}^{-T}{w}_c^{ƒƒ}}}+\underset{{\tau }_v^{ankle}}{\underbrace{A{\mathrm{i.e}}_{v,\mathrm{right}Ot}^{-T}\left(-w_G+w_c^{imp}\right)}}.$$

Where ${\tau }_v^{Hip}$

which represents contact torques generated by voluntary movement of the upper body, here in terms of the feedforward terms, and is therefore associated with a hip strategy. The contact torques induced by gravity and impedance terms are accounted for by ${\tau }_v^{ankle}$

summarized, which are considered an ankle strategy because they indicate the resultant contact torque that would be present if a fixed body posture were maintained, i.e. if $w_c^{ƒƒ}=0$

wo die Vorsteuerungs-Kraft und das -Drehmoment ${ƒ}_c^{ƒƒ}$

bzw. ${\tau }_b^{ƒƒ}$

sind. Das Vorsteuerungs-Drehmoment kann als die Änderungsrate des Schwerpunktdrehimpulses interpretiert werden, der sich aus der kommandierten Trajektorie ergibt, die über inverse Kinematik (5) aus dem Aufgabenraum transformiert wird. Die kommandierte CoM-Geschwindigkeit und -Beschleunigung, und daher auch ${ƒ}_c^{ƒƒ},$

ist für Ausbalancierungsszenarios als null definiert. Infolgedessen wird das Hüft-Kontaktdrehmoment vereinfacht zu $${\tau }_v^{hip}={\tau }_b^{ƒƒ}=i_c^{cmd},$$

wo $i_c^{cmd}$

die Änderungsrate des kommandierten Schwerpunktdrehimpuls ist.The feedforward terms in (8) can be reformulated using extended inertia and Coriolis matrices as follows: $$\underset{w_c^{ƒƒ}}{\underbrace{\left(\begin{array}{c}{ƒ}_c^{ƒƒ}\\ {\tau }_b^{ƒƒ}\end{array}\right)}}=\underset{M_c}{\underbrace{\left[\begin{array}{ccc}mI& 0& 0\\ 0& M_{\omega \omega }& M_{\omega q}\end{array}\right]}}\left(\begin{array}{c}{\ddot{x}}_c^{cm\mathrm{i.e}}\\ {\dot{\omega }}_b^{cm\mathrm{i.e}}\\ {\ddot{q}}^{cm\mathrm{i.e}}\end{array}\right)+\underset{C_c}{\underbrace{\left[\begin{array}{c}0\\ C_{\omega }\end{array}\right]}}\left(\begin{array}{c}{\dot{x}}_c^{cm\mathrm{i.e}}\\ {\omega }_b^{cm\mathrm{i.e}}\\ {\dot{q}}^{cm\mathrm{i.e}}\end{array}\right).$$

where the pilot force and torque ${ƒ}_c^{ƒƒ}$

or. ${\tau }_b^{ƒƒ}$

are. The feedforward torque can be interpreted as the rate of change of the centroid angular momentum resulting from the commanded trajectory transformed from the task space via inverse kinematics (5). The commanded CoM speed and acceleration, and hence ${ƒ}_c^{ƒƒ},$

is defined as zero for balancing scenarios. As a result, the hip contact torque is simplified to $${\tau }_v^{Hip}={\tau }_b^{ƒƒ}=i_c^{cm\mathrm{i.e}},$$

Where $i_c^{cm\mathrm{i.e}}$

is the rate of change of commanded centroid angular momentum.

liegen muss, wobei ƒ_v,z die gesamte vertikale Kraft ist, die auf den Boden aufgebracht wird.According to the invention, it can be provided that the limits for the xy plane are calculated on the basis of the restriction that the combined center of pressure p _v ∈ ℝ ² within a virtual support surface Sv $$p_v=\frac{1}{{ƒ}_{v,\mathrm{e.g}}}\left(\begin{array}{c}-{\tau }_{v,y}\\ {\tau }_{v,x}\end{array}\right)\in S_v,$$

where ƒ _v,z is the total vertical force applied to the ground.

C. Centroid angular momentum reference generation for force disturbance balancing scenarios

τ_v (19) werden ein Referenz-Schwerpunktdrehimpuls und daher auch ein resultierender ${\tau }_v^{hip}$

so erzeugt, dass τ_v innerhalb seiner Grenzen ${\tau }_v^{min/max}\in {ℝ}^3$

bleibt. Die Grenzen des Drehmoments um die z-Achse werden durch vordefinierte obere und untere Werte [21], [22] approximiert. Die Grenzen für die xy-Ebene werden auf Basis der Beschränkung berechnet, dass der kombinierte CoP p_v ∈ ℝ² innerhalb des virtuellen Standfußes Sv $$p_v=\frac{1}{{ƒ}_{v,z}}\left(\begin{array}{c}-{\tau }_{v,y}\\ {\tau }_{v,x}\end{array}\right)\in S_v,$$

liegen muss, wobei f_v,z die gesamte vertikale Kraft ist, die auf den Boden aufgebracht wird. Um den begrenzten Drehimpuls-Speicherkapazitäten eines Humanoiden Rechnung zu tragen, wird die Schwerpunktdrehimpuls-Referenzwerterzeugung in drei aufeinanderfolgende Phasen unterteilt: 1) die erste Phase erzeugt aktiv einen Schwerpunktdrehimpuls, um die äußeren Kräfte auszugleichen, die durch eine Störung (z.B. einen Stoß) hervorgerufen werden; 2) die zweite Phase bringt den Schwerpunktdrehimpuls-Referenzwert auf null zurück, um den Roboter anzuhalten; und 3) die letzte Phase stellt sicher, dass der Roboter zu seiner Referenzpose zurückkehrt. Ein Zurückschalten auf Phase 1 ist jederzeit möglich, wenn ein weiterer Stoß erfasst wird. Der Push-Recovery-Algorithmus wird unabhängig für jede räumliche Dimension i ∈ {x, y, z} angewendet, die an dem (möglicherweise gedrehten) virtuellen Standfuß ausgerichtet ist, d.h. es können Stöße aus allen Richtungen unabhängig in der jeweiligen Achse kompensiert werden. Um der Klarheit willen werden die Indizes ab jetzt weggelassen. Die vorgestellten Beziehungen in Skalarform gelten für jede räumliche Dimension. Der vorgeschlagene Algorithmus wird mit Simulationsergebnissen unter Verwendung der Humanoidroboterplattform OpenHRP [28] erläutert, wobei in experimentellen Verifizierungen das gleiche Robotermodell verwendet wird.In this section, a method for planning the activation time of the hip strategy is presented and a corresponding center of gravity angular momentum reference value is provided. Based on ${\tau }_v^{ankle}$

τ _v (19) becomes a reference centroid angular momentum and therefore also a resultant one ${\tau }_v^{Hip}$

generated in such a way that τ _v is within its bounds ${\tau }_v^{min/max}\in {\mathrm{<figure-callout\; id="3"\; label="R"\; filenames="DE102021112485B3\_0205.png,DE102021112485B3\_0206.png"\; state="\{\{state\}\}">ℝ</figure-callout>}}^3$

remains. The limits of the torque about the z-axis are approximated by predefined upper and lower values [21], [22]. The xy-plane bounds are computed based on the constraint that the combined CoP p _v ∈ ℝ ² is within the virtual pedestal Sv $$p_v=\frac{1}{{ƒ}_{v,\mathrm{e.g}}}\left(\begin{array}{c}-{\tau }_{v,y}\\ {\tau }_{v,x}\end{array}\right)\in S_v,$$

where fv _,z is the total vertical force applied to the ground. To accommodate the limited angular momentum storage capacities of a humanoid, the centroid angular momentum reference generation is divided into three sequential phases: 1) the first phase actively generates a centroid angular momentum to counterbalance the external forces induced by a disturbance (e.g., a shock). ; 2) the second phase returns the center of gravity angular momentum reference to zero to stop the robot; and 3) the last phase ensures that the robot returns to its reference pose. Switching back to phase 1 is possible at any time if another shock is detected. The push recovery algorithm is applied independently for each spatial dimension i ∈ {x, y, z}, which is aligned with the (possibly rotated) virtual stand, ie pushes from all directions can be compensated independently in the respective axis. For the sake of clarity, the indices are omitted from now on. The presented relationships in scalar form apply to any spatial dimension. The proposed algorithm is explained with simulation results using the humanoid robot platform OpenHRP [28], using the same robot model in experimental verifications.

einen vordefinierten Schwellenwert ${\tau }_v^{thres}$

überschreitet. Der Schwellenwert ist eine Funktion der Kontaktdrehmomentgrenzen, die sich aus (22) ergeben $${\tau }_v^{thres}=\alpha {\tau }_v^{min/max},$$

mit dem Designparameter α ∈ [0, 1]. Je größer α ist, desto später wird die Hüftstrategie aktiviert. Im Allgemeinen sollte die Hüftstrategie so spät wie möglich aktiviert werden, um zunächst die Möglichkeiten der Fußgelenksstrategie voll auszuschöpfen. Wenn die Hüftstrategie jedoch erst aktiv wird, wenn die Kontaktbeschränkungen erreicht werden (α = 1), können kleine Modellunsicherheiten oder Tracking-Fehler auch bewirken, dass der Roboter hinfällt. Daher verbessert ein Sicherheitsspielraum die Robustheit des Algorithmus, in der Praxis wurden gute Ergebnisse mit α ∈ [0.7, 0.9] erzielt. 2a zeigt einen simulierten Verlauf von ${\tau }_v^{ankle}$

von (19). Der Roboter wird von hinten mit einem Impuls von 12 Ns angestoßen, der, normalisiert auf seine Masse, einer Deltageschwindigkeit von 0,152 $\frac{\text{m}}{\text{s}}$

entspricht.1) Center of Gravity Momentum Generation Phase: The first phase is activated after a shock has taken place, which is detected when the appropriate ${\tau }_v^{ankle}$

a predefined threshold ${\tau }_v^{tH\mathrm{right}es}$

exceeds. The threshold is a function of the contact torque limits resulting from (22). $${\tau }_v^{tH\mathrm{right}es}=a{\tau }_v^{min/max},$$

with the design parameter α ∈ [0, 1]. The larger α is, the later the hip strategy is activated. In general, the hip strategy should be activated as late as possible in order to fully exploit the possibilities of the ankle strategy first. However, if the hip strategy only becomes active when the contact restrictions are reached (α = 1), small model uncertainties or tracking errors can also cause the robot to fall. Therefore, a safety margin improves the robustness of the algorithm, in practice good results have been obtained with α ∈ [0.7, 0.9]. 2a shows a simulated course of ${\tau }_v^{ankle}$

from (19). The robot is pushed from behind with an impulse of 12 Ns, which, normalized to its mass, has a delta velocity of 0.152 $\frac{\text{m}}{\text{s}}$

is equivalent to.

The rate of change of reference centroid angular momentum is defined as the difference in angular torque and its threshold. $$l_c^{\mathrm{right}eƒ}\left(t\right)={\tau }_v^{tH\mathrm{right}es}\left(t\right)-{\tau }_v^{ankle}\left(t\right).$$

kann durch Integrieren von (24) in Bezug auf die Zeit erhalten werden. Die Zeit t ist innerhalb des Intervalls t ∈ [t₁, t₂) definiert, wobei der Moment der Aktivierung der ersten Phase durch t = t₁ gekennzeichnet ist. Die Phase 1 ist abgeschlossen, wenn das Fußgelenksdrehmoment wieder unter dem vordefinierten Schwellenwert liegt, d.h. $\left|{\tau }_v^{ankle}\left(t\right)\right|<{\tau }_v^{thres}\left(t\right),$

dieser Zeitpunkt wird definiert durch t = t₂. 2b zeigt den resultierenden Schwerpunktdrehimpuls-Referenzwert für unterschiedliche Werte von α.This difference can be interpreted as the additional hip torque in (19) needed to keep τ _v within its limits. The corresponding reference centroid angular momentum $l_c^{\mathrm{right}eƒ}\left(t\right)$

can be obtained by integrating (24) with respect to time. The time t is defined within the interval t ∈ [t ₁ , t ₂ ), where the moment of activation of the first phase is marked by t = t ₁ . Phase 1 is complete when the ankle torque is back below the predefined threshold, ie $\left|{\tau }_v^{ankle}\left(t\right)\right|<{\tau }_v^{tH\mathrm{right}es}\left(t\right),$

this point in time is defined by t=t ₂ . 2 B shows the resulting centroid angular momentum reference value for different values of α.

auf Überschreitung eines vordefinierten Schwellenwertes ${\tau }_v^{thres}$

überprüft wird, wobei der Schwellenwert als Funktion der Kontaktdrehmomentgrenzen bestimmt wird: $${\tau }_v^{thres}=\alpha {\tau }_v^{min/max},$$

mit dem Designparameter α ∈ [0, 1] und wobei die Änderungsrate des Referenzschwerpunktdrehimpulses definiert wird als die Differenz des Fußgelenkdrehmoments und dessen Schwellenwerts $$l_c^{reƒ}\left(t\right)={\tau }_v^{thres}\left(t\right)-{\tau }_v^{ankle}\left(t\right).$$

It can be provided that in method step b) the contact torque of the virtual supporting leg or the support surface is used as the contact torque τ _ext ${\tau }_v^{ankle}$

for exceeding a predefined threshold ${\tau }_v^{tH\mathrm{right}es}$

is verified, determining the threshold as a function of the contact torque limits: $${\tau }_v^{tH\mathrm{right}es}=a{\tau }_v^{min/max},$$

with the design parameter α ∈ [0, 1] and where the rate of change of the reference centroid angular momentum is defined as the difference of the ankle torque and its threshold $$l_c^{\mathrm{right}eƒ}\left(t\right)={\tau }_v^{tH\mathrm{right}es}\left(t\right)-{\tau }_v^{ankle}\left(t\right).$$

kann durch Integrieren von (24) in Bezug auf die Zeit erhalten werden, wobei die Zeit t innerhalb des Intervalls t ∈ [t₁, t₂) definiert wird, wobei der Moment der Aktivierung der ersten Phase durch t = t₁ gekennzeichnet ist und wobei die Phase 1 abgeschlossen ist, wenn das Fußgelenksdrehmoment wieder unter dem vordefinierten Schwellenwert liegt, d.h. $\left|{\tau }_v^{ankle}\left(t\right)\right|<{\tau }_v^{thres}\left(t\right),$

dieser Zeitpunkt wird definiert durch t = t₂.This difference can be interpreted as the additional hip torque in (19) needed to keep τ _v within its limits. The corresponding reference centroid momentum $l_c^{\mathrm{right}eƒ}\left(t\right)$

can be obtained by integrating (24) with respect to time, defining the time t within the interval t ∈ [t ₁ , t ₂ ), where the moment of activation of the first phase is characterized by t = t ₁ and phase 1 is completed when the ankle torque is again below the predefined threshold, ie $\left|{\tau }_v^{ankle}\left(t\right)\right|<{\tau }_v^{tH\mathrm{right}es}\left(t\right),$

this point in time is defined by t=t ₂ .

According to the present method, it can also be provided that in method step c3) the total duration for the reduction of the reference center of gravity pulse to 0 is defined as T=t ₃ -t ₂ with t ₂ as the start time and t ₃ as the end time of the reduction of the reference center of gravity angular momentum, specifying the reference centroid angular momentum via the following relationship: $$l_c^{\mathrm{right}eƒ}\left(t\right)={\displaystyle \sum _{j=0}^3{a}_j\frac{{\left(t-t_2\right)}^j}{T^j}},$$

als Beziehung zwischen der Gesamtdauer T, dem Referenzschwerpunktdrehimpuls am Anfang der Reduktion des Schwerpunktdrehimpulses $l_{c,t_2}^{reƒ},$

Phase 2, und seiner maximalen Änderungsrate ${\dot{l}}_c^{max}>0$

wie folgt erhalten: $$T=\frac{3\left|l_{c,t_2}^{reƒ}\right|}{2{\dot{l}}_c^{max}}.$$

wobei ${\dot{l}}_c^{max}$

als Funktion der höchsten Änderungsrate des Schwerpunktdrehimpulses während Phase 1 gewählt wird, erhalten durch: $${\dot{l}}_c^{max}=\beta \left(\underset{t_1\le t<t_2}{\text{max}}\left|{\dot{l}}_c^{reƒ}\left(t\right)\right|\right),$$

mit dem Designparameter β, der innerhalb des Intervalls β ∈ (0, 1] definiert wird.Determine the of T based on the critical points of $l_c^{\mathrm{right}eƒ}$

as the relationship between the total duration T, the reference centroid angular momentum at the beginning of the reduction of the centroid angular momentum $l_{c,t_2}^{\mathrm{right}eƒ},$

Phase 2, and its maximum rate of change ${\dot{l}}_c^{max}>0$

received as follows: $$T=\frac{3\left|l_{c,{\mathrm{<figure-callout\; id="2"\; label="t"\; filenames="DE102021112485B3\_0204.png,DE102021112485B3\_0205.png"\; state="\{\{state\}\}">t</figure-callout>}}_2}^{\mathrm{right}eƒ}\right|}{2{\dot{l}}_c^{max}}.$$

whereby ${\dot{l}}_c^{max}$

is chosen as a function of the highest rate of change of centroid angular momentum during phase 1, obtained by: $${\dot{l}}_c^{max}=\beta \left(\underset{t_1\le t<t_2}{\text{Max}}\left|{\dot{l}}_c^{\mathrm{right}eƒ}\left(t\right)\right|\right),$$

with the design parameter β defined within the interval β ∈ (0, 1].

2) Centroid angular momentum reduction phase:

zu vermeiden.During phase 1, the reference centroid angular momentum increases monotonically. After the bump is balanced, the reference centroid angular momentum has a non-zero value that must be smoothly reduced to zero to bring the robot back to rest. Therefore, a third-order polynomial is used to generate a trajectory while ensuring C ¹ continuity to avoid jumps in the resulting ${\tau }_v^{Hip}$

to avoid.

wobei die Koeffizienten a_j auf Basis der Grenzbedingungen bestimmt werden können. Der letzte freie Parameter ist die Gesamtdauer der Phase 2, die durch T = t₃ - t₂ bezeichnet wird. Auf Basis der kritischen Punkte von ${\dot{l}}_c^{reƒ}$

wird eine Beziehung zwischen der Gesamtdauer, dem Referenz-Schwerpunktdrehimpuls am Anfang der Phase 2 $l_{c,t_2}^{reƒ}$

und seiner maximalen Änderungsrate ${\dot{l}}_c^{max}>0$

wie folgt erhalten: $$T=\frac{3\left|l_{c,t_2}^{reƒ}\right|}{2{\dot{l}}_c^{max}}.$$

The system state when phase 2 is activated is used as the initial condition for the polynomial. To stop the robot after phase 2, the center of mass angular momentum and its time derivative must be zero at the end of the trajectory. Phase 2 is defined within the time interval t ∈ [t ₂ , t ₃ ]. The centroid angular momentum reference trajectory is formulated as $$l_c^{\mathrm{right}eƒ}\left(t\right)={\displaystyle \sum _{j=0}^3{a}_j\frac{{\left(t-t_2\right)}^j}{T^j}},$$

where the coefficients a _j can be determined based on the boundary conditions. The last free parameter is the total duration of phase 2, denoted by T = t ₃ - t ₂ . Based on the critical points of ${\dot{l}}_c^{\mathrm{right}eƒ}$

becomes a relationship between the total duration, the reference centroid angular momentum at the beginning of phase 2 $l_{c,{\mathrm{<figure-callout\; id="2"\; label="t"\; filenames="DE102021112485B3\_0204.png,DE102021112485B3\_0205.png"\; state="\{\{state\}\}">t</figure-callout>}}_2}^{\mathrm{right}eƒ}$

and its maximum rate of change ${\dot{l}}_c^{max}>0$

received as follows: $$T=\frac{3\left|l_{c,{\mathrm{<figure-callout\; id="2"\; label="t"\; filenames="DE102021112485B3\_0204.png,DE102021112485B3\_0205.png"\; state="\{\{state\}\}">t</figure-callout>}}_2}^{\mathrm{right}eƒ}\right|}{2{\dot{l}}_c^{max}}.$$

als Funktion der höchsten Änderungsrate des Schwerpunktdrehimpulses während Phase 1 gewählt, erhalten durch $${\dot{l}}_c^{max}=\beta \left(\underset{t_1\le t<t_2}{\text{max}}\left|{\dot{l}}_c^{reƒ}\left(t\right)\right|\right),$$

mit dem Designparameter, der innerhalb des Intervalls β ∈ (0, 1] definiert ist, siehe 2c.It should be noted that T cannot be chosen arbitrarily large without reaching kinematic limits, while a small T induces a high maximum rate of change of centroid angular momentum. To find a compromise, ${\dot{l}}_c^{max}$

chosen as a function of the highest rate of change of centroid angular momentum during phase 1, obtained by $${\dot{l}}_c^{max}=\beta \left(\underset{t_1\le t<t_2}{\text{Max}}\left|{\dot{l}}_c^{\mathrm{right}eƒ}\left(t\right)\right|\right),$$

with the design parameter defined within the interval β ∈ (0, 1], see 2c .

Small values of β increase the risk of activating position limits, while large values can result in an aggressive reduction in centroid angular momentum. Both scenarios can distract the CoP towards the edge of the support surface and possibly cause the robot to fall over. Good results have been obtained with β ∈ [0.3, 0.5]. This formulation gives a good indication of how quickly the center of gravity angular momentum should be reduced, but does not provide a formal guarantee that kinematic limits or contact limitations will not be reached.

3) Posture Recovery Phase:

After phase 2 is completed, the reference centroid angular momentum is reduced to zero, but the robot configuration still deviates from its reference pose. At this point, the posture task (14b) of optimizing movement becomes dominant. It provides a rapid return to the initial robot configuration while inducing only a small centroid angular momentum of opposite sign to that generated in phases 1 and . Centroid angular momentum tracking depends on weight matrix selection. Increasing Q _l in (13) while keeping Q _p constant gives better centroid angular momentum tracking, but also increases the convergence time with respect to the reference pose and consequently the overall duration of phase 3, see 2d .

wobei w_qrf die zusammengeführten Kontaktkraftwindungen der Aufstützfläche des Roboters bezeichnet und die Abweichung von den kommandierten Trajektorien durch $\mathrm{\Delta }\nu ={\nu }_c-{\nu }_c^{cmd}\text{ und }\mathrm{\Delta }\dot{q}=\dot{q}-{\dot{q}}^{cmd}$

wiedergegeben werden; wobei die kommandierten Trajektorien x^cmd im Aufgabenraum erzeugt werden über das Verfahren gemäß einem der Ansprüche 1 bis 6 und die entsprechenden kommandierten Schwerpunkts- und Gelenkwerte über inverse Kinematik berechnet werden: $$\left(\begin{array}{c}{\nu }_c^{cmd}\\ {\dot{q}}^{cmd}\end{array}\right)=J^{-1}{\dot{x}}^{cmd}.$$

wobei die Schwerpunkts-assoziierten Impedanzen definiert werden durch: $$w_c^{imp}=\left(\begin{array}{c}{K}_c\left(x_c-x_c^{cmd}\right)+D_c\left({\dot{x}}_c-{\dot{x}}_c^{cmd}\right)\\ {\tau }_r\left({\mathrm{\Sigma }}_b,{\left(R_b^{cmd}\right)}^T{R}_b\right)+B_b\left({\omega }_b-{\omega }_b^{cmd}\right)\end{array}\right),$$

wobei die Linear- und Rotationssteifigkeitsmatrizes K_c > 0 und Σ_b > 0 sowie die Linear- und Rotationsdämpfungsmatrizes D_c > 0 und B_b > 0 symmetrisch und positiv definit sind;
wobei die kartesische Ausrichtung der Hüfte von einer virtuellen Rotationsfeder ${\tau }_r\left({\mathrm{\Sigma }}_b,{\left(R_b^{cmd}\right)}^T{R}_b\right)$

gesteuert wird, während die Impedanz der Gelenkaufgabe durch $${\tau }_{ƒ}^{imp}=K_{ƒ}\left(q_{ƒ}-q_{ƒ}^{cmd}\right)+D_{ƒ}\left({\dot{q}}_{ƒ}-{\dot{q}}_{ƒ}^{cmd}\right),$$

realisiert wird, mit den Matrizes der positiven definiten und linearen Feder K_ƒ > 0 und D_ƒ > 0.According to a second aspect of the present invention, a method for controlling the whole body of a robot to compensate for external interference affecting the robot, comprising: at least two body segments, at least one joint with the number of n degrees of freedom for the articulated connection of the body segments with at least one actuating device for actively influencing the Position q of the at least one joint, and a support surface; the control loop behavior for controlling the at least one actuating device being: $$M\left(\begin{array}{c}\mathrm{\Delta }{\dot{v}}_c\\ \mathrm{\Delta }\ddot{q}\end{array}\right)+C\left(\begin{array}{c}\mathrm{\Delta }{v}_c\\ \mathrm{\Delta }\dot{q}\end{array}\right)={\tau }_{ext}-J^T\left(\begin{array}{c}{w}_c^{imp}\\ w_{G\mathrm{right}ƒ}\\ {\tau }_{ƒ}^{imp}\end{array}\right),$$

where w _{qrf denotes} the combined contact force turns of the robot's support surface and the deviation from the commanded trajectories by $\mathrm{\Delta }v=v_c-v_c^{cm\mathrm{i.e}}\text{and}\mathrm{\Delta }\dot{q}=\dot{q}-{\dot{q}}^{cm\mathrm{i.e}}$

be reproduced; wherein the commanded trajectories x ^cmd are generated in the task space using the method according to one of claims 1 to 6 and the corresponding commanded center of gravity and joint values are calculated using inverse kinematics: $$\left(\begin{array}{c}{v}_c^{cm\mathrm{i.e}}\\ {\dot{q}}^{cm\mathrm{i.e}}\end{array}\right)=J^{-1}{\dot{x}}^{cm\mathrm{i.e}}.$$

where the centroid-associated impedances are defined by: $$w_c^{imp}=\left(\begin{array}{c}{K}_c\left(x_c-x_c^{cm\mathrm{i.e}}\right)+D_c\left({\dot{x}}_c-{\dot{x}}_c^{cm\mathrm{i.e}}\right)\\ {\tau }_{\mathrm{right}}\left({\mathrm{\Sigma }}_b,{\left(R_b^{cm\mathrm{i.e}}\right)}^T{R}_b\right)+B_b\left({\omega }_b-{\omega }_b^{cm\mathrm{i.e}}\right)\end{array}\right),$$

where the linear and rotational stiffness matrices K _c > 0 and Σ _b > 0 and the linear and rotational damping matrices D _c > 0 and B _b > 0 are symmetric and positive definite;
where the Cartesian alignment of the hip from a virtual torsional spring ${\tau }_{\mathrm{right}}\left({\mathrm{\Sigma }}_b,{\left(R_b^{cm\mathrm{i.e}}\right)}^T{R}_b\right)$

is controlled while the impedance of the joint task through $${\tau }_{ƒ}^{imp}=K_{ƒ}\left(q_{ƒ}-q_{ƒ}^{cm\mathrm{i.e}}\right)+D_{ƒ}\left({\dot{q}}_{ƒ}-{\dot{q}}_{ƒ}^{cm\mathrm{i.e}}\right),$$

is realized, with the matrices of the positive definite and linear spring K _ƒ > 0 and D _ƒ > 0.

Calculating the final control torques: $$\tau =M_q\left(\begin{array}{c}{\dot{v}}_c^{cm\mathrm{i.e}}\\ {\ddot{q}}^{cm\mathrm{i.e}}\end{array}\right)+C_q\left(\begin{array}{c}{v}_c^{cm\mathrm{i.e}}\\ {\dot{q}}^{cm\mathrm{i.e}}\end{array}\right)-{\left(J\text{'}\right)}^T{w}_{G\mathrm{right}ƒ}-S_{ƒ}^T{\tau }_{ƒ}^{imp}.$$

According to a third aspect of the present invention, a controller can be provided comprising at least one computing unit and one memory unit, instructions for executing the balancing method according to the first aspect of the present invention or for executing the whole-body control according to the second aspect of the present invention being stored on the memory unit by the Arithmetic unit are stored.

According to a further aspect, according to the invention, a robot can be provided comprising at least two body segments, at least one joint for the articulated connection of the body segments with at least one actuating device for actively influencing the at least one joint as well as a support surface and a computing unit, which use the method according to the invention to balance the robot the first aspect of the present invention.

impulsiv angestoßen. Die Optimierungsvariable ${\dot{x}}_a^{opt}$

in (13) schließt die Geschwindigkeiten der Hüfte um alle drei Achsen, die Gelenksgeschwindigkeit im Torso sowie die ersten vier Gelenkgeschwindigkeiten in jedem Arm ein. Um den Stoß abzuwehren, erzeugt der Roboter einen Schwerpunktdrehimpuls durch Vorwärtsbeugen und Rückwärtsbewegen der Arme.Two scenarios are presented to evaluate the performance of the push recovery method in the real system. In the first scenario, the robot stands upright, supporting itself in two places, and is restrained within the sagittal plane (along the x-axis) at hip height with a maximum force of 65 N and an impulse of 18 Ns (0.227 $\frac{\text{m}}{\text{s}}$

impulsively initiated. The optimization variable ${\dot{x}}_a^{Opt}$

in (13) includes the hip velocities about all three axes, the joint velocities in the torso, and the first four joint velocities in each arm. To absorb the impact, the robot creates a center of gravity angular momentum by bending its arms forward and moving them backward.

wird das resultierende Kontaktdrehmoment τ_v in seinen Grenzen gehalten und der Roboter findet sein Gleichgewicht nach dem Stoß wieder. 3d zeigt die entsprechende kombinierte Abweichung des CoP und CoM. Der Verlauf des Referenz-, des Befehls- und des gemessenen Schwerpunktdrehimpulses durch alle Phasen und ihre Änderungsrate sind in 3b bzw. 3c, abgebildet. Es gilt zu beachten, dass der gemessene Schwerpunktdrehimpuls direkt steigt, wenn der Stoß ausgeübt wird, da die externe Kraft an sich einen Drehimpuls in dem System hervorruft. Der Referenz-Schwerpunktdrehimpuls steigt mit Verzögerung an, abhängig von der Aktivierungszeit der Phase 1.The ankle, hip and resulting contact torque of the virtual foot are in 3a shown. The pure ankle torque exceeds its limit, ie the combined CoP would have reached the edge of the supporting surface if a fixed posture had been maintained, causing the robot to fall over. The control of Section II-B alone could not have compensated for the shock without the presented center of gravity-based motion optimization of Section III. Due to the additionally generated center of gravity angular momentum and the corresponding ${\tau }_v^{Hip}$

the resulting contact torque τ _v is kept within its limits and the robot regains its balance after the impact. 3d shows the corresponding combined deviation of the CoP and CoM. The history of the reference, command, and measured centroid angular momentum through all phases and their rate of change are in 3b or. 3c , pictured. It should be noted that the measured centroid angular momentum increases directly when the impact is applied, since the external force itself creates angular momentum in the system. The reference centroid angular momentum increases with a delay dependent on the phase 1 activation time.

angestoßen. Die Ergebnisse sind in 4 gezeigt. Zu beachten ist, dass die translationale kartesische Geschwindigkeit des linken Fußes zu den Optimierungsvariablen des vorherigen Versuchs mit der doppelten Abstützung addiert wird; die entsprechende CoM-gemappte Impedanzauslenkung muss in (8) und in (18) zu dem Winkeldrehmoment addiert werden (für weitere Einzelheiten siehe [22]).In the second attempt, the robot balances on its right leg and is pushed from the front with a maximum force of 75 N and an impulse of 22.5 Ns (0.284 $\frac{\text{m}}{\text{s}}$

triggered. The results are in 4 shown. Note that the translational Cartesian velocity of the left foot is added to the optimization variables from the previous experiment with the double brace; the corresponding CoM-mapped impedance excursion has to be added to the angular torque in (8) and in (18) (for more details see [22]).

• [20] C. Ott, M. A. Roa, and G. Hirzinger, „Posture and balance control for biped robots based on contact force optimization,“ in Proc. 11th IEEERAS Int. Conf. Humanoid Robots, 2011, pp. 26-33.
• [21] B. Henze, M. A. Roa, and C. Ott, „Passivity-based whole-body balancing for torque-controlled humanoid robots in multi-contact scenarios,“ Int. J. Robot. Res., vol. 35, no. 12, pp. 1522-1543, 2016.
• [22] G. Mesesan et al., „Dynamic walking on compliant and uneven terrain using DCM and passivity-based whole-body control,“ in Proc. 19^th IEEE-RAS Int. Conf. Humanoid Robots, 2019, pp. 25-32.
• [23] D. E. Orin, A. Goswami, and S.-H. Lee, „Centroidal dynamics of a humanoid robot,“ Auton. Robots, vol. 35, no. 2-3, pp. 161-176, Oct. 2013.
• [24] J. Englsberger, G. Mesesan, and C. Ott, „Smooth trajectory generation and push-recovery based on divergent component of motion,“ in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., Sep. 2017, pp. 4560-4567.
• [25] G. Mesesan, J. Englsberger, C. Ott, and A. Albu-Schäffer, „Convex properties of center-of-mass trajectories for locomotion based on divergent component of motion,“ IEEE Robot. Autom. Lett., vol. 3, no. 4, pp. 3449-3456, 2018.
• [26] B. Paden and R. Panja, „Globally asymptotically stable ‚PD+‘ controller for robot manipulators,“ Int. J. Control, vol. 47, no. 6, pp. 1697-1712, 1988.
• [27] J. Englsberger, „Combining reduced dynamics models and whole-body control for agile humanoid locomotion,“ Ph.D. dissertation, Tech. Univ. Munich, Munich, Dec. 2016.
• [28] F. Kanehiro, H. Hirukawa, and S. Kajita, „OpenHRP: Open architecture humanoid robotics platform,“ Int. J. Robot. Res., vol. 23, no. 2, pp. 155-165, Feb. 2004.
• [29] J. Englsberger et al., „Overview of the torque-controlled humanoid robot TORO,“ in Proc. IEEE-RAS Int. Conf. Humanoid Robots, 2014, pp. 916-923. [30] H. J. Ferreau, H. G. Bock, and M. Diehl, „An online active set strategy to overcome the limitations of explicit MPC,“ Int. J. Robust Nonlinear Control, vol. 18, no. 8, pp. 816-830, 2008.
• [31] A. Albu-Schäffer et al., „The DLR lightweight robot: design and control concepts for robots in human environments,“ Ind. Robot: Int. J., vol. 34, no. 5, pp. 376-385, 2007.

It should be noted that the robot with single support is able to absorb impacts in the sagittal plane of greater magnitude compared to an identical experimental setup with double support. The extent of the support area in the x-direction is the same for both configurations, but in simple support the free leg generates almost half of the peak centroid angular momentum in the y-direction, see 4d .

• [20] C Ott, MA Roa, and G Hirzinger, "Posture and balance control for biped robots based on contact force optimization," in Proc. 11th IEEERAS Int. conf Humanoid Robots, 2011, pp. 26-33.
• [21] B. Henze, MA Roa, and C. Ott, "Passivity-based whole-body balancing for torque-controlled humanoid robots in multi-contact scenarios," Int. J Robot. Res., vol. 35, no. 12, pp. 1522-1543, 2016.
• [22] G. Mesesan et al., "Dynamic walking on compliant and uneven terrain using DCM and passivity-based whole-body control," in Proc. 19th IEEE ^RAS Int. conf Humanoid Robots, 2019, pp. 25-32.
• [23] DE Orin, A. Goswami, and S.-H. Lee, "Centroidal dynamics of a humanoid robot," Auton. Robots, vol. 35, no. 2-3, pp. 161-176, Oct. 2013
• [24] Englsberger, J, Mesesan, G, and Ott, C, "Smooth trajectory generation and push-recovery based on divergent component of motion," in Proc. IEEE/RSJ Int. conf intel. Robots Syst., Sep. 2017, pp. 4560-4567.
• [25] G Mesesan, J Englsberger, C Ott, and A Albu-Schäffer, "Convex properties of center-of-mass trajectories for locomotion based on divergent component of motion," IEEE Robot. Autom. Lett., vol. 3, no. 4, pp. 3449-3456, 2018.
• [26] B Paden and R Panja, "Globally asymptotically stable 'PD+' controller for robot manipulators," Int. J. Control, vol. 47, no. 6, pp. 1697-1712, 1988.
• [27] J. Englsberger, "Combining reduced dynamics models and whole-body control for agile humanoid locomotion," Ph.D. dissertation, tech. university Munich, Munich, Dec. 2016
• [28] F Kanehiro, H Hirukawa, and S Kajita, "OpenHRP: Open architecture humanoid robotics platform," Int. J Robot. Res., vol. 23, no. 2, pp. 155-165, Feb. 2004.
• [29] J. Englsberger et al., "Overview of the torque-controlled humanoid robot TORO," in Proc. IEEE RAS Int. conf Humanoid Robots, 2014, pp. 916-923. [30] HJ Ferreau, HG Bock, and M. Diehl, "An online active set strategy to overcome the limitations of explicit MPC," Int. J. Robust Nonlinear Control, vol. 18, no. 8, pp. 816-830, 2008.
• [31] A. Albu-Schäffer et al., "The DLR lightweight robot: design and control concepts for robots in human environments," Ind. Robot: Int. J., vol. 34, no. 5, pp. 376-385, 2007.

Claims (8)

A method for balancing a robot to compensate for external interference, the robot comprising: at least two body segments, at least one joint with the number of $\overline{n}$

actuated degrees of freedom for the articulated connection of the body segments with at least one adjusting device for actively influencing the position q of the at least one joint, and a support surface; the method comprising the steps: a) monitoring and determining the contact torques τ _ext acting externally on the robot; b) checking whether the determined contact torques τ _ext exceed at least one predetermined limit value τ ^thres ; c) if the at least one predetermined limit value τ ^{thres is exceeded} in step b) execution of sub-steps c1) to c3): c1) calculation of a required reference centroid angular momentum (CAM) $l_c^{\mathrm{right}eƒ},$

to keep the robot's center of pressure (CoP) within the support surface, c2) inducing the required reference centroid angular momentum $l_c^{\mathrm{right}eƒ}$

by changing the position q of at least one joint by means of at least one actuating device of the robot, and c3) reducing the generated reference center of gravity angular momentum $l_c^{\mathrm{right}eƒ}$

by changing the position q of at least one joint by means of at least one actuating device of the robot; d) Calculating whole-body motions x ^cmd using a whole-body motion optimizer that uses the reference centroid angular momentum $l_c^{\mathrm{right}eƒ}$

and uses a reference pose x ^ref as input variables and generates joint trajectories that can be implemented dynamically and kinematically and ensures that the robot converges to the specified reference pose x ^ref and that, if steps c1) to c3) are carried out, only small angular momenta with the opposite sign are subsequently generated the angular momenta generated in steps c2) and c3); e) inducing the calculated whole-body movements x ^cmd by changing the position q of at least one joint by means of at least one actuating device of the robot; and f) continuously repeating the aforementioned process steps. Procedure for balancing claim 1 , wherein during the execution of steps c1) to c3) the method steps a) and b) are executed in parallel and continuously and if at least one predetermined limit value τ ^thres in step b) is exceeded, the ongoing execution of steps c1) to c3) is aborted and steps c1) to c3) are carried out again. Procedure for balancing claim 1 or 2 , wherein method step d) comprises the following steps: d1) dividing the total number of $\overline{n}$

Actuated degrees of freedom in the task space in: k degrees of freedom to be influenced with the associated task variables x _a and in $\overline{n}-k$

unchanged degrees of freedom with the associated task variables x _u , with $k\in \left\{\mathrm{0,}...,\overline{n}\right\};$

d2) determining the optimized speeds satisfying the relationship: $$l_c^{\mathrm{right}eƒ}={\overline{A}}_a{\dot{x}}_a^{Opt}+{\overline{A}}_{\mathrm{and}}{\dot{x}}_{\mathrm{and}}^{\mathrm{right}eƒ},$$

d3) Solving the relation in d2) by formulating a quadratic optimization problem with constraints to: $$\underset{{\dot{x}}_a^{Opt}}{\text{at least}}\left(\frac{1}{2}{\delta }_l^T{Q}_l{\delta }_l+\frac{1}{2}{\delta }_p^T{Q}_p{\delta }_p\right),$$

where Q _L and Q _R > 0 represent weight matrices, both of which are positive definite and symmetric with the residuals: $${\delta }_l={\overline{A}}_a{\dot{x}}_a^{Opt}+{\overline{A}}_{\mathrm{and}}{\dot{x}}_{\mathrm{and}}^{\mathrm{right}eƒ}-l_c^{\mathrm{right}eƒ},$$

$${\delta }_p={\dot{x}}_a^{Opt}-{\dot{x}}_a^{\mathrm{i.e}},$$

taking into account the boundary conditions: $${\dot{x}}_a^{min}\le {\dot{x}}_a^{Opt}\le {\dot{x}}_a^{max}.$$

where the desired speed ${\dot{x}}_a^{\mathrm{i.e}}$

based on the deviation of the optimized position from its reference position $x_a^{\mathrm{right}eƒ}$

is calculated: $${\dot{x}}_a^{\mathrm{i.e}}={\dot{x}}_a^{\mathrm{right}eƒ}+K_p\left(x_a^{\mathrm{right}eƒ}-x_a^{Opt}\right),$$

where reference speed ${\dot{x}}_a^{\mathrm{right}eƒ}$

for the variables to be optimized and the associated reference position $x_a^{\mathrm{right}eƒ}$

are known as input variables, where the convergence of the robot to the reference pose can be influenced by the design parameters K _p > 0, which is chosen to be diagonal and positive definite with the pose constraints $\left({\underset{\_}{x}}_a,{\overline{x}}_a\right)$

and the speed limits $\left({\dot{\underset{\_}{x}}}_a,{\overline{\dot{x}}}_a\right),$

$$\begin{array}{l}{\dot{x}}_a^{min}=\text{Max}\left(K_a\left({\underset{\_}{x}}_a-x_a^{Opt}\right),{\underset{\_}{\dot{x}}}_a\right),\hfill \\ {\dot{x}}_a^{max}=\text{at least}\left(K_a\left({\overline{x}}_a-x_a^{Opt}\right),{\overline{\dot{x}}}_a\right),\hfill \end{array}$$

with K _a > 0, where K _a is diagonal and positive definite; d4) Calculating the optimized position $x_a^{Opt}$

by integrating the optimized speeds ${\dot{x}}_a^{Opt}$

over time; and d5) assembling the optimized position $x_a^{Opt}$

for the variable degrees of freedom with the reference position $x_{\mathrm{and}}^{\mathrm{right}eƒ}$

of the invariable degrees of freedom ${\left(x^{cm\mathrm{i.e}}\right)}^T=\left({\left(x_a^{Opt}\right)}^T,{\left(x_{\mathrm{and}}^{\mathrm{right}eƒ}\right)}^T\right)$

and as a whole body movement. Method for balancing according to one of the preceding claims, wherein in method step a) the following steps are additionally carried out: a1) if there are a plurality of supporting surfaces, replacing the plurality of supporting surfaces with a single virtual supporting surface whose center x _v is divided by the center of the several supporting surfaces is formed and whose area, which corresponds to the sum of the several supporting surfaces, a2) Calculation of the contact torque τ _v ∈ ℝ ³ of the virtual contact area to: $${\tau }_v=\underset{A{\mathrm{i.e}}_{v,\mathrm{right}Ot}^{-T}}{\underbrace{\left[{\widehat{x}}_{v,c}I\right]}}\left(w_c^{ƒƒ}-w_G+w_c^{imp}\right),$$

where x̂ _v,c is the cross product matrix of the vector x _v , _c = x _c − x _v between the centroid position x _c and the bearing surface position x _v , where the rotation part of the inverted adjoint matrix of the virtual bearing surface, which by construction has a full rank has is represented by $A{\mathrm{i.e}}_{v,\mathrm{right}Ot}^{-T};$

a3) further splitting of the contact torques in step a2) to: $${\tau }_v=\underset{{\tau }_v^{Hip}}{\underbrace{A{\mathrm{i.e}}_{v,\mathrm{right}Ot}^{-T}{w}_c^{ƒƒ}}}+\underset{{\tau }_v^{ankle}}{\underbrace{A{\mathrm{i.e}}_{v,\mathrm{right}Ot}^{-T}\left(-w_G+w_c^{imp}\right)}},$$

whereby ${\tau }_v^{Hip}$

τ _v represents the contact torques generated by a voluntary movement of the upper body, here in terms of the feedforward terms, and is therefore associated with a hip strategy. The contact torques induced by gravity and impedance terms are accounted for by ${\tau }_v^{ankle}$

collectively, which are considered an ankle strategy because they are the resultant State the contact torque that would exist if a fixed posture were maintained, ie when $w_c^{ƒƒ}=0;$

a4) Rephrase the feedforward terms in (8) which are rephrased with augmented inertia and Coriolis matrix as follows: $$\underset{w_c^{ƒƒ}}{\underbrace{\left(\begin{array}{c}{ƒ}_c^{ƒƒ}\\ {\tau }_b^{ƒƒ}\end{array}\right)}}=\underset{M_c}{\underbrace{\left[\begin{array}{ccc}mI& 0& 0\\ 0& M_{\omega \omega }& M_{\omega q}\end{array}\right]}}\left(\begin{array}{c}{\ddot{x}}_c^{cm\mathrm{i.e}}\\ {\dot{\omega }}_b^{cm\mathrm{i.e}}\\ {\ddot{q}}^{cm\mathrm{i.e}}\end{array}\right)+\underset{C_c}{\underbrace{\left[\begin{array}{c}0\\ C_{\omega }\end{array}\right]}}\left(\begin{array}{c}{\dot{x}}_c^{cm\mathrm{i.e}}\\ {\omega }_b^{cm\mathrm{i.e}}\\ {\dot{q}}^{cm\mathrm{i.e}}\end{array}\right),$$

where the pilot force and torque ${ƒ}_c^{ƒƒ}$

or. ${\tau }_b^{ƒƒ}$

are. The feedforward torque can be interpreted as the rate of change of the centroid angular momentum resulting from the commanded trajectory transformed from the task space via inverse kinematics (5). The commanded center of gravity velocity and acceleration, and hence also ${ƒ}_c^{ƒƒ},$

is defined as zero for balancing scenarios. As a result, the hip contact torque is simplified to $${\tau }_v^{Hip}={\tau }_b^{ƒƒ}={\dot{l}}_c^{cm\mathrm{i.e}},$$

Where ${\dot{l}}_c^{cm\mathrm{i.e}}$

is the rate of change of commanded centroid angular momentum. Procedure for balancing claim 4 , where the limits for the xy-plane are calculated based on the constraint that the combined center of pressure p _v ∈ ℝ ² is within a virtual bearing surface Sv $$p_v=\frac{1}{{ƒ}_{v,\mathrm{e.g}}}\left(\begin{array}{c}-{\tau }_{v,y}\\ {\tau }_{v,x}\end{array}\right)\in S_v,$$

where f _v,Z is the total vertical force applied to the ground. Procedure for balancing claim 5 , where in step b) the contact torque rext is the contact torque of the ankle ${\tau }_v^{ankle}$

for exceeding a predefined threshold ${\tau }_v^{tH\mathrm{right}es}$

is verified, determining the threshold as a function of the contact torque limits: $${\tau }_v^{tH\mathrm{right}es}=a{\tau }_v^{min/max},$$

with the design parameter α ∈ [0, 1] and where the rate of change of the reference centroid angular momentum is defined as the difference of the ankle torque and its threshold $$l_c^{\mathrm{right}eƒ}\left(t\right)={\tau }_v^{tH\mathrm{right}es}\left(t\right)-{\tau }_v^{ankle}\left(t\right).$$

This difference can be interpreted as the additional hip torque in (19) needed to keep τ _v within its limits. The corresponding reference centroid momentum $l_c^{\mathrm{right}eƒ}\left(t\right)$

can be obtained by integrating (24) with respect to time, defining the time t within the interval t ∈ [t ₁ , t ₂ ), where the moment of activation of the first phase is characterized by t = t ₁ and phase 1 is completed when the ankle torque is again below the predefined threshold, ie $\left|{\tau }_v^{ankle}\left(t\right)\right|<{\tau }_v^{tH\mathrm{right}es}\left(t\right),$

this point in time is defined by t=t ₂ . procedure after claim 6 , wherein in method step c3) the total duration for the reduction of the reference centroid impulse to 0 is defined as T = t ₃ - t ₂ with t ₂ as the start time and t ₃ as the end time of the reduction of the reference centroid angular momentum, specification of the reference centroid angular momentum via the following relationship: $$l_c^{\mathrm{right}eƒ}\left(t\right)={\displaystyle \sum _{j=0}^3{a}_j\frac{{\left(t-t_2\right)}^j}{T^j}},$$

Determine the of T based on the critical points of ${\dot{l}}_c^{\mathrm{right}eƒ}$

as the relationship between the total duration T, the reference centroid angular momentum at the beginning of the reduction of the centroid angular momentum $l_{c,t_2}^{\mathrm{right}eƒ},$

Phase 2, and its maximum rate of change ${\dot{l}}_c^{max}>0$

received as follows: $$T=\frac{3\left|l_{c,t_2}^{\mathrm{right}eƒ}\right|}{2{\dot{l}}_c^{max}}.$$

whereby ${\dot{l}}_c^{max}$

is chosen as a function of the highest rate of change of centroid angular momentum during phase 1, obtained by: $${\dot{l}}_c^{max}=\beta \left(\underset{t_1\le t<t_2}{\text{Max}}\left|{\dot{l}}_c^{\mathrm{right}eƒ}\left(t\right)\right|\right),$$

with the design parameter β defined within the interval β ∈ (0, 1]. Method for whole-body control of a robot to compensate for external disturbances affecting the robot, comprising: at least two body segments, at least one joint with the number of n degrees of freedom for the articulated connection of the body segments with at least one actuating device for actively influencing the Position q of the at least one joint, and a support surface; the control loop behavior for controlling the at least one actuating device being: $$M\left(\begin{array}{c}\mathrm{\Delta }{\dot{v}}_c\\ \mathrm{\Delta }\ddot{q}\end{array}\right)+C\left(\begin{array}{c}\mathrm{\Delta }{v}_c\\ \mathrm{\Delta }\dot{q}\end{array}\right)={\tau }_{ext}-J^T\left(\begin{array}{c}{w}_c^{imp}\\ w_{G\mathrm{right}ƒ}\\ {\tau }_{ƒ}^{imp}\end{array}\right),$$

where ^w _{grf denotes} the combined contact deflections of the robot's support surface and the deviation from the commanded trajectories by $\mathrm{\Delta }v=v_c-v_c^{cm\mathrm{i.e}}\text{and}\mathrm{\Delta }\dot{q}=\dot{q}-{\dot{q}}^{cm\mathrm{i.e}}$

be reproduced; wherein the commanded trajectories x ^cmd are generated in the task space via the method according to one of Claims 1 until 6 and the corresponding commanded center of gravity and joint values are calculated via inverse kinematics: $$\left(\begin{array}{c}{v}_c^{cm\mathrm{i.e}}\\ {\dot{q}}^{cm\mathrm{i.e}}\end{array}\right)=J^{-1}{\dot{x}}^{cm\mathrm{i.e}}.$$

where the centroid-associated impedances are defined by: $$w_c^{imp}=\left(\begin{array}{c}{K}_c\left(x_c-x_c^{cm\mathrm{i.e}}\right)+D_c\left({\dot{x}}_c-{\dot{x}}_c^{cm\mathrm{i.e}}\right)\\ {\tau }_{\mathrm{right}}\left({\mathrm{\Sigma }}_b,{\left(R_b^{cm\mathrm{i.e}}\right)}^T{R}_b\right)+B_b\left({\omega }_b-{\omega }_b^{cm\mathrm{i.e}}\right)\end{array}\right),$$

where the linear and rotational stiffness matrices K _c > 0 and Σ _b > 0 and the linear and rotational damping matrices D _c > 0 and B _b > 0 are symmetric and positive definite; where the Cartesian alignment of the hip from a virtual torsional spring ${\tau }_{\mathrm{right}}\left({\mathrm{\Sigma }}_b,{\left(R_b^{cm\mathrm{i.e}}\right)}^T{R}_b\right)$

is controlled while the impedance of the joint task through $${\tau }_{ƒ}^{imp}=K_{ƒ}\left(q_{ƒ}-q_{ƒ}^{cm\mathrm{i.e}}\right)+D_{ƒ}\left({\dot{q}}_{ƒ}-{\dot{q}}_{ƒ}^{cm\mathrm{i.e}}\right),$$

is realized with the positive definite linear spring and damper matrices K _ƒ > 0 and D _ƒ > 0 . Calculating the final control torques: $$\tau =M_q\left(\begin{array}{c}{\dot{v}}_c^{cm\mathrm{i.e}}\\ {\ddot{q}}^{cm\mathrm{i.e}}\end{array}\right)+C_q\left(\begin{array}{c}{v}_c^{cm\mathrm{i.e}}\\ {\dot{q}}^{cm\mathrm{i.e}}\end{array}\right)-{\left(J\text{'}\right)}^T{w}_{G\mathrm{right}ƒ}-S_{ƒ}^T{\tau }_{ƒ}^{imp}.$$

Controller comprising at least one computing unit and one memory unit, wherein on the memory unit instructions for executing the balancing method according to one of Claims 1 until 7 are stored by the processing unit. Robot comprising: at least two body segments, at least one joint for the articulated connection of the body segments with at least one adjusting device for actively influencing the position of the at least one joint, as well as a support surface and a computing unit, which uses a method according to one of Claims 1 until 7 executes

Images