IROS2019国际学术会议论文集 0597

Learning footstep planning on irregular surfaces with partial placements Germ an Castro and Claude Sammut Abstract We present two contributions built upon on a previous footstep planner based on the ARA search Firstly we have developed an improved foothold selection method using support polygons to increase foothold availability in rough terrain Secondly we present a footstep classifi cation method using the C5 0 algorithm that takes advantage of cost similarity between adjacent steps This is intended to learn feasibility and approximate transition costs for the ARA planner These contributions extend capabilities of the planner by increasing footstep availability and allowing to generate more complex plans without compromising safety I INTRODUCTION Research on humanoid robots has increased signifi cantly in recent years given their great versatility to operate in human environments and use tools Some usage scenarios include replacing humans in dangerous situations such as rescue collaborating with humans or supporting disabled people In cases like rescue operations a crucial task is traversing irregular terrain such as rubble from a crumbling building and adapting to different terrain with varying levels of diffi culty and limited availability of foot placements In the present work we build on the footstep planning framework proposed by Stumpf et al 1 which can use search methods from the A family such as ARA 2 to plan sequences of footsteps in 3D environments In Section II we present related work Section V describes a new approach to evaluating footholds based on support polygon stability 3 defi ning a support area and risk for each placement In Section VI we propose a classifi cation method to approximate cost transitions for the ARA step planner and defi ne reachable steps with the aim of exploiting better performance from a black box walking controller Lastly in Section VII we compare our method to the original planner developed by Stumpf et al 1 II RELATEDWORK Various authors have addressed the problem of foot place ment and footstep planning Chung and Khatib 4 describe a whole body planner which is mostly focused on body movement planning and contact forces rather than terrain profi le for foot placement Contact regions are extracted from a point cloud or given by an operator Some authors have proposed specifying footholds by parameters such as convexity and slippage An example is Belter et al 5 6 who compute three features to describe the ground surface and the immediate surroundings which are later classifi ed School ofComputerScienceandEngineering TheUniversity ofNewSouthWales Sydney Australia german castro c sammut unsw edu au as poor or good in a simulation In that work planning is separated into high level path planning where A generates an overall path using traversability cost Then a lower level RRT 7 defi nes a sequence of feasible movements in a subsection of the overall path defi ned by A Experiments are performed with two different hexapod robots A commonly used stability criterion for statically moving robots is the support polygon Kolter et al 8 use this concept to traverse rough terrain using a quadruped robot whereas in this work we apply this concept for foothold selection on a biped robot In contrast most humanoid robot dynamic walking uses ZMP controllers 9 which produce faster gaits that are more similar to humans but with higher control complexity In a recent work Wiedebach et al 10 present a method to estimate and balance on partial foot placements without previous information This is an important ability to deal with unexpected terrain or modelling noise even though the best performance can be achieved with an a priori footstep plan combined with this approach Norouzi et al 11 compare RRT and A including the concept of a stability measure to discard states where the robot has a high probability of tipping over To calculate the cost with A they include a parameter that selects between shorter versus riskier paths However this parameter is not automatically tuned to produce an optimum path balancing risk and cost Additionally the use of a tracked vehicle allows a series of simplifi cations that cannot be made for humanoid robots Buchli et al 12 optimise footstep planning for Little Dog an 18DOF quadruped robot traversing rough terrain They use ARA where the cost to select footholds is learned by imitation therefore it requires a human expert to demonstrate which foot placements are preferred over other alternatives Garimort et al 13 present a search based 2D footstep planning method for humanoids based on D Lite 14 and implemented with ROS Extensions made by Hornung et al 15 16 17 use variants of A and introduce height maps generated online from a stereo camera even though its 3D planning capability is limited to fl at surfaces Stumpf et al 18 extend a previous implementation of Hornung et al to footstep plans in full 3D Their frame work 1 has been implemented with ROS and made avail able as open source The work presented here builds upon this framework therefore more details will be discussed in section III Kanoulas et al 19 present an extension to Stumpf et 2019 IEEE RSJ International Conference on Intelligent Robots and Systems IROS Macau China November 4 8 2019 978 1 7281 4003 2 19 31 00 2019 IEEE1421 al 1 using contact analysis between curved patches to determine foot placement on rough terrain as well as in unstructured fl at terrain The method only evaluates invalid states that are not feasible with the original planner The foot placement extension presented here replaces the original method therefore it can co exist with Kanoulas et al foot placement in complex terrain Further research comparing the effi ciency of both methods is needed however that will not be addressed in this paper III FOOTSTEPPLANNERFRAMEWORK This paper builds upon the work of Stumpf et al 18 and their open source framework 1 therefore we briefl y introduce some of its main characteristics A model of the 3D environment is obtained from a LIDAR scan which is aggregated to a point cloud and fi ltered to reduce noise Height is extracted from the smoothed point cloud to create a discrete 2 5D grid map additionally some estimations are made to fi ll holes from the LIDAR data Using the point cloud normals to the surface are pre calculated to maintain only one vector per cell of the discrete height map These normals will later determine roll and pitch for the foot using Principle Component Analysis in that location The state representation of a step in 3D space is defi ned by s x y z 1 s0 State of supporting foot s Initial state of swing foot s0 Final state of swing foot where z are defi ned by the discrete height map and the normal in the position x y with foot rotation yaw Discrete values for x0 y0 are sampled using a reachability polygon which describes a 2D limit region relative to the supporting foot s0 while 0is limited to a set of discrete values The quality of a foot placement is estimated by sampling its contact distance with the ground Two thresholds are defi ned a maximum intersection distance of the ground with the foot and a maximum gap distance If either threshold is exceeded the foothold is considered invalid This combined with the reachability polygon defi ne the valid states s0for a given s0 Lastly the total cost for a transition c s s0 in the ARA heuristic is calculated as a hierarchical composition of a set of criteria such as euclidean distance constant cost or dynamic constraints IV PLATFORM The experiments described in this work were performed in a simulated environment with Gazebo and ROS using a model of the ROBOTIS THORMANG3 robot The ROS interface with the robot and simulator are based on Stumpf et al 1 while the current walking controller is the default dynamic gait for THORMANG3 provided by ROBOTIS For simplicity a point cloud created from a 3D CAD model of the arena is used to represent the environment as a substitute for a point cloud produced by a LIDAR Experiments ran in a deca core desktop CPU Intel Core i9 7900X CPU with 64GB RAM V FOOTHOLD SELECTION When a bipedal robot moves over fl at surfaces it has many locations to place its feet however in rough terrain the number of options decreases with the diffi culty of the terrain Consequently the ability to choose good footholds can increase the chances of success of a plan by giving a better range of opportunities Stumpf et al 18 use the information of the discrete height map to estimate normal vectors to the area of the foothold using Principle Component Analysis These vectors are used to approximate the roll and pitch for each placement Finally the distance of each point is compared with the foot placement to fi nd out how well supported it is This technique is fast and simple for relatively fl at areas where most of the foot is supported but is has problems with noise and more complex surfaces where overhanging placements are unavoidable In this section we propose a new method to select stable foot holds for humanoid walking Instead of attempting to describe properties of the terrain such as normal to the surface or convexity we simulate the collision of a rigid foot on the ground to approximate the contact points and therefore a support polygon for the placement The process uses the same discrete height map described above to perform the analysis The procedure for analysing possible foot placements is divided in three stages Firstly a support polygon for the foothold is calculated a support ratio is determined using the polygon area see section V A If the support ratio is below a minimum threshold the placement is rejected Then risk of the placement is calculated based on the surrounding area see V B If the risk is over a specifi ed threshold the placement is rejected Finally if the foothold is valid we determine an offset value which is required for the walking engine to correct the Centre of Pressure CoP of the placement see section V C A fl owchart that describes this process can be seen in fi gure 1 Fig 1 This fl owchart represents the process to select a valid foothold for details on node getSupport see fi gure 2 1422 A Ground Support Approximation Here we describe how we approximate the support poly gon for each placement and then obtain the support ratio which is defi ned by support s0 polygon area foot area 0 1 Below we describe the four components of our method to approximate the support polygon Figure 2 shows a fl owchart of how these components interact getSupport Accept init simulation collision sim edge collision support polygon collides has converged max iterations Reject No Yes No No YesYes Fig 2 Flowchart for Ground Support Approximation 1 Initialise simulation This is an iterative method there fore some memory cache operations are used to avoid calculation of values multiple times This is explained further in Section V B 2 Collision simulation This is a simplifi ed physics sim ulation where a plane representing the foot of the robot is allowed to rest on top of the surface represented by the discrete height map The simulation ends when a maximum number of iterations is reached or when the resulting torque applied to the foot is small enough to consider that the foot has stopped moving on a stable position Algorithm 1 searches for values z roll pitch that min imise the torque x y on the foot to fi nd an equilibrium point When angular acceleration is small it is considered a stable pose for the foot The parameter INTRUSION is used to ensure that there are multiple contact points between the 2 solid bodies otherwise there would not be enough points to calculate the support polygon We assume a uniform mass distribution for the foot therefore we consider the mass of each sample mass x y 1 for convenience 3 Edge Collision After the foot is resting on a stable position on the ground we check that the immediate border surrounding the foot is not too close to an edge that could interfere with foot movement If the check fails the foothold is deemed to be an invalid placement 4 Support Polygon We assume that it is possible to obtain a good approximation of the support polygon using four points Therefore we divide the foot into four equal quadrants each of which may contain only one point of the support polygon After these assumptions it is easy to fi nd the point of the foot that touches the ground with the largest Manhattan distance from the centre of the foot This heuristic Algorithm 1 Collision simulation 1 procedureCOLLIDE 2 maxDiff 3 x footLenght 2 footLenght 2 4 y footWidth 2 footWidth 2 5 T x y z T Foot global coordinates 6 kRk Foot global rotation 7 gh x y ground height on global coortinates 8 fh x y foot height relative to foot centre 9 INTRUSION Constant value 10 GF Ground force constant 2 0 11 for all pair x y do 12 diff gh T x y fh kRk x y 13 if maxDiff 0 then 17 forcez g g 1 diff GF 18 else 19 forcez g 20 x x forcez 21 y y forcez 22 foot mass P mass x y 23 x x foot mass 24 y y foot mass 25 T z maxDiff 26 R roll x timeStep2 27 R pitch y timeStep2 return x y will return conservative values for a foot that is supported least 50 For values under this threshold a different method should be considered B Risk approximation Our concept of risk is based how accurate the step should be allowing some margin for mistakes if the robot steps within the vicinity of s0 Therefore we approximate risk by moving the original placement s0to eight surrounding loca tions and testing if these adjacent points are valid placements Four points are tested in an inner area a small distance from s0and four outer points have a bigger distance to the original point see fi gure 5 All eight placements are analysed following the procedure described in section V A then Risk s0 is approximated as I1 I4 O1 O4 1invalid 0valid Risk s0 0 7 4 X n 1 In 4 0 3 4 X n 1 On 4 2 Equation 2 returns a value between 0 no risk and 1 maximum risk The requirement for this calculation is based on how precise the placement must be or if there is 1423 Foot centre Contact point Ground reference Support polygon i ii iii iv Fig 3 Some examples of support polygon are illustrated i Fully supported foot ii partially supported foot at least 1 point of contact per quadrant iii partially supported foot 1 quadrant is not supported iv partial support with all quadrant supported Fig 4 These are examples of support for some placements the red marks represent the points for the support polygon Results have been approximated from simulation of colliding the foot with a point cloud any margin of error Additionally it helps to fi lter possible singularities produced by errors in the support polygon approximation Coeffi cients used to calculate risk were de termined to allow failure on the outer points while avoiding failure of inner points for example failure of all outer points has a 30 risk on the other hand failure of one inner point and two outer points has 32 5 risk Then using 25 or lower in the planner will produce a reasonably robust placement Besides contact support of each tested location an addi tional check is performed on the pitch and roll angles If these are too different from s0 we assume that the ground surface may be discontinuous and the tested location is considered invalid Inner and Outer area sizes are defi ned by the discrete height map resolution Inner area is defi ned as 1 cell distance and Outer area as 2 cells distance In our experiments we use 1cm cell discretisation C Offset The offset is simply the difference between the centroid of the support polygon and the centre of the foot x y This value is given to the walking engine so it can correct the Centre of Pressure to deal with partial placements VI FOOTSTEP CLASSIFICATION Stumpf et al 18 developed new hierarchical heuristics to calculate the state transition cost between footsteps treating the risk of the steps as an independent variable from the cost I1I2 I3 I4 O1 O4 O3 O2 S Fig 5 Risk is tested by moving the placement s0a small distance within an Inner area then further within an Outer area A weighted sum of each test produces Risk s0 to determine feasibility of the steps One of the cost functions uses Gaussian Process Regression to do the estimation based on previously collected data However it is diffi cult to maintain an updated value when mechanical upgrades are done to the robot Therefore the learned transition cost is not included in all experiments Here rather than approximating an exact cost function the aim of this classifi cation is to group geometrically similar steps exploiting the similarity of cost between steps that are around the same vicinity This classifi cation also defi nes a diffi culty level which can be used to compare different gait generators parameters their strengths and limitations regarding stepping range In this section we describe a step trial as two steps starting with both legs together executing the given step and fi nishing with a closing step i e both feet together For this experiment the target position has been limited to a 3D step with variable forward side height and yaw coordinates Pitch and roll remain to be zero for all trials Data for 160k step trials with randomly generated target coordinates forward side height yaw have been run in simulation and collected for analysis Approximately 130k step trials were used for training and 30k for testing A Fitness function We defi ne a fi tness function to rank the step diffi culty for classifi cation acceleration Instant acceleration poseError Distance between fi nal position and target position offLimits Number of times that a joint reaches limit fallen 1000000robot fell 0valid step A criterion for evaluating step diffi culty is how much the robot struggles to keep balanced when it is completing the step If the step is easy the robot will be well balanced on the other hand if the step is diffi cult it will fi nish the step wobbling P acceleration is measured