A Model-Based Control Approach for Locomotion Control of Legged Robots

Transcription

1 Biorobotics Laboratory A Model-Based Control Approach for Locomotion Control of Legged Robots Semester project Master Program: Robotics and Autonomous Systems Micro-Technique Department Student: Salman Faraji Supervisors: Soha Pouya, Rico Möckel Professor Auke Jan Ijspeert Lausanne, Switzerland

2 Abstract In this project, a new method is proposed for controlling legged robots. This method takes dynamics of the robot into account to have better nominal tracking of desired trajectories and more compliant environmental interactions at the same time. We have incorporated also natural dynamics of the robot into the system by using off-line calculated gaits extracted from optimizations on energy. So the main control loop in this method consists of a feed-forward block that generates actuator torques given desired trajectories and also a feedback block designed and tuned specifically for the structure of the robot. A trajectory planner uses known optimal trajectories together with some control laws that modify these trajectories to have better robustness in different situations. We successfully tested this method on two robots. The first one was a monopod robot with two actuators and five degrees of freedom. On this robot, we integrated a high level controller that made the robot capable of performing locomotion on sloped terrains up to ±20 and rough terrains up to variance of 0.03 in terrain surface. On second robot which was a 2D quadruped robot using bounding gait, we ended up with stabilization of feed-forward, feedback and trajectory planner. This robot showed more robustness to the roughness of the terrain. The proposed control method could be a good candidate for control problems concerning compliance environmental interactions and energy optimality. i

3 Contents List of figures iii List of tables 1 1 Introduction 2 2 Literature review Virtual model control(vmc) Virtual Model control on ALoF Compliant control of multi-contact and center of mass behaviours in humanoid robotics Virtual model control for bipedal locomotion Operational space control (OSC) Inverse dynamics control Impedance control Conclusion Monopod Platform Pure feed-forward control Adding feedback to pure feed-forward scenario Design of trajectory planner Design of feedback for trajectory planner Performance of feed-forward + feedback and the trajectory planner Flat terrain Rough terrain High-level SOC controller Control approach Control approach Transition to quadruped Quadruped Platform Pure feed-forward control Adding feedback to pure feed-forward scenario Design of trajectory planner Performance of feed-forward + feedback and the trajectory planner Conclusion 37 ii

4 Bibliography 38 iii

5 List of Figures 1.1 Two ends of control methods: Left: Passive walker (taken from [1]) that naturally goes down a slope without using any actuator, Right: Assimo by Honda uses a lot of computations for its movement and still doesn t have a natural movement like humans Virtual model control on ALoF (taken from [2]), Left: 2D model shown in stance phase with specific configuration of a virtual component for this phase, Right: the actual robot and all virtual forces created by virtual components between contact points Humanoid robot with multiple-point contacts (taken from [3]) (taken from [4]) Spring Turkey robot: Left: Granny walker mechanism used in stance control, Right: Dogtrack Bunny mechanism used for locomotion (taken from [4]) Spring Turkey robot: State machine for using different control methods Obstacle avoidance using potential fields (taken from [5]) Expected controller architecture Left: Notations of continuous state variables for monopod robot shown in blue, Right: Simulation environment (taken from [6]) Monopod robot: Extracted optimal open-loop solution for one cycle Monopod robot: Hip torque (left) and Prismatic force (right) for extracted optimal trajectory Pure feed-forward strategy Response of pure feed-forward strategy. Note the deviation beginning from second period due to instability of extracted open-loop solution and error accumulations Feedback and feed-forward blocks working together Arc-shape trajectory of foot to avoid obstacles in front. This law helps the robot working more robustly on rough terrains as being tested later Effect of hip angle controller on contact forces right before lift off. We amplify trajectories to help the robot lifting up easily Integration of a high level trajectory generator with feed-forward and feedback control of monopod robot. In stance phase we use both feedback and feed-forward. But in flight phase feedback tries to impose trajectory planning laws iv

6 3.10 Result of trajectory planner + feedback + feed forward on monopod for long time simulation on flat terrain. The robot goes in x direction with a natural speed and also stabilized Result of trajectory planner for one period in steady state. As we use only feedback in flight phase and there is no rule to control x and y, desired values for them are set to zero Steady state actuator variables verify feasibility of this scenario regarding actuator constraints defined (to be between -5 and 5). For prismatic actuator in stance phase, feed-forward method has the major contribution in calculating required force. In flight phase, the contribution of this block is zero Second Scenario: Effect of the rough terrain on the distance passed by robot. The performance of trajectory planner and control blocks decreases with more roughness in the terrain. Test of the robot for 500 seconds and 5 times on terrains having different roughness measures. The terrain has a Gaussian distribution with zero-mean and specified variance sampled each Configuration of high level controller that creates a stimuli used for better trajectory modification Effect of T α on ẋ, s potential for using high level controller Left: U terrain of test-1, Right: sloped terrain of test test-1: SOC controller response on a U shaped terrain. plots are showing robot s variables over time in more than 50 cycles. y trajectory which corresponds to the location of the base is similar to the terrain in shape. x trajectory is increasing with a considerably constant speed which shows the performance of high level controller. Also the variations of base attitude φ are very low test-2: SOC controller response on slope and with variable desired speeds. Despite being on a sloped terrain, the high level controller is able to follow desired speeds. The slope of y (which is ẏ) on right diagram corresponds to these desired speeds Different phase transitions during bounding gait of quadruped robot Notations of seven continuous state variables for quadruped robot Performance of pure feed-forward controller. Error accumulation will cause deviation from nominal trajectories and eventually, the robot falls over Stability of feed-forward torque generator after integration of feedback. Actuator variables are also in the desired range (between -5 to 5) which verify the feasibility of this method Arc shape trajectory of legs after lift off which helps the robot to do locomotion on rough terrains more robustly. This specific shape is obtained by tuning time constants in trajectory transition law Performance of control laws on rough terrain. Test of the robot for 50 seconds 5 times on terrains having different roughness measures. The terrain has a Gaussian distribution with zero-mean and specified variance sampled each v

7 List of Tables 2.1 Different approaches used for controlling of a humanoid robot Comparison of different approaches according to our short literature review Feedback gain for different phases in monopod Comparison of different steps taken to develop the proposed control method Contact Jacobian in different phases Feedback gain for different phases

8 Chapter 1 Introduction Although wheeled robots are widely used for different applications, legged robots have also their own advantages in locomotion. They can potentially perform much better on rough terrains and complex environments. These robots are being mostly inspired from animals, but with simpler structures. However, the complexity of structure, considerable number of actuators and degrees of freedom has made control problem a great challenge for the engineers. There are various intelligent approaches developed that control the robot for different tasks, but not so general to handle any kind of terrains or obstacles potentially existing in a complex environment. Each control method has its own advantages and disadvantages regarding task properties. Among them, Virtual Model Control uses virtual components to compensate existing redundancy in the robot. Operational Space Control tries to navigate the body through the environment using attractive and repulsive potential fields. Impedance control tries to solve the control problem by representing the manipulator, environment and control laws as impedances or admittances and then modulating these elements to obtain desired performance. Inverse dynamics optimally calculates actuator inputs considering the current dynamics of the robot and contact constraints. At BioRob, a set of dynamics modelling is prepared for some legged robots including planar monopod, biped and quadruped (with rigid and flexible spine) and additionally a 3D quadruped model with flexible spine. Also for some of these models thanks to an optimization, a periodic optimal sequence (gait) is obtained for actuator inputs. The extracted gaits are obtained so far to optimize locomotion performance metrics such as speed, energy efficiency and stability rather than producing the desired movement patterns such as running at a certain speed or jumping height. So previous works done in BioRob are: Mathematical dynamics modelling for the legged robots (2D monopod and 2D and 3D Quadrupeds). 2

9 Open loop optimal trajectories for the periodic and energy-optimized solutions (gaits). State feedback control loops (LQR) to stabilize the gaits. Making use of these previous developments, a new complementary approach is needed to improve the controllability of the robot. Moreover, to replicate the off-line extracted trajectories on the real robot, one need to design feedback controllers to sustain the desired trajectories against the model uncertainties. So the goal of this project is to design a control method to help the robot follow the optimal trajectories robustly. In addition, we want to take advantage of this knowledge in our proposed method to make the robot more flexible for some different scenarios. The main advantage of incorporating a model into the control loop is to achieve better nominal tracking of desired trajectories for a specific task knowing the dynamics of the robot. Environmental interactions could be improved also using this knowledge. Figure 1.1: Two ends of control methods: Left: Passive walker (taken from [1]) that naturally goes down a slope without using any actuator, Right: Assimo by Honda uses a lot of computations for its movement and still doesn t have a natural movement like humans Overall, the two ends of control approaches could be shown on Fig.1.1. On left, the robot can walk down a sloop in a completely passive manner while on right side, the well known Assimo humanoid robot by Honda uses a lot of computations and consumes considerable amounts of energy to walk, but still doesn t look as natural as humans themselves. However, the desired standing point is to use as less energy as possible while being capable of handling different tasks by incorporating more knowledge about robot s static and dynamic properties into controllers. In this project, first of all we are going to have a review on above-mentioned approaches and describe their roles and places in the whole control loop in Chapter 2 together with comparing their advantages and disadvantages. Regarding our goal, the best method will be chosen for this project. Then to get better familiar with simulator platform [6] and mathematical models, the algorithm is going to be simulated for a monopod robot in Chapter 3. The idea is to investigate the methodologies both for implementation and analysis first on this simpler setup to provide good insights and knowledge for more complicated models. Since the goal of this project is to control a quadruped robot eventually, the algorithm will be extended to such a robot in Chapter 4 as much as possible. Finally, the report is being wrapped up by a qualitative conclusion. 3

10 Chapter 2 Literature review In this section, we will go through a review of several approaches for controlling the locomotion of a legged robot, contact force treatment, self-collision avoidance and path planning through environment. What we arelooking for is an approach that stably controls the robot, tries to track desired trajectories we feed in nominally and also leads to more compliant environmental reactions. Several approaches are addressed in literature for this task in context of mobile robots and industrial manipulators. Essentially the control problem of a fixed base manipulator is much simpler than a mobile robot. Usually manipulators have as much degrees of freedom as required regarding repetitive task they are assigned to do. Nearly everything is defined in such environment. But in case of mobile robots, there are many uncertainties that the robot has to deal with. Moreover, usually these robots are either under-actuated or over-actuated. For example in this project, we will implement our algorithm on a 2D monopod robot that only has 2 actuators. The most important characterization of mobile robots is maybe their hybrid state space which makes control problem complicated. Most famous control approaches introduced till now are: Virtual Model Control Operational Space Control Impedance control Inverse dynamics To review each, we have studied some papers to understand the main properties and key ideas introduced in literature. In some cases, the approach is initially applied to industrial manipulators and then extended to mobile robots. In the rest of this section, we will briefly introduce each method with most important features that may help in our project and finally, a comprehensive comparison is done to select the best that fits our problem. 2.1 Virtual model control(vmc) This approach uses virtual components to compensate existing redundancy in the robot. In a typical legged or humanoid robots, there are several actuators used for different legs. 4

11 This method puts virtual elements between contact points. These elements which are spring-damper pairs, generate forces F that are translated to actuator torques T using contact Jacobian J C by following equation to produce a desired motion based on contact behaviours required. T = J C T F (2.1) By modulating these virtual elements then, a robot can move and work in different hybrid states. More specifically, new variables appeared in equations are set-points of springs. Here, we will briefly explain three instances of works profiting from this method: Virtual Model control on ALoF In this master thesis [2], a control model is proposed and applied on a real robot based on virtual components. At first, details are explained for a 2D biped robot and then, it is extended to a quadruped robot which is AloF robot, developed at ETHZ 1. Figure 2.1: Virtual model control on ALoF (taken from [2]), Left: 2D model shown in stance phase with specific configuration of a virtual component for this phase, Right: the actual robot and all virtual forces created by virtual components between contact points Features of this work could be summarized as follows: There is no sensor for contact forces and they use contact dependent virtual component. A higher level controller dictates positions of CoG and lower level controllers follow these commands. Three main tasks are described in this work: Stance Control, Motion Control and Standing Up. In Fig.2.1 on left, we can see a virtual component defined between base and mid-point of contacts which is used in stance phase, when both legs are on the ground. On right image, necessary contact forces to be generated are shown between feet. 1 Swiss Federal Institute of Technology Zurich 5

12 2.1.2 Compliant control of multi-contact and center of mass behaviours in humanoid robotics In this work [3] done by Khatib, there are multiple approaches used for different tasks. In humanoid robotics, the control problem is usually more complex because the robot is standing on two legs and is less stable compared to a quadruped robot for example. So, usually contact points are larger in humanoid robots and they can not be modelled as point contacts. In this work, the concept of Center of Pressure is introduced which helps to model multiple-point contacts better hence the humanoid robot has large feet and hands as shown in Fig.2.2. Main tasks and associated policies are listed bellow. Table 2.1: Different approaches used for controlling of a humanoid robot Task Contact Support Joint limits Self collision Balance of CoM Hand movement Policy Optimal contact force locking attractors repulsion field position control force and position control The concept of virtual components is also used here for locomotion. They use a method which calculates optimal internal joint torques given a desired multi-point contact force. However, this method does not use the dynamical model of the system. So the question of having optimal contact forces is remained unanswered. Figure 2.2: Humanoid robot with multiple-point contacts (taken from [3]) Virtual model control for bipedal locomotion In this work [4], The concept of virtual components is used in two ways: stance control and motion. Stance control is done with the aim of granny walker mechanism which is shown on Fig.2.3 Two virtual components help to control attitude and height of the robot using redundancies. For motion control, a virtual component is used between robot and a constantly 6

13 Figure 2.3: (taken from [4]) Spring Turkey robot: Left: Granny walker mechanism used in stance control, Right: Dogtrack Bunny mechanism used for locomotion moving object. This so called Dogtrack Bunny mechanism is shown on Fig.2.3. They combine these two approaches using a state machine which is shown in Fig.2.4. The benefit of using this method is the intrinsic flexibility in stance control and prioritization of tasks in low level controllers. However, exact position control of CoG is complicated and this method will guaranty position and attitude of the base to vary in a small range. Figure 2.4: (taken from [4]) Spring Turkey robot: State machine for using different control methods Overall, having reviewed some instances that use VMC, we came to the conclusion that this method has many advantages in particular simplicity and less computations needed, since it does not use mass matrix. However, it is blind to the dynamics of the system and may not track desired trajectories as we want. Moreover, contact forces are not guaranteed to be optimal and thus the robot s interaction with environment is not possibly as compliant as what we are looking for. 2.2 Operational space control (OSC) For the first time, the concept of potential fields appeared in this work [5] by khatib on It was used on industrial robots to avoid collision with environment and also a bit later, used for self-collision avoidance. Here, the environment is modelled with a set of primitive shapes and for each of them, the algorithm uses special form of repulsive forces. In this way, the destination point exerts an attractive force on manipulator, while obstacles or other robots exert a repulsive one. Later on, this approach was extended to environments with multiple manipulators and also containing mobile robots. While 7

14 the main purpose of this approach is path planning, it could be used also for treating actuator saturations. Simplicity of motion control in robotic arms is due to the fact that we already know contact forces required to calculate actuator torques, since we know the type of task the robot is doing. An example of obstacle avoidance is shown in Fig.2.5. Figure 2.5: Obstacle avoidance using potential fields (taken from [5]) In conclusion, this method uses Jacobian to translate contact forces to actuator torques from Eqn.2.1 which means no inclusion of dynamics of the robot. Sometimes people use terms to compensate for gravity to make the robot more compliant, but it is not the best way and needs tuning. In this case, knowing the dynamics of the robot can help. 2.3 Inverse dynamics control Full dynamics of a robot can be expressed in an equation called Equation Of Motion (EOM): M(q) q + h(q, q) = τ + J T C(q)λ (2.2) Where q is system variables, M is the mass matrix, h is gravitational and Corriolis forces, τ is actuator torques, J C is the Jacobian of contacts and λ is contact forces. This method tries to calculate τ from this equation by inversion of the dynamics of the robot. Other variables may be measured directly or estimated from previous time step. A major problem for controlling the robot is the fact that contact forces depend on actuator torques and vice versa. Since physical sensors are not so accurate, suffer from considerable noise and also have a delay, control algorithms tend not to rely on them. A common approach is to calculate contact forces by knowing actuator forces in previous step and use this estimation at current step to obtain new actuator torques. This method leads to unstable control loop in adverse situations, but is acceptable in fast enough control loops, since joint angle sensors are reliable for calculating other terms in EOM. This paper [7] has proposed a new method. By Q-R factorization 2 of Jacobian matrix at each time step and projecting the main equation of motion by Q, we will have two different equations where there is no contact force in one of them. Using this equation, we can easily control the robot and also satisfy contact constraints. Other features of this approach are: 2 A QR factorization is a decomposition of a matrix A into a product A = QR of an orthogonal matrix Q and an upper triangular matrix R. 8

15 Proper torques generating a specific desired trajectory are obtained using a pseudoinverse method. This is done by the equation which does not contain contact forces. The result of pseudo-inverse method is joint torques that lead to optimal contact forces in terms of energy. Redistribution of control torques can be done by using different weightings for joints in pseudo-inverse algorithm. This will make the system not optimal any more, but we have the option to tune torques for different actuators regarding their sizes. Although Q may have discontinuities, as it is used in control equation twice, first to project into unconstrained space, and second to transfer back into the original configuration space, it does not affect calculated torques. With this method, we can also calculate joint torques in a way that lead to minimal contact forces (refer to appendix of [7] for more information), meaning compliance control together with exact position control at the same time. The authors have simulated their proposed method on a humanoid robot. They have also implemented it on the well known little dog robot and tested in some real scenarios. 2.4 Impedance control This approach [8] tries to solve the control problem by representing the manipulator, environment and control laws as impedances or admittances. Dynamical expressions for each of them behave as one of these circuit elements. For example, seen from manipulators, the environment is a kind of admittance. Because the manipulator applies force (voltage) and sees some movement (current) as result. Controller is also seen as a physical object and is represented by a circuit element. In this approach, analysis of the system is done using bond graphs. Impedance control is in fact another form of virtual model control where virtual components are viewed as circuit elements. In this method also, the dynamics of the system is not used for control. It just uses Jacobian and kinematic model of the robot. In fact at the time this approach was introduced, computers were not fast enough to compute mass matrix or inversions needed when using dynamic model of the robot. Other features of this approach could be listed as: Control is done by modulation of control impedances using feedbacks of Cartesian distances. It acts like a PD controller. This impedance modulation could be done also without a feedback. The reason is that fast dynamics of multiple collisions have problem if we have feedback delays. Multiple collisions happen when a leg for example touches the ground with a relatively high speed. This is often referred to as bouncing effect. Using kinematic redundancies instead of determining everything with generalized pseudo-inverse methods (no need to inversion of mass matrix). Obstacle avoidance could be done also using superposition of impedances. Finally, this approach was very successful at its time. But since only the kinematics of the robot is taken into account and is tuned to compensate for dynamics, possibly it does not generate optimal joint torque distribution and may not be flexible in complex terrains. 9

16 2.5 Conclusion Control of a robot may require different tasks as we saw in above-mentioned methods. Main important tasks could be listed as: Balance Control Locomotion control Compliant Interaction with environment Self collision avoidance In addition to these general tasks, problem constraints are also important: actuator saturation and joint limits. These are constraints on inputs or states of the system and should be considered when designing a controller. Different model-based approaches are described in literature regarding handling of all these tasks. But they mostly rely on kinematic model of the robot. Table.2.2 shows the final qualitative comparison coming from our short literature review. Table 2.2: Comparison of different approaches according to our short literature review feature VMC OSC Inverse dynamics Impedance control Using kinematics yes yes yes yes Using Dynamics no no yes no Nominal tracking Compliance Computational resources The main point of this project is to implement a control algorithm that ensures compliant environmental interaction and nominal tracking at the same time in different situations. Among existing control approaches, the inverse dynamics method is chosen for this project regarding above-mentioned requirements. It can handle both nominal tracking and compliant contact forces using proper pseudo-inverse methods. Knowing dynamics of the robot, we can calculate actuator variables such that the they induce energy optimal contact forces. In general, structure of our controller will be similar to Fig.2.6. Figure 2.6: Expected controller architecture We desire to use dynamics model of the robot which is used inside the controller as a feed-forward torque generator. A feedback is also required in case of uncertainties to 10

17 help the feed-forward block. In addition, we have pre-calculated optimal trajectories that contain in fact natural dynamic behaviour of the robot. This data shall be used in our controller too. Therefore, we design a trajectory planner block whose responsibility is to generate trajectories based on a given scenario and then control blocks use the dynamics model of the system for better tracking of these trajectories and also for having more compliant environmental interactions. The underlying assumption is availability of a quite precise map of the environment either being preloaded or obtained on-line using various sensors. In this work also we provide the robot with such map for different scenarios we want to test the robot. In the next section, this control approach will be explained more precisely and will be tested on a monopod robot as well. 11

18 Chapter 3 Monopod Platform Among various legged robot structures, a monopod one is interesting to begin testing locomotion algorithms due to its simplicity. The challenge in control of these robots is the fact that they don not have static stability. The dimensionality of the system is small and also states to be controlled, either continuous or discrete are not so complicated. Therefore, we begin with applying this algorithm to a monopod platform so that to find out key elements and strategies required for controlling the robot. Another goal is also to get familiar with available platform of simulating these robots and principles of modelling such structures. The 2D Monopod structure under investigation consists of a prismatic joint (acting as a knee to change the leg length) and a rotary hip joint both with series elastic components (spring and damper). The trunk-leg dynamics is modelled with three inertial components (mass + inertia); one presenting the trunk dynamics and two presenting the leg dynamics (upper one as the thigh and lower one as the shank). In total, there are 3 degrees of freedom for translation and attitude of the base and 2 degrees for joints fully describing the state of the robot. All these variables are depicted on Fig.3.1. q = [x, y, φ, α, l] T (3.1) Figure 3.1: Left: Notations of continuous state variables for monopod robot shown in blue, Right: Simulation environment (taken from [6]) A big challenge in locomotion control of legged robots is hybrid dynamics regarding combination of contacts. For each combination, different constraints are imposed on the 12

19 main equation of motion which makes control problem complex. For this robot, hybrid states consist of flight phase and stance phase described by a discrete state variable. In the flight phase, the robot has no contact with the ground and thus not constrained, but there is no actuation on center of mass location and orientation. Whereas in the stance phase, there are two constraints imposed by contact point and two actuators, totally making the robot under-actuated regarding available degrees of freedom. Fig.3.1 shows the robot in general coordinates and simulation environment implemented in MATLAB [6]. Within the simulator, this dynamical system is described by five above-mentioned variables and their derivatives. All the variables are normalized so that to make simulations independent of SI dimensions. Also, we have added two saturations of ±10 for actuator forces or torques and also extension and contraction of at most 25% for prismatic joint. In the current simulation platform, the leg can exceed this 25% which should be fix in future, but in control level we currently apply no force in this case. The research previously done on this platform [9] has extracted an off-line openloop periodic solution for controlling this robot. This solution includes both the gait initialization parameters and periodic actuation profiles which result in a periodic solution of this non-linear hybrid system (so called gait). The off-line solutions are optimized either for forward velocity or cost of transportation allowing open-loop unstable behaviour. For instance, the selected solution for monopod robot through this chapter is an open-loop unstable periodic solution of the system with high velocity. Instability is due to the ẋ state whose Eigenvalue is bigger than one (Eigenvalues are calculated based on Floquet theory and using Poincare map calculation [6]). The solution is in fact periodic inputs for actuators that control the robot for one period and brings it back to initial state. Fig.3.2 shows these trajectories and required actuator torques are depicted in Fig.3.3. Note the range of variables in these figures and amounts of torques or forces generated by actuators. Figure 3.2: Monopod robot: Extracted optimal open-loop solution for one cycle Like many other control design approaches we approve unstable open-loop solutions to keep the selected criterion optimal and then we use feedback design to stabilize the closed loop system. To have a closed-loop controller, we would calculate the required actuator variables by feed-forward method and then use feedback to compensate uncertainties. In many control problems, feed-forward controllers are used to reduce the burden of feedback. Fig.3.4 shows the configuration of control blocks and variables for pure feed-forward controlling approach before integrating any feedback. 13

20 Figure 3.3: Monopod robot: Hip torque (left) and Prismatic force (right) for extracted optimal trajectory Figure 3.4: Pure feed-forward strategy In a perfect environment which is here a flat terrain, the output of the system in response to the forces generated by feed-forward method should exactly match the desired initial open-loop trajectories. We will develop our algorithm step by step to solve appearing problems. The final goal is to be capable of following a desired average speed first on the flat terrain and if possible, also on slopped and rough terrains. Each of these scenarios require more complicated design components that will be introduced in the next parts. To begin, how could we design the feed-forward controller to generate actuator forces or torques properly? In next part, we will discuss the core method used for this project. 3.1 Pure feed-forward control As we know, contact forces highly depend on actuator force or torques and also these quantities depend on contact forces when the robot is trying to jump, while being in contact with the ground. To calculate these contact forces: Some approaches use sensors placed in contact points to have at least a noisy measurement of induced forces. Although this method may yield in good results in some cases, but it highly depends on perfectness of sensor instruments and may loose performance or robustness on complex terrains or because of sensor delays. Other approaches also try to estimate these forces from previous time step. This approximate solution works if the control loop is fast enough, but can become unstable in adverse situations. The main problem is coupling of actuator forces, contact forces and also dynamics of the robot. A recent work [7] tries to decouple these forces by projecting the equations on null space of actuators. By decoupling the main equation of motion, it tries to obtain two equations where contact forces do not appear in one. This equation is therefore used for calculating required forces to be applied to actuators knowing desired accelerations. The 14

21 only sensor required is thus contact detection. The main equation of motion is: With variables defined as: M(q) q + h(q, q) = S T τ + J T C(q)λ (3.2) M(q) R n+3 n+3 : the floating base inertia matrix h(q, q) R n+3 : the floating base centripetal, Coriolis and gravity forces S = [0 n 3 I n n ]: the actuated joint selection matrix τ R n : the vector of actuated joint torques J C R k n+3 : the Jacobian of k linearly independent constraints λ R k : the vector of k linearly independent constraint forces if there exists any q = [x, y, φ, α, l] : x and y are base position, φ is base attitude, α is hip angle and l is leg s length. In case of our 2D monoped robot, we have k = 2 regarding two translation constraints and n = 2 as we have two actuators. Principally, the algorithm calculates QR decomposition of J T C first. [ ] R J T C = Q (3.3) 0 Where Q is orthogonal (QQ T = Q T Q = I), and R is an upper triangle matrix of rank k. Then it projects Eq.3.2 onto null-space of actuators by multiplying with Q T. The result is decomposition of the rigid-body dynamics into two independent equations: S c Q T (M q + h) = S c Q T S T τ + Rλ (3.4) S u Q T (M q + h) = S u Q T S T τ Where S c and S u are used to select the top and lower portions of the full equation. The main point is that contact forces are not appearing in the second equation. This allows to calculate actuator torques without knowing contact forces. S c = [I k k 0 k n+3 k ] (3.5) S u = [0 n+3 k k I n+3 k n+3 k ] Using the known periodic solution, we feed torques and forces resulted from Eq.3.6 into the forward model where initial joint variables and their derivatives exactly match with initial desired pre-optimized solution. Here the pseudo-inverse method shown by + sign calculates joint torques that induce optimal contact forces in terms of energy (refer to [7]). τ = (S u Q T S T ) + S u Q T [M q d + h] (3.6) 15

22 Figure 3.5: Response of pure feed-forward strategy. Note the deviation beginning from second period due to instability of extracted open-loop solution and error accumulations. The result of this pure feed-forward approach is shown in Fig.3.5 for one period. In this case, robot s response has considerably precise tracking of the desired value showing that the feed-forward controller is implemented and working correctly. However, this tracking is good only for one period. Note the small mismatch existing at the end of first period (around t = 2) for α, i.e. hip angle. So if we extend the simulation time, discretization errors accumulate and since calculations are highly dependent on the discrete state of robot, after 2-3 period, it falls down. In addition to error accumulation, the open-loop solution is intrinsically unstable (due to eigen-value of bigger than 1 for ẋ in Poincare map, ref to[9]) and we do not expect stability of this response. Another physical insight is that in periodic solutions found, the robot s foot in flight phase moves slightly above the ground (because of energy optimality) and is subject to touch the ground if there is a small error in calculations. To test the overall functionality of the feed-forward method, we first try to design a feedback for this scenario to see if we can sustain optimal trajectories or not. Since the current method is open-loop and requires a mechanism to compensate deviations in case of uncertainties. 3.2 Adding feedback to pure feed-forward scenario In general, when the system is not under-actuated, a feed-forward controller may work together with a feedback. There are different scenarios to configure these blocks (refer to [10]). In this case, despite being under-actuated (refer to different combinations of number of constraints k and DoFs n in [7]), we decided to design a feedback to sustain known trajectories, hoping that hybrid states may help the robot stabilize. The desired block diagram for this scenario looks like bellow: The controller tested here is a PD controller with a control law defined as: F fb = K p (q des q) + K d ( q des q) (3.7) The performance of this scenario highly depends on the design of K p and K d. This design is done considering sensitive variables and also deviations observed in case of pure feed-forward scenario. For the very first step which is flat terrain, unfortunately we could not find matrices that stabilize the system. It seems that optimal solutions are stable 16

23 Figure 3.6: Feedback and feed-forward blocks working together only in a small region. In the best case, the robot can continue for some periods, but the lift off condition (zero contact force) which is very sensitive to small errors, may not be satisfied at a moment and the robot falls down. This is in fact the challenging part of monopod robot due to its stability problem. We will see in the next chapter that this feedback design leads to more stable results on quadruped robot. Overall, we need a high level trajectory planner to modify optimal trajectories and prevent the robot from falling down. This planner should also preserve continuity from phase to phase, since PD controller may generate infinite forces in case of discontinuities. 3.3 Design of trajectory planner We want to stabilize the system by making proper trajectories on-line based on precomputed optimum results available at hand. The main problem is that we can not choose arbitrary trajectories. In flight phase, the robot s center of mass is following the ballistic trajectory emerged from the lift off condition, meaning that actuators can not modify this behaviour. In stance phase also, the robot is under-actuated. Taking inspiration from the previous works, specifically Raibert s simple laws, following strategies are designed: Flight phase: Trying to decrease leg s length fast and extend it gradually again to have an arc-shaped foot trajectory. This will prevent the foot from hitting potentially available obstacles in front of it right after lift off (which happens in next steps if we change to rough terrain). Also try to adjust the attack angle of the leg in proportion to horizontal speed to improve stability by proper exchange of energy between flight and stance phase. This law was in fact used first by Raibert [11] in 1984, resulting an impressive stability on a real robot. Note that in this phase, we only use feedback terms in control loop described later. Stance phase: try to follow pre-optimized trajectories from the periodic solution found with emphasis on base s height(y) while forcing base attitude (φ) to 0. Here, feedback is used together with feed-forward to stabilize it. This law was observed to have notable contribution in stability of the robot. Essentially in case of mobile robots, large variations are not desired for φ due to constraints imposed by sensing or computational units installed on the base of the robot. To avoid discontinuities occurred in phase transitions, we try to damp each variables at the beginning of a phase while trying to smoothly achieve a new desired value at the 17

24 end. This simply means a transition in the form of weighted average between these values. The constraint is the continuity of the variable and its derivative in the beginning of new phase and also having desired values of new trajectory and its derivative at the end. So, a weighting of the form: q new (t t 0 ) = a(t t 0 ) q(t t 0 ) + b(t t 0 ) q desired (t t 0 ) (3.8) is used which requires these conditions on a and b: a(0) = 1, a(t ) = 0, b(0) = 0, b(t ) = 1 (3.9) ȧ(0) = 0, ȧ(t ) = 0, ḃ(0) = 0, ḃ(t ) = 0 Where T is the expected period of the phase. In fact, instead of using on-line variables appearing as q(t t 0 ) in Eqn.3.8, we freeze variables right at t 0. For t > t 0, q(t t 0 ) is calculated as r(t t 0 ) + s where s and r are frozen values and derivatives saved at phase change moment (t 0 ) respectively. This approach will make transition more sensitive to frozen values, but the dynamics of a variable may not make generated desired trajectory unstable. Note that in this case the actual on-line value of the variable is used in desired trajectory generation. Among various weighting choices, we selected: a(t) = e (t/τ 1) 2 (3.10) b(t) = 1 e (t/τ 2) 2 Because of its damping nature which is tunable. But the problem is to reach the desired values at T which is roughly done if we choose small time constants. When going to flight mode, we need to preserve these conditions since our feedback has a D term which goes to infinity if we have discontinuities. The same is required when going to contact mode. Because the feed-forward model just uses accelerations and the model of robot (integrated in simulator) will deviate from desired trajectory if we have mismatching initial conditions. The desired arc-shaped trajectory for foot of the robot is induced using different time constants τ 1 and τ 2. If τ 1 > τ 2, the resulting arc will look like Fig.3.7. Figure 3.7: Arc-shape trajectory of foot to avoid obstacles in front. This law helps the robot working more robustly on rough terrains as being tested later. There are also few parameters tuned by try and error to make the scenario stable: Attack angle of foot right before touch down is proportional to ẋ by following equation. This is in fact inspired form Raibert s law. α desired = ẋ/σ φ (3.11) 18

25 Known y trajectories are scaled by µ% so that to help the robot lift off properly. Since the dynamics is modelled with non-dimensional variables, these parameters are SI-unit independent and can be used for a class of robots with the same overall configuration. The desired attitude of the base φ is set to zero. This law will cause smaller variations on base pitch angle. Also before lift off, it is required to settle the φ to the desired value, because in flight we have no control over this variable and it may disturb the next cycle by having undesired values. Note that α is in fact measured with respect to base of the robot. Since we force φ to 0, the feedback exerts torques which make contact forces non-zero. This effect is shown on Fig.3.8. Having larger y trajectories helps the robot properly lift off and compensates for the feedback which generates torque at hip joint. Generally, the feedback should bring φ to zero and stabilize before lift off. The choices of parameters are σ = 3.75 and µ = 25% in this case. Different numbers were tested and these values are the mean among all values resulting in a stable controller. Figure 3.8: Effect of hip angle controller on contact forces right before lift off. We amplify trajectories to help the robot lifting up easily. In trajectory planner, we need also to shift available periodic solutions to current position of base which is determined right at the time when the foot touches the ground. This is necessary for scenarios containing rough terrains or slopes and we have assumed that a precise map of environment is already available. Finally, the phase times T 1 and T 2 are also kept fixed, equal to those from optimal trajectories. These laws all together may help the robot better stabilize in different scenarios we expect it to work. The first scenario is in fact the flat terrain. If these laws were stabilizing the robot, we will move to more difficult terrains. However we should design a different controller from what we had before to give more importance to these newly defined parameters for tuning and modifying trajectories. Next part is dedicated to this design. 3.4 Design of feedback for trajectory planner So far, we have designed trajectory modification laws that try to stabilize the robot. To impose these trajectories, we have to redesign another proper PD controller for the new architecture that containes trajectory planner. The choice of PD controller makes us dealing with stability of the robot in a simpler way compared to PID controller, since it has integrating terms. However, we make sure that steady state errors are negligible 19

26 by choosing sufficiently large gains. The controller expression is again like Eqn.3.7 and Fig.3.9 shows the block diagram of this scenario inspired from [10]. Figure 3.9: Integration of a high level trajectory generator with feed-forward and feedback control of monopod robot. In stance phase we use both feedback and feed-forward. But in flight phase feedback tries to impose trajectory planning laws. For the design of feedback: At phase 2 which is flight mode, the feedback matrix contains only terms corresponding to φ, α and l. Since α is measured with respect to φ, we add equal terms in feedback matrix and also we use the same desired values for both of them. So effectively the feedback tries to bring the φ + α to the desired value, exactly what we require. Also in the stance phase, it contains terms corresponding to y and φ to help the robot follow what we have modified in trajectory planner. Choices of parameters for K p are shown in Table.3.1 together with the diagrams that visualize with blue arrows, each actuator is being responsible to control which of the five variables. Table 3.1: Feedback gain for different phases in monopod Phase Correspondence Controller Phase Correspondence Controller 1 [ ] [ ] The K d matrix is set to be K p /10. This form of feedback design helps to enforce few laws introduced in previous part. Since the robot is under-actuated, the feed-forward control gives us optimal torques in terms of energy. The pseudo-inverse method inside feed-forward control in fact determines the distribution of torques optimally which we can not do arbitrarily. So we may not exactly know from all trajectories fed into feed-forward 20

27 block, which of them are followed better. The feedback imposes what we want ro be followed (as determined in trajectory planner). In the next part, we will examine all these blocks together with the choice of parameters mentioned previously for the different scenarios. Still, there is no guaranty that the robot reaches the desired average speed. Now the priority is given to a robot stabilized and hopping robustly. Following desired trajectories is a second goal. 3.5 Performance of feed-forward + feedback and the trajectory planner To show the performance of the control diagram of Fig.3.9, we test the robot on a flat surface as the first scenario. If stability problem was solved, we will go one step further to rough terrain Flat terrain Fig.3.10 shows the simulation result of trajectory planner together with PD feedback designed with the feed-forward model functioning as the main torque generator. Overall, the robot is stabilized after some time and goes with a constant speed which seems to be not dependent on initial conditions of the robot (as tested for some initial conditions). Note that there is no high level controller to force the robot following a desired average speed. In fact, we should make sure that the robot is stable enough before adding any extra requirement. In this stage, the robot is just hopping robustly without falling down and we observe that desired x trajectories shown in red have higher speeds, but not followed exactly. Figure 3.10: Result of trajectory planner + feedback + feed forward on monopod for long time simulation on flat terrain. The robot goes in x direction with a natural speed and also stabilized. More details of steady state trajectories could be observed in Fig In Fig.3.11, an important thing to note is that in flight phase, there is no desired trajectory used for x and y, so we set them to zero. 21

28 Figure 3.11: Result of trajectory planner for one period in steady state. As we use only feedback in flight phase and there is no rule to control x and y, desired values for them are set to zero. Figure 3.12: Steady state actuator variables verify feasibility of this scenario regarding actuator constraints defined (to be between -5 and 5). For prismatic actuator in stance phase, feed-forward method has the major contribution in calculating required force. In flight phase, the contribution of this block is zero. The steady state actuator variables are also shown in Fig.3.12 not only to verify feasibility of this approach, but also to show that in stance mode which is phase-1, most of the final prismatic actuator force is coming from feed-forward block. For the hip joint also, in the beginning of stance phase, the feed-forward control block is doing most of the job in bringing φ to 0. This was in fact the goal of integrating the feed-forward controller, i.e. to reduce contribution of feedback. Also, Fig.3.12 shows no impulsive input applied to actuators. In our control approach, especially with derivative terms in PD controller, it is probable to have discontinuities in desired trajectories. But with the help of exponential transitions we have incorporated in the trajectory planner, such infeasible inputs do not occur any more, thus making the robot move more smoothly. The Role of exponentials are shown in Fig.3.10, on bottom right, together with soft transitions Rough terrain As it was expected, this trajectory planner individually can not handle a slightly sloped terrain. Moreover, its sensitivity to roughness is relatively high, shown on Fig In this scenario, the robot is beginning from origin and moving for 500 seconds on a rough terrain. The height of the ground has a zero-mean Gaussian distribution with specified 22

29 variance, sampled every 0.1 (without dimension on x). Interestingly, the distance from origin after this time decreases drastically with increase in roughness of the ground, i.e. Gaussian variance. Figure 3.13: Second Scenario: Effect of the rough terrain on the distance passed by robot. The performance of trajectory planner and control blocks decreases with more roughness in the terrain. Test of the robot for 500 seconds and 5 times on terrains having different roughness measures. The terrain has a Gaussian distribution with zero-mean and specified variance sampled each 0.1. By now, we have succeeded to stabilize the robot so that we can observe the behaviour of our feed-forward component. What planned initially was to use feed-forward controller to reduce the burden of feedback which is quite achieved now. Since we are imitating preoptimized trajectories, this control method is not so far from optimality. More importantly, as reviewed in chapter 2, the inverse dynamics decomposition method we use has an intrinsic optimality for contact forces. These advantages are all coming from using inverse dynamics approach. Obviously it has enabled us to use optimal trajectories found before and also generates energy optimal torques resulting in less interaction with environment in case of this under-actuated monopod robot. To go one step further, how could we solve the problem of handling sloped grounds and more important, changing speed? 3.6 High-level SOC controller What we require is to modify trajectories being fed into the feed-forward controller so that we can tune the speed of our robot. For this purpose, a mechanism should exist to generate proper modifications with respect to the desired average speed of the robot. In general, the high level controller gets the average speed of robot during one period and generates a stimulus signal for necessary trajectory modifications in the next period. We call this stimulus u hereafter. The high level controller used here is Fuzzy SOC controller [12] which stands for Self Organized Controller. This controller principally consists of two look up tables. One of them is fixed and modifies the other one, depending on performance of control input generated in previous time step. The advantage of this controller is 23

30 that it can adapt itself to the on-line changes in the model of plant. Fig.3.14 shows the combination of the high level controller with the trajectory generator. Figure 3.14: Configuration of high level controller that creates a stimuli used for better trajectory modification Having this high-level mechanism to generate proper stimulus, the question is know how and where should we apply this stimulus signal to robustly increase the speed? General requirement for such robots is that they should maintain the attitude of the base as stable as possible. This will help instruments installed on base to work properly. Moreover, there is a parameter called attack angle which determines the amount of energy being transferred from one period to another. By definition, this parameter is the angle of leg with respect to ground normal just before touch down. Being close to 0 or positive means considerable energy transfer (refer to Fig.3.7). Taking these effects into account, we designed two approaches to integrate the high level controller with the current architecture Control approach 1 One way to change speed is to modify the attack angle so that we can tune the transferred energy from one period to another. This modification should take place during flight phase, i.e. phase-2. Another important effect to be taken into account is the impact of the hip joint on ẋ when trying to bring φ to zero. This effect is shown on Fig While the robot is in stance phase, small rotations of the base are easily translated to an increase or decrease in ẋ. Because the hip joint exerts some torque between the trunk and leg which causes a contact force at foot. Then this force will apply a torque to the overall system and the consequence is mostly observable in ẋ. The larger rotation desired for α, the more change observed in ẋ. Therefore, this is another option to change average speed. So, we first proposed a method which tried to force the attitude of the robot φ to some variable that can also tune the energy transfer, using this effect. Here, the high-level stimulus is applied to desired φ trajectory. An important problem is that when speed of the robot decreases, it can not jump and thus falls down. We tried also 24

31 Figure 3.15: Effect of T α on ẋ, s potential for using high level controller. to amplify y trajectories in phase 1 so that it can jump easier. This amplification of y trajectory and modification of base attitude φ are of the form: ŷ des (t) = (y des (t) y bias ) (1 + u /λ) + y bias (3.12) ˆφ des (t) = u Where u is the output of high level controller being explained before, y bias is nonezero for sloped or rough terrains, depending on the location of the robot and map of environment. Also λ = 5, chosen by try and error. These 2 control laws together with a high level controller were integrated to the system and resulted in a controller, being able to handle slopes even up to ±18. However, there was a big problem despite being robustly working. Initially, a great requirement in such robots were to keep the change of base attitude as less as possible. But this approach were directly changing this variable and in case of considerable slopes, φ were reaching up to π/2 radiants. This problem promoted us to modify attack angle as a second approach Control approach 2 We were able to improve the performance by some modifications in the previous approach. In the second one, a formulation similar to Eqn.3.11 is used, but this time including the effect of high-level controller stimulus. ˆα des (t) = (ẋ u/λ)/σ φ (3.13) With σ = 3.75 and λ = 5 respectively as before. This new control law was initially found to be very sensitive to the choice of proportion ratio. It was not working for slightly varying slope. Refer to Table.3.1 again concentrating on K p designed for stance phase. In the last line which determines control law used for prismatic actuator ([0, 20, 0, 0, 0]), the term on y (second element) was found to make the system falling down after a while when attack angle control laws were used together with trajectory amplification. We changed this matrix so that the new responsibility of prismatic actuator would be to control leg length ([0, 0, 0, 0, 20], regarding Eqn.3.1) and also cancelled out trajectory amplifications. Interestingly, this made the system hopping more robustly. The new approach formulated above not only handles different slopes, but also performs well on slightly rough terrains which was not the case for previous controller. This simple control law is much similar to Raibert s one [11]. The advantage is that optimal trajectories are not considerably modified or tuned any more. We only set φ to 25

32 zero in stance phase. This controller could be improved to perform better especially on rough terrains. Further modifications are needed for arc-shaped trajectory followed by the foot so that it does not hit obstacles in front, right after lift off. We have skipped over these small modifications for the purpose of this research, due to strictness of project time schedule. As for the first test of high level controller, we simulate the robot on a U shaped terrain displayed in Fig.3.16 on left. Figure 3.16: Left: U terrain of test-1, Right: sloped terrain of test-2 On Fig.3.17, the performance of all these blocks is shown over time for this terrain. Note small variations in the base attitude φ and small extensions of prismatic joint which should not exceed 25% nominally on both sides. Also the desired speed is 0.5, observed on x-time trajectory. Feasibility of the actuator forces and torques are also verified for this scenario, i.e. they lye in a range of ±3 which does not exceed extremity of ±10. In test-2 which is done on a sloped terrain shown in Fig.3.16 on right, Fig.3.18 shows the performance of this controller over time which is supposed to follow a variable speed while moving also on a slope of 10%. Here, alternations of speed around the desired point could be diminished by better tuning of high-level controller which was skipped due to lack of time. Note the slope of y over time (ẏ) which changes by different choices of desired speeds. Figure 3.17: test-1: SOC controller response on a U shaped terrain. plots are showing robot s variables over time in more than 50 cycles. y trajectory which corresponds to the location of the base is similar to the terrain in shape. x trajectory is increasing with a considerably constant speed which shows the performance of high level controller. Also the variations of base attitude φ are very low. Another test performed was on a rough terrain. The high level controller can have nominal performance, i.e. following desired average speeds on all roughness levels shown 26

33 Figure 3.18: test-2: SOC controller response on slope and with variable desired speeds. Despite being on a sloped terrain, the high level controller is able to follow desired speeds. The slope of y (which is ẏ) on right diagram corresponds to these desired speeds. on Fig The feasibility of actuator inputs are also verified in this case. Overall, we could successfully achieve all the goals we were looking for. We could handle flat terrain, rough terrain and also sloped terrains. The next step required in project description is to move on quadruped robot, trying to apply the same concepts on a more complicated robot in terms of hybrid states. 3.7 Transition to quadruped So far, we attempted to control a monoped robot based on a pre-known optimal trajectories found by a wide search. This was done in 3 main steps: We first implemented a pure feed-forward block to compute required torques and forces for the pre-defined optimal trajectories using an orthogonal decomposition method that was providing us with a control law independent of contact forces. This pure feed-forward model was able to successfully replicate one period of the available unstable open-loop solution. However, the system eas still unstable and could fall over in case of presence of any noise. To stabilize the robot, many control rules were explored and finally the research came up with tuning of attack angle in proportion to speed similar to Raibert s law. However compared to Raibert, we have a feed-forward term which reduces the burden of feedback. Also we helped the robot to jump better by amplifying the altitude of the base in desired trajectories. This made the robot less optimal due to deviating from the optimal trajectories, but helped not to fall down on rough terrains. Finally, a high level controller were integrated into the hierarchy of control blocks followed by a trajectory generator to help the robot reach desired speeds and go up and down on considerably sloped terrains. For better understanding and comparing different steps, the Table.3.2 shows evolution of this method step by step. If we take a brief look on literature, there are many approaches for controlling monoped robots beginning maybe from Mark Raibert s simple control laws [11] in 1984 that made 27

34 Table 3.2: Comparison of different steps taken to develop the proposed control method Feature pure feedforward +feedback +trajectory planner replication of one period soft transitions φ des = y amplification Raibert s law arc shaped foot trajectory hopping continuously handling rough terrain handling on sloped terrain following a desired speed high level controller a 3D robot stable with not so powerful technology of that time. Quadruped robots are more interesting, because they intrinsically have a kind of static stability. Contrarily, in monoped robots we have to make a lot of effort to stabilize the base. In the rest of this project as defined in project description, we will switch to quadruped robots, but concentrating on 2D bounding gait where we have effectively two legs to control. Is it possible to extend these control laws and trajectory generators for a more complicated robot? 28

35 Chapter 4 Quadruped Platform The method we developed in previous section could successfully stabilize a monopod robot even on considerably rough terrains. After integration of a high level controller and using laws similar to Raibert s ([11]), the robot was also able to follow a desired speed over both sloped and rough terrains. Now, it becomes more challenging to apply all these concepts on a more complex robot. The number of continuous states and also discrete states are considerably more, specially making feedback design more complicated. As defined in project description, we will work on a bounding gait to make the problem simpler and go up step by step. The quadruped robot used here consists of a base and two legs with rotary joints in hips and prismatic joints used as knees. The structure of a leg is similar to what we had in monoped. For this kind of gait, in Fig.4.1, the sequence of discrete phases could be observed together with simulation environment used for this part [6]. Figure 4.1: Different phase transitions during bounding gait of quadruped robot This gait will introduce four discrete phases to the system depending on the feet contact with the ground. We have also seven continuous states for this robot shown on Fig.4.2. q = [x, y, φ, α F, l F, α B, l B ] (4.1) In fact, for this specific gait, the robot is simplified to have effectively two pairs of 29

36 similar legs. This 2D robot will be tested at BioRob using a boom to prevent the robot from falling over. Figure 4.2: Notations of seven continuous state variables for quadruped robot We will base our controller upon optimal bounding trajectories extracted from optimizations. In fact the sequence of ground contacts shown on Fig.4.1 is corresponding to this optimal bounding gait. The time trajectories could be observable in the next part as desired trajectories. Like monoped case, we first begin with implementing feed-forward block to see the performance and accuracy level of this method. 4.1 Pure feed-forward control Different phases are defined regarding the status of foot contacts with ground which create 4 cases. For each case, we will have different contact Jacobians to be decomposed and used in Eq.3.6. For example, in case of flying, the contact Jacobians will be zero while in case of two-legs contact, it should be the concatenation of two individual Jacobians for each leg. Table.4.1 shows these combinations. Table 4.1: Contact Jacobian in different phases Jacobian phase-1 phase-2 phase-3 phase-4 J C = J B [J F, J B ] J F 0 Using these Jacobians, the feed-forward method decouples contact forces from actuator forces like before. Trajectories used here and initial conditions are all exactly coming from optimal solutions. Finally in this scenario, the control law of Fig.3.4 is resulting Fig.4.3. In the Fig.4.3 showing the simulation of robot for 3 nominal periods, we see that error accumulation stops the robot from following desired trajectories properly after some times due to unstable nature of extracted optimal solution. The feed-forward module tends to preserve x, y and φ. However, specially in lift-off phase transitions, L F and L B are getting far from what desired. This shows the sensitivity of robot to such phase transitions. Remember that in monopod case before integration of high-level controller, we were solving this problem by amplifying y trajectories so that to help robot jumping strongly. Later, in the trajectory planning section, we will come back to this issue again. To solve the problem of deviation from desired trajectories, the first step is to find at least a mechanism to follow these periodic solutions robustly. 30

37 Figure 4.3: Performance of pure feed-forward controller. Error accumulation will cause deviation from nominal trajectories and eventually, the robot falls over. 4.2 Adding feedback to pure feed-forward scenario For different phases, we have a fixed number of actuators, but varying number of constraints. The goal is to find a proper feedback in each case to help the robot following desired trajectories. For this problem, we use a PD controller on continuous state variables formulated as bellow. The feed-forward torques will be added to feed-back torques just in case of ground contact like monopod controller shown on Fig.3.6. In flight phase therefore, only the feedback is enabled. F fb = K p (q des q) + K d ( q des q) (4.2) Where q is referring to Eqn.4.1 and K p and K d are 4 7 matrices, generating forces and torques of four actuators. Table.4.2 shows design of feedback gains (K p ) for different phases. Blue arrows show effect of actuators (green) on continuous state variables (red). The derivative term is also K d = 0.1K p. The feedback strategy developed in Tabel.4.2 could be described as follows: On flying phase (4), the rotational and translational speeds of mass center can not be changed. The main control actions to take is to prepare the robot for a proper touch down. We do this with the smooth transitions of leg lengths and hip angles to have a good initial condition for next cycle. In phase-1 when back leg has reached the ground and the overall system is under actuated, the same strategy will by applied for both legs. But this time, we also 31

38 consider y in the control term corresponding to length of back leg. This will prevent back leg to lift off momentarily. SO the prismatic actuator in the back leg is performing on both y and l state variables After that in the phase-2, the front leg will touch the ground and make the system over-actuated. It depends on the distance between footnotes on the ground, but the robot is supposed to follow any desired trajectories for x, y and φ in this case. We also design a feedback matrix to emphasize length of legs. The aim is helping back leg to lift off easily in the next phase. Also, front actuators will try to preserve φ which will increase stability considerably. This is due to the fact that φ at lift off (start of ballistic trajectory) will determine the sequence of phases in the next touch down which is very important. Finally in phase-3 when the front leg is going to lift off, the feedback matrix is designed so that both actuators in front leg will try to preserve desired φ. This feedback is then integrated to feed-forward method to form a closed-loop controller like Fig.3.6. The result of this scenario is shown in Fig.4.4 where the robot is bounding for several periods. On the left part, hip joints and prismatic leg actuators are shown to successfully follow the desired trajectories. On the right images, the required torques and forces are shown to verify feasibility of this approach regarding similar constraints specified for monopod. Figure 4.4: Stability of feed-forward torque generator after integration of feedback. Actuator variables are also in the desired range (between -5 to 5) which verify the feasibility of this method. So far, the feedback has stabilized the robot so that it can follow almost perfectly the desired trajectories on flat terrain. But for a very simple rough terrain or in case of using 32

39 Table 4.2: Feedback gain for different phases Phase Correspondence Controller explanation Front leg is flying, each actuator controls its own variable. The same is applied for back leg, but it also tries to control y to avoid momentary lift off or bouncing No controller used for back hip joint to avoid problem shown on Fig.3.8. L B is controlled due to its repulsive role. Front hip controls φ, as it is important before jump. Back leg is flying, each actuator controls its own variable. Both front actuators control φ, as it is important to be correct before a full jump for next touch down. Both legs are flying, each actuator controls its own variable. In this phase there is no control over center of mass location and orientation. 33

40 lower number of iterations for ODE of simulator, it does not have such performance and the robot falls over. So the robot is stable just in a very small region. In case of monopod, we designed control laws in trajectory planner to make the robot more stable on such terrains. This will be done for quadruped robot as well in next section. 4.3 Design of trajectory planner Trajectories used in this case are those found after an optimization over different bounding gaits. Each phase has a nominal duration obtained from recorded optimal trajectories. When we use feed-forward control, it is not guaranteed that this timing will happen again because of uncertainties that disturb discrete events like touch down or lift off. To solve this problem, when a practical phase duration exceeds a nominal one, optimal trajectories are obtained using interpolation of previous trajectories by spline method which helps to continue smoothly. It means that the robot is still in a phase, but the time of being in that phase is expired and this method plans new trajectories by smoothly continuing expired trajectories. Soft transitions of trajectories are done in the same way as before, but this time using on-line values of variables instead of frozen ones (ref to Eqn.3.8). In this case, sensitivity of lift-off to frozen values is relatively high compared to monopod robot and makes the feedback design difficult. The problem with closed-loop controller in the previous section was the fact that it could perform well only in perfect conditions. Refer to the section of high level trajectory planner for monopod. There, we had inspired from Raibert s [11] work, simple control laws to adjust attack angle with respect to horizontal speed of the robot before touch down. Here, for both legs we use this rule again. It means that for each leg when flying, since α is measured with respect to body, we have: ˆα des = ẋ/σ φ (4.3) The tuning parameter σ is determined to be 3 here after investigating all possible values and averaging. Since we want to avoid obstacles existing in front of a leg right after lift off, in soft transition law we tune time constants τ 1 and τ 2 (refer to Eqn.3.10) so that to make the foot following an arc shape trajectory when flying. Specifically for both legs we have: ˆldes (t) = l(t) e t2 /τ l0 (1 e t2 /τ 2 2 ) (4.4) Where l 0 = 1, is the nominal length of the leg. This transition law will cause an arc shape after lift off. For some moments (here, 100ms) we use different time constants, τ 1 and τ 2 to avoid large obstacles (shown by circle in Fig.4.5). After this fast short period, we choose equal time constants to get to the desired constant leg length before next touch-down. Overall, the final trajectory will be of the form shown in Fig.4.5. This strategy helps the robot to be more robust on rough terrains. For this planner, we use the same feedback designed in the previous part. The only change is in phase-3, where the effect of L F on φ is reduced from 50 to 20 ([0, 0, 50, 0, 0, 0] to [0, 0, 20, 0, 0, 0]). Since for rough terrains, this coefficient may cause the front leg to have a delay in lift off which disturbs the sequence of phases (similar to Fig.3.8). The trajectory planner designed for quadruped robot finally modifies attack angles and leg lengths to have arc-shaped 34

41 Figure 4.5: Arc shape trajectory of legs after lift off which helps the robot to do locomotion on rough terrains more robustly. This specific shape is obtained by tuning time constants in trajectory transition law. trajectories. It does not manipulate main variables (x, y and φ). So with minor changes, as shown in the next part, the robot is robustly working compared to the case of feed-forward and feedback without trajectory planner. 4.4 Performance of feed-forward + feedback and the trajectory planner To test this closed-loop system, some simulations are performed on different rough terrains to verify stability. The results of these tests are shown on Fig.4.6. In this figure, the interesting point compared to Fig.3.13 is the much higher performance of quadruped robot compared to monopod as we see almost the same distances passed by robot after a specific time. Remember the case of monopod where the robot could not continue its way as roughness of the terrain increased. As expected, the higher the roughness measure, the higher the variance in the distance passed by the robot. Figure 4.6: Performance of control laws on rough terrain. Test of the robot for 50 seconds 5 times on terrains having different roughness measures. The terrain has a Gaussian distribution with zero-mean and specified variance sampled each 0.1. So far, we could stabilize the controller by proper design of feedback loop and also 35