Whole Body Kalman Filter – State estimation of a humanoid robot using nonlinear constrained optimisation


View of the real Coman robot. The robot's feet slip on the ground when performing fast torso movements.In this project, the implementation of the state estimation of a coman (COMpliant HuMANoid) robot was studied. The information obtained from state estimation can typically be used for a controller or identification. Thus, accurate state estimation is one of the necessary steps before achieving a fully autonomous system. During the course of this project, the robot itself was not available at the laboratory. However, a simulation model was implemented in Webots and used in order to test different state estimation algorithms. The model of the coman robot included the different sensors found on the real robot such as an IMU at the level of the pelvis, encoders for each joint and force / torque sensors at each ankle. State estimation implies reading the sensor measurements and fusing them together in order to yield an optimal estimate of the robot’s relation to the world. Two separate scenarios were considered for this project. The first was that the feet of the robot were static, meaning they would not slip or tilt on the ground. As such, the positions and movements of each part of the robot could be computed from this fix position using the measurement from the joints. The IMU and force/torque sensors were also used in order to yield a more accurate estimation. The goal of the second scenario was to consider slippage despite the lack of sensors capable of locating the robot’s global position. In this scenario, the usage of the IMU and force/torque sensors was considered critical in order to estimate the movement of the robot in the global frame. In particular, the idea was that the position drift caused by integration of the IMU could be limited by considering that the work produced by friction between the robot and the world can only be negative, and that the orientation of the estimated force at the position of the contact with the ground should be collinear to the estimated velocity of the robot at that point.

The project was completed by first getting an overview of the techniques used by different groups working on the implementation of state estimation in robots, in particular for humanoids. It was decided that the most reasonable approach for both scenarios would be to use nonlinear constrained optimisation methods in order to fuse sensor data and obtain an optimal estimate. This approach allowed to make direct use of the physical and geometrical relations, while attempting to minimise the error. As such, different constrained optimisation algorithms were designed for the static scenario in order to build up iteratively to the dynamic scenario. The state estimation was then implemented for both scenarios. Furthermore, the complexity of the project required to design specific tools. In particular, a GUI environment for the visualisation and control of the robot data was implemented, as well as an automatic code generation script to generate code directly from the equations set. However, the implementation of the dynamic state estimation was not fully implemented during the time of this project. On the other hand, the state estimation designed for the static scenario was implemented successfully, which is the common approach considered by different labs working on state estimation with legged robots. This report discusses how the constrained optimisation algorithms were setup and shows the results obtained. Further discussion about the interpretation of the results can also be found. 

The monitor developped in order to facilitate the implementation of the algorithms for the state estimation.




Presentation (with videos)