Foot end position estimation method, device, computer readable storage medium, and robot

Abstract

This application belongs to the field of robotics technology, and particularly relates to a foot position estimation method, apparatus, computer-readable storage medium, and robot. The method includes: performing state estimation on the robot based on a Kalman filter to obtain a state estimation result; acquiring the joint angles of the robot and determining the foot posture based on the joint angles; acquiring the joint torques of the robot and detecting whether the robot's foot is touching the ground based on the joint torques; for a foot touching the ground, determining an ankle position compensation amount based on the foot posture; and compensating the foot position in the state estimation result based on the ankle position compensation amount to obtain a compensated foot position. Through this application, the foot position of a foot touching the ground can be compensated based on the foot posture, thereby obtaining a more accurate foot position estimation result.

Description

Technical Field

[0001] This application belongs to the field of robotics technology, and in particular relates to a method, apparatus, computer-readable storage medium, and robot for estimating foot position. Background Technology

[0002] In existing technologies, robot state estimation can be performed based on Kalman filters to obtain state estimation results, including the position of the feet. However, this method usually ignores the actual physical shape of the feet for ease of calculation, simplifying the feet to a single point, which leads to a large error in the foot position estimation results. Summary of the Invention

[0003] In view of this, embodiments of this application provide a foot position estimation method, apparatus, computer-readable storage medium, and robot to solve the problem of large errors in existing foot position estimation methods.

[0004] A first aspect of this application provides a foot position estimation method, which may include: The robot's state is estimated based on a Kalman filter to obtain the robot's state estimation result; Obtain the joint angles of the robot, and determine the foot posture of the robot based on the joint angles; The joint torque of the robot is obtained, and the robot's feet are detected as touching the ground based on the joint torque. For the foot that touches the ground, the ankle position compensation amount is determined based on the foot posture; The foot position in the state estimation result is compensated based on the ankle position compensation amount to obtain the compensated foot position.

[0005] In one specific implementation of the first aspect, determining the robot's foot posture based on the joint angle may include: Based on the joint angles, positive kinematics calculations are performed to obtain a first rotation matrix; wherein, the first rotation matrix is ​​the rotation matrix of the robot's foot end in the base coordinate system; The first rotation matrix is ​​transformed to the world coordinate system to obtain the second rotation matrix; wherein, the second rotation matrix is ​​the rotation matrix of the robot's foot in the world coordinate system; The foot posture is obtained by performing Euler angle transformation on the second rotation matrix.

[0006] In one specific implementation of the first aspect, detecting whether the robot's foot is touching the ground based on the joint torque may include: The foot contact force of the robot is determined based on the joint torque and the preset Jacobian matrix. If the absolute value of the foot contact force is greater than a preset contact force threshold, the foot contact force corresponding to the foot contact force is determined to be grounded. If the absolute value of the foot contact force is less than or equal to the contact force threshold, it is determined that the foot corresponding to the foot contact force is not in contact with the ground.

[0007] In one specific implementation of the first aspect, determining the ankle position compensation amount based on the foot posture may include: Determine the position vector from the ankle to the support point based on the foot posture; The position compensation amount in the ankle coordinate system is determined based on the foot posture and the position vector; The ankle position compensation amount is obtained by performing a world coordinate system transformation on the position compensation amount in the ankle coordinate system.

[0008] In one specific implementation of the first aspect, determining the position compensation amount in the ankle coordinate system based on the foot posture and the position vector may include: The first position is determined based on the foot posture and the position vector; wherein, the first position is the position of the support point in the ankle coordinate system; The second position is determined based on the foot posture and the position vector; wherein, the second position is the position of the new ankle position in the ankle coordinate system; The position compensation amount in the ankle coordinate system is determined based on the first position and the second position.

[0009] In one specific implementation of the first aspect, the step of compensating the foot position in the state estimation result based on the ankle position compensation amount to obtain the compensated foot position may include: The ankle position compensation is superimposed on the foot position in the state estimation result to obtain the compensated foot position.

[0010] In one specific implementation of the first aspect, after detecting whether the robot's foot is touching the ground based on the joint torque, the method may further include: For feet that are not touching the ground, the foot position in the state estimation result remains unchanged.

[0011] A second aspect of this application provides a foot position estimation device, which may include: The state estimation module is used to perform state estimation of the robot based on a Kalman filter to obtain the state estimation result of the robot. A foot posture determination module is used to acquire the joint angles of the robot and determine the foot posture of the robot based on the joint angles. The ground contact detection module is used to acquire the joint torque of the robot and detect whether the robot's feet are in contact with the ground based on the joint torque; The compensation amount determination module is used to determine the ankle position compensation amount based on the foot posture when the foot touches the ground. The foot position compensation module is used to compensate the foot position in the state estimation result according to the ankle position compensation amount, so as to obtain the compensated foot position.

[0012] In one specific implementation of the second aspect, the foot posture determination module can be specifically used to: perform forward kinematics calculations based on the joint angles to obtain a first rotation matrix; wherein the first rotation matrix is ​​the rotation matrix of the robot's foot in the base coordinate system; perform world coordinate system transformation on the first rotation matrix to obtain a second rotation matrix; wherein the second rotation matrix is ​​the rotation matrix of the robot's foot in the world coordinate system; and perform Euler angle transformation on the second rotation matrix to obtain the foot posture.

[0013] In one specific implementation of the second aspect, the ground contact detection module can be specifically used to: determine the foot contact force of the robot based on the joint torque and a preset Jacobian matrix; determine that the foot is in contact with the ground if the absolute value of the foot contact force is greater than a preset contact force threshold; and determine that the foot is not in contact with the ground if the absolute value of the foot contact force is less than or equal to the contact force threshold.

[0014] In one specific implementation of the second aspect, the compensation amount determination module may include: A position vector determination unit is used to determine the position vector from the ankle to the support point based on the foot posture. A position compensation determination unit is used to determine the position compensation amount in the ankle coordinate system based on the foot posture and the position vector. The coordinate system transformation unit is used to perform world coordinate system transformation on the position compensation amount in the ankle coordinate system to obtain the ankle position compensation amount.

[0015] In one specific implementation of the second aspect, the position compensation determination unit may be specifically used to: determine a first position based on the foot posture and the position vector; wherein the first position is the position of the support point in the ankle coordinate system; determine a second position based on the foot posture and the position vector; wherein the second position is the position of the new ankle position in the ankle coordinate system; and determine the position compensation amount in the ankle coordinate system based on the first position and the second position.

[0016] In one specific implementation of the second aspect, the foot position compensation module can be specifically used to: superimpose the ankle position compensation amount on the foot position in the state estimation result to obtain the compensated foot position.

[0017] In one specific implementation of the second aspect, the foot position compensation module can also be used to: for a foot that is not touching the ground, keep the foot position in the state estimation result unchanged.

[0018] A third aspect of this application provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of any of the above-described foot position estimation methods.

[0019] A fourth aspect of this application provides a robot including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of any of the above-described foot position estimation methods.

[0020] A fifth aspect of this application provides a computer program product that, when run on a robot, causes the robot to perform the steps of any of the above-described foot position estimation methods.

[0021] The beneficial effects of this application embodiment compared with the prior art are as follows: This application embodiment performs state estimation of the robot based on a Kalman filter to obtain the state estimation result of the robot; obtains the joint angles of the robot and determines the foot posture of the robot based on the joint angles; obtains the joint torques of the robot and detects whether the robot's foot is touching the ground based on the joint torques; for the foot touching the ground, determines the ankle position compensation amount based on the foot posture; and compensates the foot position in the state estimation result based on the ankle position compensation amount to obtain the compensated foot position. Through this application embodiment, the foot position of the foot touching the ground can be compensated according to the foot posture, thereby obtaining a more accurate foot position estimation result. Attached Figure Description

[0022] To more clearly illustrate the technical solutions in the embodiments of this application, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0023] Figure 1 This is a flowchart of one embodiment of a foot position estimation method in this application. Figure 2 A schematic flowchart illustrating the determination of the robot's foot posture based on joint angles; Figure 3 A schematic flowchart illustrating whether the robot's feet are touching the ground based on joint torque; Figure 4 A schematic flowchart for determining ankle position compensation based on foot posture; Figure 5 This is a structural diagram of one embodiment of a foot position estimation device according to the present application. Figure 6 This is a schematic block diagram of a robot according to an embodiment of this application. Detailed Implementation

[0024] To make the inventive objectives, features, and advantages of this application more apparent and understandable, the technical solutions in the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the embodiments described below are only some embodiments of this application, and not all embodiments. Based on the embodiments in this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0025] It should be understood that, when used in this specification and the appended claims, the term "comprising" indicates the presence of the described features, integrals, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integrals, steps, operations, elements, components and / or collections thereof.

[0026] It should also be understood that the terminology used in this specification is for the purpose of describing particular embodiments only and is not intended to limit the scope of the application. As used in this specification and the appended claims, the singular forms “a,” “an,” and “the” are intended to include the plural forms unless the context clearly indicates otherwise.

[0027] It should also be further understood that the term “and / or” as used in this application specification and the appended claims means any combination of one or more of the associated listed items and all possible combinations, and includes such combinations.

[0028] As used in this specification and the appended claims, the term "if" may be interpreted, depending on the context, as "when," "once," "in response to determination," or "in response to detection." Similarly, the phrase "if determined" or "if [the described condition or event] is detected" may be interpreted, depending on the context, as "once determined," "in response to determination," "once [the described condition or event] is detected," or "in response to detection of [the described condition or event]."

[0029] Furthermore, in the description of this application, the terms "first," "second," "third," etc., are used only to distinguish descriptions and should not be construed as indicating or implying relative importance.

[0030] In existing technologies, robot state estimation can be performed based on Kalman filters to obtain state estimation results, including the position of the feet. However, this method usually ignores the actual physical shape of the feet for ease of calculation, simplifying the feet to a single point, which leads to a large error in the foot position estimation results.

[0031] In view of this, embodiments of this application provide a foot position estimation method, apparatus, computer-readable storage medium, and robot to solve the problem of large errors in existing foot position estimation methods. In embodiments of this application, the foot position at ground contact can be compensated according to the foot posture, thereby obtaining a more accurate foot position estimation result.

[0032] The executing entity in the embodiments of this application can be a robot, especially a bipedal robot with left and right legs.

[0033] Please see Figure 1 One embodiment of a foot position estimation method in this application may include: Step S101: Perform state estimation on the robot based on the Kalman filter to obtain the state estimation result of the robot.

[0034] In one specific implementation of this application embodiment, the state vector can be set as follows: .in, This represents the robot's base position in the world coordinate system. Let the base velocity of the robot be in the world coordinate system. Let represent the positions of the robot's two feet in the world coordinate system (the position of the left foot and the position of the right foot).

[0035] Each time step of the robot's state estimation ( This can include processes such as state prediction, covariance prediction, observation prediction, constructing actual observations, calculating Kalman gain, state update, and covariance update.

[0036] The state prediction process is shown in the following equation:

[0037] in, Here is the state transition matrix. To control the input matrix, To control the input, , The rotation matrix from the base coordinate system to the world coordinate system can be calculated using the quaternions of the Inertial Measurement Unit (IMU). Let x be the acceleration of the robot in the base coordinate system. This is the acceleration due to gravity.

[0038] The covariance prediction process is shown in the following formula:

[0039] in, Let covariance matrix be the variance matrix. Let be the process noise covariance matrix.

[0040] The observation and prediction process is shown in the following formula:

[0041] in, This is the observation matrix.

[0042] The actual observation process is constructed as shown in the following formula:

[0043]

[0044]

[0045] in, For the observation vector, Let these be the positions of the robot's two feet in the base coordinate system. Let be the velocities of the robot's two feet in the base coordinate system. This is the foot-to-ground indicator; a value of 1 indicates the foot is touching the ground, and a value of 0 indicates the foot is not touching the ground.

[0046] The process for calculating the Kalman gain is shown in the following equation:

[0047]

[0048] in, To observe the noise covariance matrix, The new information covariance matrix, For Kalman gain.

[0049] The state update process is shown in the following equation:

[0050] The covariance update process is shown in the following equation:

[0051] After completing the state estimation, it can be derived from... Extract the robot's state estimation results, i.e.: , , .

[0052] Step S102: Obtain the joint angles of the robot and determine the foot posture of the robot based on the joint angles.

[0053] In one specific implementation of this application embodiment, step S102 may include, for example: Figure 2 The process shown: Step S1021: Perform forward kinematics calculations based on the joint angles to obtain the first rotation matrix.

[0054] The process of performing forward kinematic calculations on joint angles is shown in the following formula:

[0055] in, The joint angles can be read from the robot's encoder. For the positive kinematics calculation process, It is a homogeneous transformation matrix.

[0056] In obtaining Then, the rotation matrix of the robot's feet in the base coordinate system can be extracted from it. Here, we denote it as the first rotation matrix.

[0057] Step S1022: Perform a world coordinate system transformation on the first rotation matrix to obtain the second rotation matrix.

[0058] The process of performing world coordinate system transformation on the first rotation matrix is ​​shown in the following equation:

[0059] in, Let be the rotation matrix of the robot's feet in the world coordinate system, which is denoted here as the second rotation matrix.

[0060] Step S1023: Perform Euler angle transformation on the second rotation matrix to obtain the foot posture.

[0061] The process of performing Euler angle transformation on the second rotation matrix is ​​shown in the following equation:

[0062]

[0063]

[0064] in, Let be the roll angle of the robot's feet in the world coordinate system. Let be the pitch angle of the robot's feet in the world coordinate system. Let yaw angle be the robot's foot tip in the world coordinate system.

[0065] Step S103: Obtain the joint torque of the robot and detect whether the robot's feet are touching the ground based on the joint torque.

[0066] In one specific implementation of this application embodiment, step S103 may include, for example: Figure 3 The process shown: Step S1031: Determine the contact force at the robot's foot end based on the joint torque and the preset Jacobian matrix.

[0067] The calculation process for the contact force at the robot's foot tip is shown in the following formula:

[0068] in, Let be the pseudo-inverse of the Jacobian matrix of the robot's two legs. For the joint torque of the robot, This refers to the contact force at the robot's foot.

[0069] Step S1032: Determine whether the robot's feet are touching the ground based on the contact force at the foot tip.

[0070] In this embodiment, the absolute value of the foot contact force can be compared with a preset contact force threshold. If the absolute value of the foot contact force is greater than the contact force threshold, it can be determined that the foot corresponding to the foot contact force is in contact with the ground; conversely, if the absolute value of the foot contact force is less than or equal to the contact force threshold, it can be determined that the foot corresponding to the foot contact force is not in contact with the ground. As shown in the following formula:

[0071] in, The contact force threshold is a preset value, which can be flexibly set according to the actual situation. This application embodiment does not impose a specific limitation on it. This is the foot-to-ground indicator; a value of 1 indicates the foot is touching the ground, and a value of 0 indicates the foot is not touching the ground.

[0072] Step S104: For the foot that is in contact with the ground, determine the ankle position compensation amount based on the foot posture.

[0073] In one specific implementation of this application embodiment, step S104 may include, for example: Figure 4 The process shown: Step S1041: Determine the position vector from the ankle to the support point based on the foot posture.

[0074] Here, the vector position from the ankle to the support point can be denoted as... The components of the position vector in the forward and backward directions It can be based on the pitch angle in the foot posture. Confirmed, as shown in the following formula:

[0075] in, This is a foot geometry parameter, which can be set to half the foot length. When, support is provided by the toes; when When, it is supported by the sole of the foot; when At this time, the heel provides support.

[0076] Components of the position vector in the left and right directions It can be based on the roll angle in the foot posture. Confirmed, as shown in the following formula:

[0077] in, This is a foot geometry parameter, which can be set to half the width of the foot. When When, the right side of the foot provides support; when When, it is supported by the sole of the foot; when At that time, the left side of the foot provides support.

[0078] Components of the position vector in the height direction The thickness can be determined based on the thickness of the robot's foot, as shown in the following formula:

[0079] in, The thickness of the foot is the distance from the ankle to the sole of the foot.

[0080] Step S1042: Determine the position compensation amount in the ankle coordinate system based on the foot posture and position vector.

[0081] Based on the foot posture and position vector, the position of the support point in the ankle coordinate system, and the position of the new ankle in the ankle coordinate system, can be determined respectively, as shown in the following formula:

[0082]

[0083] , ,

[0084] in, The position of the support point in the ankle coordinate system is denoted as the first position. The new ankle position is located in the ankle coordinate system, and is hereby denoted as the second position.

[0085] After obtaining the first and second positions, the position compensation amount in the ankle coordinate system can be determined based on the first and second positions, as shown in the following formula:

[0086] in, This is the position compensation amount in the ankle coordinate system.

[0087] Step S1043: Perform world coordinate transformation on the position compensation amount in the ankle coordinate system to obtain the ankle position compensation amount.

[0088] The process of performing world coordinate system transformation on the position compensation amount in the ankle coordinate system is shown in the following equation:

[0089] in, This is the compensation amount for the ankle position.

[0090] Step S105: Compensate the foot position in the state estimation result according to the ankle position compensation amount to obtain the compensated foot position.

[0091] For the foot that touches the ground ( An ankle position compensation can be added to the foot position in the state estimation results to obtain the compensated foot position; for the foot that is not in contact with the ground ( This method preserves the foot position in the state estimation result without requiring compensation. As shown in the following equation:

[0092] in, The position of the foot after compensation.

[0093] This compensation method allows ankle compensation to be applied only in the ground-contact state, while maintaining the original estimated position in the non-ground-contact state. This avoids unnecessary corrections and effectively improves the efficiency and stability of foot position estimation.

[0094] In summary, this application embodiment performs state estimation of the robot based on a Kalman filter to obtain the robot's state estimation result; acquires the robot's joint angles and determines the robot's foot posture based on the joint angles; acquires the robot's joint torques and detects whether the robot's foot is touching the ground based on the joint torques; for the foot touching the ground, determines the ankle position compensation amount based on the foot posture; and compensates the foot position in the state estimation result based on the ankle position compensation amount to obtain the compensated foot position. Through this application embodiment, the foot position of the foot touching the ground can be compensated based on the foot posture, thereby obtaining a more accurate foot position estimation result.

[0095] It should be understood that the sequence number of each step in the above embodiments does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of this application.

[0096] Corresponding to the foot position estimation method described in the above embodiments, Figure 5 This diagram illustrates a structural diagram of an embodiment of a foot position estimation device provided in this application.

[0097] In this embodiment, a foot position estimation device may include: The state estimation module 501 is used to perform state estimation of the robot based on a Kalman filter to obtain the state estimation result of the robot. The foot posture determination module 502 is used to acquire the joint angles of the robot and determine the foot posture of the robot based on the joint angles. The ground contact detection module 503 is used to acquire the joint torque of the robot and detect whether the robot's foot is in contact with the ground based on the joint torque. The compensation amount determination module 504 is used to determine the ankle position compensation amount based on the foot posture for the foot that is touching the ground. The foot position compensation module 505 is used to compensate the foot position in the state estimation result according to the ankle position compensation amount to obtain the compensated foot position.

[0098] In one specific implementation of this application, the foot posture determination module can be specifically used to: perform forward kinematics calculations based on the joint angles to obtain a first rotation matrix; wherein the first rotation matrix is ​​the rotation matrix of the robot's foot in the base coordinate system; perform world coordinate system transformation on the first rotation matrix to obtain a second rotation matrix; wherein the second rotation matrix is ​​the rotation matrix of the robot's foot in the world coordinate system; and perform Euler angle transformation on the second rotation matrix to obtain the foot posture.

[0099] In one specific implementation of this application, the ground contact detection module can be specifically used to: determine the foot contact force of the robot based on the joint torque and a preset Jacobian matrix; determine that the foot corresponding to the foot contact force is in contact with the ground when the absolute value of the foot contact force is greater than a preset contact force threshold; and determine that the foot corresponding to the foot contact force is not in contact with the ground when the absolute value of the foot contact force is less than or equal to the contact force threshold.

[0100] In one specific implementation of this application embodiment, the compensation amount determination module may include: A position vector determination unit is used to determine the position vector from the ankle to the support point based on the foot posture. A position compensation determination unit is used to determine the position compensation amount in the ankle coordinate system based on the foot posture and the position vector. The coordinate system transformation unit is used to perform world coordinate system transformation on the position compensation amount in the ankle coordinate system to obtain the ankle position compensation amount.

[0101] In one specific implementation of this application embodiment, the position compensation determination unit can be specifically used to: determine a first position based on the foot posture and the position vector; wherein, the first position is the position of the support point in the ankle coordinate system; determine a second position based on the foot posture and the position vector; wherein, the second position is the position of the new ankle position in the ankle coordinate system; and determine the position compensation amount in the ankle coordinate system based on the first position and the second position.

[0102] In one specific implementation of this application, the foot position compensation module can be specifically used to: superimpose the ankle position compensation amount on the foot position in the state estimation result to obtain the compensated foot position.

[0103] In one specific implementation of this application, the foot position compensation module can also be used to: for feet that are not touching the ground, keep the foot position in the state estimation result unchanged.

[0104] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working processes of the devices, modules, and units described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here.

[0105] In the above embodiments, the descriptions of each embodiment have different focuses. For parts that are not described in detail or recorded in a certain embodiment, please refer to the relevant descriptions of other embodiments.

[0106] Figure 6 A schematic block diagram of a robot provided in an embodiment of this application is shown. For ease of explanation, only the parts related to the embodiment of this application are shown.

[0107] like Figure 6 As shown, the robot 6 in this embodiment includes: a processor 60, a memory 61, and a computer program 62 stored in the memory 61 and executable on the processor 60. When the processor 60 executes the computer program 62, it implements the steps in the various foot position estimation method embodiments described above, for example... Figure 1 Steps S101 to S105 are shown. Alternatively, when the processor 60 executes the computer program 62, it implements the functions of each module / unit in the above-described device embodiments, for example... Figure 5 The functions of modules 501 to 505 are shown.

[0108] For example, the computer program 62 may be divided into one or more modules / units, which are stored in the memory 61 and executed by the processor 60 to complete this application. The one or more modules / units may be a series of computer program instruction segments capable of performing a specific function, which describe the execution process of the computer program 62 in the robot 6.

[0109] Those skilled in the art will understand that Figure 6 This is merely an example of robot 6 and does not constitute a limitation on robot 6. It may include more or fewer parts than shown, or combine certain parts, or different parts. For example, robot 6 may also include input / output devices, network access devices, buses, etc.

[0110] The processor 60 can be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor can be a microprocessor or any conventional processor.

[0111] The memory 61 can be an internal storage unit of the robot 6, such as a hard drive or memory. The memory 61 can also be an external storage device of the robot 6, such as a plug-in hard drive, Smart Media Card (SMC), Secure Digital (SD) card, or Flash Card. Furthermore, the memory 61 can include both internal and external storage units of the robot 6. The memory 61 is used to store the computer program and other programs and data required by the robot 6. The memory 61 can also be used to temporarily store data that has been output or will be output.

[0112] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the above-described division of functional units and modules is merely an example. In practical applications, the above functions can be assigned to different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiments can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit. Furthermore, the specific names of the functional units and modules are only for easy differentiation and are not intended to limit the scope of protection of this application. The specific working process of the units and modules in the above system can be referred to the corresponding process in the foregoing method embodiments, and will not be repeated here.

[0113] In the above embodiments, the descriptions of each embodiment have different focuses. For parts that are not described in detail or recorded in a certain embodiment, please refer to the relevant descriptions of other embodiments.

[0114] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0115] In the embodiments provided in this application, it should be understood that the disclosed devices / robots and methods can be implemented in other ways. For example, the device / robot embodiments described above are merely illustrative. For instance, the division of modules or units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between devices or units may be electrical, mechanical, or other forms.

[0116] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0117] Furthermore, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0118] If the integrated module / unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the methods of the above embodiments can also be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable storage medium can include: any entity or device capable of carrying the computer program code, a recording medium, a USB flash drive, a portable hard drive, a magnetic disk, an optical disk, a computer memory, a read-only memory (ROM), a random access memory (RAM), an electrical carrier signal, a telecommunication signal, and a software distribution medium, etc. It should be noted that the content included in the computer-readable storage medium can be appropriately added or removed according to the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, the computer-readable storage medium does not include electrical carrier signals and telecommunication signals.

[0119] The above-described embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application, and should all be included within the protection scope of this application.

Claims

1. A method for estimating foot position, characterized in that, include: The robot's state is estimated based on a Kalman filter to obtain the robot's state estimation result; The joint angles of the robot are obtained, and the foot posture of the robot is determined based on the joint angles. The joint torque of the robot is obtained, and the robot's feet are detected as touching the ground based on the joint torque. For the foot that touches the ground, the ankle position compensation amount is determined based on the foot posture; The foot position in the state estimation result is compensated based on the ankle position compensation amount to obtain the compensated foot position.

2. The foot position estimation method according to claim 1, characterized in that, Determining the robot's foot posture based on the joint angle includes: Based on the joint angles, positive kinematics calculations are performed to obtain a first rotation matrix; wherein, the first rotation matrix is ​​the rotation matrix of the robot's foot end in the base coordinate system; The first rotation matrix is ​​transformed to the world coordinate system to obtain the second rotation matrix; wherein, the second rotation matrix is ​​the rotation matrix of the robot's foot in the world coordinate system; The foot posture is obtained by performing Euler angle transformation on the second rotation matrix.

3. The foot position estimation method according to claim 1, characterized in that, The step of detecting whether the robot's foot touches the ground based on the joint torque includes: The foot contact force of the robot is determined based on the joint torque and the preset Jacobian matrix. If the absolute value of the foot contact force is greater than a preset contact force threshold, the foot contact force corresponding to the foot contact force is determined to be grounded. If the absolute value of the foot contact force is less than or equal to the contact force threshold, it is determined that the foot corresponding to the foot contact force is not in contact with the ground.

4. The foot position estimation method according to claim 1, characterized in that, Determining the ankle position compensation amount based on the foot posture includes: Determine the position vector from the ankle to the support point based on the foot posture; The position compensation amount in the ankle coordinate system is determined based on the foot posture and the position vector; The ankle position compensation amount is obtained by performing a world coordinate system transformation on the position compensation amount in the ankle coordinate system.

5. The foot position estimation method according to claim 4, characterized in that, The step of determining the position compensation amount in the ankle coordinate system based on the foot posture and the position vector includes: The first position is determined based on the foot posture and the position vector; wherein, the first position is the position of the support point in the ankle coordinate system; The second position is determined based on the foot posture and the position vector; wherein, the second position is the position of the new ankle position in the ankle coordinate system; The position compensation amount in the ankle coordinate system is determined based on the first position and the second position.

6. The foot position estimation method according to claim 1, characterized in that, The step of compensating the foot position in the state estimation result based on the ankle position compensation amount to obtain the compensated foot position includes: The ankle position compensation is superimposed on the foot position in the state estimation result to obtain the compensated foot position.

7. The foot position estimation method according to any one of claims 1 to 6, characterized in that, After detecting whether the robot's foot is touching the ground based on the joint torque, the method further includes: For feet that are not touching the ground, the foot position in the state estimation result remains unchanged.

8. A foot position estimation device, characterized in that, include: The state estimation module is used to perform state estimation of the robot based on a Kalman filter to obtain the state estimation result of the robot. A foot posture determination module is used to acquire the joint angles of the robot and determine the foot posture of the robot based on the joint angles. A ground contact detection module is used to acquire the joint torque of the robot and detect whether the robot's feet are in contact with the ground based on the joint torque; The compensation amount determination module is used to determine the ankle position compensation amount based on the foot posture when the foot touches the ground. The foot position compensation module is used to compensate the foot position in the state estimation result according to the ankle position compensation amount, so as to obtain the compensated foot position.

9. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements the steps of the foot position estimation method as described in any one of claims 1 to 7.

10. A robot comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the foot position estimation method as described in any one of claims 1 to 7.

Images