LAB

<latex>{\fontsize{16pt}\selectfont \textbf{Development and Implementation of Optimized Walking Algorithms for Exoskeleton Robots}} </latex>

<latex>{\fontsize{12pt}\selectfont \textbf{[Adrian Zeberli]}} </latex>

<latex>{\fontsize{10pt}\selectfont \textit{[Master Project EEIT]}} </latex>

<latex> {\fontsize{12pt}\selectfont \textbf{Abstract} </latex>
The interaction between a robot and the environment is still a very challenging task since the robot has to deal with instantaneous changes of contact dynamics and unknown disturbances. The proposed framework focuses on the implementation of an automated trajectory optimization tool to generate different human walking behaviors. This is done in an off-line fashion where the environment is known and no disturbances arise. The selected approach is based on the contact-invariant optimization (CIO) ¹⁾ that has been extended with a walking specific cost term. This method has demonstrated high success for planning a wide range of motions for robots with complex dynamics. The optimal contact sequences are found automatically (usually manually done in other motion planning algorithms) as well as the movement trajectory are discovered by minimizing a task specific objective function. Using a simplified model of an exoskeleton robot, different motions such as walking forward, walking sideway and balancing on one foot have been generated with success. In a last step the solution of the walking forward motion has been tested in a simulated environment with gravity, showing very promising results.

<latex> {\fontsize{12pt}\selectfont \textbf{Contact-Invariant Optimization (CIO)} </latex>
Optimizing a movement trajectory involves searching the movement trajectory of the robot for the duration of $T$ simulation time steps, where the quality of the motion is measured with an objective function. It would be appealing to define the objective function in a way that the contact sequences are found automatically. Knowing these contact sets makes it easier to optimize the remaining variables for a fully defined motion. This is the key idea behind the CIO. The concept is based on an auxiliary decision variable which specifies when and where contacts are made. This variable is denoted as $c_i \geq 0$ for a potential contact <latex>$i$</latex> and has following characteristics:

• $c_i$ large: contact $i$ must be active
• $c_i$ small: contact $i$ is not active

The contact-related variables $c_i$ appear as weights in the objective function which influences on the one hand the contact points and on the other hand the dynamics. A natural way to enter the dynamics is through the contact forces which are only allowed at contact $i$ when the corresponding $c_i$ is large, but contact forces can also act when the associated end-effector is not in contact with a surface. This concept requires that all potential contact points of the robot (e.g. the feet and hands) are defined beforehand.
The movement trajectory is split into $k=1, \dots, K$ phases, where spline functions are used to approximate the character pose between the boundaries of each phase. Following features are used to describe the solution vector $\bm{s}$:
\begin{equation*}

\bm{s}=\left[\bm{x}_{1 \dots K}^\intercal \quad \dot{\bm{x}}_{1 \dots K}^\intercal \quad \bm{c}_{1 \dots K}^\intercal \right]^\intercal}

\end{equation*} where $\bm{c}_k=[c_{1,k} \quad \dots \quad c_{N,k}]^\intercal $ is a vector containing the auxiliary contact variable for all $N$ end-effectors. The pose of the robot at each phase $k$ is defined by: \begin{equation*}

\bm{x}_k=\left[\bm{x}_{b,k}^\intercal \quad \bm{p}_{e,k}^\intercal \right]^\intercal

\end{equation*} where the vector $\bm{x}_{b,k}=[\bm{p}_{b,k}^\intercal \quad \bm{r}_{b,k}^\intercal]^\intercal$ describes the position and orientation of the base and the vector $\bm{p}_{e,k}=[\bm{p}_{1,k}^\intercal \quad \dots \quad \bm{p}_{N,k}^\intercal]^\intercal$ the positions of all $N$ end-effectors, see figure. This way the optimizer can directly work with the end-effectors position to compute the distance to the next surface.

Features to describe the pose of the robot	Division of the trajectory into $K$ phases

The features of the solution vector $\bm{s}$ can then be used to define a continuous feature trajectory. Positions and velocities are used as spline knots from which position, velocity and acceleration can be computed analytically at any time in the trajectory. Using inverse kinematics, the pose of the robot is now well defined at any time as well. The variable $c_{i}$ is constant within each phase, which means that the associated end-effector is either in contact or not in contact during the whole phase, see figure. The interval step for the cost function evaluations can also be defined, and at the end of the trajectory these costs are summed up to obtain the total cost.
An objective function can now be defined which makes use of the spline functions. The goal of the CIO algorithm is to find the optimal solution $\bm{s}^*$ by minimizing the composite objective function: \begin{equation*}

L(\bm{s}) = L_{CI}(\bm{s})+L_{Physics}(\bm{s})+L_{Task}(\bm{s})+L_{Hint}(\bm{s})+L_{Lift}(\bm{s})

\end{equation*}

• $L_{CI}$ : The contact-invariant cost handles the contact activations using the contact vector $\bm{c}$. This cost forces the end-effector $i$ to be in contact with a surface and have no relative velocity when the corresponding contact variable $\bm{c}_i$ is large.
• $L_{Physics}$ : Physics is enforced by penalizing the squared residual of the equation of motion. The contact forces and actuated joint torques needed for this cost are recovered by solving a minimization problem that includes inverse dynamics and is influenced by the contact vector $\bm{c}$.
• $L_{Task}$ : The task cost enforces the goal of the movement. This is the only cost that needs to be changed to generate a different motion.
• $L_{Hint}$ : The hint cost is used to guide the solution to a region where the true cost can be optimized efficiently. This cost increases the postural stability of the robot by keeping the ZMP in the convex hull of the support region.
• $L_{Lift}$ : The lift cost shifts a moving end-effector away from any surface, producing a more “human-like” trajectory for the flight-phase motions.

The cost $L_{Physics}$ can get minimized by increasing the values of $\bm{c}$ and contact forces get generated at all end-effectors, including those which are not in contact. However that makes the contact-invariant cost $L_{CI}$ large. Hence a trade off can be found when only the variables $c$ of the end-effectors in contact have a a large value and contact forces can only be applied at these positions.
The problem does not contain any hard constraints. Instead, soft constraints are enforced by penalizing their value in the objective function. Since there is more than one minimum, four optimization sequences with different weightings of the cost functions are used to find the desired solution.

<latex> {\fontsize{12pt}\selectfont \textbf{Software and Implementation}} </latex>
The CIO algorithm and the simulation is implemented in C++ using the ADRL Control Toolbox (CT). CT provides a collection of tools for the usage of robotic control application such as dynamic solvers, kinematics, contact models, visualizer and simulation. The individual tools can be used separately or chained together according to the intended purpose.
CT has also an interface with the generated code of the Robotics Code Generator, short RobCoGen ²⁾. RobCoGen is used to generate robot specific source code such as transformation matrices, Jacobian matrices, forward dynamic algorithms and inverse dynamic algorithms. Still, inverse kinematics has to be defined manually.
Numerical optimization of the objective function is realized using Ceres Solver ³⁾. The solver can be used for unconstrained minimization problems and suits for different needs. A variety of optimization algorithms are offered, the Limited-memory-Broyden-Fletcher-Goldfarb-Shanno (LBFGS) is used in this thesis. The gradient needed for numerical optimization is approximated using finite differences.
The constraint optimization problem to find the contact forces and actuated joint torques is solved using levmar ⁴⁾ which is based on the Levenberg-Marquardt optimization algorithm. Constraints (linear equations, inequality and box constraints) can be handled by levmar. Analytical Jacobians are used in this thesis, but a finite difference approximation is also available.

Robot model	Animation versus simulation

Only the lower part of a biped robot is considered in the floating base model, as shown in the figure. No ankles nor feet are modeled, the end-effectors are simply point feet located at the end of each leg. This setup of the robot allows to place the foot in the 3D workspace, but the foot position and its orientation can not be chosen at the same time. Furthermore the total mass of the robot is concentrated at the base, massless links are assumed to simplify the dynamics.

The robot joint trajectories found by the CIO algorithm can violate physics (e.g. the joint limits and kinematic constraints). In order to obtain more practical insights on the results it would be nice to test the optimized motion in a simulated environment, where physical consistency is enforced. The resulting actuated joint torques found in the optimization may be used as feed-forward control in this case. To increase the tracking performance of joint trajectories, a feedback control is added in form of a PD joint-space controller to the resulting reference command as described in the figure above.

<latex> {\fontsize{12pt}\selectfont \textbf{Experiments}} </latex>
2m forward and 2m sideway walking, as well as an intermediate task (balancing on one foot) have been studied. For all these motions the same objective function with a different task cost could be used, unfortunately the weights for the different cost terms needed small adjustments. The optimizer took around 900 iterations. The synthesized motions look very realistic in animation, but some undesired behaviors occurred, such as unbalanced pose of the robot, collisions with other body parts or huge contact moments.

Walking forward (animation)	Walking sideways (animation)

The output of 2m forward walking optimization is used for the simulation. The robot manages to walk three steps until it looses balance and falls backwards. This unsuccessful walking was somehow expected since the dynamics are only enforced using soft constraints. But the outcome looks very promising and a better result could probably be achieved with a more sophisticated controller that also controls the floating base position and orientation and not only the joint positions in the feedback loop.

Balancing on one foot (animation)	Walking forward (simulation)

<latex> {\fontsize{12pt}\selectfont \textbf{Conclusion}} </latex>
The presented method showed good performance for most of the studied behaviors. A movement trajectory and the contact sequence could usually be found. Finding the weights for each cost functions was the most challenging part, and unfortunately they needed small adaption for the different motions. The trade-off between enforcing the physics and being in contact is strongly influenced by these weights.
The algorithm is presented in a very general way so that it can be used with other robots where only dynamic specific functions need to be changed. The potential contact points on the robot must also be pre-defined. The algorithm has only been tested on a robot model with massless links, this simplifies the dynamics but can lead to energy inefficient motions.
The main advantage of this algorithm is the ability to discover the optimal contact sequence, which is manually done for other motion planning algorithms. Thus, the CIO algorithm can be used to figure out the contact sequence and use this information as input for a more strict algorithm where physics consistency is enforced in a hard constraint. This would generate more realistic and robust trajectories and torques to be applied to a real robot.