Method of tracking control for foot force and moment of biped robot

Abstract

The present invention discloses a method of tracking control for a foot force and moment of a biped robot. According to the method, a double-spring damping model is designed, and a force tracking controller is designed by using an LQR optimization method, so as to realize tracking of the foot force and moment of the biped robot. Further, a desired force on a foot and a desired moment on the foot are calculated through a planned ZMP distribution method, thereby eventually achieving better ZMP tracking of the biped robot and adapting to ground of certain unevenness. According to the present invention, the traditional control method of ZMP tracking to realize stable walking of a biped robot and adapting to uneven ground is abandoned; instead, a desired force and moment on a foot enabling stable walking of the robot are directly calculated, and direct control is performed to realize tracking of the force and moment on the foot, so as to carry out stable control in a more essential and easy-to-implement manner, thereby achieving faster control response, stronger capability of adapting to uneven ground, and ideal ZMP tracking effect.

Claims

1. A method of tracking control for a foot force and moment of a biped robot, comprising: distributing a desired force on a foot and a desired moment on the foot of the robot through a planned zero moment point (ZMP) of the robot, and calculating differences of the desired force on the foot and the desired moment on the foot from an actual force on the foot and an actual moment on the foot, respectively, wherein, the calculated differences are used as input of a force tracking controller, the force tracking controller outputs position adjustment quantities in three directions of an ankle joint, the position adjustment quantities are added to original ankle joint trajectory planning to obtain an adjusted ankle joint trajectory, and joint angles are obtained through inverse kinematics, so as to realize tracking of the foot force and moment of the robot and then realize ZMP tracking, and an expression of the force tracking controller is:
e=dsd(F,F.sub.old,e.sub.old,ė.sub.old,T.sub.CONTROL) where e is a sum of deformation quantities of two spring damping systems, F is an input external force, F.sub.old is an input force of a previous control period, e.sub.old is an output position adjustment quantity of the previous control period, ė.sub.old is an output position adjustment quantity derivative of the previous control period, and T.sub.CONTROL is a control period.

2. The method of the tracking control for the foot force and moment of the biped robot according to claim 1, wherein the three directions of the ankle joint comprise a vertical direction, a pitch direction, and a roll direction of the ankle joint.

3. The method of the tracking control for the foot force and moment of the biped robot according to claim 1, wherein the actual force on the foot and the actual moment on the foot are calculated through a six-dimensional force sensor in an ankle of the robot.

4. The method of the tracking control for the foot force and moment of the biped robot according to claim 1, wherein the desired force on the foot and the desired moment on the foot are calculated through a planned ZMP distribution method.

5. The method of the tracking control for the foot force and moment of the biped robot according to claim 4, wherein the specific process of calculation through the planned ZMP distribution method is: (1) calculating desired forces and moments on feet in a single-foot support period, comprising: calculating and distributing desired forces on feet in vertical directions of a left foot and a right foot according to a ratio of distances between the ZMP and ankle joints of two feet, and then respectively cross-multiplying vectors from the ZMP to positions of the ankle joints of the two feet by the two desired forces to respectively obtain desired moments on feet of the two feet; and (2) calculating desired forces and moments on feet in a double-foot support period, wherein a calculation method for the desired forces on feet is the same as that in the single-foot support period, and in calculating the desired moments on feet, first, a sum of desired moments of the two feet is calculated using the method for calculating desired moments on feet in the single-foot support period, and then desired moments on feet of the left foot and the right foot are calculated and distributed according to the ratio of the distances between the ZMP and the ankle joints of the two feet.

6. The method of the tracking control for the foot force and moment of the biped robot according to claim 1, wherein the two spring damping systems are connected in series to form a double-spring damping model, and each spring damping system is formed by a spring and a damper connected in parallel.

7. A method of tracking control for a foot force and moment of a biped robot, comprising: distributing a desired force on a foot and a desired moment on the foot of the robot through a planned zero moment point (ZMP) of the robot, and calculating differences of the desired force on the foot and the desired moment on the foot from an actual force on the foot and an actual moment on the foot, respectively, wherein, the calculated differences are used as input of a force tracking controller, the force tracking controller outputs position adjustment quantities in three directions of an ankle joint, the position adjustment quantities are added to original ankle joint trajectory planning to obtain an adjusted ankle joint trajectory, and joint angles are obtained through inverse kinematics, so as to realize tracking of the foot force and moment of the robot and then realize ZMP tracking, and the desired force on the foot and the desired moment on the foot are calculated through a planned ZMP distribution method comprising: (1) calculating desired forces and moments on feet in a single-foot support period, comprising: calculating and distributing desired forces on feet in vertical directions of a left foot and a right foot according to a ratio of distances between the ZMP and ankle joints of two feet, and then respectively cross-multiplying vectors from the ZMP to positions of the ankle joints of the two feet by the two desired forces to respectively obtain desired moments on feet of the two feet; and (2) calculating desired forces and moments on feet in a double-foot support period, wherein a calculation method for the desired forces on feet is the same as that in the single-foot support period, and in calculating the desired moments on feet, first, a sum of desired moments of the two feet is calculated using the method for calculating desired moments on feet in the single-foot support period, and then desired moments on feet of the left foot and the right foot are calculated and distributed according to the ratio of the distances between the ZMP and the ankle joints of the two feet.

8. The method of the tracking control for the foot force and moment of the biped robot according to claim 7, wherein the three directions of the ankle joint comprise a vertical direction, a pitch direction, and a roll direction of the ankle joint.

9. The method of the tracking control for the foot force and moment of the biped robot according to claim 7, wherein the actual force on the foot and the actual moment on the foot are calculated through a six-dimensional force sensor in an ankle of the robot.

10. The method of the tracking control for the foot force and moment of the biped robot according to claim 7, wherein an expression of the force tracking controller is: e=dsd(F, F.sub.old, e.sub.old, ė.sub.old, T.sub.CONTROL), where e is a sum of deformation quantities of two spring damping systems, F is an input external force, F.sub.old is an input force of a previous control period, e.sub.old is an output position adjustment quantity of the previous control period, ė.sub.old is an output position adjustment quantity derivative of the previous control period, and T.sub.CONTROL is a control period.

11. The method of the tracking control for the foot force and moment of the biped robot according to claim 10, wherein the two spring damping systems are connected in series to form a double-spring damping model, and each spring damping system is formed by a spring and a damper connected in parallel.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) FIG. 1 is a flowchart of tracking control for a foot force and moment of a biped robot in the present invention;

(2) FIG. 2 is a schematic diagram of a process that a six-dimensional force sensor calculates an actual force on a foot;

(3) FIG. 3 is a schematic diagram illustrating mapping of a force, in a z direction, on a foot of the biped robot in the present invention;

(4) FIG. 4 is a schematic diagram illustrating planning of ankle joint positions of two feet and a ZMP position of the robot in a world coordinate system;

(5) FIG. 5 is a schematic diagram of a double-spring damping model in the present invention; and

(6) FIG. 6A and FIG. 6B are curve diagrams illustrating experimental data of a tracking effect of a foot force, where FIG. 6A is a curve diagram illustrating a tracking situation of a force on a left foot, and FIG. 6B is a curve diagram illustrating a tracking situation of a force on a right foot.

DETAILED DESCRIPTION OF THE EMBODIMENTS

(7) In order to enable those skilled in the art to better understand the solutions of the present invention, the technical solutions in the embodiments of the present invention will be clearly and fully described below with reference to the accompanying drawings in the embodiments of the present invention. It is obvious that the embodiments to be described are only a part rather than all of the embodiments of the present invention. All other embodiments derived by those of ordinary skill in the art based on the embodiments of the present invention without carrying out creative efforts should fall within the protection scope of the present invention.

(8) FIG. 1 shows a method of tracking control for a foot force and moment of a biped robot. A desired force on a foot and a desired moment on the foot of the robot are distributed through a planned zero moment point (ZMP) of the robot, differences of the desired force on the foot and the desired moment on the foot from an actual force on the foot and an actual moment on the foot are respectively calculated, and the differences are used as input of a force tracking controller, the force tracking controller outputs position adjustment quantities in three directions of an ankle joint (which are respectively a vertical direction, a pitch direction Pitch, and a roll direction Roll of the ankle joint), and the position adjustment quantities of the ankle joint are added to original ankle joint trajectory planning to obtain an adjusted ankle joint trajectory, and joint angles are obtained through inverse kinematics (IK), so as to realize tracking of the foot force and moment of the robot and then realize ZMP tracking.

(9) The actual force on the foot and the actual moment on the foot are measured and calculated through a six-dimensional force sensor in an ankle of the robot. The specific calculation process is as follows:

(10) As shown in FIG. 2, F.sub.z.sensor is a force in a z direction in a foot coordinate system that is measured by the force sensor, F.sub.y.sensor is a force in a y direction in the foot coordinate system that is measured by the force sensor, and τ.sub.sensor is a moment in an x direction in the foot coordinate system that is measured by the force sensor. An actual force and moment of the robot in a waist coordinate system need to be calculated according to the force and moment measured by the force sensor.

(11) When the foot of the robot rotates, the posture of the foot coordinate system is no longer the same as that of the waist coordinate system. As a result, F.sub.z.sensor measured by the force sensor is different from F.sub.z used in the control method of the present invention, and F.sub.z.sensor needs to be first mapped to the waist coordinate system to obtain F.sub.z. As shown in FIG. 3, the coordinate system O is the waist coordinate system of the robot and is an effective coordinate system relative to the robot itself. According to the order of degrees of freedom of the robot leg, the foot first rotates in the Pitch direction and then rotates in the Roll direction, and the rotation angles thereof are respectively q5 and q6. Next, F.sub.z.sensor is projected to the z axis of the coordinate system O to calculate the actual force F.sub.z:
F.sub.z=F.sub.z.senor.Math.cos q.sub.6.Math.cos q.sub.5  (1)

(12) Then, the moment is calculated. Using calculation of the moment in the x direction as an example, the actual moment τ is calculated using the following equation:
τ=F.sub.y.sensor.Math.H.sub.Ankle+τ.sub.sensor  (2)

(13) where H.sub.Ankle is a height difference between a geometric center of the sensor and an axis center of the ankle joint.

(14) The desired force on the foot and the desired moment on the foot of the robot are calculated using a planned ZMP distribution method. First, a desired force on the foot and a desired moment on the foot, namely, a desired force and moment applied by the foot of the robot to the ground, are calculated, and the desired force and the desired moment on the foot are the opposite of the desired force and moment applied by the foot to the ground. As shown in FIG. 4, equations of desired forces and moments in a single-foot support period are as follows:

(15) $\begin{array}{cc}\{\begin{array}{c}{f}_L^d=-\alpha \mathrm{Mg}\\ f_R^d=-\left(1-\alpha \right)\mathrm{Mg}\\ {\tau }_L^d=\left(p_{\mathrm{Ankle\_L}}-p_{\mathrm{ZMP}}^d\right)\times f_L^d\\ {\tau }_R^d=\left(p_{\mathrm{Ankle\_R}}-p_{\mathrm{ZMP}}^d\right)\times f_R^d\end{array}& \left(3\right)\end{array}$

(16) where f.sup.d.sub.L and τ.sup.d.sub.L are the desired force and moment of the left foot, JR and τ.sup.d.sub.R are the desired force and moment of the right foot, Mg is the gravity of the whole body of the robot, p.sub.Ankle_L and p.sub.Ankle_R are respectively desired positions of ankle joints of left and right feet of the robot in the world coordinate system, and p.sup.d.sub.ZMP is a desired position of the ZMP of the robot in the world coordinate system. A midpoint of a line segment connected between two ankles of the robot in a reset state is usually used as a zero point (point O in FIG. 4) of the coordinate system. Besides, a calculation method for the scale factor α is as follows:

(17) $\begin{array}{cc}\alpha =\frac{p_{\mathrm{Ankle\_R}}-p_{\mathrm{ZMP}}^d}{p_{\mathrm{Ankle\_R}}-p_{\mathrm{Ankle\_L}}}& \left(4\right)\end{array}$

(18) where p.sub.Edge_L and p.sub.Edge_R are inner edges of the left and right feet of the robot.

(19) It can be seen from equation (4) that when p.sup.d.sub.ZMP greater than p.sub.Ankle_R, α turns negative, and the desired force on the left foot has an upward direction, which is illogical; when p.sup.d.sub.ZMP is less than p.sub.Ankle_L, (1−α) turns negative, and the desired force on the right foot has an upward direction, which is also illogical. Accordingly, the range of a needs to be limited. The practical meaning is that when the planned ZMP of the robot moves to a place right below or outside the ankle joint of any foot, it is considered that the foot supports the weight of the whole robot. The judgment method is as follows:
if α>1thenα=1
if α<0thenα=0  (5)

(20) Therefore, in the single-foot support period and when the ZMP is within the support area of any foot, the desired force and moment can be calculated using the aforementioned method, but at other time of a double-foot support period, the distribution of moments of two feet needs to be considered. Since a summed moment of the forces and moments on the left and right feet of the robot is zero at the point ZMP, the calculation equation for the summed moment τ of the left and right feet is as follows:
τ=−(p.sub.Ankle_L−p.sup.d.sub.ZMP)×f.sup.d.sub.L−(p.sub.Ankle_R−p.sup.d.sub.ZMP)×f.sup.d.sub.R  (6)

(21) The foot moment distribution in the double-foot support period is as follows:

(22) $\begin{array}{cc}\{\begin{array}{c}{\tau }_L^d=\alpha \tau \\ {\tau }_R^d=\left(1-\alpha \right)\tau \end{array}& \left(7\right)\end{array}$

(23) The calculated desired moment is distributed in two directions of x (roll direction) and y (pitch direction), so as to calculate the respective desired moments of the left and right feet.

(24) The design method of the force tracking controller is as follows:

(25) As shown in FIG. 5, a constructed double-spring damping model is formed by two spring damping systems connected in series, and each spring damping system is formed by a spring and a damper connected in parallel. The spring 1 has a deformation quantity of e1, an elastic coefficient of K1, and a damping coefficient of D1, and the spring 2 has a deformation quantity of e2, an elastic coefficient of K2, and a damping coefficient of D2. Assuming that e1 and e2 of the springs in original length and the sum e of deformation quantities of the two spring damping system are all zero, e=e1+e2, and F is an input external force, the two springs respectively satisfy the following equations:

(26) $\begin{array}{cc}\{\begin{array}{c}{F}_1=K_1{e}_1+D_1{\stackrel{.}{e}}_1\\ F_2=K_2{e}_2+D_2{\stackrel{.}{e}}_2\end{array}& \left(8\right)\end{array}$

(27) Laplace transform is performed on the two equations:

(28) $\begin{array}{cc}\{\begin{array}{c}{F}_1\left(s\right)=\left(K_1+D_{1}s\right)e_1\left(s\right)\\ F_2\left(s\right)=\left(K_2+D_{2}s\right)e_2\left(s\right)\end{array}& \left(9\right)\end{array}$

(29) Laplace transform is performed on the sum of the deformation quantities of the springs to obtain:
e(s)=e.sub.1(s)+e.sub.2(s)  (10)

(30) Equation (9) is substituted into equation (10) to obtain:

(31) $\begin{array}{cc}e\left(s\right)=\frac{K_1+K_2+\left(D_1+D_2\right)s}{K_1{K}_2+\left(K_1{D}_2+K_2{D}_1\right)s+D_1{D}_2{s}^2}F\left(s\right)& \left(11\right)\end{array}$

(32) Inverse Laplace transform is performed to obtain the relationship between e and F:
K.sub.1K.sub.2e+(K.sub.1D.sub.2+K.sub.2D.sub.1)ė+D.sub.1D.sub.2ë=(K.sub.1+K.sub.2)F+(D.sub.1+D.sub.2){dot over (F)}  (12)

(33) The above equation is linearly discretized to obtain the expression of the force tracking controller as follows:
e=dsd(F,F.sub.old,e.sub.old,ė.sub.old,T.sub.CONTROL)  (13)

(34) where F.sub.old is an input force of a previous control period, e.sub.old is an output position adjustment quantity of the previous control period, ė.sub.old is an output position adjustment quantity derivative of the previous control period, and T.sub.CONTROL is a control period.

(35) Coefficients of the force tracking controller are optimized using LQR, after desirable controller coefficients are obtained, an ideal force and moment tracking effect can be achieved, and since the double-spring damping model is used, the foot has a certain compliance effect in actual application. In setting elastic coefficients and damping coefficients of the two springs, they are configured as combinations of large stiffness—medium damping and small stiffness—large damping. The specific data is shown in Table 1:

(36) TABLE-US-00001 TABLE 1 Elastic coefficients and damping coefficients of two springs K1 10000 D1 5700 K2 2000000 D2 2000

(37) In this way, the foot can have an impact-absorbing effect and produce a desirable force and moment tracking effect. FIG. 6A is a curve diagram illustrating the tracking situation of the force on the left foot, and FIG. 6B is a curve diagram illustrating the tracking situation of the force on the right foot. It can be seen from the figures that the actual force can desirably track the desired force. In spite of shaking due to excessive impact and small lag due to system inertia, the overall tracking effect meets control requirements.

(38) The above description is merely preferred embodiments of the present invention, and is not intended to limit the present invention in any form. Although the present invention has been disclosed above through the preferred embodiments, they are not intended to limit the present invention. Those skilled in the art can make many possible changes and modifications to the technical solutions of the present invention or modify the technical solutions of the present invention into equivalent embodiments of equivalent changes, by using the methods and technical content disclosed above without departing from the scope of the technical solutions of the present invention. Therefore, any simple modifications, equivalent changes and modifications made to the above embodiments according to the technical essence of the present invention without departing from the content of the technical solutions of the present invention shall fall within the protection scope of the technical solutions of the present invention.