## 3 Modeling and Trajectory Planning
### Trajectory Planning  
One aim in robotics is the more straightforward commissioning of robotic systems. The robots should adapt their behavior to an unknown environment, for example, by avoiding obstacles or automatic detection and modification of end-effector target positions. Tremendous improvements were made over the last years in the robotic field. Advanced trajectory planning algorithms were developed, see for example Reference [33]. However, trajectory planning is not the main focus of this paper. Major aim of this paper is the evaluation of the MPC- and vision-based control in trajectory tracking. For a meaningful evaluation, the robot trajectory should include a motion reversal of an axis. Furthermore, due to the combination with a vision system, an integrated obstacle avoidance mechanism in trajectory planning would be an asset for further studies.  
So far a simple semi-automatic procedure is developed to prepare continuous trajectories in an offline step. The workflow of trajectory planning is depicted in Figure 6. The procedure is the following:  
1. Based on the image of the scene, which serves as a template,2. the user specifies, that is, draws, the desired trajectory as a Bezier curve for the end-effector in Cartesian space in the program Inkscape. So far, the human user ensures an obstacle avoidance by drawing a suitable path.
3. The cubic splines of the Inkscape path are then scaled and resampled within the Cartesian space in Matlab.
4. From the resulting set of end-effector points along the trajectory \(L\) sample points are chosen.
5. Afterwards, feasible joint coordinates are calculated for each of the \(L\) sample points with the help of inverse kinematics, see Equation (10), and the Neweul-M\({}^{2}\) model of the robot, and the desired velocity at the intermediate points is specified.
6. For every joint those calculated joint coordinates are then combined into a continuous motion via a spline as described in Reference [34]. Instead of using a single polynomial of degree \(L-1\) for interpolation, which often leads to oscillations, \(L-1\) cubic polynomials are used to interpolate the trajectory parts. As stated above, the resulting function is a spline with minimal curvature. The degree of those polynomials is chosen to three to guarantee continuity of the resulting velocity and acceleration profile. In between those calculations an optimization process minimizes the overall time \(T\) needed for the trajectory subject to not exceeding maximum joint velocities \(\dot{q}_{i,\max}\) and accelerations \(\ddot{q}_{i,\max}\) for each joint \(i\). At first, the time each of the joints needs for the trajectory is minimized by computing the intermediate times \(T_{k,i}\) for each joint \(i\) and polynomial segment \(k=1,\ldots,L-1\). Afterwards, the maximum of the times \(T_{k,i}\) needed by each individual joint is taken as the resulting intermediate time for all joints, that is, \[\begin{array}{rcl}T_{k}&=&\max\limits_{i}&T_{k,i}\\ &&\text{s.t.}&|\dot{q}_{i}|\leq\dot{q}_{i,\max}\\ &&|\dot{q}_{i}|\leq\ddot{q}_{i,\max}\\ \end{array}\qquad\qquad\text{and}\qquad\qquad T=\sum_{k}T_{k}.\] (13)
7. The resampled results, containing joint positions, velocities, and accelerations as well as a time vector, are then simulated, animated, and checked for collisions in order to see offline if, for example, the joint restrictions are kept. Rank deficiencies in the Jacobian matrix \(\mathbf{J}(\mathbf{q})=\partial\mathbf{r}_{\text{ee}}(\mathbf{q})/\partial\mathbf{q}\), provided from Neweul-M\({}^{2}\) in a symbolical fashion, indicate singular configurations. Those singularities should be avoided in the trajectory. Finally, the trajectory can be processed on the actual robot.