Robust nonlinear state estimation for humanoid robots

Abstract

Center of Mass (CoM) estimation realizes a crucial role in legged locomotion. Most walking pattern generators and real-time gait stabilizers commonly assume that the CoM position and velocity are available for feedback. In this thesis we present one of the first 3D-CoM state estimators for humanoid robot walking. The proposed estimation scheme fuses effectively joint encoder, inertial, and feet pressure measurements with an Extended Kalman Filter (EKF) to accurately estimate the 3D-CoM position, velocity, and external forces acting on the CoM. Furthermore, it directly considers the presence of uneven terrain and the body’s angular momentum rate and thus effectively couples the frontal with the lateral plane dynamics, without relying on feet Force/Torque (F/T) sensing. Nevertheless, it is common practice to transform the measurements to a world frame of reference and estimate the CoM with respect to the world frame. Consequently, the robot’s base and support foot pose are mandatory and need to be co-estimated. To this end, we extend a well-established in literature floating mass estimator to account for the support foot dynamics and fuse kinematic-inertial measurements with the Error State Kalman Filter (ESKF) to appropriately handle the overparametrization of rotations. In such a way, a cascade state estimation scheme consisting of a base and a CoM estimator is formed and coined State Estimation RObot Walking (SEROW). Additionally, we employ Visual Odometry (VO) and/or LIDAR Odometry (LO) measurements to correct the kinematic drift caused by slippage during walking. Unfortunately, such measurements suffer from outliers in a dynamic environment, since frequently it is assumed that only the robot is in motion and the world around is static. Thus, we introduce the Robust Gaussian ESKF (RGESKF) to automatically detect and reject outliers without relying on any prior knowledge on measurement distributions or finely tuned thresholds. Therefore, SEROW is robustified and is suitable for dynamic human environments. In order to reinforce further research endeavors, SEROW is released to the robotic community as an open-source ROS/C++ package.Up to date control and state estimation schemes readily assume that feet contact status is known a priori. Contact detection is an important and largely unexplored topic in contemporary humanoid robotics research. In this thesis, we elaborate on a broader question: in which gait phase is the robot currently in? To this end, we propose a holistic framework based on unsupervised learning from proprioceptive sensing that accurately and efficiently addresses this problem. More specifically, we robustly detect one of the three gait- phases, namely Left Single Support (LSS), Double Support (DS), and Right Single Support(RSS) utilizing joint encoder, IMU, and F/T measurements. Initially, dimensionality reduction with Principal Components Analysis (PCA) or autoencoders is performed to extract useful features, obtain a compact representation, and reduce the noise. Next, clustering is performed on the low-dimensional latent space with Gaussian Mixture Models (GMMs) and three dense clusters corresponding to the gait-phases are obtained. Interestingly, it is demonstrated that the gait phase dynamics are low-dimensional which is another indication pointing towards locomotion being a low dimensional skill. Accordingly, given that the proposed framework utilizes measurements from sensors that are commonly available on humanoids nowadays, we offer the Gait-phase Estimation Module (GEM), an open-source ROS/Python implementation to the robotic community. SEROW and GEM have been quantitatively and qualitatively assessed in terms of accuracy and efficiency both in simulation and under real-world conditions. Initially, a simulated robot in MATLAB and NASA’s Valkyrie humanoid robot in ROS/Gazebo were employed to establish the proposed schemes with uneven/rough terrain gaits. Subsequently, the proposed schemes were integrated on a) the small size NAO humanoid robot v4.0 and b) the adult size WALK-MAN v2.0 for experimental validation. With NAO, SEROW was implemented on the robot to provide the necessary feedback for motion planning and real-time gait stabilization to achieve omni-directional locomotion even on outdoor/uneven terrains. Additionally, SEROW was used in footstep planning and also in Visual SLAM with the same robot. Regarding WALK-MAN v2.0, SEROW was executed onboard with kinematic-inertial and F/T data to provide base and CoM feedback in real-time. Furthermore, VO has also been considered to correct the kinematic drift while walking and facilitate possible footstep planning. GEM was also employed to estimate the gait phase in WALK-MAN’s dynamic gaits.Summarizing, a robust nonlinear state estimator is proposed for humanoid robot walking. Nevertheless, this scheme can be readily extended to other type of legged robots such as quadrupeds, since they share the same fundamental principles.

Η εκτίμηση του Κέντρου Μάζας (CoM) διαδραματίζει κρίσιμο ρόλο στη ρομποτική βάδιση. Οι περισσότεροι σχεδιαστές κίνησης και ελεγκτές βάδισης πραγματικού χρόνου υποθέτουν ότι η θέση και η ταχύτητα του CoM είναι διαθέσιμες για ανατροφοδότηση ανά πάσα στιγμή. Σε αυτή τη διατριβή παρουσιάζουμε έναν από τους πρώτους τρισδιάστατους εκτιμητές κατάστασης CoM για το περπάτημα των ανθρωποειδών ρομπότ. Ο προτεινόμενος εκτιμητής συνδυάζει αποτελεσματικά τις μετρήσεις από αισθητήρες πίεσης στα πόδια, κωδικοποιητές στις αρθρώσεις και αδρανειακής μονάδας (IMU) στο σώμα με ένα Εκτεταμένο Φίλτρο Κάλμαν (EKF) για την ακριβή εκτίμηση τόσο της θέσης και της ταχύτητας του CoM αλλά και των εξωτερικών δυνάμεων που δρουν πάνω σε αυτό. Επιπλέον, λαμβάνει υπόψιν την ανωμαλότητα του εδάφους και την στροφορμή του σώματος με αποτέλεσμα να συνδυάζει το μετωπικό με το πλευρικό επίπεδο κίνησης, χωρίς να βασίζεται σε αισθητήρες δύναμης / ροπής (F/T) στα πόδια. Ωστόσο, είναι κοινή πρακτική να επιχειρείται η μετατροπή των μετρήσεων σε ένα αδρανειακό σύστημα αναφοράς ώστε η εκτίμηση του CoM να γίνεται σε σχέση με αυτό. Κατά συνέπεια, για την επίτευξη του παραπάνω είναι υποχρεωτικό να συνεκτιμηθούν η βάση και το πόδι στήριξης του ρομπότ. Για το σκοπό αυτό, επεκτείνουμε έναν καθιερωμένο στη βιβλιογραφία εκτιμητή αιωρούμενης μάζας με τη δυναμική του ποδιού στήριξης χρησιμοποιώντας μετρήσεις κινηματικής και αδρανειακής μονάδας με το Φίλτρο Κάλμαν Σφάλματος Κατάστασης (ESKF) για την κατάλληλη διαχείριση της υπερ-παραμετροποίησης των περιστροφών. Με αυτό το τρόπο, δημιουργείται ένα σύστημα σειριακής εκτίμησης κατάστασης που αποτελείται από έναν εκτιμητή βάσης και έναν εκτιμητή CoM το οποίο ονομάζουμε State Estimation RObot Walking (SEROW). Επιπλέον, για να διορθώσουμε την κινηματική απόκλιση που προκαλείται από την ολίσθηση των ποδιών κατά το περπάτημα, χρησιμοποιούμε μετρήσεις Οπτικής Οδομετρίας (VO) και/ή Οδομετρίας LIDAR (LO). ∆υστυχώς, τέτοιες μετρήσεις υποφέρουν από ακραίες τιμές σε ένα δυναμικό περιβάλλον, αφού κατά τον υπολογισμό τους χρησιμοποιείται η υπόθεση ότι μόνο το ρομπότ βρίσκεται σε κίνηση και ο κόσμος γύρω του είναι στατικός. Για αυτό το λόγο, εισάγουμε το Σθεναρό Γκαουσιανό Φίλτρο Κάλμαν Σφάλματος Κατάστασης (RGESKF) για την αυτόματη ανίχνευση και απόρριψη των ακραίων μετρήσεων. Το προτεινόμενο φίλτρο δεν βασίζεται σε πρότερη γνώση σχετικά με τις κατανομές των μετρήσεων και δεν χρησιμοποιεί ειδικά ρυθμισμένα κατώφλια. Ως εκ τούτου, το SEROW γίνεται ένα σθεναρό σύστημα εκτίμησης κατάστασης, κατάλληλο για δυναμικά ανθρώπινα περιβάλλοντα. Προκειμένου να ενισχυθούν περαιτέρω οι ερευνητικές προσπάθειες, το SEROW δίνεται ελεύθερα στη ρομποτική κοινότητα ως ένα πακέτο ROS/C++ ανοικτού κώδικα. Τα σύγχρονα συστήματα ελέγχου και εκτίμησης κατάστασης ανθρωποειδών ρομπότ υποθέτουν ότι η κατάσταση επαφής ποδιών-εδάφους είναι γνωστή εκ των προτέρων. Η ανίχνευση τέτοιων επαφών είναι ένα σημαντικό και σε μεγάλο βαθμό ανεξερεύνητο θέμα στη σύγχρονη ρομποτική έρευνα. Σε αυτή τη διατριβή, διατυπώνουμε μια ευρύτερη ερώτηση: σε ποια φάση βάδισης βρίσκεται το ρομπότ; Προς το σκοπό αυτό, προτείνουμε ένα ολιστικό πλαίσιο βασισμένο σε μη-επιβλεπόμενη μάθηση από δεδομένα ιδιοδεκτικής αίσθησης που αντιμετωπίζει με ακρίβεια και αποτελεσματικότητα αυτό το πρόβλημα. Συγκεκριμένα, ανιχνεύουμε με ακρίβεια μια από τις τρεις φάσεις βάδισης, την Αριστερή Υποστήριξη (LSS), την ∆ιπλή Υποστήριξη (DS) και τη ∆εξιά Υποστήριξη (RSS), χρησιμοποιώντας μετρήσεις από κωδικοποιητές, IMU και F/T. Αρχικά, πραγματοποιείται μείωση των διαστάσεων με Ανάλυση Κύριων Στοιχείων (PCA) ή με αυτόματους κωδικοποιητές ώστε να εξαχθούν χρήσιμα χαρακτηριστικά, μια συμπαγής αναπαράσταση και να μειωθεί ο θόρυβος στα δεδομένα. Στη συνέχεια, πραγματοποιείται μια ομαδοποίηση στον χώρο χαμηλών διαστάσεων με Γκαουσιανά Μοντέλα Μίγματος (GMMs). Ως αποτέλεσμα λαμβάνονται τρία πυκνά συμπλέγματα που αντιστοιχούν στις φάσεις της βάδισης. Αυτό σημαίνει ότι η δυναμική της φάσης του βαδίσματος είναι χαμηλής διάστασης το οποίο λειτουργεί ως άλλη μια ένδειξη στο ότι ολόκληρη η διαδικασία της βάδισης είναι χαμηλής διάστασης. Επιπλέον, δεδομένου ότι το προτεινόμενο πλαίσιο χρησιμοποιεί μετρήσεις από αισθητήρες που είναι συνήθως διαθέσιμοι στα σημερινά ανθρωποειδή ρομπότ, προσφέρουμε στη ρομποτική κοινότητα το Gait-Phase Estimation Module (GEM), μια ανοικτού κώδικα εφαρμογή σε ROS/Python. Το SEROW και το GEM έχουν αξιολογηθεί ποσοτικά και ποιοτικά αναφορικά με την ακρίβεια και την αποδοτικότητα τους τόσο σε προσομοίωση όσο και σε πραγματικές συνθήκες. Αρχικά, χρησιμοποιήθηκε ένα προσομοιωμένο ρομπότ στο MATLAB και το ανθρωποειδές ρομπότ Valkyrie της NASA στο ROS/Gazebo για να τεκμηριωθούν τα προτεινόμενα σχήματα στο βάδισμα πάνω σε ανομοιόμορφο/ανώμαλο έδαφος. Στη συνέχεια, τα προτεινόμενα σχήματα ενσωματώθηκαν στο α) μικρού μεγέθους ανθρωποειδές ρομπότ NAO v4.0 και β) στο πλήρους μεγέθους ανθρωποειδές WALK-MAN v2.0 για περεταίρω πειραματική επικύρωση. Με το NAO, το SEROW εφαρμόστηκε στο ρομπότ για να παράσχει την απαραίτητη ανατροφοδότηση στον σχεδιασμό της κίνησης και τη σταθεροποίηση του βηματισμού σε πραγματικό χρόνο. Με αυτό το τρόπο επιτεύχθηκε πολυκατευθυντική βάδιση ακόμη και σε εξωτερικά/ανομοιογενή εδάφη. Επιπλέον, το SEROW χρησιμοποιήθηκε στον σχεδιασμό βημάτων για την πλοήγηση και επίσης στο Visual SLAM με το ίδιο ρομπότ. Όσον αφορά το WALK-MAN v2.0, το SEROW εφαρμόστηκε με δεδομένα κινηματικής, αδρανειακής μονάδας και F/T για να παρέχει ανατροφοδότηση βάσης και CoM σε πραγματικό χρόνο. Στην εκτίμηση λήφθηκε υπόψη και το VO για την διόρθωση της κινηματικής απόκλισης κατά το περπάτημα. Με αυτό το τρόπο διευκολύνεται σημαντικά ο πιθανός σχεδιασμός βημάτων. Τέλος, το GEM χρησιμοποιήθηκε επίσης για την εκτίμηση της φάσης της βάδισης στο δυναμικό περπάτημα του WALK-MAN. Συνοψίζοντας, σε αυτή τη διατριβή προτείνεται ένας σθεναρός μη-γραμμικός εκτιμητής κατάστασης για το βάδισμα ανθρωποειδών ρομπότ. Παρόλα αυτά, το προτεινόμενο σύστημα μπορεί εύκολα να επεκταθεί και σε άλλους τύπους ρομπότ με πόδια, όπως τα τετράποδα, μιας και διαθέτουν τις ίδιες βασικές αρχές κίνησης.