Center of mass trajectory generating method, robot and computer-readable storage medium

A method for generating a center of mass (CoM) trajectory includes determining an actual pose of a center of mass (CoM), a pose of a left foot, and a pose of a right pose of a robot; determining a first pose tracking vector of the robot according to the actual pose of the CoM and the pose of the left foot, and determining a second pose tracking vector of the robot according to the actual pose of the CoM and the pose of the right foot; and controlling a desired pose of the CoM of the robot to alternately track the pose of the left foot and the pose of the right foot, according to the first pose tracking vector and the second pose tracking vector, so as to generate a desired CoM trajectory of the robot.

BACKGROUND

1. Technical Field

The present disclosure generally relates to robots, and particularly to a center of mass (CoM) trajectory generating method, a robot, and a computer-readable storage medium.

2. Description of Related Art

A key issue in humanoid robot research is to maintain walking stability while increasing walking speed. Generally, as the pace increases, the step length of a robot will also increase, which causes that the center of mass cannot be tracked, resulting in divergent gait, poor stability, and even a fall of the robot.

Therefore, there is a need to provide a method to overcome the above-mentioned problems.

DETAILED DESCRIPTION

For convenience of description, in one embodiment, a world coordinate system Σwas shown inFIG.1may be established. In this coordinate system, the x-axis extends along the walking direction of the robot and is parallel to the frontal axis running through the body of the robot horizontally from back to front, the y-axis is parallel to the sagittal axis running through the body of the robot horizontally from left to right, and the z-axis is parallel to the vertical axis running through the body of the robot vertically from top to bottom.FIG.2shows the corresponding relationship between the coordinate axes and the rotation direction. As shown in the figure, the direction of rotation around the x-axis is rx, and the angle describing the magnitude of the rotation about the x-axis is referred to as a roll angle. The direction of rotation around the y-axis is ry, and the angle describing the magnitude of the rotation about the y-axis is referred to as a pitch angle. The direction of rotation around the z-axis is rz, and the angle describing the magnitude of the rotation about the z-axis is referred to as a yaw angle.

FIG.3shows a schematic block diagram of a robot according to one embodiment. The robot may include a processor101, a storage102, and one or more executable computer programs103that are stored in the storage102. The processor101is electrically connected to the storage105, and performs corresponding operations by executing the executable computer programs103stored in the storage102. When the processor101executes the computer programs103, the steps in the embodiments of the method for controlling the robot, such as steps S301to S303inFIG.4are implemented.

The processor101may be a central processing unit (CPU), a general-purpose processor, a digital signal processor (DSP), an application specific integrated circuit (ASIC), a field-programmable gate array (FPGA), a programmable logic device, a discrete gate, a transistor logic device, or a discrete hardware component. The general-purpose processor may be a microprocessor or any conventional processor or the like.

The storage102may be an internal storage unit of the robot, such as a hard disk or a memory. The storage102may also be an external storage device of the robot, such as a plug-in hard disk, a smart memory card (SMC), and a secure digital (SD) card, or any suitable flash cards. Furthermore, the storage102may also include both an internal storage unit and an external storage device. The storage102is used to store computer programs, other programs, and data required by the robot. The storage102can also be used to temporarily store data that have been output or is about to be output.

Exemplarily, the one or more computer programs103may be divided into one or more modules/units, and the one or more modules/units are stored in the storage102and executable by the processor101. The one or more modules/units may be a series of computer program instruction segments capable of performing specific functions, and the instruction segments are used to describe the execution process of the one or more computer programs103in the robot. For example, the one or more computer programs112may be divided into a pose determining module, a pose tracking vector determining module, and a tracking module as shown inFIG.15.

Referring toFIG.4, in one embodiment, a method for generating a center of mass (CoM) trajectory may include the following steps S301to S303.

Step S301: Determine an actual pose of a center of mass (CoM), a pose of a left foot, and a pose of a right pose of a robot.

The poses mentioned in the embodiments of the present disclosure all include a position and an attitude. The position is specified by an x-coordinate, a y-coordinate, and a z-coordinate in the coordinate system above, and the attitude is specified by the roll angle, pitch angle, and yaw angle.

Step S302: Determine a first pose tracking vector of the robot according to the actual pose of the CoM and the pose of the left foot, and determine a second pose tracking vector of the robot according to the actual pose of the CoM and the pose of the right foot.

The first pose tracking vector may include a first position tracking vector and a first attitude tracking vector, and the second pose tracking vector may include a second position tracking vector and a second attitude tracking vector.

Regarding the position, in one embodiment, an analysis may be performed based on the actual feet of the robot. The vector from the actual position of the center of mass (CoM) of the robot to the position of the left foot is determined as the first position tracking vector of the robot, and a vector from the actual position of the CoM of the robot to the position of the right foot is determined as the second position tracking vector of the robot.FIG.5shows a schematic diagram of the actual feet model of the robot, where T represents the center of mass of the robot, F1represents the left foot of the robot, F2represents the right foot of the robot, {right arrow over (dL)} represents the vector from the actual position of the center of mass to the position of the left foot, {right arrow over (DL)}=(dLx, dLy, dLz), dLx, dLy, and dLzare the components of the vector on the x-axis, y-axis and z-axis, respectively, {right arrow over (dR)} represents the vector from the actual position of the center of mass to the position of the right foot, {right arrow over (dR)}=(dRx, dRy, dRz), dRx, dRy, and dRzare the components of the vector on the x-axis, y-axis and z-axis, respectively.

In another embodiment, an analysis can be performed based on the virtual feet of the robot. The vector from the actual position of the CoM of the robot to the position of a virtual left foot is determined as the first position tracking vector of the robot, and the vector from the actual position of the CoM of the robot to the position of a virtual right foot is determined as the second position tracking vector of the robot.FIG.6shows a schematic diagram of the virtual feet model of the robot, where F′1represents the virtual left foot, F′2represents the virtual right foot, {right arrow over (dL)} represents the vector from the actual position of the CoM to the position of the virtual left foot, and {right arrow over (dL)}=(dL′x, dL′y,dL′z), dL′x, dL′y, and dL′zare the components of the vector on the x-axis, y-axis and z-axis, respectively, {right arrow over (dR)} is the vector from the actual position of the CoM to the position of the virtual right foot, and {right arrow over (dR)}=(dR′x, dR′y, dR′z), dR′x, dR′yand dR′zare the components of the vector on the x-axis, y-axis and z-axis, respectively.

The relationship between {right arrow over (dL)} and {right arrow over (dL)}, and the relationship between {right arrow over (dR)}→and {right arrow over (dR)} can be set according to the actual situation. For example, in the x-axis direction and the y-axis direction, the relationship can be set as shown in the following equations:
dL′x=a1dLx+b1;
dR′x=a2dRx+b2;
dL′y=a3dLy+b3;
dR′y=a4dRy+b4;
where a1, b1, a2, b2, a3, b3, a4, and b4are preset parameters, and the specific values of them can be set according to the actual situation.

Taking dL′x=a1dLx+b1as an example, if a fast CoM tracking is needed during walking of the robot, b1can be set to a positive value. When dLx>0 (i.e., the left foot is ahead of the right foot), a1can be set to a value greater than 1 such that the left foot can be tracked well. When dLx≤0 (i.e., the right foot is ahead of the left foot), a1can be set to a value less than 1 such that the right foot can be tracked well.FIG.7shows a schematic diagram of the relationship between dLxand dL′x, where the horizontal axis is the time axis, and the upper curve is the curve of dLxchanging with time, the lower curve is the curve of dL′xchanging with time, and dL′xand dLxhave a linear relationship.

In the z-axis direction, as shown inFIG.8, the relationship can be set as shown in the following equations: dL′z=dLz+Δz1and dR′z=dRz+Δz2, where Δz1and Δz2represent preset parameters, and the specific values of them can be set according to the actual situation. For example, the amount of change in position of the left foot and the amount of change in position of the right foot can be calculated based on conventional foot impedance control algorithms, and set them as Δz1and Δz2, respectively.

Regarding the attitude, the vector from the actual attitude of the CoM of the robot to the attitude of the left foot can be determined as the first attitude tracking vector of the robot, and the vector from the actual attitude of the CoM of the robot to the attitude of the right foot can be determined as the second attitude tracking vector of the robot.

Here, the vector from the actual attitude of the CoM to the attitude of the left foot is represented by {right arrow over (dLr)}, and {right arrow over (dLr)}=(dLrx, dLry, dLrz), where dLrx, dLry, and dLrzare the components of the vector in the three rotational directions, respectively, and the quaternion corresponding to this vector is ql. The vector from the actual attitude of the CoM to the attitude of the right foot is represented by {right arrow over (dRr)}, and {right arrow over (dRr)}=(dRrx, dRry, dRrz), where dRrx, dRry, and dRrzare the components of the vector in the three rotational directions, respectively, and the quaternion corresponding to this vector is qr.

Step S303: Control a desired pose of the CoM of the robot to alternately track the pose of the left foot and the pose of the right foot, according to the first pose tracking vector and the second pose tracking vector, so as to generate a desired CoM trajectory of the robot.

There are many paths for the CoM to track the feet alternately.FIG.9shows two ideas of linear interpolation (i.e., path 1) and spherical interpolation (i.e., path 2).

Path 1 can be used for position interpolation, but cannot be used for attitude interpolation. Since the arc length near the middle of path 1 is longer, and the arc length near the two ends is shorter, this means that when the time changes uniformly, the angular velocity representing the attitude vector does not change uniformly. Therefore, for the attitude, path 2 can be used for spherical interpolation. When path 2 interpolation is used, the Euler angle attitude is usually converted into a quaternion attitude.

Specifically, in terms of position, the desired position of the CoM of the robot can be controlled to alternately track the position of the left foot and the position of the right foot according to the following equations:

FIG.9is a schematic diagram of the desired position of the CoM alternately tracking the positions of the two feet, where the horizontal axis is the time axis. Taking the x-axis direction as an example, the two solid lines are the curves of dLxand dRxchanging with time, respectively, and the dotted line is the curve of x1changing with time.

It should be noted that the above process is for the actual feet model. For the virtual feet model, dLxneeds to be replaced with dL′x, dRxneeds to be replaced with dR′x, dLyneeds to be replaced with dL′y, dRyneeds to be replaced with dR′y, dLzneeds to be replaced with dL′z, and dRzneeds to be replaced with dR′z.

Regarding the attitude, the desired attitude of the CoM of the robot can be controlled to alternately track the attitude of the left foot and the attitude of the right foot according to the following equations:

q1=q1⁢sin⁡((1-s4(t))θ)+qr⁢sin⁡(s4(t)θ)sinθql-1⁢qr=(x,y,z,w)θ=2*atan⁢2⁢(x2+y2+z2,w)
where qlrepresents a quaternion of the desired attitude of the CoM of the robot, θ represents the angle between qland qr, and s4(t) represents a preset activation function.

The activation functions used in the embodiments of the present disclosure will be described in detail below. For s1(t), s2(t), and s3(t) for position control, their domain is [0, +∞), and their range is [−1,1]. For s4(t) for attitude control, the domain is [0, +∞), and the range is [0, 1]. These activation functions are all periodic functions, and their period is the gait cycle of the robot.

In one embodiment, the activation functions can be set according to the following equations:

ω=2⁢πT,
T represents a gait cycle of the robot, ϕ1, ϕ2, ϕ3, and ϕ4are preset phases respectively, and their specific values can be set according to actual situations.

A more general description of the generation process of the original activation functions will be made below.

In one embodiment, a monotonically increasing function whose domain and range are both [0, 1] can be used as an activation metafunction. Such functions are generally few, but there are many monotonically increasing functions. Here, a monotonically increasing function defined in a certain interval can be transformed into an activation metafunction through standard transformation. For example, the known function y=h(x) is a monotonic function on the domain [a1, a2], and its range is [b1, b2]. Then its standard transformation is as follows:

FIG.11shows the activation metafunctions obtained by standard transformation of several commonly used functions including the sine function (sin), the hyperbolic tangent function (tan h), the trigonometric function (triag) and the trapezoidal function (trape). The activation metafunctions are denoted by j=f1(c), where c∈[0, 1], and j∈[0, 1].

In one embodiment, the activation basis function s=f2(t, T, ϕ) may be used to transform an activation metafunction into an original activation function.FIG.12shows the excitation basis function with a period of T=2 and a phase of ϕ=0.

The general original excitation function can be expressed as g(t)=sign(s)f1(|s|). An intermediate state s can be calculated by substituting the time t, period T and phase φ into the activation basis function, and the original activation function can be calculated by substituting the sign function sign(s) of the intermediate state s and the absolute value |s| into the activation metafunction.

FIG.13is a schematic diagram of the original activation functions corresponding to the activation metafunctions inFIG.11. It can be seen from the figure that the proportion of the four curves is between 0.8 and 1, the hyperbolic tangent activation curve has the longest time, and the triangular activation curve is the shortest. The longer the proportion of time, the closer to the peak value. The closer to the peak value, the longer the center of mass is in the position near the feet, and the shorter the time in the middle of the two feet, indicating that the tracking effect of the center of mass is satisfactory.

In another embodiment, activation functions can also be set according to the following equations:
s1(t)=g1(t)*flag
s2(t)=g2(t)*flag
s3(t)=g3(t)*flag
s4(t)=g4(t)*flag

flag={1,t≤N*T/20,t>N*T/2
where N is the preset number of steps of the robot, and its specific value can be set according to the actual situation. In this way, the robot can be controlled to stop quickly when the number of steps reaches the preset number, which is more suitable for situations that require precise walking, such as humanoid robot performances on large stages.

By implementing the method including steps S301to S303disclosed in the embodiments above, the CoM trajectory is generated according to the control idea of CoM alternately tracking the feet, which effectively improves the stability of the robot.

It should be understood that in the embodiments of the present disclosure, sequence numbers of steps do not mean an execution order. The execution order of the steps should be determined according to functions and internal logic of the steps, and should not be construed as any limitation on the implementation processes of the embodiments of the present disclosure.

Corresponding to the method for generating a CoM trajectory described in the embodiments above,FIG.15shows a block diagram of a CoM trajectory generating device according to an embodiment of the present disclosure.

In one embodiment, the CoM trajectory generating device may include a pose determining module1401, a pose tracking vector determining module1402, and a tracking module1403. The pose determining module1401is to determine the actual pose of the CoM, the pose of the left foot, and the pose of the right pose of a robot. The pose tracking vector determining module1402is to determine a first pose tracking vector of the robot according to the actual pose of the CoM and the pose of the left foot, and determine a second pose tracking vector of the robot according to the actual pose of the CoM and the pose of the right foot. The tracking module1403is to control a desired pose of the CoM of the robot to alternately track the pose of the left foot and the pose of the right foot, according to the first pose tracking vector and the second pose tracking vector, so as to generate a desired CoM trajectory of the robot.

In one embodiment, the tracking module1403may include a position tracking unit. The position tracking unit is to controlling a desired position of the CoM of the robot to alternately track a position of the left foot and a position of the right foot according to the following equations:

x1=12⁢((dLx+dRx)+(dLx-dRx)⁢s1(t))y1=12⁢((dLy+dRy)+(dLy-dRy)⁢s2(t))z1=12⁢((dLz+dRz)+(dLz-dRz)⁢s3(t))
where (dLx, dLy, dLz) represents a first position tracking vector of the robot, (dRx, dRy, dRz) represents a second position tracking vector of the robot, t represents a time variable, s1(t), s2(t)s3(t) represent preset activation functions, and (x1,y1, z1) represents desired position of the CoM of the robot.

In one embodiment, the activation functions are set according to the following equations:
s1(t)=g1(t)*flag
s2(t)=g2(t)*flag
s3(t)=g3(t)*flag

flag={1,t≤N*T/20,t>N*T/2
where g1(t), g2(t)g3(t) represent preset original activation functions, N represents a preset number of steps of the robot, and T represents a gait cycle of the robot.

In one embodiment, the pose tracking vector determining module1402may include a first position tracking vector determining unit and a second position tracking vector determining unit. The first position tracking vector determining unit is to determine a vector from an actual position of the CoM to a position of the left foot as a first position tracking vector of the robot. The second position tracking vector determining unit is to determining a vector from an actual position of the CoM to a position of the right foot as a second position tracking vector of the robot.

In one embodiment, the pose tracking vector determining module1402may further include a first position tracking vector correcting unit and a second t position tracking vector correcting unit. The first position tracking vector correcting unit is to correct the first position tracking vector of the robot to obtain a first corrected position tracking vector according to the following equations:
dL′x=a1dLx+b1;
dL′y=a3dLy+b3;
dL′z=dLz+Δz1;
where (dLx, dLy, dLz) represents the first position tracking vector, a1, b1, a3, b3, and Δz1represent preset parameters, and (dL′x, dL′y, dL′z) represents the first corrected position tracking vector.

The second position tracking vector correcting unit is to correct the second position tracking vector of the robot to obtain a second corrected position tracking vector according to the following equations:
dR′x=a2dRx+b2;
dR′y=a4dRy+b4;
dR′z=dRz+Δz2;
where (dRx, dRy, dRz) represents the second position tracking vector, a2, b2, a4, b4, and Δz2represent preset parameters, and (dR′x, dR′y, dR′z) represents the second corrected position tracking vector.

In one embodiment, the pose tracking vector determining module1402may include a first attitude tracking vector determining unit and a second attitude tracking vector determining unit. The first attitude tracking vector is to determine a vector from an actual attitude of the CoM of the robot to an attitude of the left foot as a first attitude tracking vector of the robot. The second attitude tracking vector determining unit is to determine a vector from the actual attitude of the CoM to an attitude of the right foot as a second attitude tracking vector of the robot.

The tracking module may include an attitude tracking unit that is to control the desired attitude of the CoM of the robot to alternately track the attitude of the left foot and the attitude of the right foot according to the following equation:

q1=ql⁢sin⁡((1-s4(t))⁢θ)+qr⁢sin⁡(s4(t)⁢θ)sin⁢θ
where qlrepresents a quaternion of the first attitude tracking vector of the robot, qrrepresents a quaternion of the second attitude tracking vector of the robot, θ represents the angle between qland qr, t represents a time variable, s4(t) represents a preset activation function, and q1represents a quaternion of the desired attitude of the CoM of the robot. In one embodiment, the activation function is set according to the following equations:
s4(t)=g4(t)*flag;

flag={1,t≤N*T/20,t>N*T/2:
where g4(t) represents a preset original activation function, N represents a preset number of steps of the robot, and T represents a gait cycle of the robot.

A person skilled in the art can clearly understand that for the purpose of convenient and brief description, for specific working processes of the device, modules and units described above, reference may be made to corresponding processes in the embodiments of the foregoing method, which are not repeated herein.

In the embodiments above, the description of each embodiment has its own emphasis. For parts that are not detailed or described in one embodiment, reference may be made to related descriptions of other embodiments.

A person having ordinary skill in the art may clearly understand that, for the convenience and simplicity of description, the division of the above-mentioned functional units and modules is merely an example for illustration. In actual applications, the above-mentioned functions may be allocated to be performed by different functional units according to requirements, that is, the internal structure of the device may be divided into different functional units or modules to complete all or part of the above-mentioned functions. The functional units and modules in the embodiments may be integrated in one processing unit, or each unit may exist alone physically, or two or more units may be integrated in one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional unit. In addition, the specific name of each functional unit and module is merely for the convenience of distinguishing each other and are not intended to limit the scope of protection of the present disclosure. For the specific operation process of the units and modules in the above-mentioned system, reference may be made to the corresponding processes in the above-mentioned method embodiments, and are not described herein.

A person having ordinary skill in the art may clearly understand that, the exemplificative units and steps described in the embodiments disclosed herein may be implemented through electronic hardware or a combination of computer software and electronic hardware. Whether these functions are implemented through hardware or software depends on the specific application and design constraints of the technical schemes. Those ordinary skilled in the art may implement the described functions in different manners for each particular application, while such implementation should not be considered as beyond the scope of the present disclosure.

In the embodiments provided by the present disclosure, it should be understood that the disclosed apparatus (device)/terminal device and method may be implemented in other manners. For example, the above-mentioned apparatus (device)/terminal device embodiment is merely exemplary. For example, the division of modules or units is merely a logical functional division, and other division manner may be used in actual implementations, that is, multiple units or components may be combined or be integrated into another system, or some of the features may be ignored or not performed. In addition, the shown or discussed mutual coupling may be direct coupling or communication connection, and may also be indirect coupling or communication connection through some interfaces, devices or units, and may also be electrical, mechanical or other forms.

The functional units and modules in the embodiments may be integrated in one processing unit, or each unit may exist alone physically, or two or more units may be integrated in one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional unit.

When the integrated module/unit is implemented in the form of a software functional unit and is sold or used as an independent product, the integrated module/unit may be stored in a non-transitory computer-readable storage medium. Based on this understanding, all or part of the processes in the method for implementing the above-mentioned embodiments of the present disclosure may also be implemented by instructing relevant hardware through a computer program. The computer program may be stored in a non-transitory computer-readable storage medium, which may implement the steps of each of the above-mentioned method embodiments when executed by a processor. In which, the computer program includes computer program codes which may be the form of source codes, object codes, executable files, certain intermediate, and the like. The computer-readable medium may include any primitive or device capable of carrying the computer program codes, a recording medium, a USB flash drive, a portable hard disk, a magnetic disk, an optical disk, a computer memory, a read-only memory (ROM), a random-access memory (RAM), electric carrier signals, telecommunication signals and software distribution media. It should be noted that the content contained in the computer readable medium may be appropriately increased or decreased according to the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to the legislation and patent practice, a computer readable medium does not include electric carrier signals and telecommunication signals.

The embodiments above are only illustrative for the technical solutions of the present disclosure, rather than limiting the present disclosure. Although the present disclosure is described in detail with reference to the above embodiments, those of ordinary skill in the art should understand that they still can modify the technical solutions described in the foregoing various embodiments, or make equivalent substitutions on partial technical features; however, these modifications or substitutions do not make the nature of the corresponding technical solution depart from the spirit and scope of technical solutions of various embodiments of the present disclosure, and all should be included within the protection scope of the present disclosure.