MPC Algorithm

The main idea of MPC can be summarized as follows: A large optimal control problem is divided into several smaller sub-problems for a finite prediction hori-zon Thorizon starting from the current time increment t^k. Once the sub-problem is solved, the initial part (usually smaller than the prediction horizon) of the cal-culated input trajectory u^?(τ_k⁰) with τ_k⁰ := [t_k,t_k +∆tmpc] and ∆tmpc < T_horizon is applied to the system while the sub-problem of the subsequent time window τ_k+1is being solved. The basic scheme is depicted in alg. 6.

Algorithm 6Model Predictive Control(MPC) Algorithm tstart =t⁰, T_end =t^N,∆tmpc =t^k+1−^tk

fork =0 toNdo

τ_k ←[t_k,t^k+T_horizon] τ_k⁰ ←[t_k,t^k+∆tmpc]

determineu^?(τ_k)usingu⁰(τ_k) =u_previous^? (τ_k) applyu^?(τ_k⁰)

end for

5.3.2 Implementation

Real Time Capability

In order to assure the real time capability of the implementation, each calculation routine should provide a feasible solution within a fixed time span. The numeri-cal solution of the dynamic optimization problem contradicts this hard real time constraint since the number of iterations to convergence varies. Assuming that each iteration requires a similar computation time, this issue can obviously be resolved by limiting the number of iterations. This is only applicable in case the

"not yet converged" orsub-optimalsolution still provides a feasible trajectory. This requirement is inherently fulfilled by the considered system formulation shown in section 5.2.1. Using a "warm start", i.e. the previous solutionu^?_previousinitializes the subsequent optimization problem, convergence to the optimal solution over several iterations is shown by Graichen and Kugi (2010).

In Schütz, Buschmann, et al. (2014) the author reports the real time implemen-tation for the 9-DOF CROPS manipulator allowing one iteration per sub-problem.

Thereby, the prediction horizon is 0.15 s with a sampling time of 10 ms.

Continuity Considerations

While solving the optimization problem for a moving horizon and subsequently applying the optimal control inputsu^?(τ⁰), one has to assure that the overall con-trol trajectoryu^?(t)is continuous. Solving the OCP for the system formulation on the velocity level eq. (5.5) by a MPC scheme illustrates this problem: As depicted in fig. 5.9-leftfor a single joint velocity of a 4-DOF collision avoidance example, the resulting trajectory (blue) is non-smooth, although the single trajectories solv-ing the sub-problem are individually smooth. This can be explained by the fact that the solution has non-zero initial nullspace velocities since nullspace accelera-tions are not taken into account by the cost function. This issue is encountered in the extended system formulation presented in section 5.1.3. Using a simple state augmentation, the new system input ˆu for accelerations instead of velocities in nullspace is defined. This extension results in continuity on the velocity level as shown in fig. 5.9-right.

Prediction Horizon

The prediction horizonT_horizonhas a significant influence on the optimization re-sult. This insight is evident: The larger the prediction horizon, the earlier the optimization takes future events into account. I.e., the manipulator can adapt its motion earlier to choose a more favorable configuration. Regarding the squared joint velocities q˙^Tq˙ of the self-collision scenario (depicted in fig. 5.10), this effect

0 1 2 3 4 5

−¹

−^0.8

−^0.6

−^0.4

−^0.2 0

Time[s]

˙q[rad/s]

Velocity

0 1 2 3 4 5

Time[s] Acceleration

Instantaneous Trajectory Single Trajectory

Figure 5.9: Joint velocity for the 4-DOF robot at the collision avoidance scenario us-ing the MPC solution. Thereby, the system formulation with an input on the velocity level (left) shows discontinuities while the one on the acceleration level as presented in section 5.1.3 remains smooth (right).

can be quantified: The manipulator moves on a straight-line task space path from bottom to top. While moving, the manipulator prevents a self-collision of the arm with the prismatic joint by an evasive nullspace movement. Following the instan-taneous solution (gray) as shown in fig. 5.11, high joint velocities are needed to avoid the collision. Using a large prediction horizon (e.g.T_horizon =2 s,green), the collision is anticipated early and high velocities are prevented. By comparison, the smaller horizon ofT_horizon =0.15 s (blue) yields a much smaller improvement compared to the larger horizon.

5.4 Remarks

In this chapter, a novel method for the optimization of inverse kinematics for redundant robots has been introduced. By applying the method in a moving horizon calculation scheme, a predictive optimization of the respective nullspace motion can be applied in real time. Thus, future constraints and events can already be taken into account during the calculation of the current time incre-ment. This method is based on a suitable system formulation for an indirect op-timization scheme. Two contributions of this thesis, the decoupling and state augmentation enable its efficient solution. Besides the numerical solution as a TPBVP, the (conjugate) gradient method is shown in greater detail. Using the example of a redundant 4-DOF pendulum, the efficiency and robustness of sev-eral conjugate gradient versions and adaptive step size algorithms are examined.

By evaluating numerous random scenarios of this example, the combination of the Fletcher/Reeves conjugate gradient and the adaptive line search algorithm

Figure 5.10:Collision Avoidance scenario for 9-DOF harvesting manipulator (prototype 1). The end effector moves on a straight-line path from bottom to top. A self-collision of the arm with the manipulator’s prismatic joint is avoided by suitable nullspace move-ments.

showed the best results regarding success rate and computation time. Further-more, the real time application of the proposed method in a moving horizon scheme was introduced.

The solution of optimal control problems by an indirect method has several advantages: First, it offers highly accurate solutions to the dynamic problem.

Second, the numerical solution e.g. by the conjugate gradient method is compu-tationally efficient. Iterations can be interrupted before the problem is fully con-verged, since the current estimate of the optimal solution is always applicable.

This fact is of particular importance regarding real time applications. However, several drawbacks have to be considered when applying the proposed method.

The trajectory is optimizedlocallyaround the current estimate of the optimal so-lution. This makes it sensitive to the initial guess. Since optimization takes place

0 1 2 3 4 5

0 0.5 1

Time[s]

˙qT ˙q[rad2 /s2]

Instantaneous T_horizon=0.15 s T_horizon=2 s

Figure 5.11: Squared joint velocities of revolute joints in self-collision scenario for the harvesting robot calculated by local inverse kinematics and MPC approach with a mov-ing horizon of0.15sand2s.

only in nullspace for the proposed system formulation, an initialization of zeros or using the gradient∇qHof the current timestep leads to good results. Although the optimization may end in a local minimum, the solution can be expected to be more favorable compared with the instantaneous calculation scheme (cf. sec-tion 4.1.2). Another drawback is the sensitivity of the method to the relative weights within the cost functional. The coefficientsζ_(·) have to be adjusted care-fully according to the respective system and scenario. Furthermore, constraints are only taken into account as soft constraints by penalty terms. In the experi-ments, the latter fact has shown to be no limitation for practical usage.

While the proposed approach shows great potential for the efficient and op-timized redundancy resolution, two main aspects may be addressed in further research: the improvement of the collision avoidance formulation and the exten-sion of the prediction horizon. Using strictly convex geometries and/or a hierar-chical scheme from large to more detailed geometry models may be beneficial for convergence of the collision avoidance cost function. A more efficient implemen-tation of the proposed approach and a faster CPU allows a larger prediction time window and/or more iterations at each planning step.

Trajectory Optimization by Direct