WO2024037155A1 - 用于足式机器人的状态估计方法、装置、计算机设备及计算机可读存储介质 - Google Patents

用于足式机器人的状态估计方法、装置、计算机设备及计算机可读存储介质 Download PDF

Info

Publication number
WO2024037155A1
WO2024037155A1 PCT/CN2023/099676 CN2023099676W WO2024037155A1 WO 2024037155 A1 WO2024037155 A1 WO 2024037155A1 CN 2023099676 W CN2023099676 W CN 2023099676W WO 2024037155 A1 WO2024037155 A1 WO 2024037155A1
Authority
WO
WIPO (PCT)
Prior art keywords
information
state
footed robot
kalman filter
sensor
Prior art date
Application number
PCT/CN2023/099676
Other languages
English (en)
French (fr)
Inventor
吴闫明
迟万超
王海涛
姜鑫洋
张晟浩
郑宇�
Original Assignee
腾讯科技（深圳）有限公司
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 腾讯科技（深圳）有限公司 filed Critical 腾讯科技（深圳）有限公司
Publication of WO2024037155A1 publication Critical patent/WO2024037155A1/zh

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments

Definitions

  • the embodiments of the present application relate to the technical field of footed robots, specifically to the technical field of footed robot planning and control, and in particular to a state estimation method for a footed robot, a state estimation device for a footed robot, and computer equipment, as well as computer-based Read storage media.
  • Embodiments of the present application provide a state estimation method, device, computer equipment and computer-readable storage medium for a footed robot, which can improve the accuracy of the state estimation value while obtaining the state estimation value of the footed robot in real time.
  • Embodiments of the present application provide a state estimation method for a footed robot, which is executed by a computer device.
  • the method includes: obtaining first sensor information and second sensor information of the footed robot, and based on the third sensor information.
  • One sensor information and second sensor information use the first Kalman filter to determine the first state information of the footed robot, and save the first state information for a period of time as the historical information of the second Kalman filter; obtain the The third generation of legged robots sensor information, and using the second Kalman filter to determine second state information of the footed robot based on the third sensor information and the historical information; and based on the second state of the footed robot information, and update the first status information of the footed robot at the current moment to determine the status information of the footed robot at the current moment.
  • embodiments of the present application provide a state estimation device for a footed robot, the device including: a first sensor configured to obtain first sensor information of the footed robot; a second sensor, configured to obtain the second sensor information of the footed robot; a third sensor configured to obtain the third sensor information of the footed robot; a first Kalman filter configured to be based on the first sensor information and the second sensor information, determine the first state information of the footed robot, and save the first state information for a period of time as historical information; a second Kalman filter configured to based on the third sensor information and the historical information, and a second Kalman filter is used to determine the second state information of the footed robot; wherein the first Kalman filter is further configured to be based on the second state information and the The first state information corresponding to the footed robot at the current moment determines the state information of the footed robot at the current moment.
  • embodiments of the present application provide a computer device, including an input interface and an output interface, and further including: a processor, adapted to implement one or more instructions; and, a computer-readable storage medium; the computer-readable The storage medium stores one or more instructions, and the one or more instructions are suitable for the processor to load and execute the above-mentioned state estimation method of the legged robot.
  • embodiments of the present application provide a computer-readable storage medium that stores one or more instructions, and the one or more instructions are suitable for being loaded by a processor and executing the above-mentioned instructions.
  • State estimation method for legged robots
  • embodiments of the present application provide a computer program product, which includes a computer program; when the computer program is executed by a processor, the above-mentioned state estimation method of a footed robot is implemented.
  • various aspects of the embodiments of the present application integrate the output information of different sensors operating at different frequencies, and use two Kalman filters to solve the problems of different frequencies and different delays in fusing different sensor information, and achieve high real-time performance. , Highly robust and highly effective multi-sensor fusion state estimation, significantly reducing the cumulative error caused by state estimation when the footed robot moves for a long time.
  • FIG. 1 is a schematic diagram showing a footed robot according to an embodiment of the present application.
  • FIG. 2 is a flowchart illustrating a state estimation method for a footed robot according to an embodiment of the present application.
  • FIG. 3 is a schematic diagram illustrating a state estimation method for a footed robot according to an embodiment of the present application, in which first state information and second state information are schematically shown.
  • FIG. 4 is a partial structural diagram showing a state estimation device for a footed robot according to an embodiment of the present application.
  • FIG. 5 is a schematic diagram illustrating an execution method of a state estimation device for a footed robot according to an embodiment of the present application.
  • Figure 6 is a comparison diagram showing the execution effect of the method according to the embodiment of the present application and the traditional solution.
  • Figure 7 shows a schematic diagram of an electronic device according to an embodiment of the present application.
  • FIG. 8 shows a schematic diagram of the architecture of an exemplary computing device according to an embodiment of the present application.
  • Figure 9 shows a schematic diagram of a storage medium according to an embodiment of the present application.
  • first and second are used for descriptive purposes only and cannot be understood as indicating or implying relative importance or implicitly indicating the quantity of indicated technical features.
  • features defined as “first” and “second” may explicitly or implicitly include one or more of the described features.
  • “plurality” means two or more, unless otherwise explicitly and specifically limited.
  • connection should be understood in a broad sense.
  • it can be a fixed connection or a fixed connection.
  • Detachable connection, or integral connection it can be mechanical connection, electrical connection or mutual communication; it can be directly connected, or it can be indirectly connected through an intermediate medium, it can be internal connection of two elements or two elements. interactive relationship.
  • connection should be understood in a broad sense.
  • it can be a fixed connection or a fixed connection.
  • the first feature being “above” or “below” the second feature may include direct contact between the first and second features, or may include the first and second features being in direct contact. Two features are not in direct contact but are in contact through another feature between them.
  • the terms “above”, “above” and “above” a first feature on a second feature include the first feature being directly above and diagonally above the second feature, or simply mean that the first feature is higher in level than the second feature.
  • “Below”, “under” and “under” the first feature is the second feature includes the first feature being directly below and diagonally below the second feature, or simply means that the first feature is less horizontally than the second feature.
  • the footed robot in the embodiment of the present application is a footed robot that uses legs to move. It uses animals as bionic objects, and its purpose is to simulate the movement forms of animals and replicate the movement abilities of animals based on engineering technology and scientific research results. Footed robots have strong adaptability to various environments, including structured environments (such as roads, railways, treated smooth roads, etc.) and unstructured environments (such as mountains, swamps, rough roads, etc.). It can adapt to various changes in terrain, climb over higher obstacles, and can effectively reduce the load and improve the energy utilization efficiency of the system. According to the number of feet, legged robots can be divided into single-legged, bipedal, quadrupedal, six-legged, eight-legged, etc.
  • quadrupedal robots have superior movement ability and have better static stability than bipedal robots. , and its movements are simpler and more flexible than six-legged and eight-legged robots. Therefore, four-legged robots are a common choice for studying footed robots.
  • the gait of a quadruped robot is the coordinated relationship between its four legs in time and space in order for the quadruped robot to move continuously.
  • the gait of a quadruped robot is derived from the gait of a quadruped mammal, which can include but is not limited to the following three simplified forms: walk, trot, and bound.
  • the method of controlling a legged robot in the embodiment of the present application may be based on artificial intelligence (Artificial intelligence, AI).
  • Artificial intelligence is the use of digital computers or machines controlled by digital computers
  • Theories, methods, technologies and application systems that simulate, extend and expand human intelligence, perceive the environment, acquire knowledge and use knowledge to obtain the best results.
  • artificial intelligence is a comprehensive technology of computer science that attempts to understand the nature of intelligence and produce a new intelligent machine that can respond in a similar way to human intelligence.
  • it can plan the movement trajectory and gait of the footed robot in a manner similar to how humans guide the movement of living animals, making the movement of the footed robot more flexible and bionic.
  • artificial intelligence enables the method of controlling a footed robot in the embodiment of the present application to automatically and efficiently design the subsequent movement trajectory and gait of the footed robot based on the current movement state of the footed robot. function.
  • FIG. 1 is a schematic diagram showing a footed robot 100 according to an embodiment of the present application. As shown in Figure 1, taking a four-legged footed robot as an example, the left and right pictures in Figure 1 show the internal perspective view and the external structural diagram of the example footed robot respectively.
  • each mechanical leg may include a thigh and a calf, and may include a thigh and a calf, and each mechanical leg may include at least one joint.
  • each robotic leg may include multiple lower limb joints, such as a hip joint with two degrees of freedom and a knee joint with one degree of freedom.
  • the word "joint" in the embodiment of this application indicates a movable connection between two or more components of the footed robot.
  • the joint can move under the control of the force and torque output by the joint motor.
  • a joint can rotate at an angle so that other joints and their linkage mechanisms can move a certain amount in space, thereby causing a change in the posture/motion state of the footed robot 100 .
  • each robotic leg can also be configured with multiple motors, which can be used to control respectively or in combination to control the two degrees of freedom of the hip joint and one degree of freedom of the knee joint of the quadruped robot.
  • the footed robot can also be equipped with various measurement components, such as IMU sensors and joint encoders.
  • the IMU sensor can provide the acceleration and attitude information of the footed robot in real time
  • the joint encoder can Provides joint angle information of each joint of the footed robot in real time (such as joint angle angle, angular velocity feedback value, etc.).
  • the example legged robot can already achieve a variety of movements under the control of the multiple motors mentioned above, including, for example, somersaults or jumps.
  • controlling the footed robot to perform these actions often requires accurate status data. If the status data is inaccurate, it will be difficult for various control schemes to achieve accurate and flexible control of the legged robot.
  • related technologies provide a variety of means to determine the status of the footed robot in real time (such as position and attitude information, etc.).
  • a variety of sensors can be used to estimate the body state of a legged robot.
  • a variety of methods have been proposed in the industry to fuse IMU data, joint encoder data, laser data, motion capture data, etc. to determine the body state of a footed robot.
  • these methods cannot solve the problem of sensor state estimation for body sensing.
  • the state drift of the controller during long-term operation will inevitably lead to cumulative errors.
  • embodiments of the present application provide a state estimation method for a footed robot.
  • the method includes: obtaining the first sensor information and the second sensor information of the footed robot, and based on the The first sensor information and the second sensor information use the first Kalman filter to determine the first state information of the footed robot, and save the first state information for a period of time as the historical information of the second Kalman filter; obtain all Describe the third sensor information of the footed robot, and use the second Kalman filter to determine the second state information of the footed robot based on the third sensor information and the historical information; and based on the footed robot
  • the second state information of the footed robot is updated to the first state information of the footed robot at the current moment to determine the state information of the footed robot at the current moment.
  • embodiments of the present application also provide a state estimation device for a footed robot, the device including: a first sensor configured to obtain first sensor information of the footed robot; a second sensor , configured to obtain the second sensor information of the footed robot; a third sensor configured to obtain the third sensor information of the footed robot; a first Kalman filter configured to obtain the third sensor information of the footed robot based on the first The sensor information and the second sensor information determine the first state information of the footed robot, and save the first state information for a period of time as historical information; the second Kalman filter is configured to be based on the third The sensor information and the historical information are used to determine the second state information of the footed robot using a second Kalman filter; wherein the first Kalman filter is further configured to be based on the second state information and the The first state information corresponding to the footed robot at the current moment determines the state information of the footed robot at the current moment.
  • various aspects of the embodiments of the present application fuse sensor information of sensors with different operating frequencies.
  • Two Kalman filters are used to solve the problem that in related technology solutions, only lower frequencies can be fused.
  • Different sensor information leads to problems such as high delays. It achieves multi-sensor fusion state estimation with high real-time performance, high robustness, and high effectiveness, and significantly reduces the cumulative error caused by state estimation when the footed robot moves for a long time.
  • FIG. 2 is a flowchart illustrating a state estimation method 20 for a footed robot according to an embodiment of the present application.
  • FIG. 3 is a schematic diagram illustrating a state estimation method 20 for a footed robot according to an embodiment of the present application, in which first state information and second state information are schematically shown.
  • the state estimation method 20 for a footed robot may include operations S201-S203 as shown in FIG. 2 .
  • method 20 includes one or all of operations S201 to S203, and may also include more operations.
  • the execution subjects of operations S201 to S203 may be various components located on the footed robot (for example, various sensors and computing components located on the footed robot), or may be various components located outside the footed robot (for example, Various sensors and computing components located outside the legged robot).
  • the embodiment of the present application does not limit the execution subject of operations S201 to S203.
  • the method 20 can be processed by the device 40 described in detail later.
  • method 20 may be performed by any computing device.
  • the computer device here may be a terminal or a server; or the computer device here may be jointly executed by a terminal and a server, which is not limited.
  • the terminal can be a smartphone, a computer (such as a tablet, a laptop, a desktop computer, etc.), a smart wearable device (such as a smart watch, smart glasses), a smart voice interaction device, a smart home appliance (such as a smart TV), a vehicle-mounted terminal or aircraft, etc.
  • the server can be an independent physical server, or a server cluster or distributed system composed of multiple physical servers, or it can provide cloud services, cloud databases, cloud computing, cloud functions, cloud storage, network services, cloud Communication, middleware services, domain name services, security services, CDN (Content Delivery Network, content distribution network), and cloud servers for basic cloud computing services such as big data and artificial intelligence platforms, etc.
  • CDN Content Delivery Network, content distribution network
  • cloud servers for basic cloud computing services such as big data and artificial intelligence platforms, etc.
  • the terminal and server can be located within the blockchain network or outside the blockchain network, and there is no limit to this; further, the terminal and server can also upload any data stored internally to the blockchain network for storage. , to prevent internally stored data from being tampered with and improve data security.
  • first sensor information and second sensor information of the footed robot are obtained, and based on the first sensor information and second sensor information, a first Kalman filter is used to determine the footed robot.
  • the first state information of the robot is stored for a period of time as the historical information of the second Kalman filter.
  • the first sensor information may be any information collected by the first sensor.
  • the first sensor information is time stamped.
  • the first sensor may operate at the first frequency to obtain first sensor information corresponding to multiple timestamps in the first timestamp set. That is, the first sensor information may include information collected by the first sensor at each timestamp in the first timestamp set.
  • the first frequency may be relatively high, thereby ensuring the real-time performance of the first sensor.
  • An exemplary first frequency may be 0.01kHZ to 1kHZ.
  • the first sensor may include at least one of: a tactile sensor, a force and torque sensor, an inertial measurement unit (IMU), and the like.
  • IMU inertial measurement unit
  • the first sensor may further include force and torque sensors corresponding to the mechanical arm.
  • the first sensor may also include a tactile sensor on the finger end.
  • the first sensor can be physically attached to the footed robot, and its attachment position can change as the configuration of the footed robot changes. The embodiments of the present application are not limited to this. In some embodiments, the first sensor may not be physically attached to the footed robot, and the embodiments of the present application are not limited to this.
  • the second sensor information may be any information collected by the second sensor.
  • the second sensor is also timestamped.
  • the second sensor may operate at a second frequency to obtain second sensor information corresponding to multiple timestamps in the second timestamp set.
  • the second sensor information may be used to calculate information related to movements and postures of the footed robot.
  • the second sensor may be a joint encoder, which is used to collect the second sensor information of the footed robot, thereby obtaining joint angle information (such as joint angle angle, angle) of each joint of the footed robot. speed feedback value, etc.).
  • the joint encoder can also work at the second frequency.
  • An exemplary second frequency may be 0.1 to 1 kHz.
  • the second frequency may be slightly lower than the first frequency.
  • the first Kalman filter can be either a standard Kalman filter or an extended Kalman filter.
  • the Kalman filter is a high-efficiency recursive filter (autoregressive filter) that can obtain from a series of incomplete and noisy measurement information (for example, first sensor information and second sensor information) , to estimate the state of the legged robot.
  • the Kalman filter will consider the joint distribution at each time based on the values of each measurement information at different times, and then generate an estimate of the unknown variable (for example, the first state information), so it will be better than the one based only on a single measurement value.
  • the estimation method is more accurate.
  • the so-called extended Kalman filter is an extended form of the standard Kalman filter in nonlinear situations.
  • the first sensor information and the second sensor information carry a timestamp, and based on the first sensor information and the second sensor information, a first Kalman filter is used to determine the third value of the footed robot.
  • Status information use the first sensor information and the second sensor information with timestamps, and use the first sensor information and the second sensor information as the input of the first Kalman filter in sequence; and pass the first Kalman filter Perform state estimation to obtain the first state information of the footed robot with a timestamp, and save the first state information with the timestamp as historical information, where the first state information includes the output of the first Kalman filter. Multiple state estimation results with timestamps.
  • the current time is T b
  • multiple timestamps are included in the time period corresponding to time T a to time T b .
  • the first sensor information and the second sensor information corresponding to each timestamp in the above multiple timestamps can be used as the input of the first Kalman filter in sequence, so as to perform state estimation through the first Kalman filter, and obtain the following formula:
  • the first state information includes multiple state estimation results corresponding to multiple timestamps output by the first Kalman filter.
  • the first status information may be a time series value sequence composed of the following data: centroid and foot-related information corresponding to each time step (each time step corresponds to a timestamp).
  • Each element in the time series value sequence corresponds to a state estimation result.
  • each element in the time series value sequence can have multiple dimensions, which respectively represent the position of the center of mass and the foot end in the x-axis direction, the position of the center of mass in the y-axis direction, and the center of mass corresponding to a certain time step.
  • the above process can be performed in real time, that is, every time the first sensor information and the second sensor information corresponding to a timestamp are collected, they are input to the first Kalman filter to obtain a state estimation result (for example, represented by the curve in Figure 3
  • the first state information of , each point in the curve corresponds to status information at a moment in time).
  • the state estimation result output by the first Kalman filter is S a .
  • the state estimation result output by the first Kalman filter is S b-1 .
  • the term "state estimation results Sa to S b-1" includes each timestamp Ti in the historical time period corresponding to time T a to time T b-1 ( T a ⁇ T i ⁇ T b-1 ) corresponding state estimation result Si .
  • the first state information for a period of time (for example, the first state estimation results Sa to S b -1 corresponding to each timestamp in the time period [T a , T b -1 ]) can be saved as the second state information Mann filtering history information.
  • third sensor information of the footed robot is obtained, and based on the third sensor information and the historical information, the second Kalman filter is used to determine the third sensor information of the footed robot. 2. Status information.
  • the third sensor information may be any information collected by a third sensor.
  • the third sensor may operate at a third frequency to obtain only third sensor information corresponding to one of the plurality of times in a fixed time period.
  • the third frequency may be relatively low, such as lower than the first frequency.
  • An exemplary third frequency may be 10 to 50HZ.
  • the working frequency and accuracy of the sensor are mutually restricted. That is, the higher the working frequency, the accuracy decreases. Since the real-time nature of the third sensor information collected by the third sensor is lower than that of the first sensor, the accuracy is relatively low. Higher than the first sensor, errors will not accumulate with the movement of the footed robot.
  • the third sensor may include at least one of the following devices: a distance sensor (TOF), a camera or camera providing visual information, a radar, a position sensor (such as a GPS sensor), a laser sensor, or the like.
  • a distance sensor TOF
  • a camera or camera providing visual information
  • a radar a position sensor (such as a GPS sensor)
  • a laser sensor or the like.
  • the second Kalman filter can also be a standard Kalman filter or an extended Kalman filter.
  • the timestamp corresponding to the third sensor information can be determined, and the historical information and the third sensor information can be used as the input of the second Kalman filter; and state estimation can be performed through the second Kalman filter. , obtaining the state estimation result of the timestamp corresponding to the third sensor information as the second state information.
  • only the state estimation result corresponding to the timestamp corresponding to the third sensor information in the above historical information and the third sensor information can be used as the input of the second Kalman filter , to perform state estimation through the second Kalman filter to obtain the state estimation result of the timestamp corresponding to the third sensor information, and use the state estimation result as the second state information.
  • the process of state estimation by the second Kalman filter can be briefly described as follows: first, based on the timestamp corresponding to the third sensor information, determine the state estimation result corresponding to the timestamp in the historical information, and The state estimation result corresponding to the timestamp means that the timestamp of the state estimation result corresponding to the timestamp is the same as the timestamp corresponding to the third sensor information; then, based on the third sensor information and the The state estimation result corresponding to the timestamp in the historical information is used to perform state estimation using the second Kalman filter to Obtain the state estimation result corresponding to the timestamp, and use the state estimation result as the second state information.
  • the camera or camera may open the shutter at time Ta to capture the image of the footed robot at time Ta , and then process the image, Finally, at the current time T b , the third sensor information corresponding to time Ta is provided to the second Kalman filter.
  • the second Kalman filter can output the status of each timestamp in the time period Ta to T b - 1 based on the first status information corresponding to time Ta and combined with the third sensor information at time Ta. Estimate the results.
  • the state estimation result S a of the first Kalman filter and the state estimation result S a of the second Kalman filter There is a state increment ⁇ S, which represents the state estimation result S a of the first Kalman filter and the state estimation result of the second Kalman filter. difference between.
  • the second Kalman filter can superimpose the state increment ⁇ S to the first state information S b corresponding to the footed robot at the current time T b to determine the current time of the footed robot.
  • the state increment ⁇ S is superimposed on the first state information S b corresponding to the footed robot at the current time T b , and the obtained superposition result is used as the state information of the footed robot at the current time.
  • the second Kalman filter may correct the state estimation results determined by the first Kalman filter at each time stamp in the time period Ta to T b -1 one by one based on the state increment ⁇ S. (The state estimation results determined by each timestamp are recorded as Sa to S b-1 ) to obtain the second state information. to in, It is the state estimation result corresponding to time T b-1 estimated by the third sensor information.
  • the state estimation result corresponding to each timestamp in the first state information can be corrected through the following formula: Among them, a ⁇ i ⁇ b.
  • the second Kalman filter can use the state estimation result S a of the first Kalman filter corresponding to the time T a and the state estimation result S a of the second Kalman filter.
  • the offset between is the initial value, and the state estimation results of each timestamp in the time period T a to T b-1 are iteratively updated to obtain the second state information.
  • the second Kalman filter can be further combined with the leg kinematics model or dynamics model of the footed robot, based on the state estimation results of the second Kalman filter. Sequentially update the state estimation results of each timestamp in the time period T a to T b-1 to obtain the second state information to The embodiments of the present application do not limit this.
  • the dynamic model is a mathematical equation that describes the relationship between the force and motion of the robot.
  • the dynamic model can be a Virtual Model Controller (VMC) model, a Spring Loaded Inverted Pendulum, SLIP) and floating body models.
  • VMC Virtual Model Controller
  • SLIP Spring Loaded Inverted Pendulum
  • first state information corresponding to the footed robot at the current moment is obtained, and based on the second state information of the footed robot, the state information of the footed robot at the current moment is determined.
  • the explanation continues taking the current time T b as an example.
  • the first sensor can collect the first sensor information corresponding to time T b
  • the joint encoder can collect the second sensor information corresponding to time T b .
  • the first Kalman filter can take the first sensor information and the second sensor information corresponding to time T b as input, obtain the first state estimation result S b corresponding to time T b , and use the state increment in operation S202 ⁇ S corrects the first state estimation result S b to obtain The accumulated error in S b is eliminated through the state increment ⁇ S, thereby obtaining a relatively accurate That is to say, the status information of the footed robot at the current moment is the information fused by the first Kalman filter and the second Kalman filter, and its accuracy is relatively high.
  • the method 20 of the embodiment of the present application fuses the sensor information of different sensors operating at different frequencies and the second sensor information, and uses two Kalman filters to solve the problems of low frequency and high delay in merging different sensor information, and achieves It achieves multi-sensor fusion state estimation with high real-time performance, high robustness and high effectiveness, and significantly reduces the cumulative error caused by the state estimation of the footed robot during long-term movement.
  • Method 20 simplifies the analysis and adjustment of the Kalman filter and reduces running time.
  • the second Kalman filter can be turned off directly when the third sensor information is unavailable without affecting the operation of the entire state estimation device.
  • FIG. 4 is a partial structural diagram showing a state estimation device 40 for a footed robot according to an embodiment of the present application.
  • the state estimation device 40 for a legged robot optionally includes a first sensor, a second sensor, a third sensor, a first Kalman filter and a second Kalman filter.
  • the first Kalman filter can operate in a high-frequency and low-delay state
  • the second Kalman filter can operate in a low-frequency and high-delay state.
  • the first sensor is configured to obtain the first sensor information corresponding to each time stamp of the footed robot in the first time stamp set and the first sensor information corresponding to the footed robot at the current moment.
  • the first sensor in the footed robot may include an IMU sensor.
  • the IMU sensor may collect posture information and acceleration of the footed robot at multiple times at a first frequency as the first sensor information corresponding to that time.
  • the IMU sensor can be configured to acquire the three-axis attitude angle (or angular rate) and acceleration of each joint corresponding to these moments at multiple different discrete moments.
  • the data output by the IMU sensor includes timestamp, three-dimensional acceleration, three-dimensional angular velocity, orientation, etc., and the embodiments of the present application are not limited to this.
  • the joint encoder as the second sensor is optionally configured to obtain the second sensor information corresponding to each timestamp of the footed robot in the second timestamp set, and to obtain the current time of the footed robot.
  • the second sensor information corresponding to the time.
  • the joint encoder can collect the joint angle information (such as joint angle, angular velocity feedback value, etc.) of each joint on each mechanical leg of the footed robot at each moment as the second sensor information corresponding to that moment.
  • the joint encoder can be configured to obtain the angles q[] and joint angular velocities of all joints corresponding to these timestamps for multiple different timestamps.
  • the first Kalman filter is configured to determine the first status information of the footed robot based on the first sensor information and the second sensor information corresponding to the multiple timestamps.
  • the first Kalman filter also includes an observation model and a prediction model.
  • the observation model is used for state observation, and state observation data can be obtained through state observation.
  • the prediction model is used for state prediction, and state prediction data can be obtained through state prediction.
  • the prediction model obtains the prediction state and the corresponding covariance through the first sensor information
  • the observation model obtains the observation state and the corresponding covariance through the second sensor information.
  • the embodiments of the present application are not limited to this.
  • both the first timestamp set and the second timestamp set include timestamps Ti .
  • the first Kalman filter performs state estimation to obtain the state estimation result corresponding to the timestamp Ti , including: inputting the first sensor information corresponding to the timestamp Ti into the observation model to obtain the time The state observation data corresponding to the timestamp T i ; input the state estimation result corresponding to the previous timestamp T i-1 of the timestamp T i and the first sensor information corresponding to the timestamp T i into the prediction model to obtain the timestamp T i The corresponding state prediction data; and using the measurement value obtained by the second sensor corresponding to the timestamp Ti to correct the state prediction data corresponding to the timestamp Ti to obtain the state estimation result corresponding to the timestamp Ti .
  • the measurement value is obtained by kinematic solution of the second sensor information corresponding to the timestamp Ti .
  • the observation model is based on the observation matrix H, which is used to map observations to state observation data.
  • the state observation data corresponding to the timestamp Ti is recorded as Yi
  • a predictive model can be expressed as Where m i ⁇ N(0,Q i ) is the prediction noise, A is the state transition matrix, and B is the control matrix.
  • the general principle of the first Kalman filter to determine the first state information of the footed robot at time T i is as follows:
  • the IMU sensor in the footed robot can be called to collect the first sensor information of the footed robot at time T i , where the first sensor information at time T i includes the acceleration information of the footed robot (which may include the acceleration information of the footed robot at time T i acceleration in multiple directions (such as vertical direction, horizontal direction)) and attitude information, and calling the second sensor joint encoder to determine the joint angle information (such as the angle of the joint angle) of each joint of the footed robot at time T i , angular velocity feedback value, etc.).
  • the first sensor information at time T i includes the acceleration information of the footed robot (which may include the acceleration information of the footed robot at time T i acceleration in multiple directions (such as vertical direction, horizontal direction)) and attitude information
  • the second sensor joint encoder to determine the joint angle information (such as the angle of the joint angle) of each joint of the footed robot at time T i , angular velocity feedback value, etc.).
  • the state observation data can include the values of multiple state observation quantities.
  • the values of multiple state observations include: the observed position of the foot end of each mechanical leg of the footed robot at time T i , and the observed position of the center of mass of the footed robot at time T i .
  • the observation model can output a set of pose data identified by timestamps.
  • the pose data optionally includes the values of the XYZ coordinate axes of the center of mass and the foot end in the world coordinate system/body coordinate system, orientation data (expressed as a quaternion), and the position of the footed robot 100 in the world coordinate system/body coordinate system.
  • orientation data expressed as a quaternion
  • the position of the footed robot 100 in the world coordinate system/body coordinate system is optionally included.
  • the state prediction data may include values of multiple state predictors and a priori covariances.
  • the values of multiple state prediction quantities include the predicted position of the foot end of each mechanical leg of the footed robot at time T i , the predicted position of the center of mass of the footed robot at time T i , and the predicted position of the center of mass of the footed robot at time T i . speed etc.
  • the first Kalman filter can determine the state estimation result corresponding to time Ti based on the state observation data at time Ti and the state prediction data at time Ti .
  • the goal of the first Kalman filter is to use the state observation data and measurement values to correct the state prediction data to obtain the corresponding state estimation results given the state observation data, measurement values and state prediction data at time T i .
  • the third sensor is configured to obtain third sensor information corresponding to one of the plurality of timestamps of the footed robot.
  • the third sensor in the footed robot can include a camera or camera that provides visual information (hereinafter also called a visual sensor).
  • the visual odometer can convert the image information collected by the visual sensor into the world coordinate system of the footed robot.
  • the third sensor is, for example, a binocular single-channel camera, and takes a photo of 1456*1088 pixels each time, and each pixel in the photo is represented by eight bits.
  • the third sensor can trigger a shooting every 0.1 s.
  • the embodiments of the present application are not limited to this.
  • the third sensor can also trigger a shooting at a longer or shorter time interval.
  • the third sensor is also, for example, a monocular single-channel camera.
  • the resolution of the photos taken by it is 640x576 or 512x512, and each pixel in the photo is represented by 16 bits.
  • the embodiments of the present application are not limited to this.
  • the second Kalman filter is configured to use the second Kalman filter to determine the location of the footed robot based on the third sensor information corresponding to one of the plurality of timestamps and the first state information. Second status information.
  • the second Kalman filter also includes visual odometry. Visual odometry can determine the orientation and position of a legged robot by analyzing a series of image sequences.
  • the general principle of the second Kalman filter to determine the second state information of the footed robot at timestamp T i is as follows:
  • the visual sensor can be called to collect the third sensor information of the footed robot at timestamp T ic .
  • the third sensor information collected by the timestamp T ic may be calculated by the visual odometer only at the timestamp T i and the measurement information of the timestamp T ic is parsed.
  • the maximum value of the third sensor information delay is the duration corresponding to c timestamps.
  • the measurement information may include values of multiple state observations. Multiple measurement information values include: the position of the center of mass of the footed robot at timestamp T ic , the speed of the center of mass of the footed robot at timestamp Tic , etc.
  • historical information may be obtained from the first Kalman filter, which includes the state estimation results from timestamp T ic to timestamp Ti calculated by the first Kalman filter. That is, the historical data window of the first Kalman filter is the duration corresponding to c timestamps.
  • the second Kalman filter can determine the corrected timestamp T ic based on the measurement information of the timestamp T ic obtained by the visual odometry and the state estimation result of the timestamp T ic from the first Kalman filter in the historical information. state estimation knot The result is the second status information of timestamp T ic . Based on the state estimation result of the corrected timestamp T ic , the second Kalman filter can predict the state estimation result of the timestamp T i-c+1 . And so on, until the second Kalman filter calculates the state estimation result of timestamp Ti as the second state information.
  • the first Kalman filter is further configured to determine the state of the footed robot at the current moment based on the second state information and the first state information corresponding to the footed robot at the current moment. information.
  • the second Kalman filter can also calculate the state increment ⁇ S based on the first state estimation result Sa provided by the first Kalman filter and the third sensor information corresponding to the timestamp Ta , which represents the first Kalman filter. The difference between the first state estimation result Sa provided by the filter and the third sensor information corresponding to the timestamp Ta .
  • the first Kalman filter can calculate the state information corresponding to time T b based on the above information.
  • the first Kalman filter may output the status information of the footed robot at the current moment to the controller at a first frequency (for example, 0.1 to 1 kHz shown in FIG. 4 ).
  • the second Kalman filter may output the second state information to the first Kalman filter at a second frequency (eg, 10 to 50 HZ as shown in FIG. 4). Therefore, the first Kalman filter can adjust the state information at the current moment based on the second state information at an interval of 0.02s to 0.1s, thereby avoiding cumulative errors.
  • the device 40 of the embodiment of the present application fuses the sensor information of different sensors operating at different frequencies and the second sensor information, and uses two Kalman filters to solve the problems of fusion of different sensor information, different delays, etc., and achieves It achieves multi-sensor fusion state estimation with high real-time performance, high robustness and high effectiveness, and significantly reduces the cumulative error caused by the state estimation of the footed robot during long-term movement.
  • the device 40 for executing the method 20 simplifies the analysis and adjustment of the Kalman filter and reduces the running time.
  • the second Kalman filter can be turned off directly when the third sensor information is unavailable without affecting the operation of the entire state estimation device.
  • FIG. 5 is a schematic diagram illustrating the method 20 performed by the state estimation device 40 for a footed robot according to an embodiment of the present application.
  • FIG. 6 is a comparison diagram showing the execution effect of the method 20 according to the embodiment of the present application and the traditional solution.
  • a prediction model can be used to observe each state quantity (this process is also called state prediction).
  • Inputs to the predictive model may optionally include first sensor information.
  • the first sensor information includes: the rotation matrix R between the world coordinate system and the fuselage coordinate system (hereinafter also recorded as W R B , indicating the rotation matrix of the fuselage coordinate system B relative to the world coordinate system W), the fuselage The angular velocity ⁇ in the coordinate system, the acceleration a in the fuselage coordinate system, etc.
  • W R B the rotation matrix of the fuselage coordinate system B relative to the world coordinate system W
  • p represents the position of the body center of mass of the legged robot in the world coordinate system
  • v represents the speed of the body center of mass in the world coordinate system
  • f i is the index of the mechanical leg.
  • the prediction model can respectively predict the body position, body speed and foot end position of the footed robot based on the first equation (1) to the third equation (3) described below to obtain the above-mentioned x.
  • the first equation (1) is also called the discrete prediction equation of the footed robot body position.
  • the second equation (2) is also called the discrete prediction equation of the body speed of the footed robot.
  • the third equation (3) is also called the discrete prediction equation of the foot end position of the footed robot.
  • a k is the acceleration in the body coordinate system
  • dt is the time step
  • g represents the gravity acceleration
  • subscript k represents the index of the time step.
  • the observation model can also be used to observe each state quantity (this process is also called state observation).
  • the observation model may make state observations based at least in part on the leg kinematics model shown in FIG. 5 .
  • the observation model may take first sensor information and second sensor information as input, and use state observation data as output.
  • the second sensor information includes joint angle information p.
  • the state observation data includes the position q rel of the foot end relative to the center of mass in the world coordinate system, and the velocity of the foot end relative to the center of mass in the world coordinate system.
  • the observation model can also use the foot contact detector as input to obtain more accurate foot contact data.
  • the leg kinematics model is obtained by mathematical modeling of the legs of the footed robot.
  • MIT Cheetah kinematic modeling technology can be used.
  • the leg kinematics model is used to represent the relationship between the leg end motion state and the joint motion state of the footed robot, such as the position and sum between the center of mass of the footed robot and the foot end. Velocity relative relationship, which is used to eliminate the state drift between the center of mass and the foot end.
  • using a first Kalman filter to determine the first state information of the footed robot based on the first sensor information and the second sensor information may further include: based on the second sensor information Sensor information, determining movement information of the footed robot based on a leg kinematics model of the footed robot; and determining, based at least in part on the movement information, a first Kalman filter of the footed robot. a status message.
  • the observation model may be based on a leg kinematics model of the footed robot, Calculate the relative position measurement residual between the body and the foot of the footed robot, and the relative speed measurement residual between the body and the foot of the footed robot, and then compare the body and the foot of the footed robot.
  • the relative position measurement residuals between the foot ends and the relative speed measurement residuals between the body and the foot ends of the footed robot are used as the state observation data.
  • the observation model can correspondingly calculate the measurement residuals according to the fourth equation (4) and the fifth equation (5).
  • the fourth equation (4) is used to calculate the relative position measurement residual error between the foot robot body and the foot end
  • the fifth equation (5) is used to calculate the relative velocity measurement residual error between the foot robot body and the foot end.
  • ⁇ b is the angular velocity in the body coordinate system.
  • the first Kalman filter may be configured to determine the first status information of the footed robot based on the first sensor information and the second sensor information corresponding to the multiple timestamps. Assume that the first Kalman filter obtains the control input u k of each joint motor at time T k and the first sensor information collected by the first sensor at time T k at time T k (corresponding to the k-th time step). zk . At the same time, the first Kalman filter also knows the prior state corresponding to time T k-1 The goal of the first Kalman filter is to estimate the state mean ⁇ and covariance ⁇ based on each of the above values.
  • the state space equation of the first Kalman filter can be shown in the sixth equation (6) and the seventh equation (7).
  • x k Ax k-1 +Bu k +m k (6)
  • z k Cx k-1 +n k (7)
  • the first Kalman filter can process the state observation data and the state prediction data according to the eighth equation (8) to the twelfth equation (12) to fuse and obtain the state estimation result corresponding to time step k.
  • P is the error covariance matrix
  • the initial value can be set to an empirical value or an arbitrary value.
  • K is the Kalman gain matrix
  • I is the identity matrix
  • x - represents the prior value of x. represents the estimated value of x. That is, Identifies the prior estimate of P k . Identifies the prior estimate of x k . in,
  • the value of is the combination of the relative position measurement residual between the foot robot body and the foot end calculated by the fourth equation (4) and the relative speed measurement residual calculated by the fifth equation (5).
  • z k is also called the measured value, which is obtained by kinematic solution of the second sensor information; Also called observations or state observation data. thereby, That is, it can be defined as the residual.
  • the first Kalman filter can estimate the state estimation result corresponding to time T k , and can combine multiple times
  • the state estimation result corresponding to the intermediate step is used as the first state information, and is output to the second Kalman filter after information synchronization.
  • the first Kalman filter estimates that the state estimation result corresponding to time T k includes the posterior state corresponding to time T k and posterior covariance
  • the third sensor is the aforementioned visual sensor.
  • z′ s C′ s x s +n′ s (13)
  • the third sensor information has an output delayed by N time steps (time stamps) relative to the above-mentioned information of the first Kalman filter. Therefore, the third sensor and the first status information can be synchronized by information to obtain a measurement error for correcting the first status information.
  • the third sensor information corresponding to time step s, the state estimation result of the first Kalman filter corresponding to time step s, and the error covariance matrix can be used as iterations in the subsequent equations Parameters are fused based on the above-mentioned Kalman filter principle.
  • the third sensor may use the fourteenth equation (14) to obtain the relative position measurement residual p vo -p and the relative velocity measurement residual v vo -v for correcting the first state information.
  • the second Kalman filter can fuse the above time-synchronized first state information and the third sensor information based on the fifteenth equation (15) to the seventeenth equation (17) to obtain the second state information.
  • the relative position measurement residual and relative velocity measurement residuals can be used to form the seventeenth equation
  • P′ is the error covariance matrix used by the second Kalman filter.
  • K′ is the Kalman gain matrix used by the second Kalman filter, I is the identity matrix, and x - represents the prior value of x.
  • the posterior state and posterior covariance obtained by the first Kalman filter are used as the prior state and prior covariance of the second Kalman filter to be compared with the observations of the second Kalman filter.
  • the information is fused, and the state estimation result corresponding to time step s is obtained as the second state information.
  • the first Kalman filter can also combine the first sensor information and the second sensor information corresponding to time T k after receiving the second state information corresponding to time T k , based on the above-mentioned first equation (1) to the second Twelve equations (12) determine the state estimation result corresponding to time T k .
  • the first Kalman filter can be based on and The state increment ⁇ S between them is superimposed to the state estimation result corresponding to the first Kalman filter at time T k , so as to correct the state estimation result at time step k.
  • the second Kalman filter can also output the state estimation result corresponding to time T k as the second state information from the estimate corresponding to time step s by iterating N times to the first Kalman filter. device to correct the current status information.
  • the embodiments of the present application are not limited to this.
  • the device 40 of the embodiment of the present application fuses the sensor information of different sensors operating at different frequencies and the second sensor information, and uses two Kalman filters to solve the problems of different frequencies and different delays in fusing different sensor information. It achieves high real-time, high robustness, and high effectiveness multi-sensor fusion state estimation, which significantly reduces the cumulative error caused by state estimation when the footed robot moves for a long time.
  • the device 40 for executing the method 20 simplifies the analysis and adjustment of the Kalman filter and reduces the running time.
  • the second Kalman filter can be turned off directly when the third sensor information is unavailable without affecting the operation of the entire state estimation device 40 .
  • embodiments of the present application also provide a footed robot, including a memory and a processor.
  • a computer program is stored in the memory.
  • the processor executes the computer program, it implements the steps in the above method embodiments.
  • Embodiments of the present application also provide a computer-readable storage medium for storing computer programs.
  • the computer-readable storage medium can be applied to a footed robot, and the computer program causes the footed robot to execute corresponding processes in the state estimation method in the embodiments of the present application. For the sake of brevity, details will not be repeated here.
  • Embodiments of the present application also provide a computer program product.
  • the computer program product includes computer instructions, and the computer instructions are stored in a computer-readable storage medium.
  • the processor of the legged robot reads the computer instructions from the computer-readable storage medium, and the processor executes the computer instructions, causing the legged robot to execute the corresponding processes in the state estimation method in the embodiments of the present application. For the sake of simplicity, this is not mentioned here. Again.
  • An embodiment of the present application also provides a computer program.
  • the computer program includes computer instructions, and the computer instructions are stored in a computer-readable storage medium.
  • Processor design for legged robots The computer-readable storage medium reads the computer instructions, and the processor executes the computer instructions, causing the legged robot to execute corresponding processes in the state estimation method in the embodiments of the present application. For the sake of brevity, details will not be repeated here.
  • FIG. 7 shows a schematic diagram of an electronic device 2000 according to an embodiment of the present application.
  • the electronic device 2000 may include one or more processors 2010 and one or more memories 2020 .
  • the memory 2020 stores computer readable code, and when the computer readable code is run by the one or more processors 2010, the method as described above can be executed.
  • the processor in the embodiment of the present application may be an integrated circuit chip with signal processing capabilities.
  • the above-mentioned processor may be a general-purpose processor, a digital signal processor (DSP), an application-specific integrated circuit (ASIC), an off-the-shelf programmable gate array (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components.
  • DSP digital signal processor
  • ASIC application-specific integrated circuit
  • FPGA off-the-shelf programmable gate array
  • Each method, operation, and logical block diagram disclosed in the embodiments of this application can be implemented or executed.
  • the general-purpose processor can be a microprocessor or the processor can be any conventional processor, etc., which can be of X86 architecture or ARM architecture.
  • various example embodiments of embodiments of the present application may be implemented in hardware or special purpose circuits, software, firmware, logic, or any combination thereof. Some aspects may be implemented in hardware, while other aspects may be implemented in firmware or software that may be executed by a controller, microprocessor, or other computing device.
  • aspects of embodiments of the present application are illustrated or described as block diagrams, flow diagrams, or using some other graphical representation, it will be understood that the blocks, devices, systems, techniques, or methods described herein may serve as non-limiting examples. Implemented in hardware, software, firmware, special purpose circuits or logic, general purpose hardware or controllers or other computing devices, or some combination thereof.
  • computing device 3000 may include a bus 3010, one or more CPUs 3020, read only memory (ROM) 3030, random access memory (RAM) 3040, communication port 3050 connected to a network, input/output components 3060, hard disk 3070, etc.
  • the storage device in the computing device 3000 such as the ROM 3030 or the hard disk 3070, can store various data or files used for processing and/or communication of the methods provided by the embodiments of the present application, as well as program instructions executed by the CPU.
  • Computing device 3000 may also include user interface 3080.
  • the architecture shown in FIG. 8 is only exemplary, and when implementing different devices, one or more components in the computing device shown in FIG. 8 may be omitted according to actual needs.
  • Figure 9 shows a schematic diagram of a storage medium 4000 according to an embodiment of the present application.
  • Non-volatile memory may be read-only memory (ROM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), electrically erasable programmable read-only memory (EEPROM), or flash memory.
  • Volatile memory may be random access memory (RAM), which acts as an external cache.
  • RAM Direct Memory Bus Random Access Memory
  • SRAM static random access memory
  • DRAM dynamic random access memory
  • SDRAM synchronous dynamic random access memory
  • DDRSDRAM double data rate synchronous dynamic Random Access Memory
  • ESDRAM Enhanced Synchronous Dynamic Random Access Memory
  • SLDRAM Synchronous Linked Dynamic Random Access Memory
  • DR RAM Direct Memory Bus Random Access Memory
  • Embodiments of the present application also provide a computer program product or computer program.
  • the computer program product or computer program includes computer instructions, and the computer instructions are stored in a computer-readable storage medium.
  • the processor of the computer device reads the computer instructions from the computer-readable storage medium, and the processor executes the computer instructions, so that the computer device performs the method according to the embodiment of the present application.
  • each block in the flowchart or block diagrams may represent a module, segment, or portion of code that contains one or more logic for implementing the specified Function executable instructions.
  • the functions noted in the block may occur out of the order noted in the figures. For example, two blocks shown one after another may actually execute substantially in parallel, or they may sometimes execute in the reverse order, depending on the functionality involved.
  • each block of the block diagram and/or flowchart illustration, and combinations of blocks in the block diagram and/or flowchart illustration can be implemented by special purpose hardware-based systems that perform the specified functions or operations. , or can be implemented using a combination of specialized hardware and computer instructions.
  • various example embodiments of embodiments of the present application may be implemented in hardware or special purpose circuits, software, firmware, logic, or any combination thereof. Some aspects may be implemented in hardware, while other aspects may be implemented in firmware or software that may be executed by a controller, microprocessor, or other computing device.
  • aspects of embodiments of the present application are illustrated or described as block diagrams, flow diagrams, or using some other graphical representation, it will be understood that the blocks, devices, systems, techniques, or methods described herein may serve as non-limiting examples. Implemented in hardware, software, firmware, special purpose circuits or logic, general purpose hardware or controllers or other computing devices, or some combination thereof.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Manipulator (AREA)

Abstract

本申请公开了一种用于足式机器人的状态估计方法、状态估计装置、计算机设备、计算机可读存储介质、一种计算机程序产品。方法包括：获取足式机器人的第一传感器信息和第二传感器信息，并基于第一传感器信息和第二传感器信息，利用第一卡尔曼滤波器确定足式机器人的第一状态信息，并保存一段时间的第一状态信息作为第二卡尔曼滤波的历史信息；获取足式机器人的第三传感器信息，并基于第三传感器信息和所述历史信息，利用第二卡尔曼滤波器确定足式机器人的第二状态信息；以及基于足式机器人的第二状态信息，对足式机器人在当前时刻的第一状态信息进行更新，以确定足式机器人的当前时刻的状态信息。

Description

用于足式机器人的状态估计方法、装置、计算机设备及计算机可读存储介质
相关申请的交叉引用
本申请基于申请号为202210985184.6、申请日为2022年8月17日的中国专利申请提出，并要求该中国专利申请的优先权，该中国专利申请的全部内容在此引入本申请作为参考。
技术领域
本申请实施例涉及足式机器人技术领域，具体涉及足式机器人规划控制技术领域，尤其涉及用于足式机器人的状态估计方法、用于足式机器人的状态估计装置、及计算机设备、以及计算机可读存储介质。
背景技术
在足式机器人的运动过程中，相关技术通常可使用多种手段来实时确定足式机器人的状态(如位置和姿态信息等)。例如，可以使用多种传感器融合来估计足式机器人的本体状态。然而使用惯性测量单元(Inertial Measurement Unit，IMU)和腿部编码器等基于本体感知的传感器进行状态估计时，由于足式机器人在运动过程中往往会出现足端侧滑、变形、机械结构误差等，并且传感器噪音、漂移等也会影响状态估计结果，从而导致估计值不准确。随着足式机器人的运动时长的增加，误差值也会累积。由于足式机器人的控制通常需要当前状态估计值来控制各个关节电机，不准确的状态估计值可能导致难以准确控制足式机器人。
因此，需要对相关技术的状态估计方法进行改进，以在保证实时获取状态估计值的情形下尽可能地消除累积误差。
发明内容
本申请实施例提供了一种用于足式机器人的状态估计方法、装置、计算机设备及计算机可读存储介质，可以在实时获取足式机器人的状态估计值的情况下提高状态估计值的精度。
本申请实施例提供了一种用于足式机器人的状态估计方法，由计算机设备执行，所述方法包括：获取所述足式机器人的第一传感器信息和第二传感器信息，并基于所述第一传感器信息和第二传感器信息，利用第一卡尔曼滤波器确定所述足式机器人的第一状态信息，并保存一段时间的第一状态信息作为第二卡尔曼滤波的历史信息；获取所述足式机器人的第三传 感器信息，并基于所述第三传感器信息和所述历史信息，利用所述第二卡尔曼滤波器确定所述足式机器人的第二状态信息；以及基于所述足式机器人的第二状态信息，对足式机器人在当前时刻的第一状态信息进行更新，以确定所述足式机器人的当前时刻的状态信息。
又一方面，本申请实施例提供了一种用于足式机器人的状态估计装置，所述装置包括：第一传感器，被配置为获取所述足式机器人的第一传感器信息；第二传感器，被配置为获取所述足式机器人的第二传感器信息；第三传感器，被配置为获取所述足式机器人的第三传感器信息；第一卡尔曼滤波器，被配置为基于所述第一传感器信息和所述第二传感器信息，确定所述足式机器人的第一状态信息，并保存一段时间的第一状态信息作为历史信息；第二卡尔曼滤波器，被配置为基于所述第三传感器信息和所述历史信息，利用第二卡尔曼滤波器确定所述足式机器人的第二状态信息；其中，所述第一卡尔曼滤波器还被配置为基于所述第二状态信息以及所述足式机器人在当前时刻对应的第一状态信息，确定所述足式机器人在当前时刻的状态信息。
又一方面，本申请实施例提供了一种计算机设备，包括输入接口和输出接口，还包括：处理器，适于实现一条或多条指令；以及，计算机可读存储介质；所述计算机可读存储介质存储有一条或多条指令，所述一条或多条指令适于由所述处理器加载并执行上述的足式机器人的状态估计方法。
又一方面，本申请实施例提供了一种计算机可读存储介质，所述计算机可读存储介质存储有一条或多条指令，所述一条或多条指令适于由处理器加载并执行上述的足式机器人的状态估计方法。
又一方面，本申请实施例提供了一种计算机程序产品，所述计算机程序产品包括计算机程序；所述计算机程序被处理器执行时，实现上述的足式机器人的状态估计方法。
由此，本申请实施例的各个方面融合了以不同频率工作的不同传感器的输出信息，采用两个卡尔曼滤波器解决了融合不同传感器信息的频率不同、延迟不同等问题，实现了高实时性、高鲁棒性、高有效性的多传感器融合状态估计，显著减小了足式机器人长时间运动时状态估计带来的累积误差。
附图说明
为了更清楚地说明本申请实施例的技术方案，下面将对实施例的描述中所需要使用的附图作简单的介绍。显而易见地，下面描述中的附图仅仅是本申请的一些示例性实施例，对于本领域普通技术人员来说，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。以下附图并未刻意按实际尺寸等比例缩放绘制，重点在于示出本申请实施例的主旨。
图1是示出根据本申请实施例的足式机器人的示意图。
图2是示出根据本申请实施例的用于足式机器人的状态估计方法的流程图。
图3是示出根据本申请实施例的根据用于足式机器人的状态估计方法的示意图，其中示意性地示出了第一状态信息和第二状态信息。
图4是示出根据本申请实施例的用于足式机器人的状态估计装置的部分结构图。
图5是示出根据本申请实施例的用于足式机器人的状态估计装置执行方法的示意图。
图6是示出根据本申请实施例的方法的执行效果与传统方案的对比图。
图7示出了根据本申请实施例的电子设备的示意图。
图8示出了根据本申请实施例的示例性计算设备的架构的示意图。
图9示出了根据本申请实施例的存储介质的示意图。
具体实施方式
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请实施例，本领域技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请实施例保护的范围。
在本申请实施例的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”、“顺时针”、“逆时针”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本申请实施例和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本申请实施例的限制。此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个所述特征。在本申请实施例的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。
在本申请实施例的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接或可以相互通讯；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本申请实施例中的具 体含义。
在本申请实施例中，除非另有明确的规定和限定，第一特征在第二特征之“上”或之“下”可以包括第一和第二特征直接接触，也可以包括第一和第二特征不是直接接触而是通过它们之间的另外的特征接触。而且，第一特征在第二特征“之上”、“上方”和“上面”包括第一特征在第二特征正上方和斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征“之下”、“下方”和“下面”包括第一特征在第二特征正下方和斜下方，或仅仅表示第一特征水平高度小于第二特征。
下文的公开提供了许多不同的实施方式或例子用来实现本申请实施例的不同结构。为了简化本申请实施例，下文中对特定例子的部件和设置进行描述。当然，它们仅仅为示例，并且目的不在于限制本申请实施例。此外，本申请实施例可以在不同例子中重复参考数字和/或参考字母，这种重复是为了简化和清楚的目的，其本身不指示所讨论各种实施方式和/或设置之间的关系。此外，本申请实施例提供了的各种特定的工艺和材料的例子，但是本领域普通技术人员可以意识到其他工艺的应用和/或其他材料的使用。
虽然本申请实施例对根据本申请实施例的装置中的某些模块做出了各种引用，然而，任何数量的不同模块可以被使用并运行在用户终端和/或服务器上。所述模块仅是说明性的，并且所述装置和方法的不同方面可以使用不同模块。
本申请实施例中使用了流程图用来说明根据本申请实施例的方法和装置所执行的操作。应当理解的是，前面或下面操作不一定按照顺序来精确地执行。相反，根据需要，可以按照倒序或同时处理各种步骤。同时，也可以将其他操作添加到这些过程中，或从这些过程移除某一步或数步操作。
本申请实施例的足式机器人是一种使用腿进行移动的足式机器人，其以动物为仿生对象，目的是依据工程技术和科学研究成果模拟动物的运动形式和复制动物的运动能力。足式机器人对各种环境(包括结构化环境(诸如公路、铁路、经处理的平整路面等)和非结构化环境(诸如山地、沼泽、崎岖不平的路面等))具有很强的适应能力，其能够适应地形的各种变化、翻越较高的障碍物，而且能够有效地减小载荷、提高系统的能量利用效率。足式机器人按照足数可分为单足、双足、四足、六足、八足等，其中，四足足式机器人具有超强的运动能力，其比双足足式机器人静态稳定性好，且比六足、八足足式机器人运动简单灵活，因此，四足足式机器人是研究足式机器人的一种常见选择。四足足式机器人的步态是四足足式机器人为了能够进行连续的移动，其四条腿在时间和空间上的协调关系。四足足式机器人的步态来自于四足哺乳动物的步态，其可以包括但不限于以下简化的三种形式：行走(walk)、小跑(trot)和跳跃(bound)。
本申请实施例的控制足式机器人的方法可以是基于人工智能(Artificial intelligence，AI)的。人工智能是利用数字计算机或者数字计算机控制的机 器模拟、延伸和扩展人的智能，感知环境、获取知识并使用知识获得最佳结果的理论、方法、技术及应用系统。换句话说，人工智能是计算机科学的一个综合技术，它企图了解智能的实质，并生产出一种新的能以人类智能相似的方式做出反应的智能机器。例如，对于基于人工智能的控制足式机器人的方法而言，其能够以类似于人类引导活体动物运动的方式来规划足式机器人的运动轨迹和步态，使得足式机器人的运动更灵活仿生。人工智能通过研究各种智能机器的设计原理与实现方法，使本申请实施例的控制足式机器人的方法具有自动高效地基于足式机器人的当前运动状态设计足式机器人的后续运动轨迹和步态的功能。
综上所述，本申请实施例提供的方案涉及人工智能、机器学习等技术，下面将结合附图对本申请实施例进行进一步地描述。
图1是示出根据本申请实施例的足式机器人100的示意图。如图1所示，以四足足式机器人为例，图1中的左图和右图分别示出了示例足式机器人的内部透视图和外部结构图。
该示例足式机器人能够基于四条机械腿进行移动。其中，每条机械腿可以包括大腿和小腿，且可包括大腿和小腿，且每条机械腿可包括至少一个关节。例如，每条机械腿上可以包括多个下肢关节，例如髋关节两个自由度和膝关节1个自由度。值得注意的是，“关节”一词在本申请实施例中指示足式机器人的两个或两个以上的组件之间能够活动的连接。关节可以在关节电机输出的力和力矩的控制下进行运动。例如，关节可以通过转动一个角度，以使得其他关节及其连杆机构能够在空间中产生一定的移动量，从而导致足式机器人100的姿态/运动状态的改变。
除此之外，每条机械腿还可配置有多个电机，这些电机可以分别用来控制或组合控制四足足式机器人的髋关节两个自由度和膝关节1个自由度。需要说明的是，足式机器人上还可配置各式各样的测量组件，如IMU传感器和关节编码器等；其中，IMU传感器可以实时地提供足式机器人的加速度和姿态信息，关节编码器可以实时地提供足式机器人的各个关节的关节角信息(如关节角的角度、角速度反馈值等)。
示例足式机器人在上述提及的多个电机的控制下，已经能够实现多种运动，例如，包括空翻或跳跃等动作运动。然而控制该足式机器人执行这些动作往往需要准确的状态数据。如果状态数据不准确，各项控制方案就难以实现足式机器人的准确灵活控制。
在控制足式机器人的运动过程中，相关技术提供了多种手段来实时确定足式机器人的状态(如位置和姿态信息等)。例如，可以使用多种传感器来估计足式机器人的本体状态。例如，在工业界已经提出了多种融合IMU数据、关节编码器的数据、激光数据、动作捕捉数据等等来确定足式机器人本体状态的方法，但这些方法都无法解决本体感知的传感器状态估计器在长时间运行下的状态漂移，导致不可避免产生累积误差。
因此，针对上述问题，本申请实施例提供了一种用于足式机器人的状态估计方法，所述方法包括：获取所述足式机器人的第一传感器信息和第二传感器信息，并基于所述第一传感器信息和第二传感器信息，利用第一卡尔曼滤波器确定所述足式机器人的第一状态信息，并保存一段时间的第一状态信息作为第二卡尔曼滤波的历史信息；获取所述足式机器人的第三传感器信息，并基于所述第三传感器信息和所述历史信息，利用所述第二卡尔曼滤波器确定所述足式机器人的第二状态信息；以及基于所述足式机器人的第二状态信息，对足式机器人在当前时刻的第一状态信息进行更新，以确定所述足式机器人的当前时刻的状态信息。
又一方面，本申请实施例还提供了一种用于足式机器人的状态估计装置，所述装置包括：第一传感器，被配置为获取所述足式机器人的第一传感器信息；第二传感器，被配置为获取所述足式机器人的第二传感器信息；第三传感器，被配置为获取所述足式机器人的第三传感器信息；第一卡尔曼滤波器，被配置为基于所述第一传感器信息和所述第二传感器信息，确定所述足式机器人的第一状态信息，并保存一段时间的第一状态信息作为历史信息；第二卡尔曼滤波器，被配置为基于所述第三传感器信息和所述历史信息，利用第二卡尔曼滤波器确定所述足式机器人的第二状态信息；其中，所述第一卡尔曼滤波器还被配置为基于所述第二状态信息以及所述足式机器人在当前时刻对应的第一状态信息，确定所述足式机器人在当前时刻的状态信息。
相比于传统的状态估计方案，本申请实施例的各个方面融合了以不同工作频率的传感器的传感器信息，采用两个卡尔曼滤波器解决了相关技术的的方案中仅能以较低频率融合不同传感器信息进而导致的延迟高等问题，实现了高实时性、高鲁棒性、高有效性的多传感器融合状态估计，显著减小了足式机器人长时间运动时状态估计带来的累积误差。
以下参考图2至图9来进一步描述本申请实施例的各个方面的示例。本领域技术人员应当理解本申请实施例并不以此为限。
图2是示出根据本申请实施例的用于足式机器人的状态估计方法20的流程图。图3是示出根据本申请实施例的根据用于足式机器人的状态估计方法20的示意图，其中示意性地示出了第一状态信息和第二状态信息。
根据本申请实施例的用于足式机器人的状态估计方法20可以包括如图2所示的操作S201-S203。如图2所示，方法20包括操作S201至操作S203之一或全部，也可以包括更多的操作。操作S201至操作S203的执行主体可以是位于足式机器人上的各个组件(例如，位于足式机器人上的各种传感器和计算组件)，也可以是位于足式机器人外部的各种组件(例如，位于足式机器人外部的各种传感器和计算组件)。本申请实施例并不对操作S201至操作S203的执行主体进行限制。方法20可以由后续详述的装置40进行处理。
作为一个示例，方法20可以由任意的计算设备执行。此处的计算机设备可以是终端或服务器；或者，此处的计算机设备也可以是由终端和服务器共同执行，对此不作限定。其中，终端可以是智能手机、电脑(如平板电脑、笔记本电脑、台式计算机等)、智能穿戴设备(如智能手表、智能眼镜)、智能语音交互设备、智能家电(如智能电视)、车载终端或飞行器等；服务器可以是独立的物理服务器，也可以是多个物理服务器构成的服务器集群或者分布式系统，还可以是提供云服务、云数据库、云计算、云函数、云存储、网络服务、云通信、中间件服务、域名服务、安全服务、CDN(Content Delivery Network，内容分发网络)、以及大数据和人工智能平台等基础云计算服务的云服务器，等等。进一步的，终端和服务器可位于区块链网络内或区块链网络外，对此不作限定；更进一步的，终端和服务器还可将内部所存储的任一数据上传至区块链网络进行存储，以防止内部所存储的数据被篡改，提升数据安全性。
例如，在操作S201中，获取所述足式机器人的第一传感器信息和第二传感器信息，并基于所述第一传感器信息和第二传感器信息，利用第一卡尔曼滤波器确定所述足式机器人的第一状态信息，并将保存一段时间的第一状态信息作为第二卡尔曼滤波的历史信息。
可选地，所述第一传感器信息可以是通过第一传感器采集的任意信息。可选地，第一传感器信息带有时间戳。第一传感器可以采用第一频率工作，以获得第一时间戳集合中的多个时间戳对应的第一传感器信息。也即，所述第一传感器信息可以包括第一传感器在第一时间戳集合中的每个时间戳采集的信息。第一频率可能相对较高，进而保证第一传感器的实时性。示例性的第一频率可以为0.01kHZ至1kHZ。例如，第一传感器可以包括以下各项设备中的至少一项：触觉传感器、力和力矩传感器、惯性测量单元(IMU)等等。本申请实施例并不对传感器信息的种类和具体的信号形式进行限制。又例如，在足式机器人100还包括机械臂的情况下，所述第一传感器还可以包括机械臂对应的力和力矩传感器。在机械臂上还包括手指端的情况下，所述第一传感器还可以包括手指端的触觉传感器等。在一些实施例中，第一传感器可以物理地附着在足式机器人上，其附着位置可以随着足式机器人的构型的变化而变化，本申请实施例并不以此为限。在又一些实施例中，第一传感器也可以未物理地附着在足式机器人上，本申请实施例并不以此为限。
可选地，所述第二传感器信息可以是通过第二传感器采集的任意信息。可选地，第二传感器也带有时间戳。第二传感器可以以第二频率工作，以获得第二时间戳集合中的多个时间戳对应的第二传感器信息。所述第二传感器信息可以用于计算足式机器人的动作和姿态相关的信息。例如，第二传感器可以是关节编码器，其用于采集所述足式机器人的第二传感器信息，从而获得所述足式机器人的各个关节的关节角信息(如关节角的角度、角 速度反馈值等)。可选地，所述关节编码器也可以以第二频率工作。示例性的第二频率可以为0.1至1kHZ。第二频率可以略低于第一频率。
第一卡尔曼滤波器(Kalman Filter)既可以是一种标准的卡尔曼滤波器，也可以是一种扩展卡尔曼滤波器。具体地，卡尔曼滤波器是一种高效率的递归滤波器(自回归滤波器)，它能够从一系列的不完全及包含噪音的测量信息(例如，第一传感器信息和第二传感器信息)中，估计足式机器人的状态。卡尔曼滤波会根据各测量信息在不同时间下的值，考虑各时间下的联合分布，再产生对未知变量的估计(例如，第一状态信息)，因此会比只以单一测量值为基础的估计方式更准确。所谓的扩展卡尔曼滤波是标准卡尔曼滤波在非线性情形下的一种扩展形式，其可将非线性函数进行泰勒展开，省略高阶项，保留展开项的一阶项，以此来实现非线性函数线性化。以下的各个示例虽然是以标准的卡尔曼滤波器为例进行的说明，但是本领域技术人员应当理解本申请实施例并不以此为限。
在一个具体实现中，第一传感器信息和第二传感器信息带有时间戳，所述基于所述第一传感器信息和第二传感器信息，利用第一卡尔曼滤波器确定所述足式机器人的第一状态信息：将带有时间戳的第一传感器信息和第二传感器信息，将第一传感器信息和第二传感器信息，依次作为第一卡尔曼滤波器的输入；以及通过第一卡尔曼滤波器进行状态估计，得到带有时间戳的足式机器人的第一状态信息，并将带有时间戳的第一状态信息保存为历史信息，所述第一状态信息包括第一卡尔曼滤波器输出的带有时间戳的多个状态估计结果。
可选的，参考图3，假设当前时刻为Tb，而多个时间戳被包括从时刻Ta到时刻Tb对应的时间段内。可将上述多个时间戳中的每个时间戳对应的第一传感器信息和第二传感器信息依次作为第一卡尔曼滤波器的输入，以通过第一卡尔曼滤波器进行状态估计，得到足式机器人在时间段[Ta,Tb]中的每个时间戳对应的第一状态信息。值得注意的是，第一状态信息包括第一卡尔曼滤波器输出的多个时间戳对应的多个状态估计结果。
在一个示例中，所述第一状态信息可以为以下数据组成的时序数值序列：各个时间步(每个时间步对应一个时间戳)对应的质心和足端相关信息。时序数值序列中的每个元素对应一个状态估计结果。可选地，该时序数值序列中的每个元素可以具有多个维度，其分别表示某个时间步对应的质心和足端在x轴方向上的位置、质心在y轴方向上的位置、质心在z方向(重力方向)上的位置、足式机器人的倾斜角(pitch)、足式机器人的偏航角(yaw)以及足式机器人的滚动角(roll)等等，本申请实施例并不以此为限。
上述过程可以实时进行，也即，每采集一个时间戳对应的第一传感器信息和第二传感器信息，就输入至第一卡尔曼滤波器以得到一个状态估计结果(例如，图3中以曲线表示的第一状态信息，曲线中的每个点对应于 一个时刻的状态信息)。
举例来说，在时间戳Ta，第一卡尔曼滤波器输出的状态估计结果为Sa。在时间戳Tb-1，第一卡尔曼滤波器输出的状态估计结果为Sb-1。以下，术语“状态估计结果Sa至Sb-1”包括从时刻Ta到时刻Tb-1对应的历史时间段内的每个时间戳Ti(Ta<Ti<Tb-1)对应的状态估计结果Si。之后将参考图4和图5中描述的示例进一步说明通过第一卡尔曼滤波器获得第一状态信息的示例方案，本申请实施例在此就不再赘述。
然后，可以保存一段时间的第一状态信息(例如，时间段[Ta,Tb-1]中的每个时间戳对应的第一状态估计结果Sa至Sb-1)作为第二卡尔曼滤波的历史信息。
接着，在操作S202中，获取所述足式机器人的第三传感器信息，并基于所述第三传感器信息和所述历史信息，利用所述第二卡尔曼滤波器确定所述足式机器人的第二状态信息。
可选地，所述第三传感器信息可以是通过第三传感器采集的任意信息。第三传感器可以以第三频率工作，以在固定时间段仅获得所述多个时刻中的一个时刻对应的第三传感器信息。第三频率可能相对较低，例如低于第一频率。示例性的第三频率可以为10至50HZ。传感器的工作频率和准确度是相互制约的，即工作频率越高，则准确性随之下降，由于第三传感器采集的第三传感器信息的实时性相较于第一传感器低，但准确度相较第一传感器高，不会随着足式机器人的运动累积误差。
可选地，所述第三传感器可以包括以下各项设备中的至少一项：距离传感器(TOF)、提供视觉信息的相机或摄像头、雷达、位置传感器(例如GPS传感器)、激光传感器等等。
类似地，第二卡尔曼滤波器也可以是一种标准的卡尔曼滤波器或一种扩展卡尔曼滤波器。可选的，可确定所述第三传感器信息对应的时间戳，将所述历史信息和所述第三传感器信息作为第二卡尔曼滤波器的输入；并通过第二卡尔曼滤波器进行状态估计，得到所述第三传感器信息对应的时间戳的状态估计结果作为所述第二状态信息。此外，在另一实施例中，为进一步缩减计算量，可仅将上述历史信息中与第三传感器信息对应的时间戳对应的状态估计结果以及第三传感器信息作为第二卡尔曼滤波器的输入，以通过第二卡尔曼滤波器进行状态估计，得到所述第三传感器信息对应的时间戳的状态估计结果，将状态估计结果作为所述第二状态信息。
具体地，第二卡尔曼滤波器进行状态估计的过程可以被简述如下：首先，基于所述第三传感器信息对应的时间戳，确定历史信息中与所述时间戳对应的状态估计结果，与所述时间戳对应的状态估计结果是指，与所述时间戳对应的状态估计结果的时间戳，基于所述第三传感器信息对应的时间戳相同；然后，基于所述第三传感器信息以及所述历史信息中与所述时间戳对应的状态估计结果，利用所述第二卡尔曼滤波器进行状态估计，以 获取所述时间戳对应的状态估计结果，将状态估计结果作为所述第二状态信息。
例如，参考图3，以第三传感器为提供视觉信息的相机或摄像头为例，相机或摄像头可能在时刻Ta开启快门以捕获足式机器人在时刻Ta的图像，然后对该图像进行处理，最后在当前时刻Tb，将时刻Ta对应的第三传感器信息提供给第二卡尔曼滤波器。在时刻Tb，第二卡尔曼滤波器可以根据时刻Ta对应的第一状态信息，结合时刻Ta的第三传感器信息，输出时间段Ta至Tb-1中的各个时间戳的状态估计结果。
如图3所示，在时刻Ta，第一卡尔曼滤波器的状态估计结果Sa与第二卡尔曼滤波器的状态估计结果之间存在状态增量ΔS，表示第一卡尔曼滤波器的状态估计结果Sa与第二卡尔曼滤波器的状态估计结果之间的差异。在一个示例中，第二卡尔曼滤波器可以将该状态增量ΔS叠加到所述足式机器人在当前时刻Tb对应的第一状态信息Sb，以确定所述足式机器人的当前时刻的状态信息也就是说，将状态增量ΔS叠加到所述足式机器人在当前时刻Tb对应的第一状态信息Sb，将得到的叠加结果作为所述足式机器人的当前时刻的状态信息
此外，在另一个示例中，第二卡尔曼滤波器可以基于该状态增量ΔS一一校正第一卡尔曼滤波器在时间段Ta至Tb-1中的各个时间戳确定的状态估计结果(各个时间戳确定的状态估计结果记为Sa至Sb-1)，以得到第二状态信息至其中，为第三传感器信息估计的时刻Tb-1对应的状态估计结果。例如，可以通过以下公式对第一状态信息中的每个时间戳对应的状态估计结果进行校正：其中，a<i<b。在另一个示例中，第二卡尔曼滤波器可以以时刻Ta对应的第一卡尔曼滤波器的状态估计结果Sa与第二卡尔曼滤波器的状态估计结果之间的偏移量为初始值，迭代地更新时间段Ta至Tb-1中的各个时间戳的状态估计结果，以获得第二状态信息至在另一个示例中，第二卡尔曼滤波器还可以进一步结合足式机器人的腿部运动学模型或动力学模型，基于第二卡尔曼滤波器的状态估计结果依次更新时间段Ta至Tb-1中的各个时间戳的状态估计结果以获得第二状态信息至本申请实施例对此不进行限制。
动力学模型是描述机器人的力和运动之间的关系的数学方程，作为示例，动力学模型可以是虚拟模型控制器(Virtual Model Controller，VMC)模型，弹簧负载倒立摆模(Spring Loaded Inverted Pendulum，SLIP)和浮动机体模型。
之后将参考图4和图5中描述的示例进一步说明通过第二卡尔曼滤波器获得第二状态信息的示例方案，本申请实施例在此就不再赘述。
在操作S203中，获取所述足式机器人在当前时刻对应的第一状态信息，并基于所述足式机器人的第二状态信息，确定所述足式机器人的当前时刻的状态信息。
参考图3，继续以当前时刻为Tb为例进行说明。为了得到当前时刻Tb的状态Sb，在时刻Tb，第一传感器可以采集Tb时刻对应的第一传感器信息，关节编码器可以采集Tb时刻对应的第二传感器信息。此时，第一卡尔曼滤波器可以Tb时刻对应的第一传感器信息和第二传感器信息为输入，得到时刻Tb对应的第一状态估计结果Sb，并以操作S202中的状态增量ΔS对第一状态估计结果Sb进行校正，以得到通过状态增量ΔS消除了Sb中的累积误差，进而得到了相对准确的也即，所述足式机器人的当前时刻的状态信息为经第一卡尔曼滤波器和第二卡尔曼滤波器融合后的信息，其准确度较高。
由此，本申请实施例的方法20融合了以不同频率工作的不同传感器的传感器信息以及第二传感器信息，采用两个卡尔曼滤波器解决了融合不同传感器信息的频率低、延迟高等问题，实现了高实时性、高鲁棒性、高有效性的多传感器融合状态估计，显著减小了足式机器人长时间运动时状态估计带来的累积误差。尤其是，方法20简化了卡尔曼滤波器的分析与调整，减少了运行时间。此外，第二卡尔曼滤波器可以在第三传感器信息不可用时直接被关闭，而不影响整个状态估计装置的运行。
接下来，参考图4，结合图1描述足式机器人100以及图2至图3描述的方法20，进一步描述操作S201至操作S203的一些细节。其中，图4是示出根据本申请实施例的用于足式机器人的状态估计装置40的部分结构图。
如图4所示，用于足式机器人的状态估计装置40可选地包括第一传感器、第二传感器、第三传感器、第一卡尔曼滤波器和第二卡尔曼滤波器。其中，第一卡尔曼滤波器可以以高频低延时状态运行，而第二卡尔曼滤波器可以以低频高延时状态运行。
第一传感器，被配置为获取所述足式机器人在第一时间戳集合中的每个时间戳对应的第一传感器信息以及所述足式机器人在当前时刻对应的第一传感器信息。由前述可知，足式机器人中的第一传感器可包括IMU传感器，IMU传感器可以以第一频率在多个时刻采集足式机器人的姿态信息和加速度，以作为该时刻对应的第一传感器信息。IMU传感器可以被配置为在多个不同的离散时刻，获取这些时刻对应的各个关节的三轴姿态角(或角速率)以及加速度。IMU传感器输出的数据包括时间戳、三维加速度、三维角速度、朝向等等，本申请实施例并不以此为限。
例如，作为第二传感器的关节编码器可选地被配置为获取所述足式机器人在第二时间戳集合中的每个时间戳对应的第二传感器信息，以及获取所述足式机器人在当前时刻对应的第二传感器信息。作为一个示例，关节编码器可以在每个时刻采集足式机器人的每条机械腿上的各个关节的关节角信息(如关节角度、角速度反馈值等)，以作为该时刻对应的第二传感器信息。例如，关节编码器可以被配置为针对多个不同的时间戳，获取这些时间戳对应的所有关节的角度q[]和关节角速度
第一卡尔曼滤波器，被配置为基于所述多个时间戳对应的第一传感器信息和所述第二传感器信息，确定所述足式机器人的第一状态信息。可选地，第一卡尔曼滤波器还包括观测模型和预测模型。所述观测模型用于状态观测，通过状态观测可获取状态观测数据。所述预测模型用于状态预测，通过状态预测可获取状态预测数据。如之后详述的，所述预测模型通过第一传感器信息获取预测状态和对应协方差，所述观测模型通过第二传感器信息获得观测状态和对应协方差。当然本申请实施例并不以此为限。
具体地，假设第一时间戳集合和第二时间戳集合均包括时间戳Ti。针对时间戳Ti，所述第一卡尔曼滤波器进行状态估计以获取时间戳Ti对应的状态估计结果，包括：将时间戳Ti对应的第一传感器信息输入至观测模型，以获取时间戳Ti对应的状态观测数据；将时间戳Ti的前一时间戳Ti-1对应的状态估计结果和时间戳Ti对应的第一传感器信息输入至预测模型，以获取时间戳Ti对应的状态预测数据；以及使用基于时间戳Ti对应的第二传感器获得的测量值，对时间戳Ti对应的状态预测数据进行校正，以获取时间戳Ti对应的状态估计结果。例如，所述测量值是时间戳Ti对应的第二传感器信息经过运动学解算后得到的。
作为一个示例，假设观测模型是基于观测矩阵H的，用于实现观测量到状态观测数据的映射，时间戳Ti对应的状态观测数据记为Yi，观测模型可以被表示为Yi＝Hxi，其中xi为时间戳Ti对应的观测量，其是基于第一传感器信息而确定的。作为又一个示例，预测模型可以被表示为 其中mi～N(0,Qi)为预测噪音，A为状态转移矩阵，B为控制矩阵。
在具体实现中，第一卡尔曼滤波器确定足式机器人在时刻Ti的第一状态信息的大致原理如下：
首先，可调用足式机器人中的IMU传感器，采集足式机器人在时刻Ti的第一传感器信息，其中，时刻Ti的第一传感器信息包括时刻Ti的加速度信息(其可包括足式机器人在多个方向(如竖直方向、水平方向)上的加速度)和姿态信息，以及调用第二传感器关节编码器确定足式机器人的各个关节在时刻Ti的关节角信息(如关节角的角度、角速度反馈值等)。
然后，可将时刻Ti的姿态信息和关节角信息汇入观测模型计算出状态观测数据，该状态观测数据可包括：多个状态观测量的值。多个状态观测量的值包括：足式机器人的各条机械腿的足端在时刻Ti的观测位置、足式机器人的质心在时刻Ti的观测位置。作为一个示例，观测模型可以输出一组以时间戳标识的位姿数据。例如，所述位姿数据可选地包括世界坐标系/机体坐标系下质心和足端的XYZ坐标轴的值、朝向数据(以一个四元数表示)、以及足式机器人100在世界坐标系/机体坐标系下沿XYZ坐标轴的速度等等。
同时，还可将足式机器人在时刻Ti的前一时刻Ti-1的状态估计结果、足式机器人在时刻Ti的加速度信息、姿态信息中的至少一项汇入至预测模型， 以计算出时刻Ti的状态预测数据。该状态预测数据可包括多个状态预测量的值以及先验协方差。多个状态预测量的值包括足式机器人的各条机械腿的足端在时刻Ti的预测位置、足式机器人的质心在时刻Ti的预测位置、足式机器人的质心在时刻Ti的速度等。
接着，第一卡尔曼滤波器可以基于时刻Ti的状态观测数据和时刻Ti状态预测数据确定时刻Ti对应的状态估计结果。第一卡尔曼滤波器的目标是在给定时刻Ti的状态观测数据、测量值和状态预测数据的情况下，使用状态观测数据和测量值对状态预测数据进行校正以获取对应的状态估计结果。
第三传感器，被配置为获取所述足式机器人在所述多个时间戳中的一个时间戳对应的第三传感器信息。由前述可知，足式机器人中的第三传感器可包括提供视觉信息的相机或摄像头(以下又叫做视觉传感器)，视觉里程计可以将视觉传感器采集到的图像信息转化为足式机器人在世界坐标系下的位置信息和姿态信息。第三传感器例如是一种双目单通道的相机，并且每次拍摄1456*1088像素的照片，且该照片中的每个像素用八比特来表示。第三传感器可以每隔0.1s触发一次拍摄，当然本申请实施例并不以此为限，第三传感器还可以以更长或更短的时间间隔触发一次拍摄。第三传感器还例如是一种单目单通道的相机，其拍摄的照片的分辨率为640x576或512x512，相片中每个像素用16比特表示。当然本申请实施例并不以此为限。
第二卡尔曼滤波器，被配置为基于所述多个时间戳中的一个时间戳对应的第三传感器信息和所述第一状态信息，利用第二卡尔曼滤波器确定所述足式机器人的第二状态信息。可选地，第二卡尔曼滤波器还包括视觉里程计(Visual odometry)。视觉里程计可以通过分析一系列图像序列，来确定足式机器人的朝向和位置。
在具体实现中，第二卡尔曼滤波器确定足式机器人在时间戳Ti的第二状态信息的大致原理如下：
首先，可调用视觉传感器采集足式机器人在时间戳Ti-c的第三传感器信息。如前述可知，第三传感器采集的数据存在延迟。时间戳Ti-c采集到的第三传感器信息可能会在时间戳Ti才被视觉里程计计算结束并解析出时间戳Ti-c的测量信息。作为一个示例，第三传感器信息延时的最大值为c个时间戳对应的时长。类似地，该测量信息可包括：多个状态观测量的值。多个测量信息值包括：足式机器人的质心在时间戳Ti-c的位置、足式机器人的质心在时间戳Ti-c的速度等等。
然后，可从第一卡尔曼滤波器中获取历史信息，其包括第一卡尔曼滤波器计算的时间戳Ti-c至时间戳Ti的状态估计结果。也即，第一卡尔曼滤波器的历史数据窗口为c个时间戳对应的时长。第二卡尔曼滤波器可以基于视觉里程计获取的时间戳Ti-c的测量信息以及历史信息中来自第一卡尔曼滤波器的时间戳Ti-c的状态估计结果，确定校正后的时间戳Ti-c的状态估计结 果作为时间戳Ti-c的第二状态信息。基于校正后的时间戳Ti-c的状态估计结果，第二卡尔曼滤波器可以预测时间戳Ti-c+1的状态估计结果。以此类推，直到第二卡尔曼滤波器计算出时间戳Ti的状态估计结果作为第二状态信息。
进一步地，所述第一卡尔曼滤波器，还被配置为基于所述第二状态信息以及所述足式机器人在当前时刻对应的第一状态信息，确定所述足式机器人在当前时刻的状态信息。
如上所示，假设当前时刻为Tb，上述的第一传感器已经采集到了时刻Tb的第一传感器信息并且关节编码器也采集到了时刻Tb的第二传感器信息。同时，第二卡尔曼滤波器还可以基于第一卡尔曼滤波器提供的第一状态估计结果Sa和时间戳Ta对应的第三传感器信息，计算出状态增量ΔS，表征第一卡尔曼滤波器提供的第一状态估计结果Sa和时间戳Ta对应的第三传感器信息之间的差异。此时，第一卡尔曼滤波器可以基于上述信息，计算出时刻Tb对应的状态信息。相比于第一卡尔曼滤波器直接使用由第一卡尔曼滤波器在Tb-1估计的状态估计结果来估计时刻Tb的状态信息，使用来自第二卡尔曼滤波器的状态增量ΔS所估计的时刻Tb的状态信息更为准确。
如图4所示，第一卡尔曼滤波器可以以第一频率(例如，图4示出的0.1至1kHZ)向控制器输出所述足式机器人的当前时刻的状态信息。同时，第二卡尔曼滤波器可以以第二频率(例如，图4示出的10至50HZ)向第一卡尔曼滤波器输出第二状态信息。因此，第一卡尔曼滤波器可以间隔0.02s至0.1s，基于第二状态信息调整当前时刻的状态信息，从而避免了累积误差。
由此，本申请实施例的装置40融合了以不同频率工作的不同传感器的传感器信息以及第二传感器信息，采用两个卡尔曼滤波器解决了融合不同传感器信息的不同、延迟不同等问题，实现了高实时性、高鲁棒性、高有效性的多传感器融合状态估计，显著减小了足式机器人长时间运动时状态估计带来的累积误差。尤其是，执行方法20的装置40简化了卡尔曼滤波器的分析与调整，减少了运行时间。此外，第二卡尔曼滤波器可以在第三传感器信息不可用时直接被关闭，而不影响整个状态估计装置的运行。
接下来，参考图5和图6，结合图1描述足式机器人100、图2至图3描述的方法20以及参考图4描述的装置40，进一步描述上述各个组件执行上述各个操作的一些细节。其中，图5是示出根据本申请实施例的用于足式机器人的状态估计装置40执行方法20的示意图。图6是示出根据本申请实施例的方法20的执行效果与传统方案的对比图。
参考图5，结合上述内容，可以使用预测模型来对各个状态量进行观测(这一过程又称为状态预测)。预测模型的输入可以可选地包括第一传感器信息。其中，第一传感器信息包括：世界坐标系和机身坐标系之间的旋转矩阵R(以下又记为WRB，表示机身坐标系B相对于世界坐标系W的旋转矩阵)，机身坐标系下角速度ω、机身坐标系下的加速度a等等。令预测模型输出x为x＝[pT vT p1 T p2 T p3 T p4 T]T。
其中，p表示足式机器人在世界坐标系下的机身质心位置，v表示世界坐标系下的机身质心速度，表示世界坐标系下的足端位置，表示世界坐标系下足端速度(假设足端速度为零，也即足端和地面无滑动)，fi为机械腿的索引。以图1中示出足式机器人100为例，在fi＝1时，pfi表示左前腿在世界坐标系下的足端位置，表示左前腿在世界坐标系下足端速度。在fi＝2时，表示右前腿在世界坐标系下的足端位置，表示右前腿在世界坐标系下足端速度。在fi＝3时，表示左后腿在世界坐标系下的足端位置，表示左后腿在世界坐标系下足端速度。在fi＝4时，表示右后腿在世界坐标系下的足端位置，表示右后腿在世界坐标系下足端速度。
具体地，预测模型可以基于以下描述的第一方程(1)至第三方程(3)来分别预测足式机器人的本体位置、本体速度和足端位置以获取上述的x。第一方程(1)又称为足式机器人本体位置离散预测方程。第二方程(2)又称为足式机器人本体速度离散预测方程。第三方程(3)又称为足式机器人足端位置离散预测方程。
vk+1＝vk+(WRBak+g)dt       (2)
其中，ak为机身坐标系下的加速度，dt为时间步长，g表示重力加速度，下标k表示时间步的索引。
参考图5，结合上述内容，还可以使用观测模型来对各个状态量进行观测(这一过程又称为状态观测)。观测模型可以至少部分地基于图5示出的腿部运动学模型来进行状态观测。观测模型可以以第一传感器信息和第二传感器信息为输入，以状态观测数据为输出。
第二传感器信息包括关节角信息p。状态观测数据包括世界坐标系下足端相对于质心的位置qrel、世界坐标系下足端相对于质心的速度可选地，观测模型还可以以足端触地检测器为输入，以得到更精确的足端观测数据。
具体地，腿部运动学模型是对足式机器人的腿部进行数学建模得到的。例如可以采用MIT Cheetah运动学建模技术，腿部运动学模型用于表示足式机器人的腿部末端运动状态和关节运动状态之间的关系，例如足式机器人质心与足端之间的位置和速度相对关系，其用来消除质心与足端之间的状态漂移。例如，在一个具体实现中，所述基于所述第一传感器信息和第二传感器信息，利用第一卡尔曼滤波器确定所述足式机器人的第一状态信息还可以包括：基于所述第二传感器信息，基于所述足式机器人的腿部运动学模型，确定所述足式机器人的运动信息；至少部分地基于所述运动信息，利用第一卡尔曼滤波器确定所述足式机器人的第一状态信息。
在一个示例中，观测模型可以基于所述足式机器人的腿部运动学模型， 计算所述足式机器人的本体与足端之间的相对位置测量残差、以及所述足式机器人的本体与足端之间的相对速度测量残差，然后将所述足式机器人的本体与足端之间的相对位置测量残差、以及所述足式机器人的本体与足端之间的相对速度测量残差(也即上述的运动信息)，作为所述状态观测数据。
观测模型可以对应地以第四方程(4)和第五方程(5)计算测量残差。其中，第四方程(4)用于计算足式机器人本体与足端之间的相对位置测量残差，第五方程(5)用于计算足式机器人本体与足端之间的相对速度测量残差。其中，ωb为机身坐标系下角速度。

如上所述，第一卡尔曼滤波器可被配置为基于所述多个时间戳对应的第一传感器信息和所述第二传感器信息，确定所述足式机器人的第一状态信息。假设第一卡尔曼滤波器在时刻Tk(对应于第k个时间步)，获取到各个关节电机在时刻Tk的控制输入uk、第一传感器在时刻Tk采集到的第一传感器信息zk。同时，第一卡尔曼滤波器还知晓时刻Tk-1对应的先验状态第一卡尔曼滤波器的目标是基于上述各个值来估计状态均值μ和协方差σ。第一卡尔曼滤波器的状态空间方程可以以第六方程(6)和第七方程(7)示出。
xk＝Axk-1+Buk+mk       (6)
zk＝Cxk-1+nk         (7)
其中mk～N(0,Qk)为预测噪音，nk～N(0,Rk)为观测噪音，A为状态转移矩阵，B为控制矩阵，C为观测矩阵。第一卡尔曼滤波器可以以第八方程(8)至第十二方程(12)，对状态观测数据和状态预测数据进行处理，以融合得到时间步k对应的状态估计结果。




其中P为误差协方差矩阵，初始值由可以设定经验值或任意值。K为卡尔曼增益矩阵，I为单位矩阵，x-表示x的先验值。表示x的估计值。也即，标识Pk的先验估计值。标识xk的先验估计值。其中，的值为第四方程(4)计算得到的足式机器人本体与足端之间的相对位置测量残差和第五方程(5)计算得到的相对速度测量残差的组合。其中，zk又称为测量值，其是通过对第二传感器信息进行运动学解算得到的；又称为观测量或状态观测数据。从而，即可以被定义为残差。由此，第一卡尔曼滤波器能够估计出时刻Tk对应的状态估计结果，并可以将多个时 间步对应的状态估计结果作为第一状态信息，经信息同步后输出至第二卡尔曼滤波器。作为一个示例，第一卡尔曼滤波器估计出时刻Tk对应的状态估计结果包括时刻Tk对应的后验状态和后验协方差
继续参考图5，假设第三传感器为前述的视觉传感器。视觉传感器输出的第三传感器信息z′s为z′s＝[pvo T vvo T]，其中，pvo为视觉传感器在时间步s测量得到的世界坐标系下机身位置，vvo为视觉传感器在时间步s测量得到的世界坐标系下机身速度。由此，第二卡尔曼滤波器对应的状态空间方程可以第十三方程(13)示出。
z′s＝C′sxs+n′s          (13)
其中，n′k～N(0,R′k),s＝k-N。也即，第三传感器信息相对于上述的第一卡尔曼滤波器的信息存在延迟N个时间步(时间戳)的输出。因此，第三传感器和第一状态信息可以通过信息同步，以获得用于校正第一状态信息的测量误差。
具体地，如上所述，由于视觉数据具有一定的延迟，所以优先地，可以将第一状态信息以及误差协方差矩阵，与第三传感器信息z和第三传感器对应的误差协方差矩阵进行时间同步。也即，可以如上所述，将对应于时间步s的第三传感器信息、与对应于时间步s的第一卡尔曼滤波器的状态估计结果、以及误差协方差矩阵，作为后续方程中的迭代参数，基于上述的卡尔曼滤波器的原理进行融合。
例如，第三传感器可以使用第十四方程(14)获取用于校正第一状态信息的相对位置测量残差pvo-p和相对速度测量残差vvo-v。
接着第二卡尔曼滤波器可以基于第十五方程(15)至第十七方程(17)来将上述的经时间同步后的第一状态信息和第三传感器信息进行融合，以获得第二状态信息。其中，相对位置测量残差和相对速度测量残差可以用于组成第十七方程中


其中P′为第二卡尔曼滤波器使用的误差协方差矩阵。K′为第二卡尔曼滤波器使用的卡尔曼增益矩阵，I为单位矩阵，x-表示x的先验值。标识第二卡尔曼滤波器是输出的状态估计值的先验估计值。如上所述，等于第一卡尔曼滤波器在时间步s输出的后验状态等于第一卡尔曼滤波器在时间步s输出的后验协方差由此，实现了将第一卡尔曼滤波器获得的后验状态和后验协方差作为所述第二卡尔曼滤波器的先验状态和先验协方差来与第二卡尔曼滤波器的观测信息进行融合，进而获取时间步s对应的状态估计结果作为所述第二状态信息。
由于第一卡尔曼滤波器在接收到时刻Tk对应的第二状态信息之后，还能结合时刻Tk对应的第一传感器信息和第二传感器信息，基于上述的第一方程(1)至第十二方程(12)确定时刻Tk对应的状态估计结果。
此时，第一卡尔曼滤波器可以基于和之间的状态增量ΔS，将状态增量ΔS叠加到第一卡尔曼滤波器在时刻Tk对应的状态估计结果，以实现对时间步k处的状态估计结果进行校正。
此外，在另一些示例中，第二卡尔曼滤波器还能够通过反复迭代N次，从时间步s对应的估计出时刻Tk对应的状态估计结果作为第二状态信息输出至第一卡尔曼滤波器，以对当前的状态信息进行校正。本申请实施例并不以此为限。
如图6所示，示出了动作捕捉系统真值数据、传统方法估计结果、以及以采用方法20获得的估计结果对应的三条曲线，其横轴均为时间，竖轴为位置数据。在足式机器人自由运动200秒后，X、Y方向位置的累积误差分别减少了96.08％，62.52％，均方根误差分别减少了92.94％，58.63％。另外速度的均方根误差也分别减小了31.05％和16.47％。可见，方法20融合了视觉数据后可以明显减小累积误差。
由此，本申请实施例的装置40融合了以不同频率工作的不同传感器的传感器信息以及第二传感器信息，采用两个卡尔曼滤波器解决了融合不同传感器信息的频率不同、延迟不同等问题，实现了高实时性、高鲁棒性、高有效性的多传感器融合状态估计，显著减小了足式机器人长时间运动时状态估计带来的累积误差。尤其是，执行方法20的装置40简化了卡尔曼滤波器的分析与调整，减少了运行时间。此外，第二卡尔曼滤波器可以在第三传感器信息不可用时直接被关闭，而不影响整个状态估计装置40的运行。
可选的，本申请实施例还提供了一种足式机器人，包括存储器和处理器，存储器中存储有计算机程序，该处理器执行计算机程序时实现上述各方法实施例中的步骤。
本申请实施例还提供了一种计算机可读存储介质，用于存储计算机程序。该计算机可读存储介质可应用于足式机器人，并且该计算机程序使得足式机器人执行本申请实施例中的状态估计方法中的相应流程，为了简洁，在此不再赘述。
本申请实施例还提供了一种计算机程序产品，该计算机程序产品包括计算机指令，该计算机指令存储在计算机可读存储介质中。足式机器人的处理器从计算机可读存储介质读取该计算机指令，处理器执行该计算机指令，使得足式机器人执行本申请实施例中的状态估计方法中的相应流程，为了简洁，在此不再赘述。
本申请实施例还提供了一种计算机程序，该计算机程序包括计算机指令，计算机指令存储在计算机可读存储介质中。足式机器人的处理器从计 算机可读存储介质读取该计算机指令，处理器执行该计算机指令，使得足式机器人执行本申请实施例中的状态估计方法中的相应流程，为了简洁，在此不再赘述。
根据本申请实施例的又一方面，还提供了一种电子设备，用于实施根据本申请实施例的方法。图7示出了根据本申请实施例的电子设备2000的示意图。
如图7所示，所述电子设备2000可以包括一个或多个处理器2010，和一个或多个存储器2020。其中，所述存储器2020中存储有计算机可读代码，所述计算机可读代码当由所述一个或多个处理器2010运行时，可以执行如上所述的方法。
本申请实施例中的处理器可以是一种集成电路芯片，具有信号的处理能力。上述处理器可以是通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现成可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本申请实施例中的公开的各方法、操作及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，可以是X86架构或ARM架构的。
一般而言，本申请实施例的各种示例实施例可以在硬件或专用电路、软件、固件、逻辑，或其任何组合中实施。某些方面可以在硬件中实施，而其他方面可以在可以由控制器、微处理器或其他计算设备执行的固件或软件中实施。当本申请实施例的各方面被图示或描述为框图、流程图或使用某些其他图形表示时，将理解此处描述的方框、装置、系统、技术或方法可以作为非限制性的示例在硬件、软件、固件、专用电路或逻辑、通用硬件或控制器或其他计算设备，或其某些组合中实施。
例如，根据本申请实施例的方法或装置也可以借助于图8所示的计算设备3000的架构来实现。如图8所示，计算设备3000可以包括总线3010、一个或多个CPU 3020、只读存储器(ROM)3030、随机存取存储器(RAM)3040、连接到网络的通信端口3050、输入/输出组件3060、硬盘3070等。计算设备3000中的存储设备，例如ROM 3030或硬盘3070可以存储本申请实施例提供的方法的处理和/或通信使用的各种数据或文件以及CPU所执行的程序指令。计算设备3000还可以包括用户界面3080。当然，图8所示的架构只是示例性的，在实现不同的设备时，根据实际需要，可以省略图8示出的计算设备中的一个或多个组件。
根据本申请实施例的又一方面，还提供了一种计算机可读存储介质。图9示出了根据本申请实施例的存储介质4000的示意图。
如图9所示，所述计算机可读存储介质4020上存储有计算机可读指令4010。当所述计算机可读指令4010由处理器运行时，可以执行参照以上附图描述的根据本申请实施例的方法。本申请实施例中的计算机可读存储介质可以是易失性存储器或非易失性存储器，或可包括易失性和非易失性存 储器两者。非易失性存储器可以是只读存储器(ROM)、可编程只读存储器(PROM)、可擦除可编程只读存储器(EPROM)、电可擦除可编程只读存储器(EEPROM)或闪存。易失性存储器可以是随机存取存储器(RAM)，其用作外部高速缓存。通过示例性但不是限制性说明，许多形式的RAM可用，例如静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、同步动态随机存取存储器(SDRAM)、双倍数据速率同步动态随机存取存储器(DDRSDRAM)、增强型同步动态随机存取存储器(ESDRAM)、同步连接动态随机存取存储器(SLDRAM)和直接内存总线随机存取存储器(DR RAM)。应注意，本文描述的方法的存储器旨在包括但不限于这些和任意其它适合类型的存储器。应注意，本文描述的方法的存储器旨在包括但不限于这些和任意其它适合类型的存储器。
本申请实施例还提供了一种计算机程序产品或计算机程序，该计算机程序产品或计算机程序包括计算机指令，该计算机指令存储在计算机可读存储介质中。计算机设备的处理器从计算机可读存储介质读取该计算机指令，处理器执行该计算机指令，使得该计算机设备执行根据本申请实施例的方法。
需要说明的是，附图中的流程图和框图，图示了按照本申请实施例各种实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，所述模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
一般而言，本申请实施例的各种示例实施例可以在硬件或专用电路、软件、固件、逻辑，或其任何组合中实施。某些方面可以在硬件中实施，而其他方面可以在可以由控制器、微处理器或其他计算设备执行的固件或软件中实施。当本申请实施例的各方面被图示或描述为框图、流程图或使用某些其他图形表示时，将理解此处描述的方框、装置、系统、技术或方法可以作为非限制性的示例在硬件、软件、固件、专用电路或逻辑、通用硬件或控制器或其他计算设备，或其某些组合中实施。
在上面详细描述的本申请实施例的示例实施例仅仅是说明性的，而不是限制性的。本领域技术人员应该理解，在不脱离本申请实施例的原理和精神的情况下，可对这些实施例或其特征进行各种修改和组合，这样的修改应落入本申请实施例的范围内。

Claims (16)

  1. 一种用于足式机器人的状态估计方法，由计算机设备执行，所述方法包括：
    获取所述足式机器人的第一传感器信息和第二传感器信息；
    基于所述第一传感器信息和第二传感器信息，利用第一卡尔曼滤波器确定所述足式机器人的第一状态信息，并保存一段时间的第一状态信息作为第二卡尔曼滤波的历史信息；
    获取所述足式机器人的第三传感器信息，并基于所述第三传感器信息和所述历史信息，利用所述第二卡尔曼滤波器确定所述足式机器人的第二状态信息；以及
    基于所述足式机器人的第二状态信息，对所述足式机器人在当前时刻的第一状态信息进行更新，以确定所述足式机器人的当前时刻的状态信息。
  2. 如权利要求1所述的方法，其中，所述第一传感器信息是由以第一频率工作的第一传感器采集的，所述第二传感器信息是由以第二频率工作的第二传感器采集的，所述第三传感器信息是由以第三频率工作的第三传感器采集的，所述第一频率、所述第二频率和所述第三频率互不相同。
  3. 如权利要求2所述的方法，其中，所述第二传感器是关节编码器，所述基于所述第一传感器信息和第二传感器信息，利用第一卡尔曼滤波器确定所述足式机器人的第一状态信息还包括：
    基于所述第二传感器信息和所述足式机器人的腿部运动学模型，确定所述足式机器人的运动信息；
    至少部分地基于所述运动信息，利用第一卡尔曼滤波器确定所述足式机器人的第一状态信息。
  4. 如权利要求1至3任一项所述的方法，其中，
    所述第一传感器信息和所述第二传感器信息分别带有时间戳；
    所述基于所述第一传感器信息和第二传感器信息，利用第一卡尔曼滤波器确定所述足式机器人的第一状态信息，包括：
    将所述第一传感器信息和所述第二传感器信息，依次作为第一卡尔曼滤波器的输入；以及
    通过第一卡尔曼滤波器进行状态估计，得到带有时间戳的足式机器人的第一状态信息，并将带有时间戳的第一状态信息保存为历史信息，其中，所述第一状态信息包括第一卡尔曼滤波器输出的带有时间戳的多个状态估计结果。
  5. 如权利要求4所述的方法，其中，所述利用第二卡尔曼滤波器确定所述足式机器人的第二状态信息，包括：
    确定所述第三传感器信息对应的时间戳；
    将与所述第三传感器信息的时间戳对应的历史信息和所述第三传感器信息作为第二卡尔曼滤波器的输入；以及
    通过第二卡尔曼滤波器进行状态估计，得到所述第三传感器信息对应的时间戳的状态估计结果作为所述第二状态信息。
  6. 如权利要求5所述的方法，其中，所述通过第二卡尔曼滤波器进行状态估计，得到足式机器人的第二状态信息，包括：
    基于所述第三传感器信息对应的时间戳，确定所述历史信息中与所述时间戳对应的状态估计结果；
    基于所述第三传感器信息以及所述历史信息中与所述时间戳对应的状态估计结果，利用所述第二卡尔曼滤波器进行状态估计，以获取所述时间戳对应的状态估计结果，将所述状态估计结果作为所述第二状态信息。
  7. 如权利要求6所述的方法，其中，
    所述对所述足式机器人在当前时刻的第一状态信息进行更新，以确定所述足式机器人的当前时刻的状态信息，包括：
    将所述第二状态信息相对于所述第一状态信息的状态增量，叠加到所述足式机器人在当前时刻对应的第一状态信息，以确定所述足式机器人的当前时刻的状态信息。
  8. 如权利要求4至7任一项所述的方法，其中，所述第一卡尔曼滤波器包括预测模型和观测模型，所述预测模型用于通过所述第一传感器信息获取预测状态和对应协方差，所述观测模型通过第二传感器信息获得观测状态和对应协方差。
  9. 如权利要求6所述的方法，其中，所述利用所述第二卡尔曼滤波器进行状态估计，以获取所述时间戳对应的状态估计结果，包括：
    以第三传感器信息延时的最大值作为历史数据窗口，保存所述历史数据窗口区间内由所述第一卡尔曼滤波器获得的后验状态和后验协方差；
    将所述后验状态和后验协方差对应作为所述第二卡尔曼滤波器的先验状态和先验协方差，将所述先验状态、所述先验协方差与所述第二卡尔曼滤波器的观测信息进行融合，以获取所述时间戳对应的状态估计结果。
  10. 如权利要求8所述的方法，其中，所述观测模型用于至少部分地基于所述足式机器人的腿部运动学模型来进行状态观测，所述腿部运动学模型用于指示足式机器人质心与足端之间的相对位置和速度关系。
  11. 如权利要求10所述的方法，其中，所述观测模型进行所述状态观测的方式包括：
    基于所述足式机器人的腿部运动学模型，计算所述足式机器人的本体与足端之间的相对位置测量残差、以及所述足式机器人的本体与足端之间的相对速度测量残差，
    将所述足式机器人的本体与足端之间的相对位置测量残差以及所述足式机器人的本体与足端之间的相对速度测量残差，作为状态观测数据。
  12. 如权利要求6所述的方法，其中，所述方法还包括：
    基于所述第三传感器信息，获取所述第三传感器信息对应的时间戳的所述足式机器人的本体在世界坐标系下的位置和速度；以及
    基于所述第三传感器信息对应的时间戳的所述足式机器人的本体在世界坐标系下的位置和速度，计算用于校正所述第一卡尔曼滤波的状态估计信息的累积误差。
  13. 一种用于足式机器人的状态估计装置，所述装置包括：
    第一传感器，被配置为获取所述足式机器人的第一传感器信息；
    第二传感器，被配置为获取所述足式机器人的第二传感器信息；
    第三传感器，被配置为获取所述足式机器人的第三传感器信息；
    第一卡尔曼滤波器，被配置为基于所述第一传感器信息和所述第二传感器信息，确定所述足式机器人的第一状态信息，并将保存一段时间的第一状态信息作为历史信息；
    第二卡尔曼滤波器，被配置为基于所述第三传感器信息和所述历史信息，利用第二卡尔曼滤波器确定所述足式机器人的第二状态信息；
    其中，所述第一卡尔曼滤波器还被配置为基于所述第二状态信息以及所述足式机器人在当前时刻对应的第一状态信息，确定所述足式机器人在当前时刻的状态信息。
  14. 一种计算机设备，包括输入接口和输出接口，还包括：
    处理器，适于实现一条或多条指令；以及，计算机可读存储介质；
    所述计算机可读存储介质存储有一条或多条指令，所述一条或多条指令适于由所述处理器加载并执行如权利要求1-12任一项所述的足式机器人的状态估计方法。
  15. 一种计算机可读存储介质，所述计算机可读存储介质存储有一条或多条指令，所述一条或多条指令适于由处理器加载并执行如权利要求1-12任一项所述的足式机器人的状态估计方法。
  16. 一种计算机程序产品，所述计算机程序产品包括计算机程序；所述计算机程序被处理器执行时，实现如权利要求1-12任一项所述的足式机器人的状态估计方法。
PCT/CN2023/099676 2022-08-17 2023-06-12 用于足式机器人的状态估计方法、装置、计算机设备及计算机可读存储介质 WO2024037155A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202210985184.6A CN115355905A (zh) 2022-08-17 2022-08-17 用于足式机器人的状态估计方法及装置、计算机设备
CN202210985184.6 2022-08-17

Publications (1)

Publication Number Publication Date
WO2024037155A1 true WO2024037155A1 (zh) 2024-02-22

Family

ID=84002987

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2023/099676 WO2024037155A1 (zh) 2022-08-17 2023-06-12 用于足式机器人的状态估计方法、装置、计算机设备及计算机可读存储介质

Country Status (2)

Country Link
CN (1) CN115355905A (zh)
WO (1) WO2024037155A1 (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115355905A (zh) * 2022-08-17 2022-11-18 腾讯科技（深圳）有限公司 用于足式机器人的状态估计方法及装置、计算机设备

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109781117A (zh) * 2017-11-15 2019-05-21 百度在线网络技术（北京）有限公司 一种组合定位方法和系统
CN110160524A (zh) * 2019-05-23 2019-08-23 深圳市道通智能航空技术有限公司 一种惯性导航系统的传感器数据获取方法及装置
CN111086001A (zh) * 2019-12-25 2020-05-01 广东省智能制造研究所 一种足式机器人多模态感知的状态估计方法及系统
CN111993391A (zh) * 2020-08-25 2020-11-27 深圳市优必选科技股份有限公司 机器人位姿估计方法、装置、仿人机器人及存储介质
KR20210026795A (ko) * 2019-09-02 2021-03-10 경북대학교 산학협력단 Ｉｍｕ 센서와 카메라를 이용한 하이브리드 실내 측위 시스템
CN114046800A (zh) * 2021-11-09 2022-02-15 浙江大学 一种基于双层滤波框架的高精度里程估计方法
CN114216456A (zh) * 2021-11-27 2022-03-22 北京工业大学 一种基于imu与机器人本体参数融合的姿态测量方法
CN115355905A (zh) * 2022-08-17 2022-11-18 腾讯科技（深圳）有限公司 用于足式机器人的状态估计方法及装置、计算机设备

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109781117A (zh) * 2017-11-15 2019-05-21 百度在线网络技术（北京）有限公司 一种组合定位方法和系统
CN110160524A (zh) * 2019-05-23 2019-08-23 深圳市道通智能航空技术有限公司 一种惯性导航系统的传感器数据获取方法及装置
KR20210026795A (ko) * 2019-09-02 2021-03-10 경북대학교 산학협력단 Ｉｍｕ 센서와 카메라를 이용한 하이브리드 실내 측위 시스템
CN111086001A (zh) * 2019-12-25 2020-05-01 广东省智能制造研究所 一种足式机器人多模态感知的状态估计方法及系统
CN111993391A (zh) * 2020-08-25 2020-11-27 深圳市优必选科技股份有限公司 机器人位姿估计方法、装置、仿人机器人及存储介质
CN114046800A (zh) * 2021-11-09 2022-02-15 浙江大学 一种基于双层滤波框架的高精度里程估计方法
CN114216456A (zh) * 2021-11-27 2022-03-22 北京工业大学 一种基于imu与机器人本体参数融合的姿态测量方法
CN115355905A (zh) * 2022-08-17 2022-11-18 腾讯科技（深圳）有限公司 用于足式机器人的状态估计方法及装置、计算机设备

Also Published As

Publication number Publication date
CN115355905A (zh) 2022-11-18

Similar Documents

Publication Publication Date Title
Xinjilefu et al. Decoupled state estimation for humanoids using full-body dynamics
Hartley et al. Contact-aided invariant extended Kalman filtering for legged robot state estimation
Indelman et al. Factor graph based incremental smoothing in inertial navigation systems
CN110986939B (zh) 一种基于imu预积分的视觉惯性里程计方法
WO2020253260A1 (zh) 时间同步处理方法、电子设备及存储介质
Hartley et al. Hybrid contact preintegration for visual-inertial-contact state estimation using factor graphs
WO2024037155A1 (zh) 用于足式机器人的状态估计方法、装置、计算机设备及计算机可读存储介质
WO2023082050A1 (zh) 一种基于双层滤波框架的高精度里程估计方法
CN109724597B (zh) 一种基于函数迭代积分的惯性导航解算方法及系统
CN110887493B (zh) 基于局部地图匹配的轨迹推算方法、介质、终端和装置
CN110068326B (zh) 姿态计算方法、装置、电子设备以及存储介质
CN113867333B (zh) 基于视觉感知的四足机器人楼梯攀爬规划方法及其应用
CN112596534A (zh) 基于深度强化学习的四足机器人的步态训练方法、装置、电子设备及介质
Admiraal et al. Improved formulation of the IMU and MARG orientation gradient descent algorithm for motion tracking in human-machine interfaces
Schmid et al. Local reference filter for life-long vision aided inertial navigation
US20220203526A1 (en) Humanoid robot control method, humanoid robot using the same, and computer readable storage medium
Rollinson et al. State estimation for snake robots
Huai et al. Real-time large scale 3D reconstruction by fusing Kinect and IMU data
Rollinson et al. Robust state estimation with redundant proprioceptive sensors
Colombo et al. Affine visual servoing: A framework for relative positioning with a robot
CN114046800B (zh) 一种基于双层滤波框架的高精度里程估计方法
CN112284380A (zh) 一种基于光流和imu融合的非线性估计方法及系统
CN114161402B (zh) 机器人稳定控制方法、模型构建方法、装置和机器人
Liu et al. State estimation of a heavy-duty hexapod robot with passive compliant ankles based on the leg kinematics and IMU data fusion
Zhang et al. Research on UAV attitude data fusion algorithm based on quaternion gradient descent

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 23854060

Country of ref document: EP

Kind code of ref document: A1